Skip to content

feat: improve solvers #233

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
gabrielfrasantos wants to merge 1 commit into
base: main
Choose a base branch
from
Open

feat: improve solvers #233

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
64 changes: 40 additions & 24 deletions numerical/solvers/LevinsonDurbin.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@ namespace solvers

private:
bool IsToeplitz(const InputMatrix& A) const;
static T WeightedTail(const InputVector& r, const SolutionVector& v, size_t n);
};

// Implementation //
Expand All @@ -35,41 +36,56 @@ namespace solvers
{
really_assert(IsToeplitz(A));

auto [first_row, first_col] = math::ToeplitzMatrix<T, N>::ExtractToeplitzVectors(A);
auto toeplitz = math::ToeplitzMatrix<T, N>(first_row, first_col);
auto first_row = math::ToeplitzMatrix<T, N>::ExtractToeplitzVectors(A).first;

SolutionVector phi;
SolutionVector prev_phi;
SolutionVector k;
SolutionVector x;
SolutionVector y;
SolutionVector prev_y;

T prev_error = first_row.at(0, 0);
const T r0 = first_row.at(0, 0);
T error = r0;
T reflection = T(0.0f);

k.at(0, 0) = b.at(0, 0) / prev_error;
phi.at(0, 0) = k.at(0, 0);
prev_error *= (T(1.0f) - k.at(0, 0) * k.at(0, 0));
x.at(0, 0) = b.at(0, 0) / r0;

for (size_t n = 1; n < N; ++n)
if constexpr (N > 1)
{
T alpha = b.at(n, 0);
y.at(0, 0) = -first_row.at(1, 0) / r0;
reflection = y.at(0, 0);
}

for (size_t j = 0; j < n; ++j)
alpha -= phi.at(j, 0) * first_row.at(n - j, 0);
for (size_t n = 1; n < N; ++n)
{
error *= (T(1.0f) - reflection * reflection);

k.at(n, 0) = alpha / prev_error;
const T mu = (b.at(n, 0) - WeightedTail(first_row, x, n)) / error;

for (size_t j = 0; j < n; ++j)
prev_phi.at(j, 0) = phi.at(j, 0);

for (size_t j = 0; j <= n; ++j)
if (j < n)
phi.at(j, 0) = prev_phi.at(j, 0) + k.at(n, 0) * prev_phi.at(n - 1 - j, 0);
else
phi.at(n, 0) = k.at(n, 0);

prev_error *= (T(1.0f) - k.at(n, 0) * k.at(n, 0));
x.at(j, 0) += mu * y.at(n - 1 - j, 0);
x.at(n, 0) = mu;

if (n + 1 < N)
{
reflection = -(first_row.at(n + 1, 0) + WeightedTail(first_row, y, n)) / error;

for (size_t j = 0; j < n; ++j)
prev_y.at(j, 0) = y.at(j, 0);
for (size_t j = 0; j < n; ++j)
y.at(j, 0) = prev_y.at(j, 0) + reflection * prev_y.at(n - 1 - j, 0);
y.at(n, 0) = reflection;
}
}

return phi;
return x;
}

template<typename T, std::size_t N>
T LevinsonDurbin<T, N>::WeightedTail(const InputVector& r, const SolutionVector& v, size_t n)
{
T acc = T(0.0f);
for (size_t i = 1; i <= n; ++i)
acc += r.at(i, 0) * v.at(n - i, 0);
return acc;
}

template<typename T, std::size_t N>
Expand Down
38 changes: 36 additions & 2 deletions numerical/solvers/test/TestConditionNumber.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,14 +25,38 @@ TEST_F(ConditionNumberTest, Identity)
EXPECT_NEAR(*result, 1.0f, math::Tolerance<float>());
}

TEST_F(ConditionNumberTest, WellConditioned)
TEST_F(ConditionNumberTest, WellConditioned2x2)
{
auto result = solvers::ConditionNumber(a);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 3.2f, math::Tolerance<float>());
}

TEST_F(ConditionNumberTest, SingularReturnsNullopt)
TEST_F(ConditionNumberTest, DiagonalScaling3x3)
{
math::SquareMatrix<float, 3> diag{
{ 1.0f, 0.0f, 0.0f },
{ 0.0f, 2.0f, 0.0f },
{ 0.0f, 0.0f, 4.0f }
};
auto result = solvers::ConditionNumber(diag);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(*result, 4.0f, math::Tolerance<float>());
}

