Voltage sensor angle residual mod 2pi

docs/user_manual/components.md

@@ -627,10 +627,12 @@ A sensor only has output for state estimation. For other calculation types, sens
 $$
    \begin{eqnarray}
         & u_{\text{residual}} = u_{\text{measured}} - u_{\text{state}} \\
-        & \theta_{\text{residual}} = \theta_{\text{measured}} - \theta_{\text{state}}
+        & \theta_{\text{residual}} = \theta_{\text{measured}} - \theta_{\text{state}} \pmod{2 \pi}
    \end{eqnarray}
 $$
 
+The $\pmod{2\pi}$ is handled such that $-\pi \lt \theta_{\text{angle},\text{residual}} \leq \pi$.
+
 ### Generic Power Sensor
 
 * type name: `generic_power_sensor`

power_grid_model_c/power_grid_model/include/power_grid_model/common/three_phase_tensor.hpp

@@ -176,6 +176,14 @@ inline ComplexValue<asymmetric_t> phase_shift(ComplexValue<asymmetric_t> const&
     return {phase_shift(m(0)), phase_shift(m(1)), phase_shift(m(2))};
 }
 
+// arg(e^(i * phase)) = phase (mod 2pi). By convention restrict to [-pi, pi].
+inline auto phase_mod_2pi(double phase) {
+    return RealValue<symmetric_t>{arg(ComplexValue<symmetric_t>{exp(1.0i * phase)})};
+}
+inline auto phase_mod_2pi(RealValue<asymmetric_t> const& phase) {
+    return RealValue<asymmetric_t>{arg(ComplexValue<asymmetric_t>{exp(1.0i * phase)})};
+}
+
 // calculate kron product of two vector
 inline double vector_outer_product(double x, double y) { return x * y; }
 inline DoubleComplex vector_outer_product(DoubleComplex x, DoubleComplex y) { return x * y; }

power_grid_model_c/power_grid_model/include/power_grid_model/component/current_sensor.hpp

@@ -174,9 +174,7 @@ template <symmetry_tag current_sensor_symmetry_> class CurrentSensor : public Ge
             }
         }();
         output.i_residual = (cabs(i_measured_complex) - cabs(i_output)) * base_current_;
-        // arg(e^i * u_angle) = u_angle in (-pi, pi]
-        auto const unbounded_i_angle_residual = arg(i_measured_complex) - arg(i_output);
-        output.i_angle_residual = arg(ComplexValue<sym_calc>{exp(1.0i * unbounded_i_angle_residual)});
+        output.i_angle_residual = phase_mod_2pi(arg(i_measured_complex) - arg(i_output));
         return output;
     }
 };

power_grid_model_c/power_grid_model/include/power_grid_model/component/voltage_sensor.hpp

@@ -151,7 +151,7 @@ template <symmetry_tag sym> class VoltageSensor : public GenericVoltageSensor {
         } else {
             value.u_residual = (real(u1_measured) - cabs(u)) * u_rated_;
         }
-        value.u_angle_residual = arg(u1_measured) - arg(u);
+        value.u_angle_residual = phase_mod_2pi(arg(u1_measured) - arg(u));
         return value;
     }
 
@@ -160,7 +160,7 @@ template <symmetry_tag sym> class VoltageSensor : public GenericVoltageSensor {
         value.id = id();
         value.energized = 1;
         value.u_residual = (u_measured_ - cabs(u)) * u_rated_ / sqrt3;
-        value.u_angle_residual = u_angle_measured_ - arg(u);
+        value.u_angle_residual = phase_mod_2pi(u_angle_measured_ - arg(u));
         return value;
     }
 };

tests/cpp_unit_tests/test_three_phase_tensor.cpp

@@ -206,6 +206,38 @@ TEST_CASE("Three phase tensor") {
         CHECK(is_nan(vec(1)));
         CHECK(vec(2) == 6.0);
     }
