diff --git a/src/utils/ekf.hpp b/src/utils/ekf.hpp new file mode 100644 index 000000000..d4d834187 --- /dev/null +++ b/src/utils/ekf.hpp @@ -0,0 +1,308 @@ +#pragma once + +#ifndef LIBXR_NO_EIGEN + +#include +#include + +#include "libxr_def.hpp" + +namespace LibXR +{ + +using DefaultScalar = LIBXR_DEFAULT_SCALAR; + +/** + * @class EKF + * @brief 支持非线性状态转移与非线性观测的扩展卡尔曼滤波器。 + * Extended Kalman filter with optional nonlinear transition and observation. + * + * `x` 与 `P` 为公有成员,可在两次预测之间直接改写,但改写后的协方差必须仍描述新状态。 + * `x` and `P` are public and may be edited between predictions; afterwards the + * covariance must still describe the edited state. + * + * 协方差更新使用 Joseph 形式保持对称正定,新息统计使用定容环形缓冲,不做堆分配。 + * The covariance update uses the Joseph form to stay symmetric positive semi-definite; + * the innovation statistics use a fixed-capacity ring buffer with no heap allocation. + * + * @tparam Scalar 标量类型,默认为 DefaultScalar。 + * Scalar type, defaults to DefaultScalar. + * @tparam WindowCapacity NIS 失败环形缓冲容量,同时是默认窗口长度。 + * Capacity of the NIS failure ring buffer, also the default + * window length. + */ +template +class EKF +{ + public: + /// 状态向量。State vector. + using StateVector = Eigen::Matrix; + + /// 状态协方差。State covariance. + using StateCovariance = Eigen::Matrix; + + /// 状态转移矩阵,与状态协方差同形。State transition matrix, same shape as covariance. + using TransitionMatrix = StateCovariance; + + /// 观测向量。Measurement vector. + using MeasurementVector = Eigen::Matrix; + + /// 观测噪声协方差。Measurement noise covariance. + using MeasurementCovariance = Eigen::Matrix; + + /// 观测矩阵,行数为观测维度。Observation matrix, one row per measurement. + using ObservationMatrix = Eigen::Matrix; + + /// 状态增量叠加函数指针,可调用对象不得捕获任何变量。 + /// State increment function pointer; the callable must capture nothing. + using StateAddFunction = StateVector (*)(const StateVector&, const StateVector&); + + StateVector x; ///< 状态向量 State vector + StateCovariance P; ///< 状态协方差 State covariance + + /// NIS 失败环形缓冲容量。Capacity of the NIS failure ring buffer. + static constexpr size_t WINDOW_CAPACITY = WindowCapacity; + + Scalar nis_threshold = static_cast(0.711); ///< NIS 门限 NIS threshold + Scalar nees_threshold = static_cast(0.711); ///< NEES 门限 NEES threshold + + Scalar last_nis{}; ///< 最近一次更新的 NIS。NIS of the last update + Scalar last_nees{}; ///< 最近一次更新的 NEES。NEES of the last update + MeasurementVector last_residual; ///< 最近一次更新后的残差 Posterior residual + + /** + * @brief 默认状态增量叠加:普通加法。 + * Default state increment: plain addition. + */ + static StateVector PlusState(const StateVector& a, const StateVector& b) + { + return a + b; + } + + /** + * @brief 默认残差相减:普通减法。 + * Default residual subtraction: plain subtraction. + */ + static MeasurementVector MinusMeasurement(const MeasurementVector& a, + const MeasurementVector& b) + { + return a - b; + } + + /** + * @brief 构造空滤波器,状态维度为 0。 + * Constructs an empty filter with a zero-sized state. + */ + EKF() = default; + + /** + * @brief 由初始状态、初始协方差和状态增量叠加方式构造。 + * Constructs from an initial state, covariance, and state increment. + * @param x0 初始状态向量。Initial state vector. + * @param P0 初始协方差,方阵且维度与 x0 一致。Initial covariance, square and + * matching x0. + * @param x_add 状态增量叠加方式,默认普通加法;不得捕获变量。 + * State increment, plain addition by default; must capture nothing. + */ + EKF(const StateVector& x0, const StateCovariance& P0, + StateAddFunction x_add = PlusState) + : x(x0), P(P0), I_(StateCovariance::Identity(x0.rows(), x0.rows())), x_add_(x_add) + { + ASSERT(P0.rows() == P0.cols()); + ASSERT(P0.rows() == x0.size()); + } + + /** + * @brief 运行期替换状态增量叠加函数。 + * Replaces the state increment at runtime. + * @param x_add 新的叠加函数,不能为空。New addition function, must not be null. + */ + void SetStateAdd(StateAddFunction x_add) + { + ASSERT(x_add != nullptr); + x_add_ = x_add; + } + + /** + * @brief 使用线性状态转移矩阵预测一步。 + * Predicts one step with a linear transition. + * @param F 状态转移矩阵。State transition matrix. + * @param Q 过程噪声协方差。Process noise covariance. + * @return 预测后的状态。Predicted state. + */ + StateVector Predict(const TransitionMatrix& F, const StateCovariance& Q) + { + return Predict(F, Q, [&F](const StateVector& prior) { return F * prior; }); + } + + /** + * @brief 使用自定义非线性状态转移预测一步。 + * Predicts one step with a custom nonlinear transition. + * @param F 状态转移矩阵,同时用于协方差传播。State transition matrix, also used for the + * covariance propagation. + * @param Q 过程噪声协方差。Process noise covariance. + * @param f 非线性状态转移函数,按模板参数传入,编译期内联。Nonlinear transition, + * passed as a template parameter and inlined at compile time. + * @return 预测后的状态。Predicted state. + */ + template + StateVector Predict(const TransitionMatrix& F, const StateCovariance& Q, + TransitionFunction&& f) + { + ASSERT(F.rows() == F.cols()); + ASSERT(F.rows() == x.size()); + ASSERT(Q.rows() == Q.cols()); + ASSERT(Q.rows() == x.size()); + + P = F * P * F.transpose() + Q; + x = f(x); + return x; + } + + /** + * @brief 使用线性观测矩阵更新状态。 + * Updates the state with a linear observation. + * @param z 观测向量。Measurement vector. + * @param H 观测矩阵。Observation matrix. + * @param R 观测噪声协方差。Measurement noise covariance. + * @return 更新后的状态。Updated state. + */ + StateVector Update(const MeasurementVector& z, const ObservationMatrix& H, + const MeasurementCovariance& R) + { + return Update( + z, H, R, [&H](const StateVector& state) { return H * state; }, MinusMeasurement); + } + + /** + * @brief 使用自定义非线性观测函数更新状态。 + * Updates the state with a custom nonlinear observation. + * @param z 观测向量。Measurement vector. + * @param H 观测函数在当前状态处的雅可比。Jacobian of the observation. + * @param R 观测噪声协方差。Measurement noise covariance. + * @param h 非线性观测函数,按模板参数传入。Nonlinear observation, passed as a template + * parameter. + * @return 更新后的状态。Updated state. + */ + template + StateVector Update(const MeasurementVector& z, const ObservationMatrix& H, + const MeasurementCovariance& R, ObservationFunction&& h) + { + return Update(z, H, R, h, MinusMeasurement); + } + + /** + * @brief 使用自定义非线性观测与残差方式更新状态。 + * Updates the state with a custom nonlinear observation and residual. + * @param z 观测向量。Measurement vector. + * @param H 观测函数在当前状态处的雅可比。Jacobian of the observation. + * @param R 观测噪声协方差。Measurement noise covariance. + * @param h 非线性观测函数,按模板参数传入。Nonlinear observation, passed as a template + * parameter. + * @param z_subtract 残差计算方式,默认普通减法。Residual computation, plain subtraction + * by default. + * @return 更新后的状态。Updated state. + */ + template + StateVector Update(const MeasurementVector& z, const ObservationMatrix& H, + const MeasurementCovariance& R, ObservationFunction&& h, + ResidualFunction&& z_subtract) + { + ASSERT(H.cols() == x.size()); + ASSERT(H.rows() == R.rows()); + ASSERT(H.rows() == R.cols()); + ASSERT(H.rows() == z.size()); + + const StateVector x_prior = x; + const StateCovariance K = P * H.transpose() * (H * P * H.transpose() + R).inverse(); + P = (I_ - K * H) * P * (I_ - K * H).transpose() + K * R * K.transpose(); + x = x_add_(x, K * z_subtract(z, h(x))); + + const MeasurementVector residual = z_subtract(z, h(x)); + const MeasurementCovariance S = H * P * H.transpose() + R; + const Scalar nis = (residual.transpose() * S.inverse() * residual)(0, 0); + const StateVector dx = x - x_prior; + const Scalar nees = (dx.transpose() * P.inverse() * dx)(0, 0); + + if (nis > nis_threshold) + { + ++nis_count_; + } + if (nees > nees_threshold) + { + ++nees_count_; + } + ++total_count_; + + last_nis = nis; + last_nees = nees; + last_residual = residual; + + if constexpr (WINDOW_CAPACITY > 0) + { + nis_window_[nis_window_head_] = nis > nis_threshold ? 1 : 0; + nis_window_head_ = (nis_window_head_ + 1) % WINDOW_CAPACITY; + if (nis_window_count_ < WINDOW_CAPACITY) + { + ++nis_window_count_; + } + } + + return x; + } + + /** + * @brief 设置活跃窗口长度,上限为 WINDOW_CAPACITY。 + * Sets the active window length, capped at WINDOW_CAPACITY. + * @param size 期望的窗口长度,超过容量时按容量处理。Requested length, clamped to the + * capacity. + */ + void SetWindowSize(size_t size) + { + window_size_ = size < WINDOW_CAPACITY ? size : WINDOW_CAPACITY; + } + + /// 返回当前活跃窗口长度。Returns the active window length. + size_t WindowSize() const { return window_size_; } + + /** + * @brief 返回活跃窗口内 NIS 超限次数。 + * Returns NIS failures inside the active window. + */ + int NisFailureCount() const + { + const size_t length = + nis_window_count_ < window_size_ ? nis_window_count_ : window_size_; + int failures = 0; + for (size_t i = 0; i < length; ++i) + { + const size_t index = (nis_window_head_ + WINDOW_CAPACITY - 1 - i) % WINDOW_CAPACITY; + failures += nis_window_[index]; + } + return failures; + } + + /// 返回 NIS 超限的总次数。Returns the total NIS failure count. + int NisFailCount() const { return nis_count_; } + + /// 返回 NEES 超限的总次数。Returns the total NEES failure count. + int NeesFailCount() const { return nees_count_; } + + /// 返回已完成的更新次数。Returns the number of completed updates. + int UpdateCount() const { return total_count_; } + + private: + StateCovariance I_; + StateAddFunction x_add_; + std::array nis_window_{}; + size_t nis_window_head_ = 0; + size_t nis_window_count_ = 0; + size_t window_size_ = WINDOW_CAPACITY; + int nis_count_ = 0; + int nees_count_ = 0; + int total_count_ = 0; +}; + +} // namespace LibXR + +#endif diff --git a/test/automatic/CMakeLists.txt b/test/automatic/CMakeLists.txt index 63ab87294..f1dfd435b 100644 --- a/test/automatic/CMakeLists.txt +++ b/test/automatic/CMakeLists.txt @@ -159,6 +159,7 @@ add_executable( system/timer/test_timer.cpp utils/crc/test_crc.cpp utils/cycle_value/test_cycle_value.cpp + utils/ekf/test_ekf.cpp utils/flag/test_flag.cpp utils/float_encoder/test_encoder.cpp utils/inertia/test_inertia.cpp diff --git a/test/automatic/main.cpp b/test/automatic/main.cpp index 29dd1b5d7..9311fda3b 100644 --- a/test/automatic/main.cpp +++ b/test/automatic/main.cpp @@ -30,6 +30,7 @@ void test_color(); void test_crc(); void test_crc_first_call_race(); void test_float_encoder(); +void test_ekf(); void test_event(); void test_flag(); void test_inertia(); @@ -235,6 +236,7 @@ constexpr GroupedTestCase kMainTestCases[] = { {"crc_first_call_race", &RunVoidEntry, true, false}}, {"utility_tests", {"encoder", &RunVoidEntry, false}}, {"utility_tests", {"cycle_value", &RunVoidEntry, false}}, + {"utility_tests", {"ekf", &RunVoidEntry, false}}, {"utility_tests", {"print", &RunVoidEntry, false}}, {"utility_tests", {"flag", &RunVoidEntry, false}}, diff --git a/test/automatic/utils/ekf/test_ekf.cpp b/test/automatic/utils/ekf/test_ekf.cpp new file mode 100644 index 000000000..062bbdebe --- /dev/null +++ b/test/automatic/utils/ekf/test_ekf.cpp @@ -0,0 +1,307 @@ +/** + * @file test_ekf.cpp + * @brief EKF 预测、更新与新息统计测试 / EKF prediction, update, and innovation statistics + * tests. + * + * 测试项目 / Test items: + * 1. 线性匀速模型的位置与速度估计。 Linear constant-velocity state estimation. + * 2. 预测步协方差等于 F P Fᵀ + Q。 Prediction covariance equals F P Fᵀ + Q. + * 3. 角度残差归一化让更新走最短路径。 Wrapped angle residuals take the short path. + * 4. 自定义状态增量叠加后归一化 yaw。 Custom state addition wraps yaw afterwards. + * 5. 非线性观测下的一致更新不触发门限,离群更新触发门限。 + * A consistent nonlinear update stays inside the gate, an outlier trips it. + * 6. NIS 失败滑窗不超过窗口长度。 The NIS failure window never exceeds its length. + * 7. Joseph 形式更新保持协方差对称。 The Joseph-form update keeps P symmetric. + * 8. 运行期替换状态增量叠加函数。 Runtime replacement of the state addition hook. + * + * 测试原理 / Test principles: + * 1. 所有场景使用可手推的期望值,不依赖随机数据。 Every scenario uses hand-derivable + * expected values so no random data is needed. + * 2. 角度场景选取跨越 ±π 的观测,直接暴露未归一化残差会产生 2π 级跳变。 + * Angle scenarios use observations across ±π, where an unwrapped residual would jump + * by 2π. + */ + +#include "ekf.hpp" +#include "libxr.hpp" +#include "libxr_def.hpp" +#include "test.hpp" +#include "test_assert.hpp" + +namespace +{ +constexpr double EKF_TEST_PI = 3.14159265358979323846; + +// 角度归一化放在文件级函数里:EKF 钩子必须捕获列表为空,不能捕获局部 lambda。 +// The angle normalization lives at file scope: an EKF hook must capture nothing, so it +// cannot capture a local lambda. +double WrapAngle(double angle) +{ + while (angle > EKF_TEST_PI) + { + angle -= 2.0 * EKF_TEST_PI; + } + while (angle <= -EKF_TEST_PI) + { + angle += 2.0 * EKF_TEST_PI; + } + return angle; +} +} // namespace + +void test_ekf() +{ + using Ekf = LibXR::EKF; + using EkfState = Ekf::StateVector; + using EkfStateCov = Ekf::StateCovariance; + using EkfMeasurement = Ekf::MeasurementVector; + using EkfMeasurementCov = Ekf::MeasurementCovariance; + using EkfObservation = Ekf::ObservationMatrix; + + // 检查内容:线性匀速模型在位置观测下收敛到真值,速度被观测序列估计出来。 + // Check: a linear constant-velocity model converges to the true position and estimates + // velocity from the observation sequence. + { + EkfState x0(2); + x0 << 0.0, 0.0; + EkfStateCov p0 = EkfStateCov::Identity(2, 2); + Ekf ekf(x0, p0); + + const double dt = 0.1; + EkfStateCov f = EkfStateCov::Identity(2, 2); + f(0, 1) = dt; + + // 分段白噪声加速度过程噪声,连续匀速模型位置通道的常用形式。 + // Piecewise-white-noise acceleration process noise, the common form for the position + // channel of a constant-velocity model. + const double accel = 1.0; + EkfStateCov q = EkfStateCov::Zero(2, 2); + q(0, 0) = dt * dt * dt * dt / 4.0 * accel; + q(0, 1) = dt * dt * dt / 2.0 * accel; + q(1, 0) = q(0, 1); + q(1, 1) = dt * dt * accel; + TEST_ASSERT(q(1, 1) > 0.0); + + EkfObservation h(1, 2); + h << 1.0, 0.0; + EkfMeasurementCov r(1, 1); + r << 0.01; + + const double velocity = 0.5; + for (int i = 1; i <= 40; ++i) + { + ekf.Predict(f, q); + EkfMeasurement z(1); + z << velocity * i * dt; + ekf.Update(z, h, r); + } + + TEST_ASSERT(std::abs(ekf.x(0) - velocity * 40 * dt) < 1e-3); + TEST_ASSERT(std::abs(ekf.x(1) - velocity) < 1e-3); + TEST_ASSERT(ekf.UpdateCount() == 40); + } + + // 检查内容:只预测不更新时,协方差按 F P Fᵀ + Q 增长。 + // Check: prediction without an update grows the covariance by F P Fᵀ + Q. + { + EkfState x0(1); + x0 << 1.0; + EkfStateCov p0(1, 1); + p0 << 0.5; + Ekf ekf(x0, p0); + + EkfStateCov f(1, 1); + f << 2.0; + EkfStateCov q(1, 1); + q << 0.25; + + ekf.Predict(f, q); + TEST_ASSERT(equal(ekf.P(0, 0), 2.0 * 0.5 * 2.0 + 0.25)); + TEST_ASSERT(equal(ekf.x(0), 2.0)); + TEST_ASSERT(ekf.UpdateCount() == 0); + } + + // 检查内容:观测跨越 ±π 时,归一化残差把状态推向最近邻而不是绕远 2π。 + // Check: with an observation across ±π, the wrapped residual moves the state to the + // nearest value instead of the 2π detour. + { + EkfState x0(1); + x0 << -3.10; + EkfStateCov p0(1, 1); + p0 << 0.1; + Ekf ekf(x0, p0); + + EkfStateCov f = EkfStateCov::Identity(1, 1); + EkfStateCov q = EkfStateCov::Zero(1, 1); + ekf.Predict(f, q); + + EkfObservation h = EkfObservation::Identity(1, 1); + EkfMeasurementCov r(1, 1); + r << 0.01; + + EkfMeasurement z(1); + z << 3.10; + auto residual = [](const EkfMeasurement& a, const EkfMeasurement& b) + { + EkfMeasurement c = a - b; + c(0) = std::remainder(c(0), 2.0 * EKF_TEST_PI); + return c; + }; + ekf.Update(z, h, r, [](const EkfState& state) { return state; }, residual); + + TEST_ASSERT(std::abs(WrapAngle(ekf.x(0) - (-3.10))) < 0.2); + TEST_ASSERT(std::abs(WrapAngle(ekf.x(0) - 3.10)) < 0.2); + } + + // 检查内容:自定义状态增量叠加把越界 yaw 归一化回 [-π, π)。 + // Check: the custom state addition wraps an out-of-range yaw back into [-π, π). + { + EkfState x0(1); + x0 << 3.10; + EkfStateCov p0(1, 1); + p0 << 1.0; + auto x_add = [](const EkfState& a, const EkfState& b) + { + EkfState c = a + b; + c(0) = WrapAngle(c(0)); + return c; + }; + Ekf ekf(x0, p0, x_add); + + EkfObservation h = EkfObservation::Identity(1, 1); + EkfMeasurementCov r(1, 1); + r << 0.5; + + EkfMeasurement z(1); + z << 3.20; + ekf.Update(z, h, r); + + TEST_ASSERT(std::abs(ekf.x(0) - WrapAngle(3.10 + (3.20 - 3.10) * (1.0 / 1.5))) < + 1e-9); + TEST_ASSERT(ekf.x(0) < 0.0); + } + + // 检查内容:运行期替换状态增量叠加函数后,下一次更新即按新逻辑回绕 yaw。 + // Check: after replacing the state addition hook at runtime, the next update wraps yaw + // with the new logic. + { + EkfState x0(1); + x0 << 3.10; + EkfStateCov p0(1, 1); + p0 << 1.0; + Ekf ekf(x0, p0); + + EkfObservation h = EkfObservation::Identity(1, 1); + EkfMeasurementCov r(1, 1); + r << 0.5; + EkfMeasurement z(1); + z << 3.20; + + ekf.Update(z, h, r); + TEST_ASSERT(std::abs(ekf.x(0) - (3.10 + (3.20 - 3.10) * (1.0 / 1.5))) < 1e-9); + + ekf.SetStateAdd( + [](const EkfState& a, const EkfState& b) + { + EkfState c = a + b; + c(0) = WrapAngle(c(0)); + return c; + }); + ekf.Update(z, h, r); + TEST_ASSERT(ekf.x(0) < 0.0); + } + + // 检查内容:一致的非线性更新不触发新息门限,离群更新触发门限。 + // Check: a consistent nonlinear update stays inside the innovation gate, an outlier + // trips it. + { + EkfState x0(1); + x0 << 1.0; + EkfStateCov p0(1, 1); + p0 << 0.01; + Ekf ekf(x0, p0); + + EkfObservation h(1, 1); + h << 2.0; + EkfMeasurementCov r(1, 1); + r << 0.01; + auto observe = [](const EkfState& state) + { + EkfMeasurement z(1); + z << state(0) * state(0); + return z; + }; + + EkfMeasurement z(1); + z << 1.0404; + ekf.Update(z, h, r, observe); + TEST_ASSERT(ekf.NisFailCount() == 0); + TEST_ASSERT(ekf.NeesFailCount() == 0); + TEST_ASSERT(ekf.NisFailureCount() == 0); + TEST_ASSERT(ekf.UpdateCount() == 1); + + z << 3.0; + ekf.Update(z, h, r, observe); + TEST_ASSERT(ekf.last_nis > ekf.nis_threshold); + TEST_ASSERT(ekf.NisFailCount() == 1); + TEST_ASSERT(ekf.UpdateCount() == 2); + } + + // 检查内容:滑窗在更新前设为 2 时,失败记录数不超过窗口长度。 + // Check: with the window set to 2 before the updates, the recorded failures never + // exceed the window length. + { + EkfState x0(1); + x0 << 1.0; + EkfStateCov p0(1, 1); + p0 << 0.01; + Ekf ekf(x0, p0); + ekf.SetWindowSize(2); + + EkfObservation h = EkfObservation::Identity(1, 1); + EkfMeasurementCov r(1, 1); + r << 0.01; + + EkfMeasurement z(1); + for (int i = 0; i < 3; ++i) + { + z << 3.0 + 2.0 * i; + ekf.Update(z, h, r); + TEST_ASSERT(ekf.last_nis > ekf.nis_threshold); + TEST_ASSERT(ekf.NisFailureCount() <= ekf.WindowSize()); + } + TEST_ASSERT(ekf.NisFailureCount() == 2); + TEST_ASSERT(ekf.WindowSize() == 2); + TEST_ASSERT(ekf.NisFailCount() == 3); + TEST_ASSERT(ekf.UpdateCount() == 3); + + ekf.SetWindowSize(ekf.WINDOW_CAPACITY + 10U); + TEST_ASSERT(ekf.WindowSize() == ekf.WINDOW_CAPACITY); + } + + // 检查内容:多次更新后协方差保持对称,即 Joseph 形式生效。 + // Check: the covariance stays symmetric across updates, which the Joseph form + // guarantees. + { + EkfState x0(2); + x0 << 0.0, 1.0; + EkfStateCov p0(2, 2); + p0 << 1.0, 0.3, 0.3, 1.0; + Ekf ekf(x0, p0); + + EkfStateCov f = EkfStateCov::Identity(2, 2); + EkfStateCov q = EkfStateCov::Zero(2, 2); + EkfObservation h = EkfObservation::Identity(2, 2); + EkfMeasurementCov r = EkfMeasurementCov::Identity(2, 2) * 0.5; + + for (int i = 0; i < 5; ++i) + { + ekf.Predict(f, q); + EkfMeasurement z(2); + z << 0.1 * i, 1.0 + 0.05 * i; + ekf.Update(z, h, r); + TEST_ASSERT((ekf.P - ekf.P.transpose()).cwiseAbs().maxCoeff() < 1e-12); + TEST_ASSERT(ekf.P.diagonal().minCoeff() > 0.0); + } + } +}