Skip to content

Commit d89092b

Browse files
gabrielfrasantosgithub-actions[bot]claude
authored
fix: dp45, matrix init, feedlin, dare and statistics (#272)
* fix dp45, matrix init, feedlin, dare and statistics * Update numerical/controllers/implementations/Lqr.hpp Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> * fix(deadbeat): replace SVD with normal-equations pseudo-inverse to unblock underdetermined case SVD<T, Rows, Cols> requires Rows >= Cols, which contradicted the class's own static_assert (StateSize <= Steps * InputSize). Any instantiation with StateSize < ReachSize (e.g. <float,1,1,2>) would fail at the SVD's internal constraint, not at the user-facing one. Replace with Γ† = Γᵀ(ΓΓᵀ)⁻¹ via SolveSystem — valid for both the square (existing) and underdetermined (new) cases. Add <float,1,1,2> and <float,2,1,3> coverage instantiations. Fix 1e-1f tolerance to math::Tolerance<float>(). Add three tests pinning noise-gain ordering, closed-loop stability, and asymptotic convergence for the new cases. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com> * fix build * fix build 2 * fix issues 3 --------- Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: Claude Sonnet 4.6 <noreply@anthropic.com>
1 parent 69b491c commit d89092b

19 files changed

Lines changed: 258 additions & 73 deletions

‎doc/nonlinear_control/FeedbackLinearization.md‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -12,17 +12,17 @@ The technique applies to plants whose output $y \in \mathbb{R}^m$ satisfies, aft
1212

1313
$$y^{(r)} = a(x) + B(x)\, u$$
1414

15-
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.
15+
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)$.
1616

1717
### Inner Control Law (Cancellation)
1818

1919
The inner law selects $u$ so that the term $a(x)$ is cancelled and the decoupling matrix is factored out:
2020

21-
$$u = B(x)\, v + a(x)$$
21+
$$u = B^{-1}(x)\,(v - a(x))$$
2222

2323
Substituting into the plant equation yields
2424

25-
$$y^{(r)} = a(x) + B(x)\bigl(B(x)\,v + a(x)\bigr) - a(x) = v$$
25+
$$y^{(r)} = a(x) + B(x)\,B^{-1}(x)\,(v - a(x)) = a(x) + (v - a(x)) = v$$
2626

2727
leaving pure integrator chains $y^{(r)} = v$, provided $B(x)$ is nonsingular.
2828

@@ -42,16 +42,16 @@ whose eigenvalues are set by choosing $K_p, K_d$. Critical damping per channel r
4242

4343
Expanding yields the single expression evaluated on the hot path:
4444

45-
$$u = B(x)\bigl(y_d^{(r)} + K_d\,\dot{e} + K_p\, e\bigr) + a(x)$$
45+
$$u = B^{-1}(x)\bigl(y_d^{(r)} + K_d\,\dot{e} + K_p\, e - a(x)\bigr)$$
4646

47-
No matrix inversion appears on the hot path: the law multiplies by $B(x)$, not by $B(x)^{-1}$.
47+
The law requires solving the linear system $B(x)\,u = v - a(x)$ on the hot path; $B(x)$ must be nonsingular.
4848

4949
## Complexity Analysis
5050

5151
| Operation | Time | Space | Notes |
5252
|--------------|--------------------|--------------|----------------------------------------|
5353
| Construction | $O(m^2)$ | $O(m^2)$ | Copy two gain matrices |
54-
| ComputeInput | $O(m^2)$ | $O(m)$ extra | Two matrix-vector products dominate |
54+
| ComputeInput | $O(m^3)$ | $O(m)$ extra | One linear solve dominates |
5555
| Model query | $O(m^2)$–$O(nm^2)$ | $O(m^2)$ | Implementation-defined; injected model |
5656

5757
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
6666

6767
1. Compute error: $e = [0.4, 0.3]^\top$, $\dot{e} = [0, 0]^\top$.
6868
2. Compute virtual input: $v = 0 + 20 \cdot 0 + 100 \cdot [0.4, 0.3]^\top = [40, 30]^\top$.
69-
3. Inner law: $u = I \cdot [40, 30]^\top + [0.3, 0.1]^\top = [40.3, 30.1]^\top$.
69+
3. Inner law: $u = I^{-1}([40, 30]^\top - [0.3, 0.1]^\top) = [39.7, 29.9]^\top$.
7070

7171
The gravity-like drift $a(x)$ is added directly; the outer PD term drives position error to zero.
7272

‎doc/solvers/RungeKuttaIntegrators.md‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -41,13 +41,13 @@ Dormand and Prince (1980) selected a 7-stage Butcher tableau whose 5th-order pro
4141
The **5th-order** solution used to advance the state:
4242

4343
$$
44-
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)
44+
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)
4545
$$
4646

