diff --git a/doc/estimators/README.md b/doc/estimators/README.md index 08814c0f..98d548d5 100644 --- a/doc/estimators/README.md +++ b/doc/estimators/README.md @@ -17,9 +17,3 @@ Statistical estimation algorithms for fitting models to observed data and making | Algorithm | Description | |-----------------------------------------------------|--------------------------------------------------------------------------| | [Recursive Least Squares](RecursiveLeastSquares.md) | Sample-by-sample parameter estimation with exponential forgetting factor | - -## Consistency Metrics - -| Algorithm | Description | -|---------------------------------------------------------|-------------------------------------------------------------------------------------| -| [Consistency Metrics (NEES/NIS)](ConsistencyMetrics.md) | Normalised Estimation Error Squared and Normalised Innovation Squared with χ² gates | diff --git a/doc/filters/active/UnscentedKalmanFilter.md b/doc/filters/active/UnscentedKalmanFilter.md index 0a2d99a5..4bd075e0 100644 --- a/doc/filters/active/UnscentedKalmanFilter.md +++ b/doc/filters/active/UnscentedKalmanFilter.md @@ -146,7 +146,7 @@ graph LR |------------------------------------------------------------------|---------------------------------------------------------------------------------------| | [Kalman Filter](KalmanFilter.md) | The UKF reduces to the standard KF when $f$ and $h$ are linear | | [Extended Kalman Filter](ExtendedKalmanFilter.md) | Uses Jacobians instead of sigma points; simpler but less accurate for nonlinear cases | -| [Cholesky Decomposition](../../solvers/CholeskyDecomposition.md) | Used internally to generate sigma points from the covariance matrix | +| [Cholesky Decomposition](../../math/CholeskyDecomposition.md) | Used internally to generate sigma points from the covariance matrix | ## References & Further Reading diff --git a/doc/solvers/CholeskyDecomposition.md b/doc/math/CholeskyDecomposition.md similarity index 94% rename from doc/solvers/CholeskyDecomposition.md rename to doc/math/CholeskyDecomposition.md index 651e1c70..814aeb7e 100644 --- a/doc/solvers/CholeskyDecomposition.md +++ b/doc/math/CholeskyDecomposition.md @@ -64,8 +64,9 @@ $$L = \begin{bmatrix} 2 & 0 \\ 1 & 2 \end{bmatrix}$$ | Algorithm | Relationship | |-----------------------------------------------------------------------|-------------------------------------------------------------------| -| [Unscented Kalman Filter](../filters/active/UnscentedKalmanFilter.md) | Uses Cholesky to generate sigma points from the covariance matrix | -| [Gaussian Elimination](GaussianElimination.md) | General-purpose alternative; does not exploit symmetry | +| [Unscented Kalman Filter](../filters/active/UnscentedKalmanFilter.md) | Uses `Factor` to generate sigma points from the covariance matrix | +| [Consistency Metrics](ConsistencyMetrics.md) | Uses `Solve` for the SPD covariance solve behind NEES/NIS | +| [Gaussian Elimination](../solvers/GaussianElimination.md) | General-purpose alternative; does not exploit symmetry | ## References & Further Reading diff --git a/doc/estimators/ConsistencyMetrics.md b/doc/math/ConsistencyMetrics.md similarity index 95% rename from doc/estimators/ConsistencyMetrics.md rename to doc/math/ConsistencyMetrics.md index 5c68c5d3..67e5dbf4 100644 --- a/doc/estimators/ConsistencyMetrics.md +++ b/doc/math/ConsistencyMetrics.md @@ -86,7 +86,7 @@ The average NEES over a Monte-Carlo ensemble of $M$ runs and $K$ time steps yiel ## Connections to Other Algorithms -NEES and NIS are statistical companions to the standard Kalman filter update step. They depend on the covariance propagation produced by the `filters/active` family (KF, EKF, UKF). The linear solve reuses `solvers::GaussianElimination`, and the state-error representation aligns with `estimators::EstimationMetrics`. +NEES and NIS are statistical companions to the standard Kalman filter update step. They depend on the covariance propagation produced by the `filters/active` family (KF, EKF, UKF). The covariance being symmetric positive-definite, the linear solve reuses `math::CholeskyDecomposition::Solve` (factorisation plus forward/back substitution), keeping the metric a dependency-free `math` primitive. ## References & Further Reading diff --git a/doc/math/README.md b/doc/math/README.md index 491bf8ba..16f7b0eb 100644 --- a/doc/math/README.md +++ b/doc/math/README.md @@ -11,7 +11,9 @@ Core mathematical primitives for numerical computation. | [MatrixNorms](MatrixNorms.md) | Frobenius, 1-norm, infinity-norm on matrices; vector L2 norm/normalize | | [Householder Transform](HouseholderTransform.md) | Householder reflector for a sub-column — orthogonal, backward-stable factorization primitive | | [Givens Rotation](GivensRotation.md) | Plane rotation zeroing one entry — streaming/sparse factorization primitive | -| [Triangular Solve](TriangularSolve.md) | Upper-triangular back-substitution shared by Gaussian elimination and QR | +| [Triangular Solve](TriangularSolve.md) | Lower/upper-triangular forward and back-substitution shared by Gaussian elimination, Cholesky, and QR | +| [Cholesky Decomposition](CholeskyDecomposition.md) | SPD factorization $A=LL^T$ with `Factor` (L) and `Solve` (SPD system) — twice as fast as LU | | [Matrix Operations](MatrixOperations.md) | Structural matrix utilities — `Symmetrize` (closest symmetric matrix) | | [Step Response Metrics](StepResponseMetrics.md) | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector | | [Matrix Exponential](MatrixExponential.md) | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine | +| [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared | diff --git a/doc/solvers/JacobiEigenSolver.md b/doc/solvers/JacobiEigenSolver.md index b53e4e95..9ec43d70 100644 --- a/doc/solvers/JacobiEigenSolver.md +++ b/doc/solvers/JacobiEigenSolver.md @@ -119,11 +119,11 @@ graph LR JAC -.->|"eigenvalue floor keeps SPD"| CHOL ``` -| Algorithm | Relationship | -|----------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------| -| [QR Decomposition](QrDecomposition.md) | Both are built from orthogonal (Givens/Householder) transforms; QR underlies the alternative tridiagonal eigen-method | -| [Cholesky Decomposition](CholeskyDecomposition.md) | Requires symmetric positive-definite input; Jacobi eigenvalues certify or restore definiteness | -| [Spectral Radius](SpectralRadius.md) | Returns only the dominant eigenvalue magnitude; Jacobi returns the full spectrum and vectors | +| Algorithm | Relationship | +|------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------| +| [QR Decomposition](QrDecomposition.md) | Both are built from orthogonal (Givens/Householder) transforms; QR underlies the alternative tridiagonal eigen-method | +| [Cholesky Decomposition](../math/CholeskyDecomposition.md) | Requires symmetric positive-definite input; Jacobi eigenvalues certify or restore definiteness | +| [Spectral Radius](SpectralRadius.md) | Returns only the dominant eigenvalue magnitude; Jacobi returns the full spectrum and vectors | ## References & Further Reading diff --git a/doc/solvers/README.md b/doc/solvers/README.md index 484f7851..d88a4c6d 100644 --- a/doc/solvers/README.md +++ b/doc/solvers/README.md @@ -6,7 +6,6 @@ Numerical solvers for linear systems, polynomial roots, and matrix equations. | Algorithm | Description | |----------------------------------------------------------------------------|------------------------------------------------------------------------| -| [Cholesky Decomposition](CholeskyDecomposition.md) | Fast factorization for symmetric positive-definite matrices | | [Gaussian Elimination](GaussianElimination.md) | Direct solver for dense linear systems using partial pivoting | | [Levinson-Durbin](LevinsonDurbin.md) | Fast solver for Toeplitz linear systems exploiting structural symmetry | | [Durand-Kerner](DurandKerner.md) | Simultaneous iterative root-finder for polynomials | diff --git a/numerical/estimators/CMakeLists.txt b/numerical/estimators/CMakeLists.txt index d4362399..7f5e25c4 100644 --- a/numerical/estimators/CMakeLists.txt +++ b/numerical/estimators/CMakeLists.txt @@ -12,14 +12,8 @@ target_link_libraries(numerical.estimators ${NUMERICAL_VISIBILITY} ) target_sources(numerical.estimators PRIVATE - ConsistencyMetrics.hpp Estimator.hpp ) -numerical_add_coverage_sources(numerical.estimators - ConsistencyMetrics.cpp -) - add_subdirectory(offline) add_subdirectory(online) -add_subdirectory(test) diff --git a/numerical/estimators/offline/LinearRegression.hpp b/numerical/estimators/offline/LinearRegression.hpp index c45a35e0..947b60ad 100644 --- a/numerical/estimators/offline/LinearRegression.hpp +++ b/numerical/estimators/offline/LinearRegression.hpp @@ -4,8 +4,6 @@ #pragma GCC optimize("O3", "fast-math") #endif -#include "numerical/math/CompilerOptimizations.hpp" - #include "numerical/estimators/Estimator.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/solvers/QrDecomposition.hpp" diff --git a/numerical/estimators/offline/YuleWalker.hpp b/numerical/estimators/offline/YuleWalker.hpp index 83cb85ae..490f576d 100644 --- a/numerical/estimators/offline/YuleWalker.hpp +++ b/numerical/estimators/offline/YuleWalker.hpp @@ -6,6 +6,7 @@ #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Matrix.hpp" +#include "numerical/math/Statistics.hpp" #include "numerical/math/Toeplitz.hpp" #include "numerical/solvers/GaussianElimination.hpp" @@ -44,10 +45,7 @@ namespace estimators template OPTIMIZE_FOR_SPEED void YuleWalker::Fit(const InputVector& x) { - mean = T(0.0f); - for (std::size_t i = 0; i < Samples; ++i) - mean += x[i]; - mean = mean / T(static_cast(Samples)); + mean = math::Mean(x); auto r = ComputeAutocovariance(x, mean); diff --git a/numerical/estimators/offline/test/TestExpectationMaximization.cpp b/numerical/estimators/offline/test/TestExpectationMaximization.cpp index 774fdb85..fa114318 100644 --- a/numerical/estimators/offline/test/TestExpectationMaximization.cpp +++ b/numerical/estimators/offline/test/TestExpectationMaximization.cpp @@ -1,4 +1,5 @@ #include "numerical/estimators/offline/ExpectationMaximization.hpp" +#include "numerical/math/Tolerance.hpp" #include #include #include @@ -23,17 +24,18 @@ namespace Em em; - // Deliberately wrong initial guess — F = I, H known, Q/R inflated - EmParameters initialGuess{ - StateMatrix::Identity(), - MeasurementMatrix{ { 1.0f, 0.0f } }, - StateMatrix::Identity() * 0.1f, - MeasurementCovariance::Identity() * 0.5f, - StateVector{}, - StateMatrix::Identity() - }; - - // Hardcoded synthetic observations from true model (noisy position) + EmParameters MakeInitialGuess() + { + return EmParameters{ + StateMatrix::Identity(), + MeasurementMatrix{ { 1.0f, 0.0f } }, + StateMatrix::Identity() * 0.1f, + MeasurementCovariance::Identity() * 0.5f, + StateVector{}, + StateMatrix::Identity() + }; + } + std::array observations{ { MeasurementVector{ { 0.07f } }, MeasurementVector{ { 0.15f } }, MeasurementVector{ { 0.22f } }, @@ -45,103 +47,157 @@ namespace MeasurementVector{ { 0.64f } }, MeasurementVector{ { 0.73f } } } }; }; + + class TestExpectationMaximizationRecovery : public ::testing::Test + { + protected: + static constexpr std::size_t N4 = 4; + static constexpr std::size_t M2 = 2; + static constexpr std::size_t T20 = 20; + + using Em4 = estimators::ExpectationMaximization; + using EmParameters4 = Em4::EmParameters; + using StateMatrix4 = Em4::StateMatrix; + using StateVector4 = Em4::StateVector; + using MeasurementMatrix4 = Em4::MeasurementMatrix; + using MeasurementVector4 = Em4::MeasurementVector; + using MeasurementCovariance4 = Em4::MeasurementCovariance; + + Em4 em; + + EmParameters4 MakeInitialGuess() + { + return EmParameters4{ + StateMatrix4::Identity() * 0.8f, + MeasurementMatrix4{ { 0.5f, 0.5f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.5f, 0.5f } }, + StateMatrix4::Identity() * 0.1f, + MeasurementCovariance4::Identity() * 0.3f, + StateVector4{}, + StateMatrix4::Identity() + }; + } + + std::array MakeObservations() + { + std::array obs{}; + for (std::size_t t = 0; t < T20; ++t) + { + const float v = static_cast(t) * 0.05f; + obs[t] = MeasurementVector4{ { v }, { v * 0.5f } }; + } + return obs; + } + }; } TEST_F(TestExpectationMaximization, log_likelihood_improves_over_iterations) { - // One iteration versus ten: the log-likelihood should increase meaningfully. const float noConverge = std::numeric_limits::lowest(); - const auto result1 = em.Run(observations, T, initialGuess, 1, noConverge); + const auto result1 = em.Run(observations, T, MakeInitialGuess(), 1, noConverge); Em em10; - const auto result10 = em10.Run(observations, T, initialGuess, 10, noConverge); + const auto result10 = em10.Run(observations, T, MakeInitialGuess(), 10, noConverge); EXPECT_GT(result10.logLikelihood, result1.logLikelihood + 0.1f); } -TEST_F(TestExpectationMaximization, iteration_count_is_plausible) +TEST_F(TestExpectationMaximization, converged_flag_reflects_tolerance_outcome) { - const auto result = em.Run(observations, T, initialGuess, 50, 1e-3f); - - EXPECT_GE(result.iterations, std::size_t{ 2 }); - EXPECT_LE(result.iterations, std::size_t{ 50 }); -} - -TEST_F(TestExpectationMaximization, log_likelihood_is_finite) -{ - // The Kalman log-likelihood may be positive for a tight model (small R); - // it is not bounded above. Only finiteness is guaranteed. - const auto result = em.Run(observations, T, initialGuess, 50, 1e-3f); - - EXPECT_TRUE(std::isfinite(result.logLikelihood)); + EXPECT_FALSE(em.Run(observations, T, MakeInitialGuess(), 1, 1e-6f).converged); + Em em2; + EXPECT_TRUE(em2.Run(observations, T, MakeInitialGuess(), 50, 1000.0f).converged); } -TEST_F(TestExpectationMaximization, process_noise_is_symmetric) +TEST_F(TestExpectationMaximization, deterministic_repeated_runs_agree) { - const auto result = em.Run(observations, T, initialGuess, 50, 1e-3f); + const float noConverge = std::numeric_limits::lowest(); + const auto r1 = em.Run(observations, T, MakeInitialGuess(), 5, noConverge); + Em em2; + const auto r2 = em2.Run(observations, T, MakeInitialGuess(), 5, noConverge); - for (std::size_t i = 0; i < N; ++i) - for (std::size_t j = 0; j < N; ++j) - EXPECT_NEAR(result.parameters.Q.at(i, j), result.parameters.Q.at(j, i), 1e-5f); + EXPECT_FLOAT_EQ(r1.logLikelihood, r2.logLikelihood); + EXPECT_FLOAT_EQ(r1.parameters.H.at(0, 0), r2.parameters.H.at(0, 0)); + EXPECT_FLOAT_EQ(r1.parameters.H.at(0, 1), r2.parameters.H.at(0, 1)); } -TEST_F(TestExpectationMaximization, measurement_noise_is_symmetric) +TEST_F(TestExpectationMaximization, log_likelihood_increases_monotonically) { - const auto result = em.Run(observations, T, initialGuess, 50, 1e-3f); + const float neverConverge = std::numeric_limits::lowest(); + EmParameters params = MakeInitialGuess(); + float prevLl = neverConverge; - for (std::size_t i = 0; i < M; ++i) - for (std::size_t j = 0; j < M; ++j) - EXPECT_NEAR(result.parameters.R.at(i, j), result.parameters.R.at(j, i), 1e-5f); + for (std::size_t step = 0; step < 8; ++step) + { + Em emStep; + const auto result = emStep.Run(observations, T, params, 1, neverConverge); + if (prevLl != neverConverge) + EXPECT_GE(result.logLikelihood, prevLl - math::Tolerance()); + prevLl = result.logLikelihood; + params = result.parameters; + } } -TEST_F(TestExpectationMaximization, process_noise_diagonal_is_positive) +TEST_F(TestExpectationMaximization, noise_covariances_are_symmetric_positive_definite) { - const auto result = em.Run(observations, T, initialGuess, 50, 1e-3f); + const auto result = em.Run(observations, T, MakeInitialGuess(), 50, 1e-3f); for (std::size_t i = 0; i < N; ++i) + { EXPECT_GT(result.parameters.Q.at(i, i), 0.0f); + for (std::size_t j = i + 1; j < N; ++j) + EXPECT_NEAR(result.parameters.Q.at(i, j), result.parameters.Q.at(j, i), 1e-5f); + } + EXPECT_GT(result.parameters.R.at(0, 0), 0.0f); } -TEST_F(TestExpectationMaximization, measurement_noise_diagonal_is_positive) +TEST_F(TestExpectationMaximizationRecovery, observation_matrix_direction_recovered_from_structured_data) { - const auto result = em.Run(observations, T, initialGuess, 50, 1e-3f); + const auto obs = MakeObservations(); + EmParameters4 guess = MakeInitialGuess(); + const auto result = em.Run(obs, T20, guess, 80, 1e-4f); + + const float h00 = result.parameters.H.at(0, 0); + const float h01 = result.parameters.H.at(0, 1); + const float h10 = result.parameters.H.at(1, 0); + const float h11 = result.parameters.H.at(1, 1); + + EXPECT_TRUE(std::isfinite(h00)); + EXPECT_TRUE(std::isfinite(h01)); + EXPECT_TRUE(std::isfinite(h10)); + EXPECT_TRUE(std::isfinite(h11)); + + const float hNorm0 = std::sqrt(h00 * h00 + h01 * h01 + result.parameters.H.at(0, 2) * result.parameters.H.at(0, 2) + result.parameters.H.at(0, 3) * result.parameters.H.at(0, 3)); + const float hNorm1 = std::sqrt(h10 * h10 + h11 * h11 + result.parameters.H.at(1, 2) * result.parameters.H.at(1, 2) + result.parameters.H.at(1, 3) * result.parameters.H.at(1, 3)); + EXPECT_GT(hNorm0, 0.05f); + EXPECT_GT(hNorm1, 0.05f); + + EXPECT_GT(result.logLikelihood, std::numeric_limits::lowest() + 1.0f); + EXPECT_TRUE(std::isfinite(result.logLikelihood)); - for (std::size_t i = 0; i < M; ++i) + for (std::size_t i = 0; i < N4; ++i) + { + EXPECT_GT(result.parameters.Q.at(i, i), 0.0f); + EXPECT_TRUE(std::isfinite(result.parameters.Q.at(i, i))); + } + for (std::size_t i = 0; i < M2; ++i) EXPECT_GT(result.parameters.R.at(i, i), 0.0f); } -TEST_F(TestExpectationMaximization, initial_state_is_plausible) +TEST_F(TestExpectationMaximization, minimal_steps_returns_finite_symmetric_covariances) { - const auto result = em.Run(observations, T, initialGuess, 50, 1e-3f); + std::array minObs{}; + minObs[0] = MeasurementVector{ { 0.1f } }; + minObs[1] = MeasurementVector{ { 0.2f } }; - EXPECT_GT(result.parameters.initialState.at(0, 0), -2.0f); - EXPECT_LT(result.parameters.initialState.at(0, 0), 2.0f); -} + const auto result = em.Run(minObs, 2, MakeInitialGuess(), 5, 1e-6f); -TEST_F(TestExpectationMaximization, recovered_parameters_produce_finite_predictions) -{ - // With any converged parameters, running the smoother again must not produce NaN/Inf. - const float noConverge = std::numeric_limits::lowest(); - const auto result = em.Run(observations, T, initialGuess, 20, noConverge); - - Em em2; - const auto result2 = em2.Run(observations, T, result.parameters, 1, noConverge); - - EXPECT_TRUE(std::isfinite(result2.logLikelihood)); -} - -TEST_F(TestExpectationMaximization, log_likelihood_increases_monotonically) -{ - // Run EM one step at a time, feeding output as next input; verify non-decreasing log-likelihood - const float neverConverge = std::numeric_limits::lowest(); - EmParameters params = initialGuess; - float prevLl = neverConverge; - - for (std::size_t step = 0; step < 5; ++step) + for (std::size_t i = 0; i < N; ++i) { - const auto result = em.Run(observations, T, params, 1, neverConverge); - if (prevLl != neverConverge) - EXPECT_GE(result.logLikelihood, prevLl - 1e-3f); - prevLl = result.logLikelihood; - params = result.parameters; + EXPECT_TRUE(std::isfinite(result.parameters.Q.at(i, i))); + EXPECT_GT(result.parameters.Q.at(i, i), 0.0f); + for (std::size_t j = i + 1; j < N; ++j) + EXPECT_NEAR(result.parameters.Q.at(i, j), result.parameters.Q.at(j, i), 1e-4f); } + EXPECT_TRUE(std::isfinite(result.parameters.R.at(0, 0))); + EXPECT_GT(result.parameters.R.at(0, 0), 0.0f); + EXPECT_TRUE(std::isfinite(result.logLikelihood)); } diff --git a/numerical/estimators/offline/test/TestLinearRegression.cpp b/numerical/estimators/offline/test/TestLinearRegression.cpp index a71830fd..6e1d1c97 100644 --- a/numerical/estimators/offline/test/TestLinearRegression.cpp +++ b/numerical/estimators/offline/test/TestLinearRegression.cpp @@ -1,30 +1,26 @@ #include "numerical/estimators/offline/LinearRegression.hpp" +#include "numerical/math/QNumber.hpp" +#include "numerical/math/Tolerance.hpp" +#include #include namespace { - template - bool AreFloatsNear(T a, T b, float epsilon = 1e-4f) - { - if constexpr (std::is_same_v) - return std::abs(a - b) < epsilon; - else - return std::abs(a.ToFloat() - b.ToFloat()) < epsilon; - } - template class LinearRegressionTest : public ::testing::Test { protected: - static constexpr size_t Samples = 4; - static constexpr size_t Features = 2; + static constexpr std::size_t Samples = 4; + static constexpr std::size_t Features = 2; using EstimatorType = estimators::LinearRegression; using MatrixType = math::Matrix; using TargetType = math::Matrix; using InputType = typename EstimatorType::InputMatrix; + EstimatorType estimator; + static T MakeValue(float f) { return T(std::max(std::min(f, 0.1f), -0.1f)); @@ -33,10 +29,10 @@ namespace MatrixType MakeFeatureMatrix(const std::initializer_list>& values) { MatrixType result; - size_t i = 0; + std::size_t i = 0; for (const auto& row : values) { - size_t j = 0; + std::size_t j = 0; for (float value : row) { result.at(i, j) = MakeValue(value); @@ -50,7 +46,7 @@ namespace TargetType MakeTargetVector(const std::initializer_list& values) { TargetType result; - size_t i = 0; + std::size_t i = 0; for (float value : values) { result.at(i, 0) = MakeValue(value); @@ -62,7 +58,7 @@ namespace InputType MakeInputVector(const std::initializer_list& values) { InputType result; - size_t i = 0; + std::size_t i = 0; for (float value : values) { result.at(i, 0) = MakeValue(value); @@ -76,10 +72,8 @@ namespace TYPED_TEST_SUITE(LinearRegressionTest, TestTypes); } -TYPED_TEST(LinearRegressionTest, SimpleLinearFit) +TYPED_TEST(LinearRegressionTest, FitRecoversTwoFeatureCoefficients) { - typename TestFixture::EstimatorType estimator; - auto X = this->MakeFeatureMatrix({ { 0.02f, 0.03f }, { 0.03f, 0.04f }, { 0.04f, 0.02f }, @@ -90,42 +84,38 @@ TYPED_TEST(LinearRegressionTest, SimpleLinearFit) 0.04f * 0.05f + 0.02f * 0.03f + 0.01f, 0.05f * 0.05f + 0.05f * 0.03f + 0.01f }); - estimator.Fit(X, y); + this->estimator.Fit(X, y); - const auto& coef = estimator.Coefficients(); - EXPECT_TRUE(AreFloatsNear(coef.at(0, 0), this->MakeValue(0.01f))); - EXPECT_TRUE(AreFloatsNear(coef.at(1, 0), this->MakeValue(0.05f))); - EXPECT_TRUE(AreFloatsNear(coef.at(2, 0), this->MakeValue(0.03f))); + const auto& coef = this->estimator.Coefficients(); + EXPECT_NEAR(math::ToFloat(coef.at(0, 0)), math::ToFloat(this->MakeValue(0.01f)), math::Tolerance()); + EXPECT_NEAR(math::ToFloat(coef.at(1, 0)), math::ToFloat(this->MakeValue(0.05f)), math::Tolerance()); + EXPECT_NEAR(math::ToFloat(coef.at(2, 0)), math::ToFloat(this->MakeValue(0.03f)), math::Tolerance()); } -TYPED_TEST(LinearRegressionTest, PredictNewValues) +TYPED_TEST(LinearRegressionTest, PredictReturnsInterpolatedValue) { - typename TestFixture::EstimatorType estimator; - auto X = this->MakeFeatureMatrix({ { 0.01f, 0.01f }, { 0.02f, 0.02f }, { 0.03f, 0.01f }, { 0.02f, 0.03f } }); auto y = this->MakeTargetVector({ - 0.01f * 0.02f + 0.01f * 0.01f + 0.01f, // 0.0103 - 0.02f * 0.02f + 0.02f * 0.01f + 0.01f, // 0.0106 - 0.03f * 0.02f + 0.01f * 0.01f + 0.01f, // 0.0107 - 0.02f * 0.02f + 0.03f * 0.01f + 0.01f // 0.0107 + 0.01f * 0.02f + 0.01f * 0.01f + 0.01f, + 0.02f * 0.02f + 0.02f * 0.01f + 0.01f, + 0.03f * 0.02f + 0.01f * 0.01f + 0.01f, + 0.02f * 0.02f + 0.03f * 0.01f + 0.01f, }); - estimator.Fit(X, y); + this->estimator.Fit(X, y); auto newX = this->MakeInputVector({ 0.02f, 0.02f }); - auto predicted = estimator.Predict(newX); + auto predicted = this->estimator.Predict(newX); - EXPECT_TRUE(AreFloatsNear(predicted, this->MakeValue(0.0106f))); + EXPECT_NEAR(math::ToFloat(predicted), math::ToFloat(this->MakeValue(0.0106f)), math::Tolerance()); } -TYPED_TEST(LinearRegressionTest, NearZeroFeatures) +TYPED_TEST(LinearRegressionTest, NearZeroFeaturesYieldConstantPrediction) { - typename TestFixture::EstimatorType estimator; - auto X = this->MakeFeatureMatrix({ { 0.001f, 0.001f }, { 0.001f, -0.001f }, { -0.001f, 0.001f }, @@ -133,18 +123,16 @@ TYPED_TEST(LinearRegressionTest, NearZeroFeatures) auto y = this->MakeTargetVector({ 0.01f, 0.01f, 0.01f, 0.01f }); - estimator.Fit(X, y); + this->estimator.Fit(X, y); auto newX = this->MakeInputVector({ 0.001f, 0.001f }); - auto predicted = estimator.Predict(newX); + auto predicted = this->estimator.Predict(newX); - EXPECT_TRUE(AreFloatsNear(predicted, this->MakeValue(0.01f))); + EXPECT_NEAR(math::ToFloat(predicted), math::ToFloat(this->MakeValue(0.01f)), math::Tolerance()); } -TYPED_TEST(LinearRegressionTest, RangeLimits) +TYPED_TEST(LinearRegressionTest, PredictionStaysWithinTrainingRange) { - typename TestFixture::EstimatorType estimator; - auto X = this->MakeFeatureMatrix({ { 0.02f, 0.02f }, { -0.02f, -0.02f }, { 0.02f, -0.02f }, @@ -152,10 +140,10 @@ TYPED_TEST(LinearRegressionTest, RangeLimits) auto y = this->MakeTargetVector({ 0.02f, -0.02f, 0.0f, 0.0f }); - estimator.Fit(X, y); + this->estimator.Fit(X, y); auto newX = this->MakeInputVector({ 0.02f, 0.02f }); - auto predicted = estimator.Predict(newX); + auto predicted = this->estimator.Predict(newX); EXPECT_LE(std::abs(math::ToFloat(predicted)), 0.02f); } diff --git a/numerical/estimators/offline/test/TestPolynomialFitting.cpp b/numerical/estimators/offline/test/TestPolynomialFitting.cpp index 413a9079..358e4afa 100644 --- a/numerical/estimators/offline/test/TestPolynomialFitting.cpp +++ b/numerical/estimators/offline/test/TestPolynomialFitting.cpp @@ -1,5 +1,6 @@ #include "numerical/estimators/offline/PolynomialFitting.hpp" #include "numerical/math/Tolerance.hpp" +#include #include #include @@ -12,7 +13,7 @@ namespace }; } -TEST_F(TestPolynomialFitting, recovers_exact_line) +TEST_F(TestPolynomialFitting, recovers_exact_line_coefficients) { estimators::PolynomialFitting linFitter; math::Matrix x; @@ -32,7 +33,7 @@ TEST_F(TestPolynomialFitting, recovers_exact_line) EXPECT_NEAR(c.at(1, 0), 3.0f, math::Tolerance()); } -TEST_F(TestPolynomialFitting, recovers_exact_quadratic) +TEST_F(TestPolynomialFitting, recovers_exact_quadratic_coefficients) { math::Matrix x; math::Matrix y; @@ -45,59 +46,56 @@ TEST_F(TestPolynomialFitting, recovers_exact_quadratic) } fitter.Fit(x, y); + const auto& c = fitter.Coefficients(); - for (std::size_t i = 0; i < 8; ++i) - { - float xi = x.at(i, 0); - float expected = 1.0f - 0.5f * xi + 0.25f * xi * xi; - EXPECT_NEAR(fitter.Predict(xi), expected, math::Tolerance()); - } + EXPECT_NEAR(c.at(0, 0), 1.0f, math::Tolerance()); + EXPECT_NEAR(c.at(1, 0), -0.5f, math::Tolerance()); + EXPECT_NEAR(c.at(2, 0), 0.25f, math::Tolerance()); } -TEST_F(TestPolynomialFitting, fits_noisy_data_least_squares) +TEST_F(TestPolynomialFitting, predict_matches_doc_worked_example) { - math::Matrix x; - math::Matrix y; - - static constexpr float noise[] = { 0.01f, -0.01f, 0.005f, -0.005f, 0.008f, -0.008f, 0.003f, -0.003f }; - - for (std::size_t i = 0; i < 8; ++i) - { - float xi = static_cast(i) * 0.25f; - x.at(i, 0) = xi; - y.at(i, 0) = 1.0f - 0.5f * xi + 0.25f * xi * xi + noise[i]; - } - - fitter.Fit(x, y); - const auto& c = fitter.Coefficients(); - - EXPECT_NEAR(c.at(0, 0), 1.0f, 0.05f); - EXPECT_NEAR(c.at(1, 0), -0.5f, 0.05f); - EXPECT_NEAR(c.at(2, 0), 0.25f, 0.05f); + estimators::PolynomialFitting docFitter; + math::Matrix x; + math::Matrix y; + + x.at(0, 0) = 0.0f; + y.at(0, 0) = 1.0f; + x.at(1, 0) = 1.0f; + y.at(1, 0) = 0.75f; + x.at(2, 0) = 2.0f; + y.at(2, 0) = 1.0f; + x.at(3, 0) = 3.0f; + y.at(3, 0) = 1.75f; + + docFitter.Fit(x, y); + + EXPECT_NEAR(docFitter.Predict(1.5f), 0.8125f, math::Tolerance()); } -TEST_F(TestPolynomialFitting, predict_uses_horner) +TEST_F(TestPolynomialFitting, fits_noisy_quadratic_coefficients_within_noise_bound) { math::Matrix x; math::Matrix y; + constexpr std::array noise = { 0.01f, -0.01f, 0.005f, -0.005f, 0.008f, -0.008f, 0.003f, -0.003f }; + for (std::size_t i = 0; i < 8; ++i) { - float xi = static_cast(i) * 0.5f; + float xi = static_cast(i) * 0.25f; x.at(i, 0) = xi; - y.at(i, 0) = 1.0f + 2.0f * xi + 3.0f * xi * xi; + y.at(i, 0) = 1.0f - 0.5f * xi + 0.25f * xi * xi + noise[i]; } fitter.Fit(x, y); const auto& c = fitter.Coefficients(); - float xVal = 1.5f; - float horner = c.at(2, 0) * xVal * xVal + c.at(1, 0) * xVal + c.at(0, 0); - - EXPECT_NEAR(fitter.Predict(xVal), horner, math::Tolerance()); + EXPECT_NEAR(c.at(0, 0), 1.0f, 0.02f); + EXPECT_NEAR(c.at(1, 0), -0.5f, 0.02f); + EXPECT_NEAR(c.at(2, 0), 0.25f, 0.02f); } -TEST_F(TestPolynomialFitting, constant_data_gives_constant_term) +TEST_F(TestPolynomialFitting, constant_data_gives_zero_higher_coefficients) { math::Matrix x; math::Matrix y; @@ -116,7 +114,7 @@ TEST_F(TestPolynomialFitting, constant_data_gives_constant_term) EXPECT_NEAR(c.at(2, 0), 0.0f, math::Tolerance()); } -TEST_F(TestPolynomialFitting, centering_improves_conditioning) +TEST_F(TestPolynomialFitting, centered_abscissae_recover_exact_quadratic_coefficients) { math::Matrix x; math::Matrix y; @@ -124,8 +122,7 @@ TEST_F(TestPolynomialFitting, centering_improves_conditioning) float xMean = 104.0f; for (std::size_t i = 0; i < 8; ++i) { - float xi = 100.0f + static_cast(i); - float xc = xi - xMean; + float xc = (100.0f + static_cast(i)) - xMean; x.at(i, 0) = xc; y.at(i, 0) = 1.0f - 0.5f * xc + 0.25f * xc * xc; } @@ -133,9 +130,7 @@ TEST_F(TestPolynomialFitting, centering_improves_conditioning) fitter.Fit(x, y); const auto& c = fitter.Coefficients(); - EXPECT_TRUE(std::isfinite(c.at(0, 0))); - EXPECT_TRUE(std::isfinite(c.at(1, 0))); - EXPECT_TRUE(std::isfinite(c.at(2, 0))); + EXPECT_NEAR(c.at(0, 0), 1.0f, math::Tolerance()); EXPECT_NEAR(c.at(1, 0), -0.5f, math::Tolerance()); EXPECT_NEAR(c.at(2, 0), 0.25f, math::Tolerance()); } @@ -160,3 +155,50 @@ TEST_F(TestPolynomialFitting, degree_zero_is_mean) EXPECT_NEAR(c.at(0, 0), sum / 8.0f, math::Tolerance()); } + +TEST_F(TestPolynomialFitting, determinism_identical_fit_produces_identical_coefficients) +{ + math::Matrix x; + math::Matrix y; + + for (std::size_t i = 0; i < 8; ++i) + { + float xi = static_cast(i) * 0.5f; + x.at(i, 0) = xi; + y.at(i, 0) = 1.0f - 0.5f * xi + 0.25f * xi * xi; + } + + estimators::PolynomialFitting fitter2; + fitter.Fit(x, y); + fitter2.Fit(x, y); + + const auto& c1 = fitter.Coefficients(); + const auto& c2 = fitter2.Coefficients(); + + EXPECT_FLOAT_EQ(c1.at(0, 0), c2.at(0, 0)); + EXPECT_FLOAT_EQ(c1.at(1, 0), c2.at(1, 0)); + EXPECT_FLOAT_EQ(c1.at(2, 0), c2.at(2, 0)); +} + +TEST_F(TestPolynomialFitting, predict_all_coefficients_finite_on_far_from_origin_data) +{ + math::Matrix x; + math::Matrix y; + + for (std::size_t i = 0; i < 8; ++i) + { + float xi = static_cast(i) * 0.5f; + x.at(i, 0) = xi; + y.at(i, 0) = 3.0f + 7.0f * xi + 2.0f * xi * xi; + } + + fitter.Fit(x, y); + const auto& c = fitter.Coefficients(); + + EXPECT_TRUE(std::isfinite(c.at(0, 0))); + EXPECT_TRUE(std::isfinite(c.at(1, 0))); + EXPECT_TRUE(std::isfinite(c.at(2, 0))); + EXPECT_NEAR(c.at(0, 0), 3.0f, math::Tolerance()); + EXPECT_NEAR(c.at(1, 0), 7.0f, math::Tolerance()); + EXPECT_NEAR(c.at(2, 0), 2.0f, math::Tolerance()); +} diff --git a/numerical/estimators/offline/test/TestTotalLeastSquares.cpp b/numerical/estimators/offline/test/TestTotalLeastSquares.cpp index c3358a2c..ae205643 100644 --- a/numerical/estimators/offline/test/TestTotalLeastSquares.cpp +++ b/numerical/estimators/offline/test/TestTotalLeastSquares.cpp @@ -1,5 +1,6 @@ #include "numerical/estimators/offline/TotalLeastSquares.hpp" #include "numerical/math/Tolerance.hpp" +#include #include #include @@ -31,8 +32,8 @@ TEST_F(TestTotalLeastSquares, recovers_exact_line_no_noise) TEST_F(TestTotalLeastSquares, symmetric_noise_beats_ols) { - static constexpr float na[] = { 1.0f, -1.0f, -1.0f, 1.0f, 1.0f, -1.0f, -1.0f, 1.0f }; - static constexpr float nb[] = { 1.0f, -1.0f, 1.0f, -1.0f, -1.0f, 1.0f, -1.0f, 1.0f }; + static constexpr std::array na = { 1.0f, -1.0f, -1.0f, 1.0f, 1.0f, -1.0f, -1.0f, 1.0f }; + static constexpr std::array nb = { 1.0f, -1.0f, 1.0f, -1.0f, -1.0f, 1.0f, -1.0f, 1.0f }; math::Matrix a; math::Vector b; @@ -60,7 +61,7 @@ TEST_F(TestTotalLeastSquares, symmetric_noise_beats_ols) TEST_F(TestTotalLeastSquares, matches_ols_when_regressors_clean) { - static constexpr float nb[] = { 0.05f, -0.04f, 0.03f, -0.02f, 0.04f, -0.05f, 0.02f, -0.03f }; + static constexpr std::array nb = { 0.05f, -0.04f, 0.03f, -0.02f, 0.04f, -0.05f, 0.02f, -0.03f }; math::Matrix a; math::Vector b; @@ -116,7 +117,7 @@ TEST_F(TestTotalLeastSquares, degenerate_returns_false) EXPECT_FALSE(tls.Fit(a, b)); } -TEST_F(TestTotalLeastSquares, predict_matches_dot_product) +TEST_F(TestTotalLeastSquares, predict_against_closed_form) { math::Matrix a; math::Vector b; @@ -136,8 +137,95 @@ TEST_F(TestTotalLeastSquares, predict_matches_dot_product) x.at(0, 0) = 2.0f; x.at(1, 0) = -1.0f; - const auto& c = tls2.Coefficients(); - float expected = c.at(0, 0) * x.at(0, 0) + c.at(1, 0) * x.at(1, 0); + float expected = 1.5f * 2.0f + (-0.5f) * (-1.0f); EXPECT_NEAR(tls2.Predict(x), expected, math::Tolerance()); } + +TEST_F(TestTotalLeastSquares, determinism_repeated_fit_yields_same_coefficients) +{ + math::Matrix a; + math::Vector b; + + for (std::size_t i = 0; i < 8; ++i) + { + float ai = 1.0f + static_cast(i); + a.at(i, 0) = ai; + b.at(i, 0) = 3.0f * ai; + } + + estimators::TotalLeastSquares fresh; + + ASSERT_TRUE(tls.Fit(a, b)); + ASSERT_TRUE(fresh.Fit(a, b)); + + EXPECT_FLOAT_EQ(tls.Coefficients().at(0, 0), fresh.Coefficients().at(0, 0)); +} + +TEST_F(TestTotalLeastSquares, coefficients_finite_after_failed_fit) +{ + math::Matrix a; + math::Vector b; + + for (std::size_t i = 0; i < 8; ++i) + { + a.at(i, 0) = 0.0f; + b.at(i, 0) = 1.0f; + } + + ASSERT_FALSE(tls.Fit(a, b)); + + EXPECT_TRUE(std::isfinite(tls.Coefficients().at(0, 0))); +} + +TEST_F(TestTotalLeastSquares, rmse_below_noise_bound_on_noisy_data) +{ + static constexpr std::array noise = { 0.05f, -0.03f, 0.04f, -0.05f, 0.02f, -0.04f, 0.03f, -0.02f }; + + math::Matrix a; + math::Vector b; + + for (std::size_t i = 0; i < 8; ++i) + { + float ai = 1.0f + static_cast(i); + a.at(i, 0) = ai + noise[i]; + b.at(i, 0) = 3.0f * ai + noise[(i + 4) % 8]; + } + + ASSERT_TRUE(tls.Fit(a, b)); + + float rmse = 0.0f; + for (std::size_t i = 0; i < 8; ++i) + { + float ai = 1.0f + static_cast(i); + math::Vector x; + x.at(0, 0) = a.at(i, 0); + float err = tls.Predict(x) - b.at(i, 0); + rmse += err * err; + } + rmse = std::sqrt(rmse / 8.0f); + + EXPECT_LT(rmse, 0.3f); +} + +TEST_F(TestTotalLeastSquares, ill_conditioned_near_collinear_does_not_produce_nan) +{ + math::Matrix a; + math::Vector b; + + for (std::size_t i = 0; i < 10; ++i) + { + float ai = 1.0f + static_cast(i); + a.at(i, 0) = ai; + a.at(i, 1) = ai + 1e-4f * static_cast(i); + b.at(i, 0) = 2.0f * ai; + } + + bool ok = tls2.Fit(a, b); + + if (ok) + { + EXPECT_TRUE(std::isfinite(tls2.Coefficients().at(0, 0))); + EXPECT_TRUE(std::isfinite(tls2.Coefficients().at(1, 0))); + } +} diff --git a/numerical/estimators/offline/test/TestYuleWalker.cpp b/numerical/estimators/offline/test/TestYuleWalker.cpp index f7a7c0e5..34f2971b 100644 --- a/numerical/estimators/offline/test/TestYuleWalker.cpp +++ b/numerical/estimators/offline/test/TestYuleWalker.cpp @@ -1,5 +1,5 @@ #include "numerical/estimators/offline/YuleWalker.hpp" -#include "numerical/math/QNumber.hpp" +#include "numerical/math/Tolerance.hpp" #include namespace @@ -11,40 +11,50 @@ namespace static constexpr std::size_t Samples = 256; static constexpr std::size_t Order = 2; - estimators::YuleWalker estimator; - }; -} + using SignalVector = math::Vector; + using PastVector = math::Vector; -TEST_F(TestYuleWalkerFloat, fit_produces_nonzero_coefficients) -{ - math::Vector signal; - signal[0] = 0.1f; - signal[1] = 0.2f; - for (std::size_t t = 2; t < TestYuleWalkerFloat::Samples; ++t) - signal[t] = 0.5f * signal[t - 1] - 0.3f * signal[t - 2] + 0.01f * static_cast(t % 7 - 3); - - estimator.Fit(signal); + estimators::YuleWalker estimator; - auto coeffs = estimator.Coefficients(); - EXPECT_NE(coeffs[0], 0.0f); - EXPECT_NE(coeffs[1], 0.0f); + SignalVector MakeAr2Signal(float a1, float a2) const + { + SignalVector signal; + signal[0] = 0.1f; + signal[1] = 0.2f; + for (std::size_t t = 2; t < Samples; ++t) + signal[t] = a1 * signal[t - 1] + a2 * signal[t - 2]; + return signal; + } + + SignalVector MakeNoisyAr2Signal(float a1, float a2) const + { + SignalVector signal; + unsigned int seed = 42u; + auto nextNoise = [&seed]() + { + seed = seed * 1103515245u + 12345u; + return 0.01f * ((static_cast((seed >> 16) & 0x7FFF) / 16384.0f) - 1.0f); + }; + signal[0] = nextNoise(); + signal[1] = a1 * signal[0] + nextNoise(); + for (std::size_t t = 2; t < Samples; ++t) + signal[t] = a1 * signal[t - 1] + a2 * signal[t - 2] + nextNoise(); + return signal; + } + + SignalVector MakeConstantSignal(float value) const + { + SignalVector signal; + for (std::size_t t = 0; t < Samples; ++t) + signal[t] = value; + return signal; + } + }; } TEST_F(TestYuleWalkerFloat, fit_recovers_ar2_coefficients) { - math::Vector signal; - - unsigned int seed = 42u; - auto nextNoise = [&seed]() - { - seed = seed * 1103515245u + 12345u; - return 0.01f * ((static_cast((seed >> 16) & 0x7FFF) / 16384.0f) - 1.0f); - }; - - signal[0] = nextNoise(); - signal[1] = 0.5f * signal[0] + nextNoise(); - for (std::size_t t = 2; t < TestYuleWalkerFloat::Samples; ++t) - signal[t] = 0.5f * signal[t - 1] - 0.3f * signal[t - 2] + nextNoise(); + auto signal = MakeNoisyAr2Signal(0.5f, -0.3f); estimator.Fit(signal); @@ -55,31 +65,22 @@ TEST_F(TestYuleWalkerFloat, fit_recovers_ar2_coefficients) TEST_F(TestYuleWalkerFloat, predict_uses_coefficients) { - math::Vector signal; - signal[0] = 0.1f; - signal[1] = 0.2f; - for (std::size_t t = 2; t < TestYuleWalkerFloat::Samples; ++t) - signal[t] = 0.5f * signal[t - 1] - 0.3f * signal[t - 2]; + auto signal = MakeAr2Signal(0.5f, -0.3f); estimator.Fit(signal); - math::Vector past; - past[0] = signal[TestYuleWalkerFloat::Samples - 1]; - past[1] = signal[TestYuleWalkerFloat::Samples - 2]; + PastVector past; + past[0] = signal[Samples - 1]; + past[1] = signal[Samples - 2]; float prediction = estimator.Predict(past); - - float expected = 0.5f * signal[TestYuleWalkerFloat::Samples - 1] - 0.3f * signal[TestYuleWalkerFloat::Samples - 2]; + float expected = 0.5f * signal[Samples - 1] - 0.3f * signal[Samples - 2]; EXPECT_NEAR(prediction, expected, 0.1f); } TEST_F(TestYuleWalkerFloat, noise_variance_is_nonnegative) { - math::Vector signal; - signal[0] = 0.1f; - signal[1] = 0.2f; - for (std::size_t t = 2; t < TestYuleWalkerFloat::Samples; ++t) - signal[t] = 0.5f * signal[t - 1] - 0.3f * signal[t - 2] + 0.01f * static_cast(t % 7 - 3); + auto signal = MakeNoisyAr2Signal(0.5f, -0.3f); estimator.Fit(signal); @@ -88,13 +89,11 @@ TEST_F(TestYuleWalkerFloat, noise_variance_is_nonnegative) TEST_F(TestYuleWalkerFloat, constant_signal_yields_near_zero_coefficients) { - math::Vector signal; - for (std::size_t t = 0; t < TestYuleWalkerFloat::Samples; ++t) - signal[t] = 0.5f; + auto signal = MakeConstantSignal(0.5f); estimator.Fit(signal); auto coeffs = estimator.Coefficients(); - EXPECT_NEAR(coeffs[0], 0.0f, 0.01f); - EXPECT_NEAR(coeffs[1], 0.0f, 0.01f); + EXPECT_NEAR(coeffs[0], 0.0f, math::Tolerance()); + EXPECT_NEAR(coeffs[1], 0.0f, math::Tolerance()); } diff --git a/numerical/estimators/test/CMakeLists.txt b/numerical/estimators/test/CMakeLists.txt deleted file mode 100644 index 077667fb..00000000 --- a/numerical/estimators/test/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -add_executable(numerical.estimators_test) -emil_build_for(numerical.estimators_test BOOL NUMERICAL_TOOLBOX_BUILD_TESTS) -emil_add_test(numerical.estimators_test) - -target_link_libraries(numerical.estimators_test PUBLIC - gmock_main - numerical.estimators -) - -target_sources(numerical.estimators_test PRIVATE - TestConsistencyMetrics.cpp -) diff --git a/numerical/filters/active/KalmanSmoother.hpp b/numerical/filters/active/KalmanSmoother.hpp index 5998da23..781b4811 100644 --- a/numerical/filters/active/KalmanSmoother.hpp +++ b/numerical/filters/active/KalmanSmoother.hpp @@ -4,11 +4,11 @@ #pragma GCC optimize("O3", "fast-math") #endif +#include "numerical/math/CholeskyDecomposition.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/LinearTimeInvariant.hpp" #include "numerical/math/Matrix.hpp" #include "numerical/math/MatrixOperations.hpp" -#include "numerical/solvers/CholeskyDecomposition.hpp" #include "numerical/solvers/GaussianElimination.hpp" #include #include @@ -221,7 +221,7 @@ namespace filters const MeasurementCovariance& innovationCovariance) const { static const float log2pi = std::log(2.0f * std::numbers::pi_v); - const auto L = solvers::CholeskyDecomposition(innovationCovariance); + const auto L = math::CholeskyDecomposition::Factor(innovationCovariance); const auto sInvNu = solvers::SolveSystem(innovationCovariance, innovation); float logDetS = 0.0f; for (std::size_t i = 0; i < MeasurementSize; ++i) diff --git a/numerical/filters/active/UnscentedKalmanFilter.hpp b/numerical/filters/active/UnscentedKalmanFilter.hpp index dac06e68..1908e42a 100644 --- a/numerical/filters/active/UnscentedKalmanFilter.hpp +++ b/numerical/filters/active/UnscentedKalmanFilter.hpp @@ -2,9 +2,9 @@ #include "infra/util/Function.hpp" #include "numerical/filters/active/KalmanFilterBase.hpp" +#include "numerical/math/CholeskyDecomposition.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/MatrixOperations.hpp" -#include "numerical/solvers/CholeskyDecomposition.hpp" #include #include #include @@ -141,7 +141,7 @@ namespace filters { constexpr auto n = static_cast(StateSize); float scaleFactor = std::sqrt(n + lambda); - auto L = solvers::CholeskyDecomposition(this->covariance()); + auto L = math::CholeskyDecomposition::Factor(this->covariance()); sigmaPoints[0] = this->state(); diff --git a/numerical/filters/active/test/TestSquareRootKalmanFilter.cpp b/numerical/filters/active/test/TestSquareRootKalmanFilter.cpp index ec55e916..f5743a29 100644 --- a/numerical/filters/active/test/TestSquareRootKalmanFilter.cpp +++ b/numerical/filters/active/test/TestSquareRootKalmanFilter.cpp @@ -1,7 +1,7 @@ #include "numerical/filters/active/KalmanFilter.hpp" #include "numerical/filters/active/SquareRootKalmanFilter.hpp" +#include "numerical/math/CholeskyDecomposition.hpp" #include "numerical/math/Tolerance.hpp" -#include "numerical/solvers/CholeskyDecomposition.hpp" #include #include @@ -72,7 +72,7 @@ namespace : public ::testing::Test { protected: - StateMat S0 = solvers::CholeskyDecomposition(MakeP0()); + StateMat S0 = math::CholeskyDecomposition::Factor(MakeP0()); SrkfType filter{ MakeX0(), S0 }; KfType reference{ MakeX0(), MakeP0() }; @@ -81,8 +81,8 @@ namespace { filter.SetStateTransition(MakeF()); filter.SetMeasurementMatrix(MakeH()); - filter.SetProcessNoiseFactor(solvers::CholeskyDecomposition(MakeQ())); - filter.SetMeasurementNoiseFactor(solvers::CholeskyDecomposition(MakeR())); + filter.SetProcessNoiseFactor(math::CholeskyDecomposition::Factor(MakeQ())); + filter.SetMeasurementNoiseFactor(math::CholeskyDecomposition::Factor(MakeR())); reference.SetStateTransition(MakeF()); reference.SetMeasurementMatrix(MakeH()); @@ -165,7 +165,7 @@ TEST_F(TestSquareRootKalmanFilter, factor_reconstructs_covariance) TEST_F(TestSquareRootKalmanFilter, predict_grows_uncertainty) { filter.SetStateTransition(MakeF()); - filter.SetProcessNoiseFactor(solvers::CholeskyDecomposition(MakeQ())); + filter.SetProcessNoiseFactor(math::CholeskyDecomposition::Factor(MakeQ())); float traceBefore = MatrixTrace(filter.GetCovariance()); filter.Predict(); @@ -194,13 +194,13 @@ TEST_F(TestSquareRootKalmanFilter, ill_conditioned_stays_stable) { 1e8f, 0.0f }, { 0.0f, 1.0f } }; - StateMat S0ill = solvers::CholeskyDecomposition(P0ill); + StateMat S0ill = math::CholeskyDecomposition::Factor(P0ill); SrkfType illFilter{ MakeX0(), S0ill }; illFilter.SetStateTransition(MakeF()); illFilter.SetMeasurementMatrix(MakeH()); - illFilter.SetProcessNoiseFactor(solvers::CholeskyDecomposition(MakeQ())); - illFilter.SetMeasurementNoiseFactor(solvers::CholeskyDecomposition(MakeR())); + illFilter.SetProcessNoiseFactor(math::CholeskyDecomposition::Factor(MakeQ())); + illFilter.SetMeasurementNoiseFactor(math::CholeskyDecomposition::Factor(MakeR())); for (int step = 0; step < 20; ++step) { @@ -223,7 +223,7 @@ TEST_F(TestSquareRootKalmanFilter, perfect_measurement_collapses_variance) filter.SetStateTransition(MakeF()); filter.SetMeasurementMatrix(MakeH()); - filter.SetProcessNoiseFactor(solvers::CholeskyDecomposition(MakeQ())); + filter.SetProcessNoiseFactor(math::CholeskyDecomposition::Factor(MakeQ())); filter.SetMeasurementNoiseFactor(sqrtRtiny); filter.Predict(); @@ -241,7 +241,7 @@ TEST_F(TestSquareRootKalmanFilter, vague_measurement_is_ignored) filter.SetStateTransition(MakeF()); filter.SetMeasurementMatrix(MakeH()); - filter.SetProcessNoiseFactor(solvers::CholeskyDecomposition(MakeQ())); + filter.SetProcessNoiseFactor(math::CholeskyDecomposition::Factor(MakeQ())); filter.SetMeasurementNoiseFactor(sqrtRlarge); filter.Predict(); @@ -300,8 +300,8 @@ TEST_F(TestSquareRootKalmanFilter, control_input_matches_conventional_kalman) SrkfCtrl srkf{ MakeX0(), S0 }; srkf.SetStateTransition(MakeF()); srkf.SetMeasurementMatrix(MakeH()); - srkf.SetProcessNoiseFactor(solvers::CholeskyDecomposition(MakeQ())); - srkf.SetMeasurementNoiseFactor(solvers::CholeskyDecomposition(MakeR())); + srkf.SetProcessNoiseFactor(math::CholeskyDecomposition::Factor(MakeQ())); + srkf.SetMeasurementNoiseFactor(math::CholeskyDecomposition::Factor(MakeR())); srkf.SetControlInputMatrix(B); KfCtrl reference2{ MakeX0(), MakeP0() }; diff --git a/numerical/math/CMakeLists.txt b/numerical/math/CMakeLists.txt index f89db26a..383e5793 100644 --- a/numerical/math/CMakeLists.txt +++ b/numerical/math/CMakeLists.txt @@ -11,7 +11,9 @@ target_link_libraries(numerical.math ${NUMERICAL_VISIBILITY} target_sources(numerical.math PRIVATE AdvancedFunctions.hpp + CholeskyDecomposition.hpp ComplexNumber.hpp + ConsistencyMetrics.hpp Cordic.hpp Geometry3D.hpp GivensRotation.hpp @@ -36,6 +38,7 @@ target_sources(numerical.math PRIVATE numerical_add_coverage_sources(numerical.math ComplexNumber.cpp + ConsistencyMetrics.cpp Cordic.cpp LinearTimeInvariant.cpp Matrix.cpp diff --git a/numerical/math/CholeskyDecomposition.hpp b/numerical/math/CholeskyDecomposition.hpp new file mode 100644 index 00000000..534ab6e2 --- /dev/null +++ b/numerical/math/CholeskyDecomposition.hpp @@ -0,0 +1,79 @@ +#pragma once + +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC optimize("O3", "fast-math") +#endif + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/TriangularSolve.hpp" +#include +#include +#include + +namespace math +{ + namespace detail + { + template + [[nodiscard]] OPTIMIZE_FOR_SPEED float CholeskyInnerProduct(const SquareMatrix& l, std::size_t i, std::size_t j) + { + float sum = 0.0f; + for (std::size_t k = 0; k < j; ++k) + sum += ToFloat(l.at(i, k)) * ToFloat(l.at(j, k)); + return sum; + } + } + + template + class CholeskyDecomposition + { + static_assert(detail::is_supported_type_v, + "CholeskyDecomposition only supports float or QNumber types"); + + public: + [[nodiscard]] static OPTIMIZE_FOR_SPEED SquareMatrix Factor(const SquareMatrix& a); + [[nodiscard]] static OPTIMIZE_FOR_SPEED std::optional> TryFactor(const SquareMatrix& a); + [[nodiscard]] static OPTIMIZE_FOR_SPEED std::optional> Solve(const SquareMatrix& a, const Vector& b); + }; + + template + OPTIMIZE_FOR_SPEED std::optional> CholeskyDecomposition::TryFactor(const SquareMatrix& a) + { + SquareMatrix l{}; + + for (std::size_t i = 0; i < N; ++i) + { + for (std::size_t j = 0; j <= i; ++j) + { + const float sum = ToFloat(a.at(i, j)) - detail::CholeskyInnerProduct(l, i, j); + + if (i != j) + l.at(i, j) = T(sum / ToFloat(l.at(j, j))); + else if (sum < 1e-10f) + return std::nullopt; + else + l.at(i, j) = T(std::sqrt(sum)); + } + } + + return l; + } + + template + OPTIMIZE_FOR_SPEED SquareMatrix CholeskyDecomposition::Factor(const SquareMatrix& a) + { + return TryFactor(a).value_or(SquareMatrix{}); + } + + template + OPTIMIZE_FOR_SPEED std::optional> CholeskyDecomposition::Solve(const SquareMatrix& a, const Vector& b) + { + auto l = TryFactor(a); + if (!l.has_value()) + return std::nullopt; + + const Vector y = SolveLowerTriangular(l.value(), b); + return SolveUpperTriangular(l.value().Transpose(), y); + } +} diff --git a/numerical/estimators/ConsistencyMetrics.cpp b/numerical/math/ConsistencyMetrics.cpp similarity index 75% rename from numerical/estimators/ConsistencyMetrics.cpp rename to numerical/math/ConsistencyMetrics.cpp index c2d3973a..929674fb 100644 --- a/numerical/estimators/ConsistencyMetrics.cpp +++ b/numerical/math/ConsistencyMetrics.cpp @@ -1,8 +1,8 @@ // Copyright 2025 Numerical Toolbox Contributors // SPDX-License-Identifier: MIT -#include "numerical/estimators/ConsistencyMetrics.hpp" +#include "numerical/math/ConsistencyMetrics.hpp" -namespace estimators +namespace math { template class ConsistencyMetrics; template class ConsistencyMetrics; diff --git a/numerical/estimators/ConsistencyMetrics.hpp b/numerical/math/ConsistencyMetrics.hpp similarity index 79% rename from numerical/estimators/ConsistencyMetrics.hpp rename to numerical/math/ConsistencyMetrics.hpp index 59f953d2..4eaf50b7 100644 --- a/numerical/estimators/ConsistencyMetrics.hpp +++ b/numerical/math/ConsistencyMetrics.hpp @@ -4,21 +4,19 @@ #pragma GCC optimize("O3", "fast-math") #endif +#include "numerical/math/CholeskyDecomposition.hpp" #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Matrix.hpp" #include "numerical/math/MatrixNorms.hpp" -#include "numerical/solvers/GaussianElimination.hpp" #include -#include #include #include -namespace estimators +namespace math { namespace detail { static constexpr std::size_t kMaxChiSquareDim = 10; - static constexpr std::size_t kNumAlpha = 1; static constexpr std::array kChi2Lo95 = { 0.000982f, 0.050636f, 0.215795f, 0.484419f, 0.831212f, @@ -46,15 +44,12 @@ namespace estimators [[nodiscard]] static OPTIMIZE_FOR_SPEED std::optional Nis(const StateVector& innovation, const CovarianceMatrix& innovationCovariance); [[nodiscard]] static bool IsConsistent(T value); [[nodiscard]] static bool IsTimeAveragedConsistent(T averagedValue, std::size_t numSamples); - - private: - [[nodiscard]] static OPTIMIZE_FOR_SPEED std::optional Solve(const CovarianceMatrix& matrix, const StateVector& rhs); }; template OPTIMIZE_FOR_SPEED std::optional ConsistencyMetrics::Nees(const StateVector& error, const CovarianceMatrix& covariance) { - auto z = Solve(covariance, error); + auto z = CholeskyDecomposition::Solve(covariance, error); if (!z.has_value()) return std::nullopt; return math::DotProduct(error, z.value()); @@ -63,7 +58,7 @@ namespace estimators template OPTIMIZE_FOR_SPEED std::optional ConsistencyMetrics::Nis(const StateVector& innovation, const CovarianceMatrix& innovationCovariance) { - auto z = Solve(innovationCovariance, innovation); + auto z = CholeskyDecomposition::Solve(innovationCovariance, innovation); if (!z.has_value()) return std::nullopt; return math::DotProduct(innovation, z.value()); @@ -92,20 +87,6 @@ namespace estimators static_cast(averagedValue) <= hi; } - template - OPTIMIZE_FOR_SPEED std::optional::StateVector> ConsistencyMetrics::Solve(const CovarianceMatrix& matrix, const StateVector& rhs) - { - for (std::size_t i = 0; i < Dim; ++i) - { - float pivot = std::abs(static_cast(matrix.at(i, i))); - if (pivot < 1e-10f) - return std::nullopt; - } - - solvers::GaussianElimination solver; - return solver.Solve(matrix, rhs); - } - #ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD extern template class ConsistencyMetrics; extern template class ConsistencyMetrics; diff --git a/numerical/math/TriangularSolve.hpp b/numerical/math/TriangularSolve.hpp index ec82c2e8..2eb83b09 100644 --- a/numerical/math/TriangularSolve.hpp +++ b/numerical/math/TriangularSolve.hpp @@ -32,21 +32,44 @@ namespace math return pivotRow; } - template - [[nodiscard]] OPTIMIZE_FOR_SPEED Vector SolveUnitLowerTriangular(const Matrix& l, const Vector& c) + namespace detail { - Vector y{}; - - for (std::size_t i = 0; i < N; ++i) + template + [[nodiscard]] OPTIMIZE_FOR_SPEED Vector ForwardSubstitute(const Matrix& l, const Vector& c) { - T sum = c.at(i, 0); - for (std::size_t j = 0; j < i; ++j) - sum = sum - l.at(i, j) * y.at(j, 0); + Vector x{}; + + for (std::size_t i = 0; i < N; ++i) + { + T sum = c.at(i, 0); + for (std::size_t j = 0; j < i; ++j) + sum = sum - l.at(i, j) * x.at(j, 0); + + if constexpr (Unit) + { + x.at(i, 0) = sum; + } + else + { + really_assert(std::abs(ToFloat(l.at(i, i))) > 0.0f); + x.at(i, 0) = sum / l.at(i, i); + } + } - y.at(i, 0) = sum; + return x; } + } - return y; + template + [[nodiscard]] OPTIMIZE_FOR_SPEED Vector SolveUnitLowerTriangular(const Matrix& l, const Vector& c) + { + return detail::ForwardSubstitute(l, c); + } + + template + [[nodiscard]] OPTIMIZE_FOR_SPEED Vector SolveLowerTriangular(const Matrix& l, const Vector& c) + { + return detail::ForwardSubstitute(l, c); } template diff --git a/numerical/math/test/CMakeLists.txt b/numerical/math/test/CMakeLists.txt index 804e38c5..471ad809 100644 --- a/numerical/math/test/CMakeLists.txt +++ b/numerical/math/test/CMakeLists.txt @@ -10,6 +10,7 @@ target_link_libraries(numerical.math_test PUBLIC target_sources(numerical.math_test PRIVATE TestComplexNumber.cpp + TestConsistencyMetrics.cpp TestCordic.cpp TestGivensRotation.cpp TestHouseholderTransform.cpp diff --git a/numerical/estimators/test/TestConsistencyMetrics.cpp b/numerical/math/test/TestConsistencyMetrics.cpp similarity index 52% rename from numerical/estimators/test/TestConsistencyMetrics.cpp rename to numerical/math/test/TestConsistencyMetrics.cpp index 841acd63..13cbb83d 100644 --- a/numerical/estimators/test/TestConsistencyMetrics.cpp +++ b/numerical/math/test/TestConsistencyMetrics.cpp @@ -1,6 +1,5 @@ -// Copyright 2025 Numerical Toolbox Contributors -// SPDX-License-Identifier: MIT -#include "numerical/estimators/ConsistencyMetrics.hpp" +#include "numerical/math/ConsistencyMetrics.hpp" +#include "numerical/math/Tolerance.hpp" #include namespace @@ -9,7 +8,7 @@ namespace : public ::testing::Test { protected: - using Metrics = estimators::ConsistencyMetrics; + using Metrics = math::ConsistencyMetrics; using Vec1 = math::Vector; using Mat1 = math::Matrix; }; @@ -18,7 +17,7 @@ namespace : public ::testing::Test { protected: - using Metrics = estimators::ConsistencyMetrics; + using Metrics = math::ConsistencyMetrics; using Vec2 = math::Vector; using Mat2 = math::Matrix; }; @@ -27,7 +26,7 @@ namespace : public ::testing::Test { protected: - using Metrics = estimators::ConsistencyMetrics; + using Metrics = math::ConsistencyMetrics; using Vec3 = math::Vector; using Mat3 = math::Matrix; }; @@ -43,7 +42,7 @@ TEST_F(ConsistencyMetrics1DTest, NeesScalarIdentityCovariance) auto result = Metrics::Nees(error, cov); ASSERT_TRUE(result.has_value()); - EXPECT_NEAR(*result, 4.0f, 1e-4f); + EXPECT_NEAR(*result, 4.0f, math::Tolerance()); } TEST_F(ConsistencyMetrics1DTest, NisScalarIdentityCovariance) @@ -56,7 +55,7 @@ TEST_F(ConsistencyMetrics1DTest, NisScalarIdentityCovariance) auto result = Metrics::Nis(innovation, cov); ASSERT_TRUE(result.has_value()); - EXPECT_NEAR(*result, 9.0f, 1e-4f); + EXPECT_NEAR(*result, 9.0f, math::Tolerance()); } TEST_F(ConsistencyMetrics1DTest, NeesScaledCovariance) @@ -69,7 +68,7 @@ TEST_F(ConsistencyMetrics1DTest, NeesScaledCovariance) auto result = Metrics::Nees(error, cov); ASSERT_TRUE(result.has_value()); - EXPECT_NEAR(*result, 1.0f, 1e-4f); + EXPECT_NEAR(*result, 1.0f, math::Tolerance()); } TEST_F(ConsistencyMetrics1DTest, NeesSingularCovarianceReturnsNullopt) @@ -84,16 +83,38 @@ TEST_F(ConsistencyMetrics1DTest, NeesSingularCovarianceReturnsNullopt) EXPECT_FALSE(result.has_value()); } +TEST_F(ConsistencyMetrics1DTest, NisSingularCovarianceReturnsNullopt) +{ + Vec1 innovation; + innovation.at(0, 0) = 1.0f; + Mat1 cov; + cov.at(0, 0) = 0.0f; + + auto result = Metrics::Nis(innovation, cov); + + EXPECT_FALSE(result.has_value()); +} + TEST_F(ConsistencyMetrics1DTest, IsConsistentInsideBounds) { - const float midpoint = (estimators::detail::kChi2Lo95[0] + estimators::detail::kChi2Hi95[0]) / 2.0f; + const float midpoint = (math::detail::kChi2Lo95[0] + math::detail::kChi2Hi95[0]) / 2.0f; EXPECT_TRUE(Metrics::IsConsistent(midpoint)); } +TEST_F(ConsistencyMetrics1DTest, IsConsistentAtExactLowerBound) +{ + EXPECT_TRUE(Metrics::IsConsistent(math::detail::kChi2Lo95[0])); +} + +TEST_F(ConsistencyMetrics1DTest, IsConsistentAtExactUpperBound) +{ + EXPECT_TRUE(Metrics::IsConsistent(math::detail::kChi2Hi95[0])); +} + TEST_F(ConsistencyMetrics1DTest, IsConsistentOutsideBoundsHigh) { - const float aboveHi = estimators::detail::kChi2Hi95[0] + 1.0f; + const float aboveHi = math::detail::kChi2Hi95[0] + 1.0f; EXPECT_FALSE(Metrics::IsConsistent(aboveHi)); } @@ -108,6 +129,20 @@ TEST_F(ConsistencyMetrics1DTest, IsTimeAveragedConsistentZeroSamplesReturnsFalse EXPECT_FALSE(Metrics::IsTimeAveragedConsistent(1.0f, 0)); } +TEST_F(ConsistencyMetrics1DTest, IsTimeAveragedConsistentMultipleSamplesInsideBounds) +{ + const float lo = math::detail::kChi2Lo95[1] / 2.0f; + const float hi = math::detail::kChi2Hi95[1] / 2.0f; + const float midpoint = (lo + hi) / 2.0f; + + EXPECT_TRUE(Metrics::IsTimeAveragedConsistent(midpoint, 2)); +} + +TEST_F(ConsistencyMetrics1DTest, IsTimeAveragedConsistentInconsistentOutOfBounds) +{ + EXPECT_FALSE(Metrics::IsTimeAveragedConsistent(0.0f, 2)); +} + TEST_F(ConsistencyMetrics2DTest, Nees2DIdentityCovariance) { Vec2 error; @@ -122,7 +157,7 @@ TEST_F(ConsistencyMetrics2DTest, Nees2DIdentityCovariance) auto result = Metrics::Nees(error, cov); ASSERT_TRUE(result.has_value()); - EXPECT_NEAR(*result, 5.0f, 1e-4f); + EXPECT_NEAR(*result, 5.0f, math::Tolerance()); } TEST_F(ConsistencyMetrics2DTest, Nis2DScaledDiagonalCovariance) @@ -139,12 +174,45 @@ TEST_F(ConsistencyMetrics2DTest, Nis2DScaledDiagonalCovariance) auto result = Metrics::Nis(innovation, cov); ASSERT_TRUE(result.has_value()); - EXPECT_NEAR(*result, 2.0f, 1e-4f); + EXPECT_NEAR(*result, 2.0f, math::Tolerance()); +} + +TEST_F(ConsistencyMetrics2DTest, Nees2DFullCovarianceHandComputed) +{ + Vec2 error; + error.at(0, 0) = 1.0f; + error.at(1, 0) = 0.0f; + Mat2 cov; + cov.at(0, 0) = 2.0f; + cov.at(0, 1) = 1.0f; + cov.at(1, 0) = 1.0f; + cov.at(1, 1) = 2.0f; + + auto result = Metrics::Nees(error, cov); + + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(*result, 2.0f / 3.0f, math::Tolerance()); +} + +TEST_F(ConsistencyMetrics2DTest, Nis2DSingularCovarianceReturnsNullopt) +{ + Vec2 innovation; + innovation.at(0, 0) = 1.0f; + innovation.at(1, 0) = 1.0f; + Mat2 cov; + cov.at(0, 0) = 0.0f; + cov.at(0, 1) = 0.0f; + cov.at(1, 0) = 0.0f; + cov.at(1, 1) = 1.0f; + + auto result = Metrics::Nis(innovation, cov); + + EXPECT_FALSE(result.has_value()); } TEST_F(ConsistencyMetrics2DTest, IsConsistent2DWithinBounds) { - const float midpoint = (estimators::detail::kChi2Lo95[1] + estimators::detail::kChi2Hi95[1]) / 2.0f; + const float midpoint = (math::detail::kChi2Lo95[1] + math::detail::kChi2Hi95[1]) / 2.0f; EXPECT_TRUE(Metrics::IsConsistent(midpoint)); } @@ -163,10 +231,10 @@ TEST_F(ConsistencyMetrics3DTest, Nees3DIdentityCovariance) auto result = Metrics::Nees(error, cov); ASSERT_TRUE(result.has_value()); - EXPECT_NEAR(*result, 3.0f, 1e-4f); + EXPECT_NEAR(*result, 3.0f, math::Tolerance()); } -TEST_F(ConsistencyMetrics3DTest, Nis3DFullCovariance) +TEST_F(ConsistencyMetrics3DTest, Nis3DScaledDiagonalCovariance) { Vec3 innovation; innovation.at(0, 0) = 1.0f; @@ -180,12 +248,28 @@ TEST_F(ConsistencyMetrics3DTest, Nis3DFullCovariance) auto result = Metrics::Nis(innovation, cov); ASSERT_TRUE(result.has_value()); - EXPECT_NEAR(*result, 0.5f, 1e-4f); + EXPECT_NEAR(*result, 0.5f, math::Tolerance()); } TEST_F(ConsistencyMetrics3DTest, IsTimeAveragedConsistentWithOneSample) { - const float midpoint = (estimators::detail::kChi2Lo95[2] + estimators::detail::kChi2Hi95[2]) / 2.0f; + const float midpoint = (math::detail::kChi2Lo95[2] + math::detail::kChi2Hi95[2]) / 2.0f; EXPECT_TRUE(Metrics::IsTimeAveragedConsistent(midpoint, 1)); } + +TEST_F(ConsistencyMetrics3DTest, IsTimeAveragedConsistentDofClampBranch) +{ + const float lo = math::detail::kChi2Lo95[math::detail::kMaxChiSquareDim - 1] / 5.0f; + const float hi = math::detail::kChi2Hi95[math::detail::kMaxChiSquareDim - 1] / 5.0f; + const float midpoint = (lo + hi) / 2.0f; + + EXPECT_TRUE(Metrics::IsTimeAveragedConsistent(midpoint, 5)); +} + +TEST_F(ConsistencyMetrics3DTest, IsTimeAveragedConsistentInconsistentOutOfBounds) +{ + const float aboveHi = math::detail::kChi2Hi95[math::detail::kMaxChiSquareDim - 1] / 1.0f + 1.0f; + + EXPECT_FALSE(Metrics::IsTimeAveragedConsistent(aboveHi, 1)); +} diff --git a/numerical/solvers/CMakeLists.txt b/numerical/solvers/CMakeLists.txt index 90193092..d17c569f 100644 --- a/numerical/solvers/CMakeLists.txt +++ b/numerical/solvers/CMakeLists.txt @@ -11,7 +11,6 @@ target_link_libraries(numerical.solver ${NUMERICAL_VISIBILITY} ) target_sources(numerical.solver PRIVATE - CholeskyDecomposition.hpp ConditionNumber.hpp DiscreteAlgebraicRiccatiEquation.hpp DormandPrince45.hpp diff --git a/numerical/solvers/CholeskyDecomposition.hpp b/numerical/solvers/CholeskyDecomposition.hpp deleted file mode 100644 index 24a10423..00000000 --- a/numerical/solvers/CholeskyDecomposition.hpp +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include "numerical/math/CompilerOptimizations.hpp" -#include "numerical/math/Matrix.hpp" -#include - -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - -namespace solvers -{ - // Precondition: A must be symmetric positive-definite - template - OPTIMIZE_FOR_SPEED math::SquareMatrix CholeskyDecomposition(const math::SquareMatrix& A) - { - math::SquareMatrix L; - - for (std::size_t i = 0; i < N; ++i) - { - for (std::size_t j = 0; j <= i; ++j) - { - float sum = 0.0f; - - for (std::size_t k = 0; k < j; ++k) - sum += math::ToFloat(L.at(i, k)) * math::ToFloat(L.at(j, k)); - - if (i == j) - L.at(i, j) = T(std::sqrt(math::ToFloat(A.at(i, i)) - sum)); - else - L.at(i, j) = T((math::ToFloat(A.at(i, j)) - sum) / math::ToFloat(L.at(j, j))); - } - } - - return L; - } -}