TEST_F(ConditionNumberTest, IllConditionedHilbert3x3)
{
math::SquareMatrix<float, 3> hilbert{
{ 1.0f, 0.5f, 1.0f / 3.0f },
{ 0.5f, 1.0f / 3.0f, 0.25f },
{ 1.0f / 3.0f, 0.25f, 0.2f }
Comment on lines +50 to +52

@github-actions github-actions Bot Aug 4, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[MegaLinter] reported by reviewdog 🐶

Suggested change
{ 1.0f, 0.5f, 1.0f / 3.0f },
{ 0.5f, 1.0f / 3.0f, 0.25f },
{ 1.0f / 3.0f, 0.25f, 0.2f }
{ 1.0f, 0.5f, 1.0f / 3.0f },
{ 0.5f, 1.0f / 3.0f, 0.25f },
{ 1.0f / 3.0f, 0.25f, 0.2f }

};
auto result = solvers::ConditionNumber(hilbert);
ASSERT_TRUE(result.has_value());
EXPECT_GT(*result, 100.0f);
}

TEST_F(ConditionNumberTest, ZeroRowReturnsNullopt)
{
math::SquareMatrix<float, 2> singular{
{ 1.0f, 2.0f },
Expand All @@ -41,3 +65,13 @@ TEST_F(ConditionNumberTest, SingularReturnsNullopt)
auto result = solvers::ConditionNumber(singular);
EXPECT_FALSE(result.has_value());
}

TEST_F(ConditionNumberTest, AllTinyRowReturnsNullopt)
{
math::SquareMatrix<float, 2> nearZero{
{ 1.0f, 2.0f },

@github-actions github-actions Bot Aug 4, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[MegaLinter] reported by reviewdog 🐶

Suggested change
{ 1.0f, 2.0f },
{ 1.0f, 2.0f },

{ 5e-13f, 5e-13f }
};
auto result = solvers::ConditionNumber(nearZero);
EXPECT_FALSE(result.has_value());
}
126 changes: 101 additions & 25 deletions numerical/solvers/test/TestDiscreteAlgebraicRiccatiEquation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
#include "numerical/math/QNumber.hpp"
#include "numerical/math/Matrix.hpp"
#include "numerical/math/Tolerance.hpp"
#include "numerical/solvers/DiscreteAlgebraicRiccatiEquation.hpp"
#include <cmath>
#include <gtest/gtest.h>

namespace
Expand All @@ -8,71 +10,145 @@ namespace
{
protected:
solvers::DiscreteAlgebraicRiccatiEquation<float, 1, 1> scalarSolver;
solvers::DiscreteAlgebraicRiccatiEquation<float, 2, 1> vectorSolver;
solvers::DiscreteAlgebraicRiccatiEquation<float, 2, 1> twoByOneSolver;
solvers::DiscreteAlgebraicRiccatiEquation<float, 2, 2> twoByTwoSolver;

static float DareResidual2x1(
const math::SquareMatrix<float, 2>& P,
const math::SquareMatrix<float, 2>& A,
const math::Matrix<float, 2, 1>& B,
const math::SquareMatrix<float, 2>& Q,
const math::SquareMatrix<float, 1>& R)
{
auto BtP = B.Transpose() * P;
auto S = R + BtP * B;
auto AtP = A.Transpose() * P;
auto AtPA = AtP * A;
auto BtPA = BtP * A;
auto correction = AtP * B * (math::Matrix<float, 1, 2>{ { BtPA.at(0, 0), BtPA.at(0, 1) } } * (1.0f / S.at(0, 0)));
auto Prhs = AtPA - correction + Q;

float maxErr = 0.0f;
for (std::size_t i = 0; i < 2; ++i)
for (std::size_t j = 0; j < 2; ++j)
maxErr = std::max(maxErr, std::abs(Prhs.at(i, j) - P.at(i, j)));
return maxErr;
}
};
}

TEST_F(TestDiscreteAlgebraicRiccatiEquation, solve_converges_immediately_when_dynamics_are_zero)
TEST_F(TestDiscreteAlgebraicRiccatiEquation, scalar_unit_system_matches_golden_ratio_root)
{
math::SquareMatrix<float, 1> A{ { 0.0f } };
math::SquareMatrix<float, 1> A{ { 1.0f } };
math::Matrix<float, 1, 1> B{ { 1.0f } };
math::SquareMatrix<float, 1> Q{ { 2.0f } };
math::SquareMatrix<float, 1> Q{ { 1.0f } };
math::SquareMatrix<float, 1> R{ { 1.0f } };

auto P = scalarSolver.Solve(A, B, Q, R);

EXPECT_NEAR(P.at(0, 0), 2.0f, 1e-3f);
constexpr float expected = 1.6180339887f;
EXPECT_NEAR(P.at(0, 0), expected, 1e-3f);
}

