From 4f374d51dc90b5c093fe56b6d80e99785d4b3b2f Mon Sep 17 00:00:00 2001
From: gfs <gabriel.fra.santos@gmail.com>
Date: Fri, 14 Aug 2026 16:26:40 +0000
Subject: [PATCH] fix(controllers): apply N-bar precompensator in
 DeadbeatControl for unity DC gain
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

For square systems (StateSize == InputSize) with Steps > 1, the open-loop
pseudo-inverse gain K_r produced a closed-loop DC gain less than unity,
causing static tracking error proportional to plant decay rate. Solve
B·K_r = I − A + B·K_x at construction time to restore exact steady-state
tracking. Non-square systems retain the previous open-loop K_r unchanged.

Fixes #278.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
---
 doc/controllers/DeadbeatControl.md            | 16 ++++++--
 .../implementations/DeadbeatControl.hpp       | 13 ++++++-
 .../test/TestDeadbeatControl.cpp              | 39 +++++++++++++++++++
 3 files changed, 63 insertions(+), 5 deletions(-)

diff --git a/doc/controllers/DeadbeatControl.md b/doc/controllers/DeadbeatControl.md
index 5ac4b02..91eff37 100644
--- a/doc/controllers/DeadbeatControl.md
+++ b/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:
 
diff --git a/numerical/controllers/implementations/DeadbeatControl.hpp b/numerical/controllers/implementations/DeadbeatControl.hpp
index b801d58..9a9680a 100644
--- a/numerical/controllers/implementations/DeadbeatControl.hpp
+++ b/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
diff --git a/numerical/controllers/implementations/test/TestDeadbeatControl.cpp b/numerical/controllers/implementations/test/TestDeadbeatControl.cpp
index f4b6fef..a8e2207 100644
--- a/numerical/controllers/implementations/test/TestDeadbeatControl.cpp
+++ b/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 };