diff --git a/numerical/filters/passive/SavitzkyGolayFilter.hpp b/numerical/filters/passive/SavitzkyGolayFilter.hpp index b3bdf45..16c5b05 100644 --- a/numerical/filters/passive/SavitzkyGolayFilter.hpp +++ b/numerical/filters/passive/SavitzkyGolayFilter.hpp @@ -15,6 +15,14 @@ namespace filters::passive { namespace detail { + constexpr std::size_t Factorial(std::size_t n) + { + std::size_t result = 1; + for (std::size_t i = 2; i <= n; ++i) + result *= i; + return result; + } + template constexpr math::Matrix BuildVandermonde() { @@ -43,12 +51,18 @@ namespace filters::passive eDeriv.at(Deriv, 0) = T{ 1 }; auto z = math::CholeskyDecomposition::Solve(AtA, eDeriv); + if (!z.has_value()) + return std::array{}; + constexpr auto scale = static_cast(Factorial(Deriv)); std::array coeffs{}; for (std::size_t k = 0; k < Window; ++k) for (std::size_t d = 0; d < Order + 1; ++d) coeffs[k] += A.at(k, d) * z->at(d, 0); + for (std::size_t k = 0; k < Window; ++k) + coeffs[k] *= scale; + return coeffs; } } diff --git a/numerical/filters/passive/test/TestSavitzkyGolayFilter.cpp b/numerical/filters/passive/test/TestSavitzkyGolayFilter.cpp index c93caa1..33b9239 100644 --- a/numerical/filters/passive/test/TestSavitzkyGolayFilter.cpp +++ b/numerical/filters/passive/test/TestSavitzkyGolayFilter.cpp @@ -150,7 +150,7 @@ TEST_F(TestSavitzkyGolayFilter, second_derivative_of_quadratic_recovers_curvatur float x = static_cast(n); float out = d2.Filter(a * x * x + 5.0f * x - 2.0f); if (n >= 4) - EXPECT_NEAR(out, a, tol); + EXPECT_NEAR(out, 2.0f * a, tol); } } diff --git a/numerical/math/CMakeLists.txt b/numerical/math/CMakeLists.txt index b341959..54df957 100644 --- a/numerical/math/CMakeLists.txt +++ b/numerical/math/CMakeLists.txt @@ -36,6 +36,7 @@ target_sources(numerical.math PRIVATE numerical_add_coverage_sources(numerical.math Math.cpp + CholeskyDecomposition.cpp ComplexNumber.cpp ConsistencyMetrics.cpp Cordic.cpp diff --git a/numerical/math/CholeskyDecomposition.cpp b/numerical/math/CholeskyDecomposition.cpp new file mode 100644 index 0000000..6341c1a --- /dev/null +++ b/numerical/math/CholeskyDecomposition.cpp @@ -0,0 +1,8 @@ +#include "numerical/math/CholeskyDecomposition.hpp" + +namespace math +{ + template class CholeskyDecomposition; + template class CholeskyDecomposition; + template class CholeskyDecomposition; +} diff --git a/numerical/math/CholeskyDecomposition.hpp b/numerical/math/CholeskyDecomposition.hpp index f14391b..4cabaf9 100644 --- a/numerical/math/CholeskyDecomposition.hpp +++ b/numerical/math/CholeskyDecomposition.hpp @@ -9,6 +9,7 @@ #include "numerical/math/Matrix.hpp" #include "numerical/math/TriangularSolve.hpp" #include +#include #include namespace math @@ -50,7 +51,7 @@ namespace math if (i != j) l.at(i, j) = T(sum / ToFloat(l.at(j, j))); - else if (sum < 1e-10f) + else if (sum <= std::numeric_limits::epsilon() * math::Abs(ToFloat(a.at(i, i)))) return std::nullopt; else l.at(i, j) = T(math::Sqrt(sum)); @@ -76,4 +77,10 @@ namespace math const Vector y = SolveLowerTriangular(l.value(), b); return SolveUpperTriangular(l.value().Transpose(), y); } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class CholeskyDecomposition; + extern template class CholeskyDecomposition; + extern template class CholeskyDecomposition; +#endif } diff --git a/numerical/math/ConsistencyMetrics.hpp b/numerical/math/ConsistencyMetrics.hpp index 4eaf50b..5f59812 100644 --- a/numerical/math/ConsistencyMetrics.hpp +++ b/numerical/math/ConsistencyMetrics.hpp @@ -27,6 +27,16 @@ namespace math 5.023886f, 7.377759f, 9.348404f, 11.143480f, 12.832502f, 14.449376f, 16.012764f, 17.534546f, 19.022768f, 20.483177f }; + + static constexpr float kZ975 = 1.95996f; + + [[nodiscard]] OPTIMIZE_FOR_SPEED constexpr float Chi2QuantileApprox(float z, float dof) + { + const float mu = 1.0f - 2.0f / (9.0f * dof); + const float sigma = math::Sqrt(2.0f / (9.0f * dof)); + const float x = mu + z * sigma; + return dof * x * x * x; + } } template @@ -77,12 +87,16 @@ namespace math if (numSamples == 0) return false; const std::size_t dof = numSamples * Dim; - const float lo = (dof <= detail::kMaxChiSquareDim) - ? detail::kChi2Lo95[dof - 1] / static_cast(numSamples) - : detail::kChi2Lo95[detail::kMaxChiSquareDim - 1] / static_cast(numSamples); - const float hi = (dof <= detail::kMaxChiSquareDim) - ? detail::kChi2Hi95[dof - 1] / static_cast(numSamples) - : detail::kChi2Hi95[detail::kMaxChiSquareDim - 1] / static_cast(numSamples); + const auto fdof = static_cast(dof); + const auto fsamples = static_cast(numSamples); + const float lo_raw = (dof <= detail::kMaxChiSquareDim) + ? detail::kChi2Lo95[dof - 1] + : detail::Chi2QuantileApprox(-detail::kZ975, fdof); + const float hi_raw = (dof <= detail::kMaxChiSquareDim) + ? detail::kChi2Hi95[dof - 1] + : detail::Chi2QuantileApprox(detail::kZ975, fdof); + const float lo = lo_raw / fsamples; + const float hi = hi_raw / fsamples; return static_cast(averagedValue) >= lo && static_cast(averagedValue) <= hi; } diff --git a/numerical/math/test/CMakeLists.txt b/numerical/math/test/CMakeLists.txt index 5bbcb22..bb07325 100644 --- a/numerical/math/test/CMakeLists.txt +++ b/numerical/math/test/CMakeLists.txt @@ -11,6 +11,7 @@ target_link_libraries(numerical.math_test PUBLIC numerical_add_qemu_test(numerical.math_test) target_sources(numerical.math_test PRIVATE + TestCholeskyDecomposition.cpp TestComplexNumber.cpp TestMath.cpp TestConsistencyMetrics.cpp diff --git a/numerical/math/test/TestCholeskyDecomposition.cpp b/numerical/math/test/TestCholeskyDecomposition.cpp new file mode 100644 index 0000000..7f8ec98 --- /dev/null +++ b/numerical/math/test/TestCholeskyDecomposition.cpp @@ -0,0 +1,120 @@ +#include "numerical/math/CholeskyDecomposition.hpp" +#include "numerical/math/Tolerance.hpp" +#include +#include + +namespace +{ + class CholeskyDecompositionTest : public ::testing::Test + { + protected: + using Chol1 = math::CholeskyDecomposition; + using Chol2 = math::CholeskyDecomposition; + using Mat1 = math::SquareMatrix; + using Mat2 = math::SquareMatrix; + using Vec2 = math::Vector; + }; +} + +TEST_F(CholeskyDecompositionTest, TryFactorScalarSpdMatrixSucceeds) +{ + Mat1 a; + a.at(0, 0) = 4.0f; + + auto l = Chol1::TryFactor(a); + + ASSERT_TRUE(l.has_value()); + EXPECT_NEAR(l->at(0, 0), 2.0f, math::Tolerance()); +} + +TEST_F(CholeskyDecompositionTest, TryFactorSingularScalarReturnsNullopt) +{ + Mat1 a; + a.at(0, 0) = 0.0f; + + EXPECT_FALSE(Chol1::TryFactor(a).has_value()); +} + +TEST_F(CholeskyDecompositionTest, TryFactor2x2SpdMatrixCoversOffDiagonalBranch) +{ + Mat2 a; + a.at(0, 0) = 4.0f; + a.at(0, 1) = 2.0f; + a.at(1, 0) = 2.0f; + a.at(1, 1) = 3.0f; + + auto l = Chol2::TryFactor(a); + + ASSERT_TRUE(l.has_value()); + EXPECT_NEAR(l->at(0, 0), 2.0f, math::Tolerance()); + EXPECT_NEAR(l->at(1, 0), 1.0f, math::Tolerance()); + EXPECT_NEAR(l->at(1, 1), std::sqrt(2.0f), math::Tolerance()); +} + +TEST_F(CholeskyDecompositionTest, TryFactorNonSpdMatrixReturnsNullopt) +{ + Mat2 a; + a.at(0, 0) = 1.0f; + a.at(0, 1) = 2.0f; + a.at(1, 0) = 2.0f; + a.at(1, 1) = 1.0f; + + EXPECT_FALSE(Chol2::TryFactor(a).has_value()); +} + +TEST_F(CholeskyDecompositionTest, TryFactorSmallScaleSpdMatrixAccepted) +{ + Mat1 a; + a.at(0, 0) = 5e-11f; + + auto l = Chol1::TryFactor(a); + + ASSERT_TRUE(l.has_value()); + EXPECT_GT(l->at(0, 0), 0.0f); +} + +TEST_F(CholeskyDecompositionTest, FactorNonSpdReturnsZeroMatrix) +{ + Mat2 a; + a.at(0, 0) = 1.0f; + a.at(0, 1) = 2.0f; + a.at(1, 0) = 2.0f; + a.at(1, 1) = 1.0f; + + auto l = Chol2::Factor(a); + + EXPECT_NEAR(l.at(0, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(l.at(1, 1), 0.0f, math::Tolerance()); +} + +TEST_F(CholeskyDecompositionTest, SolveDiagonalSystemFindsExactSolution) +{ + Mat2 a; + a.at(0, 0) = 4.0f; + a.at(0, 1) = 0.0f; + a.at(1, 0) = 0.0f; + a.at(1, 1) = 9.0f; + Vec2 b; + b.at(0, 0) = 8.0f; + b.at(1, 0) = 27.0f; + + auto x = Chol2::Solve(a, b); + + ASSERT_TRUE(x.has_value()); + EXPECT_NEAR(x->at(0, 0), 2.0f, math::Tolerance()); + EXPECT_NEAR(x->at(1, 0), 3.0f, math::Tolerance()); +} + +TEST_F(CholeskyDecompositionTest, SolveNonSpdSystemReturnsNullopt) +{ + Mat2 a; + a.at(0, 0) = 1.0f; + a.at(0, 1) = 2.0f; + a.at(1, 0) = 2.0f; + a.at(1, 1) = 1.0f; + Vec2 b; + b.at(0, 0) = 1.0f; + b.at(1, 0) = 1.0f; + + EXPECT_FALSE(Chol2::Solve(a, b).has_value()); +} diff --git a/numerical/math/test/TestConsistencyMetrics.cpp b/numerical/math/test/TestConsistencyMetrics.cpp index 13cbb83..0d1edc4 100644 --- a/numerical/math/test/TestConsistencyMetrics.cpp +++ b/numerical/math/test/TestConsistencyMetrics.cpp @@ -258,13 +258,10 @@ TEST_F(ConsistencyMetrics3DTest, IsTimeAveragedConsistentWithOneSample) EXPECT_TRUE(Metrics::IsTimeAveragedConsistent(midpoint, 1)); } -TEST_F(ConsistencyMetrics3DTest, IsTimeAveragedConsistentDofClampBranch) +TEST_F(ConsistencyMetrics3DTest, IsTimeAveragedConsistentLargeDofWilsonHilferty) { - 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)); + EXPECT_TRUE(Metrics::IsTimeAveragedConsistent(2.373f, 5)); + EXPECT_FALSE(Metrics::IsTimeAveragedConsistent(6.0f, 5)); } TEST_F(ConsistencyMetrics3DTest, IsTimeAveragedConsistentInconsistentOutOfBounds) @@ -273,3 +270,9 @@ TEST_F(ConsistencyMetrics3DTest, IsTimeAveragedConsistentInconsistentOutOfBounds EXPECT_FALSE(Metrics::IsTimeAveragedConsistent(aboveHi, 1)); } + +TEST_F(ConsistencyMetrics1DTest, IsTimeAveragedConsistentLargeNumSamplesUsesApproximation) +{ + EXPECT_TRUE(Metrics::IsTimeAveragedConsistent(1.0f, 100)); + EXPECT_FALSE(Metrics::IsTimeAveragedConsistent(1.4f, 100)); +}