4747
The **4th-order** embedded solution used only for error estimation:
4848

4949
$$
50-
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)
50+
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)
5151
$$
5252

5353
### Error Norm and Step-Size Control

‎numerical/controllers/implementations/DeadbeatControl.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,8 @@
33
namespace controllers
44
{
55
template class DeadbeatControl<float, 1, 1, 1>;
6+
template class DeadbeatControl<float, 1, 1, 2>;
67
template class DeadbeatControl<float, 2, 1, 2>;
8+
template class DeadbeatControl<float, 2, 1, 3>;
79
template class DeadbeatControl<float, 2, 2, 1>;
810
}

‎numerical/controllers/implementations/DeadbeatControl.hpp‎

Lines changed: 4 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -4,11 +4,10 @@
44
#pragma GCC optimize("O3", "fast-math")
55
#endif
66

7-
#include "infra/util/ReallyAssert.hpp"
87
#include "numerical/controllers/interfaces/StateFeedbackController.hpp"
98
#include "numerical/math/CompilerOptimizations.hpp"
109
#include "numerical/math/LinearTimeInvariant.hpp"
11-
#include "numerical/solvers/SingularValueDecomposition.hpp"
10+
#include "numerical/solvers/GaussianElimination.hpp"
1211
#include <type_traits>
1312

1413
namespace controllers
@@ -125,20 +124,17 @@ namespace controllers
125124
for (std::size_t i = 0; i < Steps; ++i)
126125
AN = A * AN;
127126

128-
static constexpr T kSvdTol = T(1e-5f);
129-
solvers::SingularValueDecomposition<T, StateSize, ReachSize> svd;
130-
svd.Decompose(gamma);
131-
really_assert(svd.Rank(kSvdTol) == StateSize);
132-
133-
auto gammaPinv = svd.PseudoInverse(kSvdTol);
127+
auto gammaPinv = solvers::SolveSystem<T, StateSize, ReachSize>(gamma * gamma.Transpose(), gamma).Transpose();
134128

135129
gainRef = gammaPinv.template GetBlock<InputSize, StateSize>(0, 0);
136130
gainState = gainRef * AN;
137131
}
138132

139133
#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
140134
extern template class DeadbeatControl<float, 1, 1, 1>;
135+
extern template class DeadbeatControl<float, 1, 1, 2>;
141136
extern template class DeadbeatControl<float, 2, 1, 2>;
137+
extern template class DeadbeatControl<float, 2, 1, 3>;
142138
extern template class DeadbeatControl<float, 2, 2, 1>;
143139
#endif
144140
}

