fix: dp45, matrix init, feedlin, dare and statistics

doc/nonlinear_control/FeedbackLinearization.md

@@ -12,17 +12,17 @@ The technique applies to plants whose output $y \in \mathbb{R}^m$ satisfies, aft
 
 $$y^{(r)} = a(x) + B(x)\, u$$
 
-where $x \in \mathbb{R}^n$ is the state, $u \in \mathbb{R}^m$ is the input, $a(x) \in \mathbb{R}^m$ is the **drift term** (known nonlinear dynamics), and $B(x) \in \mathbb{R}^{m \times m}$ is the **decoupling matrix** (state-dependent input gain). The integer $r$ is the relative degree. For mechanical systems ($r = 2$), $B(x) = M(q)$ is the inertia matrix and $a(x) = C(q, \dot{q})\dot{q} + g(q)$ is the Coriolis-plus-gravity term.
+where $x \in \mathbb{R}^n$ is the state, $u \in \mathbb{R}^m$ is the input, $a(x) \in \mathbb{R}^m$ is the **drift term** (known nonlinear dynamics), and $B(x) \in \mathbb{R}^{m \times m}$ is the **decoupling matrix** (state-dependent input gain). The integer $r$ is the relative degree. For mechanical systems ($r = 2$), written as $M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = u$, the control-affine form has $B(x) = M^{-1}(q)$ and $a(x) = -M^{-1}(q)\bigl(C(q,\dot{q})\dot{q} + g(q)\bigr)$.
 
 ### Inner Control Law (Cancellation)
 
 The inner law selects $u$ so that the term $a(x)$ is cancelled and the decoupling matrix is factored out:
 
-$$u = B(x)\, v + a(x)$$
+$$u = B^{-1}(x)\,(v - a(x))$$
 
 Substituting into the plant equation yields
 
-$$y^{(r)} = a(x) + B(x)\bigl(B(x)\,v + a(x)\bigr) - a(x) = v$$
+$$y^{(r)} = a(x) + B(x)\,B^{-1}(x)\,(v - a(x)) = a(x) + (v - a(x)) = v$$
 
 leaving pure integrator chains $y^{(r)} = v$, provided $B(x)$ is nonsingular.
 
@@ -42,16 +42,16 @@ whose eigenvalues are set by choosing $K_p, K_d$. Critical damping per channel r
 
 Expanding yields the single expression evaluated on the hot path:
 
-$$u = B(x)\bigl(y_d^{(r)} + K_d\,\dot{e} + K_p\, e\bigr) + a(x)$$
+$$u = B^{-1}(x)\bigl(y_d^{(r)} + K_d\,\dot{e} + K_p\, e - a(x)\bigr)$$
 
-No matrix inversion appears on the hot path: the law multiplies by $B(x)$, not by $B(x)^{-1}$.
+The law requires solving the linear system $B(x)\,u = v - a(x)$ on the hot path; $B(x)$ must be nonsingular.
 
 ## Complexity Analysis
 
 | Operation    | Time               | Space        | Notes                                  |
 |--------------|--------------------|--------------|----------------------------------------|
 | Construction | $O(m^2)$           | $O(m^2)$     | Copy two gain matrices                 |
-| ComputeInput | $O(m^2)$           | $O(m)$ extra | Two matrix-vector products dominate    |
+| ComputeInput | $O(m^3)$           | $O(m)$ extra | One linear solve dominates             |
 | Model query  | $O(m^2)$–$O(nm^2)$ | $O(m^2)$     | Implementation-defined; injected model |
 
 All storage is in fixed-size stack arrays; the law itself performs no heap allocation.
@@ -66,7 +66,7 @@ Consider a 2-DOF planar arm with $m = 2$, $K_p = 100 I$, $K_d = 20 I$, and at on
 
 1. Compute error: $e = [0.4, 0.3]^\top$, $\dot{e} = [0, 0]^\top$.
 2. Compute virtual input: $v = 0 + 20 \cdot 0 + 100 \cdot [0.4, 0.3]^\top = [40, 30]^\top$.
-3. Inner law: $u = I \cdot [40, 30]^\top + [0.3, 0.1]^\top = [40.3, 30.1]^\top$.
+3. Inner law: $u = I^{-1}([40, 30]^\top - [0.3, 0.1]^\top) = [39.7, 29.9]^\top$.
 
 The gravity-like drift $a(x)$ is added directly; the outer PD term drives position error to zero.
 

doc/solvers/RungeKuttaIntegrators.md

@@ -41,13 +41,13 @@ Dormand and Prince (1980) selected a 7-stage Butcher tableau whose 5th-order pro
 The **5th-order** solution used to advance the state:
 
 $$
