Newton Raphson State Esimation Implementation

power_grid_model_c/power_grid_model/include/power_grid_model/calculation_parameters.hpp

@@ -112,7 +112,7 @@ using BranchIdx = std::array<Idx, 2>;
 using Branch3Idx = std::array<Idx, 3>;
 
 struct MathModelTopology {
-    Idx slack_bus_{};
+    Idx slack_bus{};
     std::vector<double> phase_shift;
     std::vector<BranchIdx> branch_bus_idx;
     std::vector<BranchIdx> fill_in;

power_grid_model_c/power_grid_model/include/power_grid_model/math_solver/block_matrix.hpp

@@ -62,6 +62,8 @@ class Block : public block_trait<T, sym, is_tensor, n_sub_block>::ArrayType {
             return (*this)(get_asym_row_idx<r>(), get_asym_col_idx<c>());
         }
     }
+
+    void clear() { *this = Block<T, sym, is_tensor, n_sub_block>{}; }
 };
 
 } // namespace power_grid_model::math_solver

power_grid_model_c/power_grid_model/include/power_grid_model/math_solver/iterative_linear_se_solver.hpp

@@ -117,7 +117,7 @@ template <bool sym> class IterativeLinearSESolver {
             sub_timer = Timer(calculation_info, 2225, "Solve sparse linear equation (pre-factorized)");
             sparse_solver_.solve_with_prefactorized_matrix(data_gain_, perm_, x_rhs_, x_rhs_);
             sub_timer = Timer(calculation_info, 2226, "Iterate unknown");
-            max_dev = iterate_unknown(output.u, measured_values.has_angle_measurement());
+            max_dev = iterate_unknown(output.u, measured_values.has_angle());
         };
 
         // calculate math result