‎numerical/controllers/implementations/Lqr.hpp‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,12 @@ namespace controllers
4848
template<typename T, std::size_t StateSize, std::size_t InputSize>
4949
Lqr<T, StateSize, InputSize>::Lqr(
5050
const StateMatrix& A, const InputMatrix& B, const StateMatrix& Q, const InputWeightMatrix& R)
51-
: riccatiSolution(solvers::DiscreteAlgebraicRiccatiEquation<T, StateSize, InputSize>{}.Solve(A, B, Q, R))
51+
: riccatiSolution([&A, &B, &Q, &R]
52+
{
53+
auto r = solvers::DiscreteAlgebraicRiccatiEquation<T, StateSize, InputSize>{}.Solve(A, B, Q, R);
54+
really_assert(r.converged);
55+
return r.value;
56+
}())
5257
, riccatiSolutionAvailable(true)
5358
{
5459
ComputeGain(A, B, riccatiSolution, R);

‎numerical/controllers/implementations/test/TestDeadbeatControl.cpp‎

Lines changed: 45 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
#include "numerical/controllers/implementations/DeadbeatControl.hpp"
66
#include "numerical/math/LinearTimeInvariant.hpp"
77
#include "numerical/math/Tolerance.hpp"
8+
#include <cmath>
89
#include <gtest/gtest.h>
910

1011
namespace
@@ -105,16 +106,16 @@ TEST_F(TestDeadbeatControl, state_gain_matches_analytic_reachability_formula)
105106
{
106107
controllers::DeadbeatControl<float, 2, 1, 2> ctrl{ A2, B2 };
107108

108-
EXPECT_NEAR(ctrl.GetStateGain().at(0, 0), kGainStateA, 1e-1f);
109-
EXPECT_NEAR(ctrl.GetStateGain().at(0, 1), kGainStateB, 1e-1f);
109+
EXPECT_NEAR(ctrl.GetStateGain().at(0, 0), kGainStateA, math::Tolerance<float>());
110+
EXPECT_NEAR(ctrl.GetStateGain().at(0, 1), kGainStateB, math::Tolerance<float>());
110111
}
111112

112113
TEST_F(TestDeadbeatControl, reference_gain_matches_analytic_reachability_formula)
113114
{
114115
controllers::DeadbeatControl<float, 2, 1, 2> ctrl{ A2, B2 };
115116

116-
EXPECT_NEAR(ctrl.GetReferenceGain().at(0, 0), kGainRefA, 1e-1f);
117-
EXPECT_NEAR(ctrl.GetReferenceGain().at(0, 1), kGainRefB, 1e-1f);
117+
EXPECT_NEAR(ctrl.GetReferenceGain().at(0, 0), kGainRefA, math::Tolerance<float>());
118+
EXPECT_NEAR(ctrl.GetReferenceGain().at(0, 1), kGainRefB, math::Tolerance<float>());
118119
}
119120

120121
TEST_F(TestDeadbeatControl, lti_constructor_matches_matrix_constructor)
@@ -129,3 +130,43 @@ TEST_F(TestDeadbeatControl, lti_constructor_matches_matrix_constructor)
129130
EXPECT_NEAR(ctrlMat.GetReferenceGain().at(0, 0), ctrlLti.GetReferenceGain().at(0, 0), math::Tolerance<float>());
130131
EXPECT_NEAR(ctrlMat.GetReferenceGain().at(0, 1), ctrlLti.GetReferenceGain().at(0, 1), math::Tolerance<float>());
131132
}
133+
134+
TEST_F(TestDeadbeatControl, two_step_gain_amplifies_noise_less_than_one_step)
135+
{
136+
math::SquareMatrix<float, 1> A{ { kScalarA } };
137+
math::Matrix<float, 1, 1> B{ { kScalarB } };
138+
controllers::DeadbeatControl<float, 1, 1, 1> ctrl1{ A, B };
139+
controllers::DeadbeatControl<float, 1, 1, 2> ctrl2{ A, B };
140+
141+
EXPECT_LT(std::abs(ctrl2.GetReferenceGain().at(0, 0)),
142+
std::abs(ctrl1.GetReferenceGain().at(0, 0)));
143+
}
144+
145+
TEST_F(TestDeadbeatControl, scalar_two_step_closed_loop_is_stable)
146+
{
147+
math::SquareMatrix<float, 1> A{ { kScalarA } };
148+
math::Matrix<float, 1, 1> B{ { kScalarB } };
149+
controllers::DeadbeatControl<float, 1, 1, 2> ctrl{ A, B };
150+
151+
const float closedLoopEig = kScalarA - kScalarB * ctrl.GetStateGain().at(0, 0);
152+
153+
EXPECT_LT(std::abs(closedLoopEig), 1.0f);
154+
}
155+
156+
TEST_F(TestDeadbeatControl, two_state_three_step_asymptotically_converges)
157+
{
158+
controllers::DeadbeatControl<float, 2, 1, 3> ctrl{ A2, B2 };
159+
160+
math::Vector<float, 2> x{ { 0.0f }, { 0.0f } };
161+
math::Vector<float, 2> r{ { 5.0f }, { 0.0f } };
162+
ctrl.SetReference(r);
163+
164+
for (int i = 0; i < 50; ++i)
165+
{
166+
const auto u = ctrl.ComputeControl(x);
167+
x = A2 * x + B2 * u;
168+
}
169+
170+
EXPECT_NEAR(x.at(0, 0), r.at(0, 0), math::Tolerance<float>());
171+
EXPECT_NEAR(x.at(1, 0), r.at(1, 0), math::Tolerance<float>());
172+
}

‎numerical/math/Matrix.hpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -151,13 +151,20 @@ namespace math
151151

152152
template<typename T, size_t Rows, size_t Cols>
153153
OPTIMIZE_FOR_SPEED constexpr Matrix<T, Rows, Cols>::Matrix(std::initializer_list<std::initializer_list<T>> init)
154+
: data{}
154155
{
155156
size_t row = 0;
156157
for (const auto& row_list : init)
157158
{
159+
#ifdef NUMERICAL_TOOLBOX_ENABLE_ASSERTIONS
160+
really_assert(row < Rows);
161+
#endif
158162
size_t col = 0;
159163
for (const auto& value : row_list)
160164
{
165+
#ifdef NUMERICAL_TOOLBOX_ENABLE_ASSERTIONS
166+
really_assert(col < Cols);
167+
#endif
161168
at(row, col) = value;
162169
++col;
163170
}

‎numerical/math/Statistics.hpp‎

Lines changed: 28 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -5,17 +5,24 @@
55

66
namespace math
77
{
8+
template<typename V>
9+
struct Result
10+
{
11+
V value{};
12+
bool valid{ false };
13+
};
14+
815
template<typename T, size_t Rows, size_t Cols>
916
[[nodiscard]] constexpr T Mean(const Matrix<T, Rows, Cols>& data)
1017
{
1118
static_assert(detail::is_supported_type_v<T>,
1219
"Statistical functions only support float or QNumber types");
1320

14-
T sum{};
21+
float sum = 0.0f;
1522
for (size_t i = 0; i < data.size; ++i)
16-
sum += data.begin()[i];
23+
sum += math::ToFloat(data.begin()[i]);
1724

18-
return T{ math::ToFloat(sum) / static_cast<float>(data.size) };
25+
return T{ sum / static_cast<float>(data.size) };
1926
}
2027

2128
template<typename T, size_t Rows, size_t Cols>
@@ -33,7 +40,9 @@ namespace math
3340
sum_sq += diff * diff;
3441
}
3542

36-
return T{ sum_sq / static_cast<float>(sample ? data.size - 1 : data.size) };
43+
const float divisor = sample ? static_cast<float>(data.size) - 1.0f : static_cast<float>(data.size);
44+
really_assert(divisor > 0.0f);
45+
return T{ sum_sq / divisor };
3746
}
3847

3948
template<typename T, size_t Rows, size_t Cols>
@@ -85,7 +94,7 @@ namespace math
8594
}
8695