+    SUBCASE("phase_mod_2pi") {
+        auto const check = [](double value) {
+            CAPTURE(value);
+            CHECK_GE(value, -pi);
+            CHECK_LE(value, pi);
+            if (value != pi && value != -pi) {
+                CHECK(phase_mod_2pi(value) == doctest::Approx(value));
+            }
+        };
+        auto const check_asym = [&check](RealValue<asymmetric_t> const& value) {
+            for (Idx i : {0, 1, 2}) {
+                CAPTURE(i);
+                check(value(i));
+            }
+        };
+        check(phase_mod_2pi(0.0));
+        check(phase_mod_2pi(2.0 * pi));
+        check(phase_mod_2pi(2.0 * pi + 1.0));
+        check(phase_mod_2pi(-1.0));
+        check(phase_mod_2pi(-pi));
+        check(phase_mod_2pi(pi));
+        check(phase_mod_2pi(-3.0 * pi));
+        check(phase_mod_2pi(3.0 * pi));
+        check(phase_mod_2pi(pi * (1.0 + std::numeric_limits<double>::epsilon())));
+        check(phase_mod_2pi(pi * (1.0 - std::numeric_limits<double>::epsilon())));
+        check(phase_mod_2pi(-pi * (1.0 + std::numeric_limits<double>::epsilon())));
+        check(phase_mod_2pi(-pi * (1.0 - std::numeric_limits<double>::epsilon())));
+
+        check_asym(phase_mod_2pi(RealValue<asymmetric_t>{0.0, 2.0 * pi, 2.0 * pi + 1.0}));
+        check_asym(phase_mod_2pi(RealValue<asymmetric_t>{0.0, 2.0 * pi, 2.0 * pi + 1.0}));
+        check_asym(phase_mod_2pi(RealValue<asymmetric_t>{0.0, 2.0 * pi, 2.0 * pi + 1.0}));
+    }
 }
 
 } // namespace power_grid_model

tests/cpp_unit_tests/test_voltage_sensor.cpp

@@ -363,6 +363,47 @@ TEST_CASE("Test voltage sensor") {
             CHECK(sym_voltage_sensor_asym_output.u_angle_residual[2] == doctest::Approx(-0.2));
         }
 
+        SUBCASE("Angle = ± pi ∓ 0.1") {
+            RealValue<symmetric_t> const u_measured{10.1e3};
+            RealValue<symmetric_t> const u_angle_measured{pi - 0.1};
+            double const u_sigma = 1.0;
+            double const u_rated = 10.0e3;
+
+            VoltageSensorInput<symmetric_t> voltage_sensor_input{};
+            voltage_sensor_input.id = 0;
+            voltage_sensor_input.measured_object = 1;
+            voltage_sensor_input.u_sigma = u_sigma;
+            voltage_sensor_input.u_measured = u_measured;
+            voltage_sensor_input.u_angle_measured = u_angle_measured;
+
+            VoltageSensor<symmetric_t> const voltage_sensor{voltage_sensor_input, u_rated};
+
+            ComplexValue<symmetric_t> const u_calc_sym{1.02 * exp(1i * (-pi + 0.1))};
+            VoltageSensorOutput<symmetric_t> sym_voltage_sensor_sym_output =
+                voltage_sensor.get_output<symmetric_t>(u_calc_sym);
+
+            ComplexValue<asymmetric_t> const u_calc_asym{1.02 * exp(1i * (-pi + 0.1)), 1.03 * exp(1i * (-pi + 0.2)),
+                                                         1.04 * exp(1i * (-pi + 0.3))};
+            VoltageSensorOutput<asymmetric_t> sym_voltage_sensor_asym_output =
+                voltage_sensor.get_output<asymmetric_t>(u_calc_asym);
+
+            // Check sym output
+            CHECK(sym_voltage_sensor_sym_output.id == 0);
+            CHECK(sym_voltage_sensor_sym_output.energized == 1);
+            CHECK(sym_voltage_sensor_sym_output.u_residual == doctest::Approx(-100.0));
+            CHECK(sym_voltage_sensor_sym_output.u_angle_residual == doctest::Approx(-0.2).epsilon(1e-12));
+
+            // Check asym output
+            CHECK(sym_voltage_sensor_asym_output.id == 0);
+            CHECK(sym_voltage_sensor_asym_output.energized == 1);
+            CHECK(sym_voltage_sensor_asym_output.u_residual[0] == doctest::Approx(-100.0 / sqrt3));
+            CHECK(sym_voltage_sensor_asym_output.u_residual[1] == doctest::Approx(-200.0 / sqrt3));
+            CHECK(sym_voltage_sensor_asym_output.u_residual[2] == doctest::Approx(-300.0 / sqrt3));
+            CHECK(sym_voltage_sensor_asym_output.u_angle_residual[0] == doctest::Approx(-0.2).epsilon(1e-12));
+            CHECK(sym_voltage_sensor_asym_output.u_angle_residual[1] == doctest::Approx(-0.3));
+            CHECK(sym_voltage_sensor_asym_output.u_angle_residual[2] == doctest::Approx(-0.4));
+        }
+
         SUBCASE("Angle = nan") {
             RealValue<symmetric_t> const u_measured{10.1e3};
             RealValue<symmetric_t> const u_angle_measured{nan};