diff --git a/numerical/solvers/LevinsonDurbin.hpp b/numerical/solvers/LevinsonDurbin.hpp index f0516f5..d9137ca 100644 --- a/numerical/solvers/LevinsonDurbin.hpp +++ b/numerical/solvers/LevinsonDurbin.hpp @@ -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 // @@ -35,41 +36,56 @@ namespace solvers { really_assert(IsToeplitz(A)); - auto [first_row, first_col] = math::ToeplitzMatrix::ExtractToeplitzVectors(A); - auto toeplitz = math::ToeplitzMatrix(first_row, first_col); + auto first_row = math::ToeplitzMatrix::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 + T LevinsonDurbin::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 diff --git a/numerical/solvers/test/TestConditionNumber.cpp b/numerical/solvers/test/TestConditionNumber.cpp index 47940bd..f5fa6ee 100644 --- a/numerical/solvers/test/TestConditionNumber.cpp +++ b/numerical/solvers/test/TestConditionNumber.cpp @@ -25,14 +25,38 @@ TEST_F(ConditionNumberTest, Identity) EXPECT_NEAR(*result, 1.0f, math::Tolerance()); } -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()); } -TEST_F(ConditionNumberTest, SingularReturnsNullopt) +TEST_F(ConditionNumberTest, DiagonalScaling3x3) +{ + math::SquareMatrix 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()); +} + +TEST_F(ConditionNumberTest, IllConditionedHilbert3x3) +{ + math::SquareMatrix 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 } + }; + auto result = solvers::ConditionNumber(hilbert); + ASSERT_TRUE(result.has_value()); + EXPECT_GT(*result, 100.0f); +} + +TEST_F(ConditionNumberTest, ZeroRowReturnsNullopt) { math::SquareMatrix singular{ { 1.0f, 2.0f }, @@ -41,3 +65,13 @@ TEST_F(ConditionNumberTest, SingularReturnsNullopt) auto result = solvers::ConditionNumber(singular); EXPECT_FALSE(result.has_value()); } + +TEST_F(ConditionNumberTest, AllTinyRowReturnsNullopt) +{ + math::SquareMatrix nearZero{ + { 1.0f, 2.0f }, + { 5e-13f, 5e-13f } + }; + auto result = solvers::ConditionNumber(nearZero); + EXPECT_FALSE(result.has_value()); +} diff --git a/numerical/solvers/test/TestDiscreteAlgebraicRiccatiEquation.cpp b/numerical/solvers/test/TestDiscreteAlgebraicRiccatiEquation.cpp index 89f813a..1f4be05 100644 --- a/numerical/solvers/test/TestDiscreteAlgebraicRiccatiEquation.cpp +++ b/numerical/solvers/test/TestDiscreteAlgebraicRiccatiEquation.cpp @@ -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 #include namespace @@ -8,71 +10,145 @@ namespace { protected: solvers::DiscreteAlgebraicRiccatiEquation scalarSolver; - solvers::DiscreteAlgebraicRiccatiEquation vectorSolver; + solvers::DiscreteAlgebraicRiccatiEquation twoByOneSolver; + solvers::DiscreteAlgebraicRiccatiEquation twoByTwoSolver; + + static float DareResidual2x1( + const math::SquareMatrix& P, + const math::SquareMatrix& A, + const math::Matrix& B, + const math::SquareMatrix& Q, + const math::SquareMatrix& 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{ { 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 A{ { 0.0f } }; + math::SquareMatrix A{ { 1.0f } }; math::Matrix B{ { 1.0f } }; - math::SquareMatrix Q{ { 2.0f } }; + math::SquareMatrix Q{ { 1.0f } }; math::SquareMatrix 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 A{ { 1.0f } }; + math::SquareMatrix A{ { 0.9f } }; math::Matrix B{ { 1.0f } }; math::SquareMatrix Q{ { 1.0f } }; math::SquareMatrix 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 A{ { 0.0f } }; + math::Matrix B{ { 1.0f } }; + math::SquareMatrix Q{ { 2.0f } }; + math::SquareMatrix R{ { 1.0f } }; + + auto P = scalarSolver.Solve(A, B, Q, R); + + EXPECT_NEAR(P.at(0, 0), 2.0f, math::Tolerance()); +} + +TEST_F(TestDiscreteAlgebraicRiccatiEquation, two_by_one_solution_satisfies_dare_residual) { math::SquareMatrix A{ - { 1.0f, 1.0f }, + { 1.0f, 0.1f }, { 0.0f, 1.0f } }; - math::Matrix B{ - { 0.5f }, - { 1.0f } + { 0.005f }, + { 0.1f } }; - auto Q = math::SquareMatrix::Identity(); math::SquareMatrix 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 A{ - { 0.5f, 0.0f }, - { 0.0f, 0.3f } + { 1.0f, 0.1f }, + { 0.0f, 1.0f } }; - math::Matrix B{ - { 1.0f }, - { 0.0f } + { 0.005f }, + { 0.1f } }; + auto Q = math::SquareMatrix::Identity(); + math::SquareMatrix R{ { 1.0f } }; + auto P = twoByOneSolver.Solve(A, B, Q, R); + + EXPECT_NEAR(P.at(0, 1), P.at(1, 0), math::Tolerance()); +} + +TEST_F(TestDiscreteAlgebraicRiccatiEquation, two_by_one_solution_is_positive_definite) +{ + math::SquareMatrix A{ + { 1.0f, 0.1f }, + { 0.0f, 1.0f } + }; + math::Matrix B{ + { 0.005f }, + { 0.1f } + }; auto Q = math::SquareMatrix::Identity(); math::SquareMatrix 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 A{ + { 0.8f, 0.2f }, + { 0.0f, 0.7f } + }; + math::Matrix B{ + { 1.0f, 0.0f }, + { 0.0f, 1.0f } + }; + auto Q = math::SquareMatrix::Identity(); + auto R = math::SquareMatrix::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()); EXPECT_GT(P.at(0, 0), 0.0f); - EXPECT_GT(P.at(1, 1), 0.0f); + EXPECT_GT(det, 0.0f); } diff --git a/numerical/solvers/test/TestDurandKerner.cpp b/numerical/solvers/test/TestDurandKerner.cpp index d71bb39..6f5eed4 100644 --- a/numerical/solvers/test/TestDurandKerner.cpp +++ b/numerical/solvers/test/TestDurandKerner.cpp @@ -1,115 +1,152 @@ #include "numerical/solvers/DurandKerner.hpp" #include +#include #include namespace { - template class TestDurandKerner : public ::testing::Test { protected: - solvers::DurandKerner solver; + solvers::DurandKerner solver; }; - - using TestTypes = ::testing::Types; - TYPED_TEST_SUITE(TestDurandKerner, TestTypes); } -TYPED_TEST(TestDurandKerner, finds_roots_of_linear_polynomial) +TEST_F(TestDurandKerner, linear_polynomial_returns_single_exact_root) { - std::array coefficients = { TypeParam(2.0), TypeParam(4.0) }; + std::array coefficients{ 2.0f, 4.0f }; - auto roots = this->solver.Solve(coefficients); + auto roots = solver.Solve(coefficients); ASSERT_EQ(roots.size(), 1u); - EXPECT_NEAR(roots[0].Real(), -2.0, 1e-4); - EXPECT_NEAR(roots[0].Imaginary(), 0.0, 1e-4); + EXPECT_NEAR(roots[0].Real(), -2.0f, 1e-4f); + EXPECT_NEAR(roots[0].Imaginary(), 0.0f, 1e-4f); } -TYPED_TEST(TestDurandKerner, finds_real_roots_of_quadratic) +TEST_F(TestDurandKerner, real_quadratic_roots_match_factored_form) { - std::array coefficients = { TypeParam(1.0), TypeParam(-3.0), TypeParam(2.0) }; + std::array coefficients{ 1.0f, -3.0f, 2.0f }; - auto roots = this->solver.Solve(coefficients); + auto roots = solver.Solve(coefficients); ASSERT_EQ(roots.size(), 2u); - EXPECT_NEAR(roots[0].Real(), 1.0, 1e-4); - EXPECT_NEAR(roots[0].Imaginary(), 0.0, 1e-4); - EXPECT_NEAR(roots[1].Real(), 2.0, 1e-4); - EXPECT_NEAR(roots[1].Imaginary(), 0.0, 1e-4); + EXPECT_NEAR(roots[0].Real(), 1.0f, 1e-4f); + EXPECT_NEAR(roots[0].Imaginary(), 0.0f, 1e-4f); + EXPECT_NEAR(roots[1].Real(), 2.0f, 1e-4f); + EXPECT_NEAR(roots[1].Imaginary(), 0.0f, 1e-4f); } -TYPED_TEST(TestDurandKerner, finds_complex_roots_of_quadratic) +TEST_F(TestDurandKerner, complex_conjugate_roots_have_unit_imaginary_magnitude) { - std::array coefficients = { TypeParam(1.0), TypeParam(0.0), TypeParam(1.0) }; + std::array coefficients{ 1.0f, 0.0f, 1.0f }; - auto roots = this->solver.Solve(coefficients); + auto roots = solver.Solve(coefficients); ASSERT_EQ(roots.size(), 2u); - EXPECT_NEAR(roots[0].Real(), 0.0, 1e-4); - EXPECT_NEAR(std::abs(roots[0].Imaginary()), 1.0, 1e-4); - EXPECT_NEAR(roots[1].Real(), 0.0, 1e-4); - EXPECT_NEAR(std::abs(roots[1].Imaginary()), 1.0, 1e-4); + EXPECT_NEAR(roots[0].Real(), 0.0f, 1e-4f); + EXPECT_NEAR(std::abs(roots[0].Imaginary()), 1.0f, 1e-4f); + EXPECT_NEAR(roots[1].Real(), 0.0f, 1e-4f); + EXPECT_NEAR(std::abs(roots[1].Imaginary()), 1.0f, 1e-4f); } -TYPED_TEST(TestDurandKerner, finds_roots_of_cubic) +TEST_F(TestDurandKerner, cubic_roots_match_factored_form_1_2_3) { - std::array coefficients = { TypeParam(1.0), TypeParam(-6.0), TypeParam(11.0), TypeParam(-6.0) }; + std::array coefficients{ 1.0f, -6.0f, 11.0f, -6.0f }; - auto roots = this->solver.Solve(coefficients); + auto roots = solver.Solve(coefficients); ASSERT_EQ(roots.size(), 3u); - EXPECT_NEAR(roots[0].Real(), 1.0, 1e-3); - EXPECT_NEAR(roots[0].Imaginary(), 0.0, 1e-3); - EXPECT_NEAR(roots[1].Real(), 2.0, 1e-3); - EXPECT_NEAR(roots[1].Imaginary(), 0.0, 1e-3); - EXPECT_NEAR(roots[2].Real(), 3.0, 1e-3); - EXPECT_NEAR(roots[2].Imaginary(), 0.0, 1e-3); + EXPECT_NEAR(roots[0].Real(), 1.0f, 1e-3f); + EXPECT_NEAR(roots[0].Imaginary(), 0.0f, 1e-3f); + EXPECT_NEAR(roots[1].Real(), 2.0f, 1e-3f); + EXPECT_NEAR(roots[1].Imaginary(), 0.0f, 1e-3f); + EXPECT_NEAR(roots[2].Real(), 3.0f, 1e-3f); + EXPECT_NEAR(roots[2].Imaginary(), 0.0f, 1e-3f); } -TYPED_TEST(TestDurandKerner, finds_roots_of_quartic) +TEST_F(TestDurandKerner, quartic_roots_match_factored_form_1_2_3_4) { - std::array coefficients = { TypeParam(1.0), TypeParam(-10.0), TypeParam(35.0), TypeParam(-50.0), TypeParam(24.0) }; + std::array coefficients{ 1.0f, -10.0f, 35.0f, -50.0f, 24.0f }; - auto roots = this->solver.Solve(coefficients); + auto roots = solver.Solve(coefficients); ASSERT_EQ(roots.size(), 4u); - EXPECT_NEAR(roots[0].Real(), 1.0, 1e-2); - EXPECT_NEAR(roots[1].Real(), 2.0, 1e-2); - EXPECT_NEAR(roots[2].Real(), 3.0, 1e-2); - EXPECT_NEAR(roots[3].Real(), 4.0, 1e-2); + EXPECT_NEAR(roots[0].Real(), 1.0f, 1e-2f); + EXPECT_NEAR(roots[0].Imaginary(), 0.0f, 1e-2f); + EXPECT_NEAR(roots[1].Real(), 2.0f, 1e-2f); + EXPECT_NEAR(roots[1].Imaginary(), 0.0f, 1e-2f); + EXPECT_NEAR(roots[2].Real(), 3.0f, 1e-2f); + EXPECT_NEAR(roots[2].Imaginary(), 0.0f, 1e-2f); + EXPECT_NEAR(roots[3].Real(), 4.0f, 1e-2f); + EXPECT_NEAR(roots[3].Imaginary(), 0.0f, 1e-2f); } -TYPED_TEST(TestDurandKerner, finds_repeated_roots) +TEST_F(TestDurandKerner, repeated_root_both_approximations_converge_to_same_value) { - std::array coefficients = { TypeParam(1.0), TypeParam(-2.0), TypeParam(1.0) }; + std::array coefficients{ 1.0f, -2.0f, 1.0f }; - auto roots = this->solver.Solve(coefficients); + auto roots = solver.Solve(coefficients); ASSERT_EQ(roots.size(), 2u); - EXPECT_NEAR(roots[0].Real(), 1.0, 1e-3); - EXPECT_NEAR(roots[1].Real(), 1.0, 1e-3); + EXPECT_NEAR(roots[0].Real(), 1.0f, 1e-3f); + EXPECT_NEAR(roots[1].Real(), 1.0f, 1e-3f); } -TYPED_TEST(TestDurandKerner, returns_empty_for_constant_polynomial) +TEST_F(TestDurandKerner, constant_polynomial_returns_empty) { - std::array coefficients = { TypeParam(5.0) }; + std::array coefficients{ 5.0f }; - auto roots = this->solver.Solve(coefficients); + auto roots = solver.Solve(coefficients); EXPECT_EQ(roots.size(), 0u); } -TYPED_TEST(TestDurandKerner, finds_roots_of_second_order_system_polynomial) +TEST_F(TestDurandKerner, second_order_control_system_roots_match_analytic_poles) { - TypeParam wn(2.0); - TypeParam zeta(0.5); - std::array coefficients = { TypeParam(1.0), TypeParam(2.0) * zeta * wn, wn * wn }; + float wn = 2.0f; + float zeta = 0.5f; + std::array coefficients{ 1.0f, 2.0f * zeta * wn, wn * wn }; - auto roots = this->solver.Solve(coefficients); + auto roots = solver.Solve(coefficients); ASSERT_EQ(roots.size(), 2u); - EXPECT_NEAR(roots[0].Real(), double(-zeta * wn), 1e-3); - EXPECT_NEAR(roots[1].Real(), double(-zeta * wn), 1e-3); - EXPECT_NEAR(std::abs(roots[0].Imaginary()), double(wn * std::sqrt(TypeParam(1.0) - zeta * zeta)), 1e-3); + float expectedReal = -zeta * wn; + float expectedImag = wn * std::sqrt(1.0f - zeta * zeta); + EXPECT_NEAR(roots[0].Real(), expectedReal, 1e-3f); + EXPECT_NEAR(roots[1].Real(), expectedReal, 1e-3f); + EXPECT_NEAR(std::abs(roots[0].Imaginary()), expectedImag, 1e-3f); +} + +TEST_F(TestDurandKerner, each_root_satisfies_polynomial_residual_near_zero) +{ + std::array coefficients{ 1.0f, -6.0f, 11.0f, -6.0f }; + + auto roots = solver.Solve(coefficients); + + ASSERT_EQ(roots.size(), 3u); + for (std::size_t i = 0; i < roots.size(); ++i) + { + float re = roots[i].Real(); + float im = roots[i].Imaginary(); + float re2 = re * re - im * im; + float im2 = 2.0f * re * im; + float re3 = re2 * re - im2 * im; + float im3 = re2 * im + im2 * re; + float pRe = re3 - 6.0f * re2 + 11.0f * re - 6.0f; + float pIm = im3 - 6.0f * im2 + 11.0f * im; + EXPECT_NEAR(pRe, 0.0f, 1e-2f); + EXPECT_NEAR(pIm, 0.0f, 1e-2f); + } +} + +TEST_F(TestDurandKerner, same_input_produces_identical_real_parts_determinism) +{ + std::array coefficients{ 1.0f, -6.0f, 11.0f, -6.0f }; + + auto roots1 = solver.Solve(coefficients); + auto roots2 = solver.Solve(coefficients); + + ASSERT_EQ(roots1.size(), roots2.size()); + for (std::size_t i = 0; i < roots1.size(); ++i) + EXPECT_FLOAT_EQ(roots1[i].Real(), roots2[i].Real()); } diff --git a/numerical/solvers/test/TestGaussianElimination.cpp b/numerical/solvers/test/TestGaussianElimination.cpp index c617c56..f3fd02b 100644 --- a/numerical/solvers/test/TestGaussianElimination.cpp +++ b/numerical/solvers/test/TestGaussianElimination.cpp @@ -1,4 +1,5 @@ #include "numerical/math/QNumber.hpp" +#include "numerical/math/Tolerance.hpp" #include "numerical/solvers/GaussianElimination.hpp" #include @@ -21,6 +22,13 @@ namespace protected: solvers::GaussianElimination solver; }; + + class TestGaussianEliminationFloat3 + : public ::testing::Test + { + protected: + solvers::GaussianElimination solver3; + }; } TYPED_TEST(TestGaussianElimination, solve_identity_matrix_returns_rhs) @@ -99,3 +107,52 @@ TEST_F(TestGaussianEliminationFloat, solve_system_delegates_per_column) EXPECT_NEAR(result.at(0, 1), 1.0f, 0.01f); EXPECT_NEAR(result.at(1, 1), 0.0f, 0.01f); } + +TEST_F(TestGaussianEliminationFloat3, general_3x3_well_conditioned_full_solve) +{ + math::SquareMatrix a{ + { 2.0f, 1.0f, -1.0f }, + { -3.0f, -1.0f, 2.0f }, + { -2.0f, 1.0f, 2.0f } + }; + math::Vector b{ { 8.0f }, { -11.0f }, { -3.0f } }; + + auto x = solver3.Solve(a, b); + + EXPECT_NEAR(x.at(0, 0), 2.0f, math::Tolerance()); + EXPECT_NEAR(x.at(1, 0), 3.0f, math::Tolerance()); + EXPECT_NEAR(x.at(2, 0), -1.0f, math::Tolerance()); +} + +TEST_F(TestGaussianEliminationFloat3, solve_system_with_identity_rhs_yields_inverse) +{ + math::SquareMatrix a{ + { 2.0f, 1.0f, 0.0f }, + { 1.0f, 3.0f, 1.0f }, + { 0.0f, 1.0f, 2.0f } + }; + auto identity = math::SquareMatrix::Identity(); + + auto inv = solvers::SolveSystem(a, identity); + auto product = a * inv; + + for (std::size_t i = 0; i < 3; ++i) + for (std::size_t j = 0; j < 3; ++j) + EXPECT_NEAR(product.at(i, j), (i == j) ? 1.0f : 0.0f, math::Tolerance()); +} + +TEST_F(TestGaussianEliminationFloat, make_gaussian_elimination_factory_produces_working_solver) +{ + auto factorySolver = solvers::MakeGaussianElimination(); + + math::SquareMatrix a{ + { 3.0f, 1.0f }, + { 1.0f, 2.0f } + }; + math::Vector b{ { 9.0f }, { 8.0f } }; + + auto x = factorySolver.Solve(a, b); + + EXPECT_NEAR(x.at(0, 0), 2.0f, math::Tolerance()); + EXPECT_NEAR(x.at(1, 0), 3.0f, math::Tolerance()); +} diff --git a/numerical/solvers/test/TestJacobiEigenSolver.cpp b/numerical/solvers/test/TestJacobiEigenSolver.cpp index 3be1cc8..c4438cf 100644 --- a/numerical/solvers/test/TestJacobiEigenSolver.cpp +++ b/numerical/solvers/test/TestJacobiEigenSolver.cpp @@ -9,6 +9,7 @@ namespace protected: solvers::JacobiEigenSolver solver2{}; solvers::JacobiEigenSolver solver3{}; + solvers::JacobiEigenSolver solver4{}; }; } @@ -118,3 +119,64 @@ TEST_F(TestJacobiEigenSolver, reconstructs_matrix_from_spectral_decomposition) for (std::size_t j = 0; j < 3; ++j) EXPECT_NEAR(reconstructed.at(i, j), a.at(i, j), 1e-4f); } + +TEST_F(TestJacobiEigenSolver, sweeps_is_zero_for_already_diagonal_input) +{ + math::Matrix a{ + { 5.0f, 0.0f, 0.0f }, + { 0.0f, 2.0f, 0.0f }, + { 0.0f, 0.0f, 8.0f } + }; + + EXPECT_TRUE(solver3.Solve(a)); + EXPECT_EQ(solver3.Sweeps(), std::size_t{ 0 }); +} + +TEST_F(TestJacobiEigenSolver, sweeps_nonzero_and_bounded_after_off_diagonal_solve) +{ + math::Matrix a{ + { 4.0f, 3.0f }, + { 3.0f, 4.0f } + }; + + EXPECT_TRUE(solver2.Solve(a)); + EXPECT_GT(solver2.Sweeps(), std::size_t{ 0 }); + EXPECT_LT(solver2.Sweeps(), std::size_t{ 50 }); +} + +TEST_F(TestJacobiEigenSolver, four_by_four_discrete_laplacian_eigenvalues) +{ + math::Matrix a{ + { 2.0f, -1.0f, 0.0f, 0.0f }, + { -1.0f, 2.0f, -1.0f, 0.0f }, + { 0.0f, -1.0f, 2.0f, -1.0f }, + { 0.0f, 0.0f, -1.0f, 2.0f } + }; + + EXPECT_TRUE(solver4.Solve(a)); + + EXPECT_NEAR(solver4.Eigenvalues().at(0, 0), 0.38197f, 1e-4f); + EXPECT_NEAR(solver4.Eigenvalues().at(1, 0), 1.38197f, 1e-4f); + EXPECT_NEAR(solver4.Eigenvalues().at(2, 0), 2.61803f, 1e-4f); + EXPECT_NEAR(solver4.Eigenvalues().at(3, 0), 3.61803f, 1e-4f); +} + +TEST_F(TestJacobiEigenSolver, determinism_same_input_produces_identical_output) +{ + math::Matrix a{ + { 3.0f, 1.0f, 0.5f }, + { 1.0f, 5.0f, 2.0f }, + { 0.5f, 2.0f, 4.0f } + }; + + solver3.Solve(a); + float ev0 = solver3.Eigenvalues().at(0, 0); + float ev1 = solver3.Eigenvalues().at(1, 0); + float ev2 = solver3.Eigenvalues().at(2, 0); + + solver3.Solve(a); + + EXPECT_FLOAT_EQ(solver3.Eigenvalues().at(0, 0), ev0); + EXPECT_FLOAT_EQ(solver3.Eigenvalues().at(1, 0), ev1); + EXPECT_FLOAT_EQ(solver3.Eigenvalues().at(2, 0), ev2); +} diff --git a/numerical/solvers/test/TestLevinsonDurbin.cpp b/numerical/solvers/test/TestLevinsonDurbin.cpp index aa2fd0e..d9fcd2f 100644 --- a/numerical/solvers/test/TestLevinsonDurbin.cpp +++ b/numerical/solvers/test/TestLevinsonDurbin.cpp @@ -1,4 +1,5 @@ #include "numerical/math/Tolerance.hpp" +#include "numerical/solvers/GaussianElimination.hpp" #include "numerical/solvers/LevinsonDurbin.hpp" #include @@ -11,6 +12,21 @@ namespace static constexpr std::size_t N = 2; solvers::LevinsonDurbin solver; }; + + class TestLevinsonDurbin3 + : public ::testing::Test + { + protected: + static constexpr std::size_t N = 3; + solvers::LevinsonDurbin solver; + solvers::GaussianElimination reference; + + math::Matrix BuildToeplitz3(float r0, float r1, float r2) + { + math::Vector row{ { r0 }, { r1 }, { r2 } }; + return math::ToeplitzMatrix(row).ToFullMatrix(); + } + }; } TEST_F(TestLevinsonDurbin, solve_symmetric_toeplitz_returns_correct_coefficients) @@ -36,3 +52,73 @@ TEST_F(TestLevinsonDurbin, solve_asserts_on_non_toeplitz_matrix) EXPECT_DEATH(solver.Solve(A, b), ""); } + +TEST_F(TestLevinsonDurbin3, solve_n3_doc_reference_example) +{ + auto A = BuildToeplitz3(4.0f, 2.0f, 1.0f); + math::Vector b{ { 2.0f }, { 1.0f }, { 0.5f } }; + + auto x = solver.Solve(A, b); + + EXPECT_NEAR(x.at(0, 0), 0.5f, math::Tolerance()); + EXPECT_NEAR(x.at(1, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(x.at(2, 0), 0.0f, math::Tolerance()); +} + +TEST_F(TestLevinsonDurbin3, solve_n3_residual_satisfies_axb) +{ + auto A = BuildToeplitz3(4.0f, 2.0f, 1.0f); + math::Vector b{ { 2.0f }, { 1.0f }, { 0.5f } }; + + auto x = solver.Solve(A, b); + + math::Vector row{ { 4.0f }, { 2.0f }, { 1.0f } }; + auto toeplitz = math::ToeplitzMatrix(row); + auto Ax = toeplitz * x; + + EXPECT_NEAR(Ax.at(0, 0), b.at(0, 0), math::Tolerance()); + EXPECT_NEAR(Ax.at(1, 0), b.at(1, 0), math::Tolerance()); + EXPECT_NEAR(Ax.at(2, 0), b.at(2, 0), math::Tolerance()); +} + +TEST_F(TestLevinsonDurbin3, solve_n3_matches_gaussian_elimination) +{ + auto A = BuildToeplitz3(6.0f, 3.0f, 1.0f); + math::Vector b{ { 1.0f }, { 2.0f }, { 3.0f } }; + + auto xLd = solver.Solve(A, b); + auto xGe = reference.Solve(A, b); + + EXPECT_NEAR(xLd.at(0, 0), xGe.at(0, 0), math::Tolerance()); + EXPECT_NEAR(xLd.at(1, 0), xGe.at(1, 0), math::Tolerance()); + EXPECT_NEAR(xLd.at(2, 0), xGe.at(2, 0), math::Tolerance()); +} + +TEST_F(TestLevinsonDurbin3, solve_n3_reflection_coefficients_bounded_for_spd_matrix) +{ + auto A = BuildToeplitz3(4.0f, 2.0f, 1.0f); + math::Vector b{ { 2.0f }, { 1.0f }, { 0.5f } }; + + auto x = solver.Solve(A, b); + + float mu0 = b.at(0, 0) / 4.0f; + float residual0 = 4.0f * (1.0f - mu0 * mu0); + float alpha1 = b.at(1, 0) - x.at(0, 0) * 2.0f; + float mu1 = alpha1 / residual0; + + EXPECT_LT(std::abs(mu0), 1.0f); + EXPECT_LT(std::abs(mu1), 1.0f); +} + +TEST_F(TestLevinsonDurbin3, solve_n3_deterministic_on_repeated_calls) +{ + auto A = BuildToeplitz3(4.0f, 2.0f, 1.0f); + math::Vector b{ { 2.0f }, { 1.0f }, { 0.5f } }; + + auto x1 = solver.Solve(A, b); + auto x2 = solver.Solve(A, b); + + EXPECT_FLOAT_EQ(x1.at(0, 0), x2.at(0, 0)); + EXPECT_FLOAT_EQ(x1.at(1, 0), x2.at(1, 0)); + EXPECT_FLOAT_EQ(x1.at(2, 0), x2.at(2, 0)); +}