-y_5 = x_n + h\left(\frac{35}{384}k_1 + \frac{500}{1113}k_3 - \frac{125}{192}k_4 + \frac{2187}{6784}k_5 + \frac{11}{84}k_6\right)
+y_5 = x_n + h\left(\frac{35}{384}k_1 + \frac{500}{1113}k_3 + \frac{125}{192}k_4 - \frac{2187}{6784}k_5 + \frac{11}{84}k_6\right)
 $$
 
 The **4th-order** embedded solution used only for error estimation:
 
 $$
-y_4 = x_n + h\left(\frac{5179}{57600}k_1 + \frac{7571}{16695}k_3 - \frac{393}{640}k_4 + \frac{92097}{339200}k_5 + \frac{187}{2100}k_6 + \frac{1}{40}k_7\right)
+y_4 = x_n + h\left(\frac{5179}{57600}k_1 + \frac{7571}{16695}k_3 + \frac{393}{640}k_4 - \frac{92097}{339200}k_5 + \frac{187}{2100}k_6 + \frac{1}{40}k_7\right)
 $$
 
 ### Error Norm and Step-Size Control

numerical/controllers/implementations/DeadbeatControl.cpp

@@ -3,6 +3,8 @@
 namespace controllers
 {
     template class DeadbeatControl<float, 1, 1, 1>;
+    template class DeadbeatControl<float, 1, 1, 2>;
     template class DeadbeatControl<float, 2, 1, 2>;
+    template class DeadbeatControl<float, 2, 1, 3>;
     template class DeadbeatControl<float, 2, 2, 1>;
 }

numerical/controllers/implementations/DeadbeatControl.hpp

@@ -4,11 +4,10 @@
 #pragma GCC optimize("O3", "fast-math")
 #endif
 
-#include "infra/util/ReallyAssert.hpp"
 #include "numerical/controllers/interfaces/StateFeedbackController.hpp"
 #include "numerical/math/CompilerOptimizations.hpp"
 #include "numerical/math/LinearTimeInvariant.hpp"
-#include "numerical/solvers/SingularValueDecomposition.hpp"
+#include "numerical/solvers/GaussianElimination.hpp"
 #include <type_traits>
 
 namespace controllers
@@ -125,20 +124,17 @@ namespace controllers
         for (std::size_t i = 0; i < Steps; ++i)
             AN = A * AN;
 
-        static constexpr T kSvdTol = T(1e-5f);
-        solvers::SingularValueDecomposition<T, StateSize, ReachSize> svd;
-        svd.Decompose(gamma);
-        really_assert(svd.Rank(kSvdTol) == StateSize);
-
-        auto gammaPinv = svd.PseudoInverse(kSvdTol);
+        auto gammaPinv = solvers::SolveSystem<T, StateSize, ReachSize>(gamma * gamma.Transpose(), gamma).Transpose();
 
         gainRef = gammaPinv.template GetBlock<InputSize, StateSize>(0, 0);
         gainState = gainRef * AN;
     }
 
 #ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
     extern template class DeadbeatControl<float, 1, 1, 1>;
+    extern template class DeadbeatControl<float, 1, 1, 2>;
     extern template class DeadbeatControl<float, 2, 1, 2>;
+    extern template class DeadbeatControl<float, 2, 1, 3>;
     extern template class DeadbeatControl<float, 2, 2, 1>;
 #endif
 }

numerical/controllers/implementations/Lqr.hpp

@@ -48,7 +48,12 @@ namespace controllers
     template<typename T, std::size_t StateSize, std::size_t InputSize>
     Lqr<T, StateSize, InputSize>::Lqr(
         const StateMatrix& A, const InputMatrix& B, const StateMatrix& Q, const InputWeightMatrix& R)
-        : riccatiSolution(solvers::DiscreteAlgebraicRiccatiEquation<T, StateSize, InputSize>{}.Solve(A, B, Q, R))
+        : riccatiSolution([&A, &B, &Q, &R]
+              {
+                  auto r = solvers::DiscreteAlgebraicRiccatiEquation<T, StateSize, InputSize>{}.Solve(A, B, Q, R);
+                  really_assert(r.converged);
+                  return r.value;
+              }())
         , riccatiSolutionAvailable(true)
     {
         ComputeGain(A, B, riccatiSolution, R);

numerical/controllers/implementations/test/TestDeadbeatControl.cpp

@@ -5,6 +5,7 @@
 #include "numerical/controllers/implementations/DeadbeatControl.hpp"
 #include "numerical/math/LinearTimeInvariant.hpp"
 #include "numerical/math/Tolerance.hpp"
+#include <cmath>
 #include <gtest/gtest.h>
 
 namespace
@@ -105,16 +106,16 @@ TEST_F(TestDeadbeatControl, state_gain_matches_analytic_reachability_formula)
 {
     controllers::DeadbeatControl<float, 2, 1, 2> ctrl{ A2, B2 };
 
-    EXPECT_NEAR(ctrl.GetStateGain().at(0, 0), kGainStateA, 1e-1f);
-    EXPECT_NEAR(ctrl.GetStateGain().at(0, 1), kGainStateB, 1e-1f);
+    EXPECT_NEAR(ctrl.GetStateGain().at(0, 0), kGainStateA, math::Tolerance<float>());
+    EXPECT_NEAR(ctrl.GetStateGain().at(0, 1), kGainStateB, math::Tolerance<float>());
 }
 
 TEST_F(TestDeadbeatControl, reference_gain_matches_analytic_reachability_formula)
 {
     controllers::DeadbeatControl<float, 2, 1, 2> ctrl{ A2, B2 };
 
-    EXPECT_NEAR(ctrl.GetReferenceGain().at(0, 0), kGainRefA, 1e-1f);
-    EXPECT_NEAR(ctrl.GetReferenceGain().at(0, 1), kGainRefB, 1e-1f);
+    EXPECT_NEAR(ctrl.GetReferenceGain().at(0, 0), kGainRefA, math::Tolerance<float>());
+    EXPECT_NEAR(ctrl.GetReferenceGain().at(0, 1), kGainRefB, math::Tolerance<float>());
 }
 
 TEST_F(TestDeadbeatControl, lti_constructor_matches_matrix_constructor)
@@ -129,3 +130,43 @@ TEST_F(TestDeadbeatControl, lti_constructor_matches_matrix_constructor)
     EXPECT_NEAR(ctrlMat.GetReferenceGain().at(0, 0), ctrlLti.GetReferenceGain().at(0, 0), math::Tolerance<float>());
     EXPECT_NEAR(ctrlMat.GetReferenceGain().at(0, 1), ctrlLti.GetReferenceGain().at(0, 1), math::Tolerance<float>());
 }
+
+TEST_F(TestDeadbeatControl, two_step_gain_amplifies_noise_less_than_one_step)
+{
+    math::SquareMatrix<float, 1> A{ { kScalarA } };
+    math::Matrix<float, 1, 1> B{ { kScalarB } };
+    controllers::DeadbeatControl<float, 1, 1, 1> ctrl1{ A, B };
+    controllers::DeadbeatControl<float, 1, 1, 2> ctrl2{ A, B };
+
+    EXPECT_LT(std::abs(ctrl2.GetReferenceGain().at(0, 0)),
+        std::abs(ctrl1.GetReferenceGain().at(0, 0)));
+}
+
+TEST_F(TestDeadbeatControl, scalar_two_step_closed_loop_is_stable)
+{
+    math::SquareMatrix<float, 1> A{ { kScalarA } };
+    math::Matrix<float, 1, 1> B{ { kScalarB } };
+    controllers::DeadbeatControl<float, 1, 1, 2> ctrl{ A, B };
+
+    const float closedLoopEig = kScalarA - kScalarB * ctrl.GetStateGain().at(0, 0);
+
+    EXPECT_LT(std::abs(closedLoopEig), 1.0f);
+}
+
+TEST_F(TestDeadbeatControl, two_state_three_step_asymptotically_converges)
+{
+    controllers::DeadbeatControl<float, 2, 1, 3> ctrl{ A2, B2 };
+
+    math::Vector<float, 2> x{ { 0.0f }, { 0.0f } };
+    math::Vector<float, 2> r{ { 5.0f }, { 0.0f } };
+    ctrl.SetReference(r);
+
+    for (int i = 0; i < 50; ++i)
+    {
+        const auto u = ctrl.ComputeControl(x);
+        x = A2 * x + B2 * u;
+    }
+
+    EXPECT_NEAR(x.at(0, 0), r.at(0, 0), math::Tolerance<float>());
+    EXPECT_NEAR(x.at(1, 0), r.at(1, 0), math::Tolerance<float>());
+}

numerical/math/Matrix.hpp

@@ -146,18 +146,25 @@
 
     template<typename T, size_t Rows, size_t Cols>
     constexpr Matrix<T, Rows, Cols>::Matrix() noexcept
         : data{}
     {}
 
     template<typename T, size_t Rows, size_t Cols>
     OPTIMIZE_FOR_SPEED constexpr Matrix<T, Rows, Cols>::Matrix(std::initializer_list<std::initializer_list<T>> init)
+        : data{}
     {
         size_t row = 0;
         for (const auto& row_list : init)
         {
+#ifdef NUMERICAL_TOOLBOX_ENABLE_ASSERTIONS
+            really_assert(row < Rows);
+#endif
             size_t col = 0;
             for (const auto& value : row_list)
             {
+#ifdef NUMERICAL_TOOLBOX_ENABLE_ASSERTIONS
+                really_assert(col < Cols);
+#endif
                 at(row, col) = value;
                 ++col;
             }

numerical/math/Statistics.hpp

@@ -5,17 +5,24 @@
 
 namespace math
 {
+    template<typename V>
+    struct Result
+    {
+        V value{};
+        bool valid{ false };
+    };
+
     template<typename T, size_t Rows, size_t Cols>
     [[nodiscard]] constexpr T Mean(const Matrix<T, Rows, Cols>& data)
     {
         static_assert(detail::is_supported_type_v<T>,
             "Statistical functions only support float or QNumber types");
 
-        T sum{};
+        float sum = 0.0f;
         for (size_t i = 0; i < data.size; ++i)
-            sum += data.begin()[i];
+            sum += math::ToFloat(data.begin()[i]);
 
-        return T{ math::ToFloat(sum) / static_cast<float>(data.size) };
+        return T{ sum / static_cast<float>(data.size) };
     }
 
     template<typename T, size_t Rows, size_t Cols>
@@ -33,7 +40,9 @@ namespace math
             sum_sq += diff * diff;
         }
 
-        return T{ sum_sq / static_cast<float>(sample ? data.size - 1 : data.size) };
+        const float divisor = sample ? static_cast<float>(data.size) - 1.0f : static_cast<float>(data.size);
+        really_assert(divisor > 0.0f);
+        return T{ sum_sq / divisor };
     }
 
     template<typename T, size_t Rows, size_t Cols>
@@ -85,7 +94,7 @@ namespace math
     }
 
     template<typename T, size_t Size>
-    [[nodiscard]] constexpr T RSquaredScore(const Vector<T, Size>& actual, const Vector<T, Size>& predicted)
+    [[nodiscard]] constexpr Result<T> RSquaredScore(const Vector<T, Size>& actual, const Vector<T, Size>& predicted)
     {
         static_assert(detail::is_supported_type_v<T>,
             "Statistical functions only support float or QNumber types");
@@ -104,11 +113,13 @@ namespace math
             residual_ss += diff_pred * diff_pred;
         }
 
-        return T{ 1.0f - (residual_ss / total_ss) };
+        if (total_ss == 0.0f)
+            return { T{}, false };
+        return { T{ 1.0f - (residual_ss / total_ss) }, true };
     }
 
     template<typename T, size_t Size>
-    [[nodiscard]] constexpr Matrix<T, Size, 1> AutoCorrelation(const Vector<T, Size>& data, size_t maxLag)
+    [[nodiscard]] constexpr Result<Matrix<T, Size, 1>> AutoCorrelation(const Vector<T, Size>& data, size_t maxLag)
     {
         static_assert(detail::is_supported_type_v<T>,
             "Statistical functions only support float or QNumber types");
@@ -124,6 +135,9 @@ namespace math
             sum_sq += diff * diff;
         }
 
+        if (sum_sq == 0.0f)
+            return { Matrix<T, Size, 1>{}, false };
+
         Matrix<T, Size, 1> result;
 
         for (size_t lag = 0; lag <= maxLag; ++lag)
@@ -133,12 +147,17 @@ namespace math
                 sum += (math::ToFloat(data.at(t, 0)) - mean_val) * (math::ToFloat(data.at(t + lag, 0)) - mean_val);
 
             if (lag == 0)
-                result.at(lag, 0) = T{ 0.9999f };
+            {
+                if constexpr (std::is_floating_point_v<T>)
+                    result.at(lag, 0) = T{ 1 };
+                else
+                    result.at(lag, 0) = T{ 0.9999f };
+            }
             else
                 result.at(lag, 0) = T{ sum / ((sum_sq / static_cast<float>(Size)) * static_cast<float>(Size - lag)) };
         }
 
-        return result;
+        return { result, true };
     }
 
     template<typename T, size_t Rows, size_t Cols>

numerical/math/test/TestMatrix.cpp

@@ -204,6 +204,18 @@ TYPED_TEST(MatrixTest, AdditionIsCommutative)
     EXPECT_TRUE(AreMatricesNear(m1 + m2, m2 + m1));
 }
 
+TYPED_TEST(MatrixTest, PartialInitializerListZeroFillsRemainder)
+{
+    typename TestFixture::MatrixType m{
+        { this->MakeValue(0.5f) }
+    };
+
+    EXPECT_NEAR(math::ToFloat(m.at(0, 0)), 0.5f, math::Tolerance<float>());
+    EXPECT_NEAR(math::ToFloat(m.at(0, 1)), 0.0f, math::Tolerance<float>());
+    EXPECT_NEAR(math::ToFloat(m.at(1, 0)), 0.0f, math::Tolerance<float>());
+    EXPECT_NEAR(math::ToFloat(m.at(1, 1)), 0.0f, math::Tolerance<float>());
+}
+
 TYPED_TEST(MatrixTest, MultiplicationByZeroMatrixYieldsZero)
 {
     auto m = this->MakeMatrix(0.5f, 0.3f, 0.2f, 0.4f);