8796
template<typename T, size_t Size>
88-
[[nodiscard]] constexpr T RSquaredScore(const Vector<T, Size>& actual, const Vector<T, Size>& predicted)
97+
[[nodiscard]] constexpr Result<T> RSquaredScore(const Vector<T, Size>& actual, const Vector<T, Size>& predicted)
8998
{
9099
static_assert(detail::is_supported_type_v<T>,
91100
"Statistical functions only support float or QNumber types");
@@ -104,11 +113,13 @@ namespace math
104113
residual_ss += diff_pred * diff_pred;
105114
}
106115

107-
return T{ 1.0f - (residual_ss / total_ss) };
116+
if (total_ss == 0.0f)
117+
return { T{}, false };
118+
return { T{ 1.0f - (residual_ss / total_ss) }, true };
108119
}
109120

110121
template<typename T, size_t Size>
111-
[[nodiscard]] constexpr Matrix<T, Size, 1> AutoCorrelation(const Vector<T, Size>& data, size_t maxLag)
122+
[[nodiscard]] constexpr Result<Matrix<T, Size, 1>> AutoCorrelation(const Vector<T, Size>& data, size_t maxLag)
112123
{
113124
static_assert(detail::is_supported_type_v<T>,
114125
"Statistical functions only support float or QNumber types");
@@ -124,6 +135,9 @@ namespace math
124135
sum_sq += diff * diff;
125136
}
126137

138+
if (sum_sq == 0.0f)
139+
return { Matrix<T, Size, 1>{}, false };
140+
127141
Matrix<T, Size, 1> result;
128142

129143
for (size_t lag = 0; lag <= maxLag; ++lag)
@@ -133,12 +147,17 @@ namespace math
133147
sum += (math::ToFloat(data.at(t, 0)) - mean_val) * (math::ToFloat(data.at(t + lag, 0)) - mean_val);
134148

135149
if (lag == 0)
136-
result.at(lag, 0) = T{ 0.9999f };
150+
{
151+
if constexpr (std::is_floating_point_v<T>)
152+
result.at(lag, 0) = T{ 1 };
153+
else
154+
result.at(lag, 0) = T{ 0.9999f };
155+
}
137156
else
138157
result.at(lag, 0) = T{ sum / ((sum_sq / static_cast<float>(Size)) * static_cast<float>(Size - lag)) };
139158
}
140159

141-
return result;
160+
return { result, true };
142161
}
143162

144163
template<typename T, size_t Rows, size_t Cols>

‎numerical/math/test/TestMatrix.cpp‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -204,6 +204,18 @@ TYPED_TEST(MatrixTest, AdditionIsCommutative)
204204
EXPECT_TRUE(AreMatricesNear(m1 + m2, m2 + m1));
205205
}
206206

207+
TYPED_TEST(MatrixTest, PartialInitializerListZeroFillsRemainder)
208+
{
209+
typename TestFixture::MatrixType m{
210+
{ this->MakeValue(0.5f) }
211+
};
212+
213+
EXPECT_NEAR(math::ToFloat(m.at(0, 0)), 0.5f, math::Tolerance<float>());
214+
EXPECT_NEAR(math::ToFloat(m.at(0, 1)), 0.0f, math::Tolerance<float>());
215+
EXPECT_NEAR(math::ToFloat(m.at(1, 0)), 0.0f, math::Tolerance<float>());
216+
EXPECT_NEAR(math::ToFloat(m.at(1, 1)), 0.0f, math::Tolerance<float>());
217+
}
218+
207219
TYPED_TEST(MatrixTest, MultiplicationByZeroMatrixYieldsZero)
208220
{
209221
auto m = this->MakeMatrix(0.5f, 0.3f, 0.2f, 0.4f);

0 commit comments

Comments
 (0)