@@ -167,7 +167,7 @@ template <bool sym> class IterativeLinearSESolver {
                 Idx const col = col_indices[data_idx_lu];
                 // get a reference and reset block to zero
                 ILSEGainBlock<sym>& block = data_gain_[data_idx_lu];
-                block = ILSEGainBlock<sym>{};
+                block.clear();
                 // get data idx of y bus,
                 // skip for a fill-in
                 Idx const data_idx = y_bus.map_lu_y_bus()[data_idx_lu];
@@ -259,14 +259,14 @@ template <bool sym> class IterativeLinearSESolver {
         std::vector<BranchIdx> const branch_bus_idx = y_bus.math_topology().branch_bus_idx;
         // get generated (measured/estimated) voltage phasor
         // with current result voltage angle
-        ComplexValueVector<sym> u = measured_value.voltage(current_u);
+        ComplexValueVector<sym> u = linearize_voltage(current_u, measured_value);
 
         // loop all bus to fill rhs
         for (Idx bus = 0; bus != n_bus_; ++bus) {
             Idx const data_idx = y_bus.bus_entry()[bus];
             // reset rhs block to fill values
             ILSERhs<sym>& rhs_block = x_rhs_[bus];
-            rhs_block = ILSERhs<sym>{};
+            rhs_block.clear();
             // fill block with voltage measurement
             if (measured_value.has_voltage(bus)) {
                 // eta += u / variance
@@ -315,18 +315,18 @@ template <bool sym> class IterativeLinearSESolver {
         }
     }
 
-    double iterate_unknown(ComplexValueVector<sym>& u, bool has_angle_measurement) {
+    double iterate_unknown(ComplexValueVector<sym>& u, bool has_angle) {
         double max_dev = 0.0;
         // phase shift anti offset of slack bus, phase a
         // if no angle measurement is present
         DoubleComplex const angle_offset = [&]() -> DoubleComplex {
-            if (has_angle_measurement) {
+            if (has_angle) {
                 return 1.0;
             }
             if constexpr (sym) {
-                return cabs(x_rhs_[math_topo_->slack_bus_].u()) / x_rhs_[math_topo_->slack_bus_].u();
+                return cabs(x_rhs_[math_topo_->slack_bus].u()) / x_rhs_[math_topo_->slack_bus].u();
             } else {
-                return cabs(x_rhs_[math_topo_->slack_bus_].u()(0)) / x_rhs_[math_topo_->slack_bus_].u()(0);
+                return cabs(x_rhs_[math_topo_->slack_bus].u()(0)) / x_rhs_[math_topo_->slack_bus].u()(0);
             }
         }();
 
@@ -350,6 +350,32 @@ template <bool sym> class IterativeLinearSESolver {
         std::tie(output.load_gen, output.source) =
             measured_value.calculate_load_gen_source(output.u, output.bus_injection);
     }
+
+    // getter of voltage value for all buses
+    // for no measurement, the voltage phasor is used from the current iteration
+    // for magnitude only measurement, angle is added from the current iteration
+    // for magnitude and angle measurement, the measured phasor is returned
+    ComplexValueVector<sym> linearize_voltage(ComplexValueVector<sym> const& current_u,
+                                              MeasuredValues<sym> const& measured_values) const {
+        ComplexValueVector<sym> u(current_u.size());
+
+        for (Idx bus = 0; bus != static_cast<Idx>(current_u.size()); ++bus) {
+            // no measurement
+            if (!measured_values.has_voltage(bus)) {
+                u[bus] = current_u[bus];
+            }
+            // no angle measurement
+            else if (!measured_values.has_angle_measurement(bus)) {
+                u[bus] = real(measured_values.voltage(bus)) * current_u[bus] /
+                         cabs(current_u[bus]); // U / |U| to get angle shift
+            }
+            // full measurement
+            else {
+                u[bus] = measured_values.voltage(bus);
+            }
+        }
+        return u;
+    }
 };
 
 template class IterativeLinearSESolver<true>;

power_grid_model_c/power_grid_model/include/power_grid_model/math_solver/math_solver.hpp

@@ -64,7 +64,7 @@ template <bool sym> class MathSolver {
 
         switch (calculation_method) {
         case default_method:
-            [[fallthrough]]; // use Newton-Raphson by default
+            [[fallthrough]]; // use iterative linear by default
         case iterative_linear:
             return run_state_estimation_iterative_linear(input, err_tol, max_iter, calculation_info, y_bus);
         case newton_raphson:

power_grid_model_c/power_grid_model/include/power_grid_model/math_solver/measured_values.hpp

@@ -57,47 +57,24 @@ template <bool sym> class MeasuredValues {
         normalize_variance();
     }
 
-    // checker of measured data, return true if measurement is available
-    bool has_voltage(Idx bus) const { return idx_voltage_[bus] >= 0; }
-    bool has_bus_injection(Idx bus) const { return bus_injection_[bus].idx_bus_injection >= 0; }
-    bool has_branch_from(Idx branch) const { return idx_branch_from_power_[branch] >= 0; }
-    bool has_branch_to(Idx branch) const { return idx_branch_to_power_[branch] >= 0; }
-    bool has_shunt(Idx shunt) const { return idx_shunt_power_[shunt] >= 0; }
-    bool has_load_gen(Idx load_gen) const { return idx_load_gen_power_[load_gen] >= 0; }
-    bool has_source(Idx source) const { return idx_source_power_[source] >= 0; }
-    bool has_angle() const { return n_angle_ > 0; }
+    constexpr bool has_angle() const { return n_voltage_angle_measurements_ > 0; }
+    constexpr bool has_voltage_measurements() const { return n_voltage_measurements_ > 0; }
+
+    constexpr bool has_voltage(Idx bus) const { return idx_voltage_[bus] >= 0; }
+    constexpr bool has_angle_measurement(Idx bus) const { return !is_nan(imag(voltage(bus))); }
+    constexpr bool has_bus_injection(Idx bus) const { return bus_injection_[bus].idx_bus_injection >= 0; }
+    constexpr bool has_branch_from(Idx branch) const { return idx_branch_from_power_[branch] >= 0; }
+    constexpr bool has_branch_to(Idx branch) const { return idx_branch_to_power_[branch] >= 0; }
+    constexpr bool has_shunt(Idx shunt) const { return idx_shunt_power_[shunt] >= 0; }
+    constexpr bool has_load_gen(Idx load_gen) const { return idx_load_gen_power_[load_gen] >= 0; }
+    constexpr bool has_source(Idx source) const { return idx_source_power_[source] >= 0; }
 
     // getter of measurement and variance
     // if the obj is not measured, it is undefined behaviour to call this function
     // use checker first
 
-    // getter of voltage variance
-    double voltage_var(Idx bus) const { return voltage_main_value_[idx_voltage_[bus]].variance; }
-    // getter of voltage value for all buses
-    // for no measurement, the voltage phasor is used from the current iteration
-    // for magnitude only measurement, angle is added from the current iteration
-    // for magnitude and angle measurement, the measured phasor is returned
-    ComplexValueVector<sym> voltage(ComplexValueVector<sym> const& current_u) const {
-        ComplexValueVector<sym> u(current_u.size());
-        for (Idx bus = 0; bus != static_cast<Idx>(current_u.size()); ++bus) {
-            // no measurement
-            if (idx_voltage_[bus] == unmeasured) {
-                u[bus] = current_u[bus];
-            }
-            // no angle measurement
-            else if (is_nan(imag(voltage_main_value_[idx_voltage_[bus]].value))) {
-                u[bus] = real(voltage_main_value_[idx_voltage_[bus]].value) * current_u[bus] /
-                         cabs(current_u[bus]); // U / |U| to get angle shift
-            }
-            // full measurement
-            else {
-                u[bus] = voltage_main_value_[idx_voltage_[bus]].value;
-            }
-        }
-        return u;
-    }
-
-    // power measurement
+    constexpr double voltage_var(Idx bus) const { return voltage_main_value_[idx_voltage_[bus]].variance; }
+    constexpr auto const& voltage(Idx bus) const { return voltage_main_value_[idx_voltage_[bus]].value; }
     constexpr auto const& bus_injection(Idx bus) const {
         return power_main_value_[bus_injection_[bus].idx_bus_injection];
     }
@@ -109,9 +86,13 @@ template <bool sym> class MeasuredValues {
     constexpr auto const& load_gen_power(Idx load_gen) const { return extra_value_[idx_load_gen_power_[load_gen]]; }
     constexpr auto const& source_power(Idx source) const { return extra_value_[idx_source_power_[source]]; }
 
+    constexpr auto first_voltage_measurement() const {
+        assert(has_voltage_measurements());
+        return first_voltage_measurement_;
+    }
+
     // getter mean angle shift
     RealValue<sym> mean_angle_shift() const { return mean_angle_shift_; }
-    constexpr bool has_angle_measurement() const { return n_angle_ > 0; }
 
     // calculate load_gen and source flow
     // with given bus voltage and bus current injection
@@ -177,11 +158,14 @@ template <bool sym> class MeasuredValues {
     // relevant for extra value
     IdxVector idx_load_gen_power_;
     IdxVector idx_source_power_;
-    // number of angle measurement
-    Idx n_angle_{};
+
+    Idx n_voltage_measurements_{};
+    Idx n_voltage_angle_measurements_{}; // number of angle measurement
+
     // average angle shift of voltages with angle measurement
     // default is zero is no voltage has angle measurement
     RealValue<sym> mean_angle_shift_;
+    Idx first_voltage_measurement_{};
 
     constexpr MathModelTopology const& math_topology() const { return *math_topology_; }
 
@@ -227,9 +211,14 @@ template <bool sym> class MeasuredValues {
         }
 
         // assign a meaningful mean angle shift, if at least one voltage has angle measurement
-        if (n_angle_ > 0) {
-            mean_angle_shift_ = angle_cum / n_angle_;
+        if (has_angle()) {
+            mean_angle_shift_ = angle_cum / n_voltage_angle_measurements_;
         }
+
+        auto const is_measured = [](auto const& value) { return value >= 0; };
+        n_voltage_measurements_ = std::ranges::count_if(idx_voltage_, is_measured);
+        first_voltage_measurement_ =
+            std::distance(idx_voltage_.begin(), std::ranges::find_if(idx_voltage_, is_measured));
     }
 
     RealValue<sym> process_bus_voltage_measurements(Idx bus, IdxRange const& sensors,
@@ -256,7 +245,7 @@ template <bool sym> class MeasuredValues {
             idx_voltage_[bus] = static_cast<Idx>(voltage_main_value_.size());
             voltage_main_value_.push_back(aggregated);
             if (angle_measured) {
-                ++n_angle_;
+                ++n_voltage_angle_measurements_;
                 // accumulate angle, offset by intrinsic phase shift
                 angle_cum = arg(aggregated.value * std::exp(-1.0i * math_topology().phase_shift[bus]));
             }
@@ -401,11 +390,7 @@ template <bool sym> class MeasuredValues {
             accumulated_inverse_variance += inv_variance;
             if constexpr (only_magnitude) {
                 ComplexValue<sym> abs_value = piecewise_complex_value<sym>(DoubleComplex{0.0, nan});
-                if (is_nan(imag(measurement.value))) {
-                    abs_value += real(measurement.value); // only keep real part
-                } else {
-                    abs_value += cabs(measurement.value); // get abs of the value
-                }
+                abs_value += cabs_or_real<sym>(measurement.value);
                 accumulated_value += abs_value * inv_variance;
             } else {
                 // accumulate value
@@ -520,10 +505,11 @@ template <bool sym> class MeasuredValues {
         }
 
         // scale
-        std::ranges::for_each(voltage_main_value_, [min_var](auto& x) { x.variance /= min_var; });
-        std::ranges::for_each(power_main_value_, [min_var](auto& x) {
-            x.p_variance /= min_var;
-            x.q_variance /= min_var;
+        auto inv_norm_var = 1.0 / min_var;
+        std::ranges::for_each(voltage_main_value_, [inv_norm_var](auto& x) { x.variance *= inv_norm_var; });
+        std::ranges::for_each(power_main_value_, [inv_norm_var](auto& x) {
+            x.p_variance *= inv_norm_var;
+            x.q_variance *= inv_norm_var;
         });
     }