|
| 1 | +#pragma once |
| 2 | + |
| 3 | +#if defined(__GNUC__) || defined(__clang__) |
| 4 | +#pragma GCC optimize("O3", "fast-math") |
| 5 | +#endif |
| 6 | + |
| 7 | +#include "numerical/math/CompilerOptimizations.hpp" |
| 8 | +#include "numerical/math/Matrix.hpp" |
| 9 | +#include "numerical/solvers/GaussianElimination.hpp" |
| 10 | +#include <array> |
| 11 | +#include <cmath> |
| 12 | +#include <cstddef> |
| 13 | +#include <optional> |
| 14 | + |
| 15 | +namespace estimators |
| 16 | +{ |
| 17 | + namespace detail |
| 18 | + { |
| 19 | + static constexpr std::size_t kMaxChiSquareDim = 10; |
| 20 | + static constexpr std::size_t kNumAlpha = 1; |
| 21 | + |
| 22 | + static constexpr std::array<float, kMaxChiSquareDim> kChi2Lo95 = { |
| 23 | + 0.000982f, 0.050636f, 0.215795f, 0.484419f, 0.831212f, |
| 24 | + 1.237344f, 1.689869f, 2.179731f, 2.700389f, 3.246973f |
| 25 | + }; |
| 26 | + |
| 27 | + static constexpr std::array<float, kMaxChiSquareDim> kChi2Hi95 = { |
| 28 | + 5.023886f, 7.377759f, 9.348404f, 11.143480f, 12.832502f, |
| 29 | + 14.449376f, 16.012764f, 17.534546f, 19.022768f, 20.483177f |
| 30 | + }; |
| 31 | + } |
| 32 | + |
| 33 | + template<typename T, std::size_t Dim> |
| 34 | + class ConsistencyMetrics |
| 35 | + { |
| 36 | + static_assert(std::is_floating_point_v<T>, "ConsistencyMetrics supports floating-point types"); |
| 37 | + static_assert(Dim >= 1 && Dim <= detail::kMaxChiSquareDim, |
| 38 | + "ConsistencyMetrics Dim must be in [1, kMaxChiSquareDim]"); |
| 39 | + |
| 40 | + public: |
| 41 | + using StateVector = math::Vector<T, Dim>; |
| 42 | + using CovarianceMatrix = math::Matrix<T, Dim, Dim>; |
| 43 | + |
| 44 | + [[nodiscard]] static OPTIMIZE_FOR_SPEED std::optional<T> Nees(const StateVector& error, const CovarianceMatrix& covariance); |
| 45 | + [[nodiscard]] static OPTIMIZE_FOR_SPEED std::optional<T> Nis(const StateVector& innovation, const CovarianceMatrix& innovationCovariance); |
| 46 | + [[nodiscard]] static bool IsConsistent(T value); |
| 47 | + [[nodiscard]] static bool IsTimeAveragedConsistent(T averagedValue, std::size_t numSamples); |
| 48 | + |
| 49 | + private: |
| 50 | + [[nodiscard]] static OPTIMIZE_FOR_SPEED std::optional<StateVector> Solve(const CovarianceMatrix& matrix, const StateVector& rhs); |
| 51 | + [[nodiscard]] static OPTIMIZE_FOR_SPEED T DotProduct(const StateVector& a, const StateVector& b); |
| 52 | + }; |
| 53 | + |
| 54 | + template<typename T, std::size_t Dim> |
| 55 | + OPTIMIZE_FOR_SPEED std::optional<T> ConsistencyMetrics<T, Dim>::Nees(const StateVector& error, const CovarianceMatrix& covariance) |
| 56 | + { |
| 57 | + auto z = Solve(covariance, error); |
| 58 | + if (!z.has_value()) |
| 59 | + return std::nullopt; |
| 60 | + return DotProduct(error, z.value()); |
| 61 | + } |
| 62 | + |
| 63 | + template<typename T, std::size_t Dim> |
| 64 | + OPTIMIZE_FOR_SPEED std::optional<T> ConsistencyMetrics<T, Dim>::Nis(const StateVector& innovation, const CovarianceMatrix& innovationCovariance) |
| 65 | + { |
| 66 | + auto z = Solve(innovationCovariance, innovation); |
| 67 | + if (!z.has_value()) |
| 68 | + return std::nullopt; |
| 69 | + return DotProduct(innovation, z.value()); |
| 70 | + } |
| 71 | + |
| 72 | + template<typename T, std::size_t Dim> |
| 73 | + bool ConsistencyMetrics<T, Dim>::IsConsistent(T value) |
| 74 | + { |
| 75 | + return value >= static_cast<T>(detail::kChi2Lo95[Dim - 1]) && |
| 76 | + value <= static_cast<T>(detail::kChi2Hi95[Dim - 1]); |
| 77 | + } |
| 78 | + |
| 79 | + template<typename T, std::size_t Dim> |
| 80 | + bool ConsistencyMetrics<T, Dim>::IsTimeAveragedConsistent(T averagedValue, std::size_t numSamples) |
| 81 | + { |
| 82 | + if (numSamples == 0) |
| 83 | + return false; |
| 84 | + const std::size_t dof = numSamples * Dim; |
| 85 | + const float lo = (dof <= detail::kMaxChiSquareDim) |
| 86 | + ? detail::kChi2Lo95[dof - 1] / static_cast<float>(numSamples) |
| 87 | + : detail::kChi2Lo95[detail::kMaxChiSquareDim - 1] / static_cast<float>(numSamples); |
| 88 | + const float hi = (dof <= detail::kMaxChiSquareDim) |
| 89 | + ? detail::kChi2Hi95[dof - 1] / static_cast<float>(numSamples) |
| 90 | + : detail::kChi2Hi95[detail::kMaxChiSquareDim - 1] / static_cast<float>(numSamples); |
| 91 | + return static_cast<float>(averagedValue) >= lo && |
| 92 | + static_cast<float>(averagedValue) <= hi; |
| 93 | + } |
| 94 | + |
| 95 | + template<typename T, std::size_t Dim> |
| 96 | + OPTIMIZE_FOR_SPEED std::optional<typename ConsistencyMetrics<T, Dim>::StateVector> ConsistencyMetrics<T, Dim>::Solve(const CovarianceMatrix& matrix, const StateVector& rhs) |
| 97 | + { |
| 98 | + for (std::size_t i = 0; i < Dim; ++i) |
| 99 | + { |
| 100 | + float pivot = std::abs(static_cast<float>(matrix.at(i, i))); |
| 101 | + if (pivot < 1e-10f) |
| 102 | + return std::nullopt; |
| 103 | + } |
| 104 | + |
| 105 | + solvers::GaussianElimination<T, Dim> solver; |
| 106 | + return solver.Solve(matrix, rhs); |
| 107 | + } |
| 108 | + |
| 109 | + template<typename T, std::size_t Dim> |
| 110 | + OPTIMIZE_FOR_SPEED T ConsistencyMetrics<T, Dim>::DotProduct(const StateVector& a, const StateVector& b) |
| 111 | + { |
| 112 | + T result{}; |
| 113 | + for (std::size_t i = 0; i < Dim; ++i) |
| 114 | + result += a.at(i, 0) * b.at(i, 0); |
| 115 | + return result; |
| 116 | + } |
| 117 | + |
| 118 | +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD |
| 119 | + extern template class ConsistencyMetrics<float, 1>; |
| 120 | + extern template class ConsistencyMetrics<float, 2>; |
| 121 | + extern template class ConsistencyMetrics<float, 3>; |
| 122 | +#endif |
| 123 | +} |
![github-actions[bot]](https://avatars.githubusercontent.com/in/15368?v=4&size=40){kind=avatar}
0 commit comments