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 = {}; }
 };
 
 } // namespace power_grid_model::math_solver

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

@@ -6,9 +6,11 @@
 #ifndef POWER_GRID_MODEL_COMMON_SOLVER_FUNCTIONS_HPP
 #define POWER_GRID_MODEL_COMMON_SOLVER_FUNCTIONS_HPP
 
+#include "measured_values.hpp"
 #include "y_bus.hpp"
 
 #include "../calculation_parameters.hpp"
+#include "../exception.hpp"
 
 namespace power_grid_model::math_solver::detail {
 
@@ -75,10 +77,10 @@ inline void calculate_load_gen_result(IdxRange const& load_gens, Idx bus_number,
 template <bool sym, typename LoadGenFunc>
     requires std::invocable<std::remove_cvref_t<LoadGenFunc>, Idx> &&
              std::same_as<std::invoke_result_t<LoadGenFunc, Idx>, LoadGenType>
-inline void calculate_result(YBus<sym> const& y_bus, PowerFlowInput<sym> const& input,
-                             grouped_idx_vector_type auto const& sources_per_bus,
-                             grouped_idx_vector_type auto const& load_gens_per_bus, MathOutput<sym>& output,
-                             LoadGenFunc&& load_gen_func) {
+inline void calculate_pf_result(YBus<sym> const& y_bus, PowerFlowInput<sym> const& input,
+                                grouped_idx_vector_type auto const& sources_per_bus,
+                                grouped_idx_vector_type auto const& load_gens_per_bus, MathOutput<sym>& output,
+                                LoadGenFunc&& load_gen_func) {
     assert(sources_per_bus.size() == load_gens_per_bus.size());
 
     // call y bus
@@ -97,6 +99,16 @@ inline void calculate_result(YBus<sym> const& y_bus, PowerFlowInput<sym> const&
     output.bus_injection = y_bus.calculate_injection(output.u);
 }
 
+template <bool sym>
+inline void calculate_se_result(YBus<sym> const& y_bus, MeasuredValues<sym> const& measured_value,
+                                MathOutput<sym>& output) {
+    // call y bus
+    output.branch = y_bus.template calculate_branch_flow<BranchMathOutput<sym>>(output.u);
+    output.shunt = y_bus.template calculate_shunt_flow<ApplianceMathOutput<sym>>(output.u);
+    output.bus_injection = y_bus.calculate_injection(output.u);
+    std::tie(output.load_gen, output.source) = measured_value.calculate_load_gen_source(output.u, output.bus_injection);
+}
+
 } // namespace power_grid_model::math_solver::detail
 
 #endif

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

@@ -6,11 +6,10 @@
 #ifndef POWER_GRID_MODEL_MATH_SOLVER_ITERATIVE_LINEAR_SE_SOLVER_HPP
 #define POWER_GRID_MODEL_MATH_SOLVER_ITERATIVE_LINEAR_SE_SOLVER_HPP
 
-/*
-iterative linear state estimation solver
-*/
+// iterative linear state estimation solver
 
 #include "block_matrix.hpp"
+#include "common_solver_functions.hpp"
 #include "measured_values.hpp"
 #include "sparse_lu_solver.hpp"
 #include "y_bus.hpp"
@@ -45,12 +44,10 @@ template <bool sym> struct ILSEUnknown : public Block<DoubleComplex, sym, false,
 template <bool sym> using ILSERhs = ILSEUnknown<sym>;
 
 // class of 2*2 (6*6) se gain block
-/*
-[
-   [G, QH]
-   [Q, R ]
-]
-*/
+// [
+//    [G, QH]
+//    [Q, R ]
+// ]
 template <bool sym> class ILSEGainBlock : public Block<DoubleComplex, sym, true, 2> {
   public:
     template <int r, int c> using GetterType = typename Block<DoubleComplex, sym, true, 2>::template GetterType<r, c>;
@@ -117,12 +114,12 @@ 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
         sub_timer = Timer(calculation_info, 2227, "Calculate Math Result");
-        calculate_result(y_bus, measured_values, output);
+        detail::calculate_se_result<sym>(y_bus, measured_values, output);
 
         // Manually stop timers to avoid "Max number of iterations" to be included in the timing.
         sub_timer.stop();
@@ -152,7 +149,7 @@ template <bool sym> class IterativeLinearSESolver {
     SparseLUSolver<ILSEGainBlock<sym>, ILSERhs<sym>, ILSEUnknown<sym>> sparse_solver_;
     typename SparseLUSolver<ILSEGainBlock<sym>, ILSERhs<sym>, ILSEUnknown<sym>>::BlockPermArray perm_;
 
-    auto diagonal_inverse(RealValue<sym> const& value) {
+    static auto diagonal_inverse(RealValue<sym> const& value) {
         return ComplexDiagonalTensor<sym>{static_cast<ComplexValue<sym>>(RealValue<sym>{1.0} / value)};
     }
 
@@ -167,7 +164,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];
@@ -188,7 +185,7 @@ template <bool sym> class IterativeLinearSESolver {
                     // shunt
                     if (type == YBusElementType::shunt) {
                         if (measured_value.has_shunt(obj)) {
-                            // G += Ys^H * (variance^-1) * Ys
+                            // G += (-Ys)^H * (variance^-1) * (-Ys)
                             auto const& shunt_power = measured_value.shunt_power(obj);
                             block.g() += dot(hermitian_transpose(param.shunt_param[obj]),
                                              diagonal_inverse(shunt_power.p_variance + shunt_power.q_variance),
@@ -259,14 +256,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_measurements(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
@@ -281,7 +278,7 @@ template <bool sym> class IterativeLinearSESolver {
                 if (type == YBusElementType::shunt) {
                     if (measured_value.has_shunt(obj)) {
                         PowerSensorCalcParam<sym> const& m = measured_value.shunt_power(obj);
-                        // eta -= Ys^H * (variance^-1) * i_shunt
+                        // eta += (-Ys)^H * (variance^-1) * i_shunt
                         rhs_block.eta() -= dot(hermitian_transpose(param.shunt_param[obj]),
                                                diagonal_inverse(m.p_variance + m.q_variance), conj(m.value / u[bus]));
                     }
@@ -315,19 +312,24 @@ 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();
-            } else {
-                return cabs(x_rhs_[math_topo_->slack_bus_].u()(0)) / x_rhs_[math_topo_->slack_bus_].u()(0);
+            auto const& voltage = x_rhs_[math_topo_->slack_bus].u();
+            auto const& voltage_a = [&voltage]() -> auto const& {
+                if constexpr (sym) {
+                    return voltage;
+                } else {
+                    return voltage(0);
+                }
             }
+            ();
+            return cabs(voltage_a) / voltage_a;
         }();
 
         for (Idx bus = 0; bus != n_bus_; ++bus) {
@@ -342,13 +344,8 @@ template <bool sym> class IterativeLinearSESolver {
         return max_dev;
     }
 
-    void calculate_result(YBus<sym> const& y_bus, MeasuredValues<sym> const& measured_value, MathOutput<sym>& output) {
-        // call y bus
-        output.branch = y_bus.template calculate_branch_flow<BranchMathOutput<sym>>(output.u);
-        output.shunt = y_bus.template calculate_shunt_flow<ApplianceMathOutput<sym>>(output.u);
-        output.bus_injection = y_bus.calculate_injection(output.u);
-        std::tie(output.load_gen, output.source) =
-            measured_value.calculate_load_gen_source(output.u, output.bus_injection);
+    auto linearize_measurements(ComplexValueVector<sym> const& current_u, MeasuredValues<sym> const& measured_values) {
+        return measured_values.combine_voltage_iteration_with_measurements(current_u);
     }
 };
 

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

@@ -102,8 +102,8 @@ template <bool sym, typename DerivedSolver> class IterativePFSolver {
     }
 
     void calculate_result(YBus<sym> const& y_bus, PowerFlowInput<sym> const& input, MathOutput<sym>& output) {
-        detail::calculate_result(y_bus, input, *sources_per_bus_, *load_gens_per_bus_, output,
-                                 [this](Idx i) { return (*load_gen_type_)[i]; });
+        detail::calculate_pf_result(y_bus, input, *sources_per_bus_, *load_gens_per_bus_, output,
+                                    [this](Idx i) { return (*load_gen_type_)[i]; });
     }
 
   private:

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

@@ -115,8 +115,8 @@ template <bool sym> class LinearPFSolver {
     }
 
     void calculate_result(YBus<sym> const& y_bus, PowerFlowInput<sym> const& input, MathOutput<sym>& output) {
-        detail::calculate_result(y_bus, input, *sources_per_bus_, *load_gens_per_bus_, output,
-                                 [](Idx /*i*/) { return LoadGenType::const_y; });
+        detail::calculate_pf_result(y_bus, input, *sources_per_bus_, *load_gens_per_bus_, output,
+                                    [](Idx /*i*/) { return LoadGenType::const_y; });
     }
 };
 

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: