fix: apply N-bar precompensator in DeadbeatControl for u…

doc/controllers/DeadbeatControl.md

@@ -40,11 +40,21 @@ where $\Gamma_N^+ = \Gamma_N^T (\Gamma_N \Gamma_N^T)^{-1}$ is the Moore-Penrose
 
 ### Precomputed Gain Matrices
 
-Only the **first** $m$ elements of $\mathbf{u}^*$ are applied at each sample (the remaining elements are recomputed at the next step). Let $\Pi_0$ denote the operator that extracts the first $m$ rows of a matrix. Define:
+Only the **first** $m$ elements of $\mathbf{u}^*$ are applied at each sample (the remaining elements are recomputed at the next step). Let $\Pi_0$ denote the operator that extracts the first $m$ rows of a matrix. Define the open-loop state-feedback gain:
 
-$$K_r = \Pi_0 \left(\Gamma_N^+\right) \in \mathbb{R}^{m \times n}$$
+$$K_x = \Pi_0 \left(\Gamma_N^+\right) \cdot A^N \in \mathbb{R}^{m \times n}$$
+
+For unity closed-loop DC gain in receding-horizon (closed-loop) operation, $K_r$ must satisfy:
+
+$$B \, K_r = I - A + B \, K_x$$
+
+For **square systems** ($m = n$), $B$ is invertible and the unique solution is:
 
-$$K_x = K_r \cdot A^N \in \mathbb{R}^{m \times n}$$
+$$K_r = B^{-1}(I - A + B \, K_x) \in \mathbb{R}^{m \times n}$$
+
+For **non-square systems** ($m < n$), the open-loop first-step gain is used directly:
+
+$$K_r = \Pi_0 \left(\Gamma_N^+\right) \in \mathbb{R}^{m \times n}$$
 
 The resulting **deadbeat control law** is:
 

numerical/controllers/implementations/DeadbeatControl.hpp

@@ -125,9 +125,18 @@ namespace controllers
             AN = A * AN;
 
         auto gammaPinv = solvers::SolveSystem<T, StateSize, ReachSize>(gamma * gamma.Transpose(), gamma).Transpose();
+        auto gainRefOl = gammaPinv.template GetBlock<InputSize, StateSize>(0, 0);
+        gainState = gainRefOl * AN;
 
-        gainRef = gammaPinv.template GetBlock<InputSize, StateSize>(0, 0);
-        gainState = gainRef * AN;
+        if constexpr (StateSize == InputSize)
+        {
+            StateMatrix rhs = StateMatrix::Identity() - A + B * gainState;
+            gainRef = solvers::SolveSystem<T, StateSize, StateSize>(B, rhs);
+        }
+        else
+        {
+            gainRef = gainRefOl;
+        }
     }
 
 #ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD

numerical/controllers/implementations/test/TestDeadbeatControl.cpp

@@ -153,6 +153,45 @@ TEST_F(TestDeadbeatControl, scalar_two_step_closed_loop_is_stable)
     EXPECT_LT(std::abs(closedLoopEig), 1.0f);
 }
 
+TEST_F(TestDeadbeatControl, scalar_two_step_closed_loop_unity_dc_gain)
+{
+    math::SquareMatrix<float, 1> A{ { kScalarA } };
+    math::Matrix<float, 1, 1> B{ { kScalarB } };
+    controllers::DeadbeatControl<float, 1, 1, 2> ctrl{ A, B };
+
+    math::Vector<float, 1> x{ { 0.0f } };
+    math::Vector<float, 1> r{ { 3.0f } };
+    ctrl.SetReference(r);
+
+    for (std::size_t i = 0; i < 50; ++i)
+    {
+        const auto u = ctrl.ComputeControl(x);
+        x = A * x + B * u;
+    }
+
+    EXPECT_NEAR(x.at(0, 0), r.at(0, 0), math::Tolerance<float>());
+}
+
+TEST_F(TestDeadbeatControl, scalar_fast_plant_two_step_unity_dc_gain)
+{
+    static constexpr float kFastA = 0.6065f;
+    math::SquareMatrix<float, 1> A{ { kFastA } };
+    math::Matrix<float, 1, 1> B{ { kScalarB } };
+    controllers::DeadbeatControl<float, 1, 1, 2> ctrl{ A, B };
+
+    math::Vector<float, 1> x{ { 0.0f } };
+    math::Vector<float, 1> r{ { 3.0f } };
+    ctrl.SetReference(r);
+
+    for (std::size_t i = 0; i < 50; ++i)
+    {
+        const auto u = ctrl.ComputeControl(x);
+        x = A * x + B * u;
+    }
+
+    EXPECT_NEAR(x.at(0, 0), r.at(0, 0), math::Tolerance<float>());
+}
+
 TEST_F(TestDeadbeatControl, two_state_three_step_asymptotically_converges)
 {
     controllers::DeadbeatControl<float, 2, 1, 3> ctrl{ A2, B2 };