TEST_F(TestDiscreteAlgebraicRiccatiEquation, solve_iterates_riccati_update)
TEST_F(TestDiscreteAlgebraicRiccatiEquation, scalar_stable_system_matches_closed_form_quadratic_root)
{
math::SquareMatrix<float, 1> A{ { 1.0f } };
math::SquareMatrix<float, 1> A{ { 0.9f } };
math::Matrix<float, 1, 1> B{ { 1.0f } };
math::SquareMatrix<float, 1> Q{ { 1.0f } };
math::SquareMatrix<float, 1> R{ { 1.0f } };

auto P = scalarSolver.Solve(A, B, Q, R);

EXPECT_GT(P.at(0, 0), Q.at(0, 0));
constexpr float expected = 1.483901f;
EXPECT_NEAR(P.at(0, 0), expected, 1e-3f);
}

TEST_F(TestDiscreteAlgebraicRiccatiEquation, solve_produces_symmetric_result)
TEST_F(TestDiscreteAlgebraicRiccatiEquation, zero_dynamics_converges_immediately_to_Q)
{
math::SquareMatrix<float, 1> A{ { 0.0f } };
math::Matrix<float, 1, 1> B{ { 1.0f } };
math::SquareMatrix<float, 1> Q{ { 2.0f } };
math::SquareMatrix<float, 1> R{ { 1.0f } };

auto P = scalarSolver.Solve(A, B, Q, R);

EXPECT_NEAR(P.at(0, 0), 2.0f, math::Tolerance<float>());
}

TEST_F(TestDiscreteAlgebraicRiccatiEquation, two_by_one_solution_satisfies_dare_residual)
{
math::SquareMatrix<float, 2> A{
{ 1.0f, 1.0f },
{ 1.0f, 0.1f },
{ 0.0f, 1.0f }
};

math::Matrix<float, 2, 1> B{
{ 0.5f },
{ 1.0f }
{ 0.005f },
{ 0.1f }
};

auto Q = math::SquareMatrix<float, 2>::Identity();
math::SquareMatrix<float, 1> R{ { 1.0f } };

auto P = vectorSolver.Solve(A, B, Q, R);
auto P = twoByOneSolver.Solve(A, B, Q, R);

EXPECT_NEAR(P.at(0, 1), P.at(1, 0), 1e-3f);
float residual = DareResidual2x1(P, A, B, Q, R);
EXPECT_NEAR(residual, 0.0f, 1e-2f);
}

TEST_F(TestDiscreteAlgebraicRiccatiEquation, solve_produces_positive_diagonal)
TEST_F(TestDiscreteAlgebraicRiccatiEquation, two_by_one_solution_is_symmetric)
{
math::SquareMatrix<float, 2> A{
{ 0.5f, 0.0f },
{ 0.0f, 0.3f }
{ 1.0f, 0.1f },
{ 0.0f, 1.0f }
};

math::Matrix<float, 2, 1> B{
{ 1.0f },
{ 0.0f }
{ 0.005f },
{ 0.1f }
};
auto Q = math::SquareMatrix<float, 2>::Identity();
math::SquareMatrix<float, 1> R{ { 1.0f } };

auto P = twoByOneSolver.Solve(A, B, Q, R);

EXPECT_NEAR(P.at(0, 1), P.at(1, 0), math::Tolerance<float>());
}

TEST_F(TestDiscreteAlgebraicRiccatiEquation, two_by_one_solution_is_positive_definite)
{
math::SquareMatrix<float, 2> A{
{ 1.0f, 0.1f },
{ 0.0f, 1.0f }
};
math::Matrix<float, 2, 1> B{
{ 0.005f },
{ 0.1f }
};
auto Q = math::SquareMatrix<float, 2>::Identity();
math::SquareMatrix<float, 1> R{ { 1.0f } };

auto P = vectorSolver.Solve(A, B, Q, R);
auto P = twoByOneSolver.Solve(A, B, Q, R);

float det = P.at(0, 0) * P.at(1, 1) - P.at(0, 1) * P.at(1, 0);
EXPECT_GT(P.at(0, 0), 0.0f);
EXPECT_GT(det, 0.0f);
}

TEST_F(TestDiscreteAlgebraicRiccatiEquation, two_by_two_full_input_solution_is_symmetric_and_positive_definite)
{
math::SquareMatrix<float, 2> A{
{ 0.8f, 0.2f },
{ 0.0f, 0.7f }
};
math::Matrix<float, 2, 2> B{
{ 1.0f, 0.0f },
{ 0.0f, 1.0f }
};
auto Q = math::SquareMatrix<float, 2>::Identity();
auto R = math::SquareMatrix<float, 2>::Identity();

auto P = twoByTwoSolver.Solve(A, B, Q, R);

float det = P.at(0, 0) * P.at(1, 1) - P.at(0, 1) * P.at(1, 0);
EXPECT_NEAR(P.at(0, 1), P.at(1, 0), math::Tolerance<float>());
EXPECT_GT(P.at(0, 0), 0.0f);
EXPECT_GT(P.at(1, 1), 0.0f);
EXPECT_GT(det, 0.0f);
}
Loading
Loading