From fd7e09fbce4337820cadf1c728f3d77b20f01580 Mon Sep 17 00:00:00 2001 From: keruth Date: Tue, 10 Nov 2026 14:22:45 +0800 Subject: [PATCH 1/5] feat(utils): add EKF math utility MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds LibXR::EKF, a header-only extended Kalman filter for nonlinear state transition and observation models, following the other Eigen-based utils under src/utils and guarded by LIBXR_NO_EIGEN. Angular states use customizable hooks: x_add defines how a state increment is applied (for example wrapping yaw into [-pi, pi) after the addition) and z_subtract defines how a residual is formed (for example wrapping an angle residual), while f and h carry the nonlinear transition and observation. The covariance update uses the Joseph form, so P stays symmetric positive semi-definite. Every update computes the normalized innovation squared (NIS) and the normalized estimation error squared (NEES) against configurable thresholds and records the result in a ring buffer whose capacity is the WindowCapacity template parameter; the statistics buffer is a std::array member and allocates no heap memory. Caller-supplied matrices are checked with ASSERT on their dimensions. The hooks take two forms: the per-call f, h and z_subtract are template parameters that inline at compile time, and x_add, supplied at construction and kept for the object lifetime, is a plain function pointer, matching RBTree's comparator. Both forms bind at compile time and add zero storage for the callable, and the filter relies only on Eigen's usual dynamic-matrix storage, so it runs unchanged on bare-metal, RTOS and Linux targets. The state vector and covariance stay public so a caller may adjust the state between predictions; the class documents that such an edit must keep the covariance consistent with the state. test_ekf covers linear constant-velocity tracking, the F P Fᵀ + Q prediction identity, wrapped angle residuals across ±pi, yaw wrapping through the state-addition hook, NIS/NEES gating for consistent versus outlier observations, the failure-window bound and its capacity clamp, and covariance symmetry. --- src/utils/ekf.hpp | 364 ++++++++++++++++++++++++++ test/automatic/CMakeLists.txt | 1 + test/automatic/main.cpp | 2 + test/automatic/utils/ekf/test_ekf.cpp | 276 +++++++++++++++++++ 4 files changed, 643 insertions(+) create mode 100644 src/utils/ekf.hpp create mode 100644 test/automatic/utils/ekf/test_ekf.cpp diff --git a/src/utils/ekf.hpp b/src/utils/ekf.hpp new file mode 100644 index 000000000..683e86bea --- /dev/null +++ b/src/utils/ekf.hpp @@ -0,0 +1,364 @@ +#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` 是公有成员,调用方可以在两次预测之间直接改写状态 + * (固定某个自由度、限制角速度、按观测修正某个分量),但改写后的协方差必须仍然描述 + * 改写后的状态,否则后续更新会按旧不确定性加权。 + * The state vector `x` and covariance `P` are public so callers may adjust the state + * between predictions (pin a degree of freedom, clamp a rate, correct one component + * from an observation); after such an edit the covariance must still describe the + * edited state, otherwise later updates weigh the state with stale uncertainty. + * + * 协方差更新使用 Joseph 形式,数值上保持对称正定,适用于过程噪声与观测噪声都接近 + * 奇异或者状态维度动态变化的场景。 + * The covariance update uses the Joseph form, which stays symmetric positive + * semi-definite and suits near-singular process or measurement noise. + * + * 角度类状态通过可自定义钩子处理:`x_add` 决定状态增量如何叠加(例如把 yaw 叠加后 + * 归一化到 [-π, π)),`z_subtract` 决定残差如何相减(例如把角度残差归一化),`f` 与 + * `h` 分别承载非线性状态转移与非线性观测。 + * Angular states use customizable hooks: `x_add` defines how a state increment is + * applied (for example wrapping yaw into [-π, π) after the addition), `z_subtract` + * defines how a residual is formed (for example wrapping an angle residual), and `f` + * and `h` carry the nonlinear transition and observation. + * + * 钩子分两种形式:每次调用传入的 `f`、`h`、`z_subtract` 是模板参数,编译期内联;构造时 + * 传入并长期保存的 `x_add` 是裸函数指针(与 RBTree 的比较器一致)。两种形式都在编译期 + * 绑定,可调用对象零存储开销;滤波器只依赖 Eigen 动态矩阵的常规存储行为,并且不依赖 + * 异常与 RTTI,可在裸金属、RTOS 与 Linux 上使用。 + * The hooks take two forms: the per-call `f`, `h` and `z_subtract` are template + * parameters that inline at compile time, and `x_add`, supplied at construction and + * kept for the object lifetime, is a plain function pointer, matching RBTree's + * comparator. Both forms bind at compile time and add zero storage for the callable; + * the filter relies only on Eigen's usual dynamic-matrix storage and needs neither + * exceptions nor RTTI, so it runs on bare-metal, RTOS and Linux targets alike. + * + * 新息统计:每次更新计算归一化新息平方(NIS)与归一化估计误差平方(NEES),与 + * `nis_threshold` / `nees_threshold` 比较后计数。近期 NIS 失败记录放在编译期固定容量的 + * 环形缓冲里,容量由 `WindowCapacity` 决定,活跃窗口长度用 `SetWindowSize` 调整; + * 统计缓冲区不分配堆内存。 + * Innovation statistics: every update computes the normalized innovation squared (NIS) + * and the normalized estimation error squared (NEES) and compares them with + * `nis_threshold` / `nees_threshold`. Recent NIS failures live in a ring buffer whose + * capacity is fixed at compile time by `WindowCapacity`, with the active window length + * set through `SetWindowSize`; the statistics buffer allocates no heap memory. + * + * @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: + using StateVector = Eigen::Matrix; + using StateCovariance = Eigen::Matrix; + using TransitionMatrix = StateCovariance; + using MeasurementVector = Eigen::Matrix; + using MeasurementCovariance = Eigen::Matrix; + using ObservationMatrix = Eigen::Matrix; + + /** + * @brief 状态增量叠加函数指针,只接受捕获列表为空的可调用对象。 + * State increment application function pointer; the callable captures nothing. + * + * 构造后长期持有的钩子使用裸函数指针(与 RBTree 的比较器一致):钩子自身不分配堆 + * 内存、可拷贝、可放在裸金属和 RTOS 目标上。需要携带状态的叠加逻辑时,把状态放在 + * 函数内的静态上下文或全局状态里。 + * A hook stored for the object lifetime is a plain function pointer, matching + * RBTree's comparator: no heap allocation, copyable, and usable on bare-metal and + * RTOS targets. Stateful addition logic keeps its state in static or global storage + * inside the function. + */ + using StateAddFunction = StateVector (*)(const StateVector&, const StateVector&); + + StateVector x; ///< 状态向量 State vector + StateCovariance P; ///< 状态协方差 State covariance + + /// NIS 失败环形缓冲容量,等于模板参数 WindowCapacity。 + /// Capacity of the NIS failure ring buffer, equal to the WindowCapacity argument. + static constexpr size_t kWindowCapacity = 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; ///< 最近一次更新的残差 Residual of the last update + + /** + * @brief 默认状态增量叠加:普通加法。 + * Default state increment application: 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。 + * Construct an empty filter with a zero-sized state. + */ + EKF() = default; + + /** + * @brief 使用初始状态、初始协方差和状态增量叠加方式构造滤波器。 + * Construct a filter from an initial state, covariance, and state increment + * application. + * + * @param x0 初始状态向量。 Initial state vector. + * @param P0 初始状态协方差,必须为方阵且维度与 x0 一致。 + * Initial state covariance, square and matching x0. + * @param x_add 状态增量叠加方式,默认普通加法。必须是捕获列表为空的可调用对象。 + * State increment application, plain addition by default; the callable + * 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 使用线性状态转移矩阵预测一步。 + * Predict one step with a linear transition matrix. + * + * @param F 状态转移矩阵。 State transition matrix. + * @param Q 过程噪声协方差。 Process noise covariance. + * @return 预测后的状态向量。 Predicted state vector. + */ + StateVector Predict(const TransitionMatrix& F, const StateCovariance& Q) + { + return Predict(F, Q, [&F](const StateVector& prior) { return F * prior; }); + } + + /** + * @brief 使用自定义非线性状态转移预测一步。 + * Predict one step with a custom nonlinear transition. + * + * @param F 状态转移矩阵,同时用于协方差传播。 + * State transition matrix, also used to propagate the covariance. + * @param Q 过程噪声协方差。 Process noise covariance. + * @param f 非线性状态转移函数,按模板参数传入,可内联且不分配内存。 + * Nonlinear transition function, taken as a template parameter so it inlines + * and allocates nothing. + * @return 预测后的状态向量。 Predicted state vector. + */ + 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 使用线性观测矩阵更新状态。 + * Update the state with a linear observation matrix. + * + * @param z 观测向量。 Measurement vector. + * @param H 观测矩阵。 Observation matrix. + * @param R 观测噪声协方差。 Measurement noise covariance. + * @return 更新后的状态向量。 Updated state vector. + */ + 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 使用自定义非线性观测函数更新状态。 + * Update the state with a custom nonlinear observation function. + * + * @param z 观测向量。 Measurement vector. + * @param H 观测函数在当前状态处的雅可比。 Jacobian of the observation at the current + * state. + * @param R 观测噪声协方差。 Measurement noise covariance. + * @param h 非线性观测函数,按模板参数传入,可内联且不分配内存。 + * Nonlinear observation function, taken as a template parameter so it + * inlines and allocates nothing. + * @return 更新后的状态向量。 Updated state vector. + */ + template + StateVector Update(const MeasurementVector& z, const ObservationMatrix& H, + const MeasurementCovariance& R, ObservationFunction&& h) + { + return Update(z, H, R, h, MinusMeasurement); + } + + /** + * @brief 使用自定义非线性观测函数和自定义残差方式更新状态。 + * Update the state with a custom nonlinear observation and residual handling. + * + * 残差与 NIS/NEES 均在状态与协方差更新之后计算,因此 `last_residual` 是更新后的 + * 后验残差,不是更新前的预测残差。 + * The residual and the NIS/NEES statistics are computed after the state and + * covariance are updated, so `last_residual` is the post-update residual rather + * than the pre-update innovation. + * + * @param z 观测向量。 Measurement vector. + * @param H 观测函数在当前状态处的雅可比。 Jacobian of the observation at the current + * state. + * @param R 观测噪声协方差。 Measurement noise covariance. + * @param h 非线性观测函数,按模板参数传入,可内联且不分配内存。 + * Nonlinear observation function, taken as a template parameter so it + * inlines and allocates nothing. + * @param z_subtract 残差计算方式,默认普通减法;同样是模板参数。 + * Residual computation, plain subtraction by default; also a + * template parameter. + * @return 更新后的状态向量。 Updated state vector. + */ + 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 (kWindowCapacity > 0) + { + nis_window_[nis_window_head_] = nis > nis_threshold ? 1 : 0; + nis_window_head_ = (nis_window_head_ + 1) % kWindowCapacity; + if (nis_window_count_ < kWindowCapacity) + { + ++nis_window_count_; + } + } + + return x; + } + + /** + * @brief 设置活跃窗口长度,取值上限为 kWindowCapacity。 + * Set the active window length, capped at kWindowCapacity. + * + * @param size 期望的窗口长度,超过容量时按容量处理。 + * Requested window length; values above the capacity are clamped. + */ + void SetWindowSize(size_t size) + { + window_size_ = size < kWindowCapacity ? size : kWindowCapacity; + } + + /** + * @brief 返回当前活跃窗口长度。 + * Return the current active window length. + */ + size_t WindowSize() const { return window_size_; } + + /** + * @brief 返回活跃窗口内 NIS 超限的次数。 + * Return the number of 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_ + kWindowCapacity - 1 - i) % kWindowCapacity; + failures += nis_window_[index]; + } + return failures; + } + + /** + * @brief 返回 NIS 超限的更新次数。 + * Return the number of updates whose NIS exceeded the threshold. + */ + int NisFailCount() const { return nis_count_; } + + /** + * @brief 返回 NEES 超限的更新次数。 + * Return the number of updates whose NEES exceeded the threshold. + */ + int NeesFailCount() const { return nees_count_; } + + /** + * @brief 返回已完成的更新次数。 + * Return 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_ = kWindowCapacity; + 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..8ae030dc5 --- /dev/null +++ b/test/automatic/utils/ekf/test_ekf.cpp @@ -0,0 +1,276 @@ +/** + * @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. + * + * 测试原理 / 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 kEKF_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 > kEKF_TEST_PI) + { + angle -= 2.0 * kEKF_TEST_PI; + } + while (angle <= -kEKF_TEST_PI) + { + angle += 2.0 * kEKF_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 * kEKF_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); + } + + // 检查内容:一致的非线性更新不触发新息门限,离群更新触发门限。 + // 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.kWindowCapacity + 10U); + TEST_ASSERT(ekf.WindowSize() == ekf.kWindowCapacity); + } + + // 检查内容:多次更新后协方差保持对称,即 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); + } + } +} From edc5b393e314f6aaf00ee08120cf455c2fb28b26 Mon Sep 17 00:00:00 2001 From: keruth Date: Wed, 11 Nov 2026 09:56:54 +0800 Subject: [PATCH 2/5] style(utils): name the EKF window capacity in SCREAMING_SNAKE_CASE The class-level constant follows the repository constant style used by PID_SIGMA instead of the k-prefixed form, and the test angle constant drops its prefix for the same reason. No behavior change. --- src/utils/ekf.hpp | 20 ++++++++++---------- test/automatic/utils/ekf/test_ekf.cpp | 16 ++++++++-------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/utils/ekf.hpp b/src/utils/ekf.hpp index 683e86bea..6f223fde1 100644 --- a/src/utils/ekf.hpp +++ b/src/utils/ekf.hpp @@ -95,7 +95,7 @@ class EKF /// NIS 失败环形缓冲容量,等于模板参数 WindowCapacity。 /// Capacity of the NIS failure ring buffer, equal to the WindowCapacity argument. - static constexpr size_t kWindowCapacity = WindowCapacity; + 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 @@ -281,11 +281,11 @@ class EKF last_nees = nees; last_residual = residual; - if constexpr (kWindowCapacity > 0) + if constexpr (WINDOW_CAPACITY > 0) { nis_window_[nis_window_head_] = nis > nis_threshold ? 1 : 0; - nis_window_head_ = (nis_window_head_ + 1) % kWindowCapacity; - if (nis_window_count_ < kWindowCapacity) + nis_window_head_ = (nis_window_head_ + 1) % WINDOW_CAPACITY; + if (nis_window_count_ < WINDOW_CAPACITY) { ++nis_window_count_; } @@ -295,15 +295,15 @@ class EKF } /** - * @brief 设置活跃窗口长度,取值上限为 kWindowCapacity。 - * Set the active window length, capped at kWindowCapacity. + * @brief 设置活跃窗口长度,取值上限为 WINDOW_CAPACITY。 + * Set the active window length, capped at WINDOW_CAPACITY. * * @param size 期望的窗口长度,超过容量时按容量处理。 * Requested window length; values above the capacity are clamped. */ void SetWindowSize(size_t size) { - window_size_ = size < kWindowCapacity ? size : kWindowCapacity; + window_size_ = size < WINDOW_CAPACITY ? size : WINDOW_CAPACITY; } /** @@ -323,7 +323,7 @@ class EKF int failures = 0; for (size_t i = 0; i < length; ++i) { - const size_t index = (nis_window_head_ + kWindowCapacity - 1 - i) % kWindowCapacity; + const size_t index = (nis_window_head_ + WINDOW_CAPACITY - 1 - i) % WINDOW_CAPACITY; failures += nis_window_[index]; } return failures; @@ -350,10 +350,10 @@ class EKF private: StateCovariance I_; StateAddFunction x_add_; - std::array nis_window_{}; + std::array nis_window_{}; size_t nis_window_head_ = 0; size_t nis_window_count_ = 0; - size_t window_size_ = kWindowCapacity; + size_t window_size_ = WINDOW_CAPACITY; int nis_count_ = 0; int nees_count_ = 0; int total_count_ = 0; diff --git a/test/automatic/utils/ekf/test_ekf.cpp b/test/automatic/utils/ekf/test_ekf.cpp index 8ae030dc5..faa198fd9 100644 --- a/test/automatic/utils/ekf/test_ekf.cpp +++ b/test/automatic/utils/ekf/test_ekf.cpp @@ -29,20 +29,20 @@ namespace { -constexpr double kEKF_TEST_PI = 3.14159265358979323846; +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 > kEKF_TEST_PI) + while (angle > EKF_TEST_PI) { - angle -= 2.0 * kEKF_TEST_PI; + angle -= 2.0 * EKF_TEST_PI; } - while (angle <= -kEKF_TEST_PI) + while (angle <= -EKF_TEST_PI) { - angle += 2.0 * kEKF_TEST_PI; + angle += 2.0 * EKF_TEST_PI; } return angle; } @@ -143,7 +143,7 @@ void test_ekf() auto residual = [](const EkfMeasurement& a, const EkfMeasurement& b) { EkfMeasurement c = a - b; - c(0) = std::remainder(c(0), 2.0 * kEKF_TEST_PI); + c(0) = std::remainder(c(0), 2.0 * EKF_TEST_PI); return c; }; ekf.Update(z, h, r, [](const EkfState& state) { return state; }, residual); @@ -244,8 +244,8 @@ void test_ekf() TEST_ASSERT(ekf.NisFailCount() == 3); TEST_ASSERT(ekf.UpdateCount() == 3); - ekf.SetWindowSize(ekf.kWindowCapacity + 10U); - TEST_ASSERT(ekf.WindowSize() == ekf.kWindowCapacity); + ekf.SetWindowSize(ekf.WINDOW_CAPACITY + 10U); + TEST_ASSERT(ekf.WindowSize() == ekf.WINDOW_CAPACITY); } // 检查内容:多次更新后协方差保持对称,即 Joseph 形式生效。 From fe3967c68125e462e6eb32e9f9ac463071e3d349 Mon Sep 17 00:00:00 2001 From: keruth Date: Wed, 11 Nov 2026 09:59:33 +0800 Subject: [PATCH 3/5] feat(utils): allow replacing the EKF state addition hook The stored hook was fixed at construction, which made a runtime change of the increment application require rebuilding the filter. Add SetStateAdd, matching the SetILimit/SetK setter style of PID: it swaps the function pointer, keeps the estimated state and covariance, and takes effect on the next update. A null hook is rejected with ASSERT, the same guard RBTree puts on its comparator. test_ekf gains a case that updates with the default addition, replaces the hook with a wrapping one, and checks that the next update wraps the yaw state back into [-pi, pi). --- src/utils/ekf.hpp | 18 ++++++++++++++++ test/automatic/utils/ekf/test_ekf.cpp | 31 +++++++++++++++++++++++++++ 2 files changed, 49 insertions(+) diff --git a/src/utils/ekf.hpp b/src/utils/ekf.hpp index 6f223fde1..4eed272d1 100644 --- a/src/utils/ekf.hpp +++ b/src/utils/ekf.hpp @@ -149,6 +149,24 @@ class EKF ASSERT(P0.rows() == x0.size()); } + /** + * @brief 运行期替换状态增量叠加函数,只接受捕获列表为空的可调用对象。 + * Replace the state increment application at runtime; the callable captures + * nothing. + * + * 下一次更新开始使用新的叠加逻辑,已估计的状态与协方差保持不变。 + * The next update uses the new application; the estimated state and covariance are + * kept as they are. + * + * @param x_add 新的状态增量叠加函数,不能为空。 + * New state increment application, must not be null. + */ + void SetStateAdd(StateAddFunction x_add) + { + ASSERT(x_add != nullptr); + x_add_ = x_add; + } + /** * @brief 使用线性状态转移矩阵预测一步。 * Predict one step with a linear transition matrix. diff --git a/test/automatic/utils/ekf/test_ekf.cpp b/test/automatic/utils/ekf/test_ekf.cpp index faa198fd9..062bbdebe 100644 --- a/test/automatic/utils/ekf/test_ekf.cpp +++ b/test/automatic/utils/ekf/test_ekf.cpp @@ -12,6 +12,7 @@ * 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 @@ -180,6 +181,36 @@ void test_ekf() 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. From 81bcbdd904701abb1d662950eec839dd98cb06d2 Mon Sep 17 00:00:00 2001 From: keruth Date: Wed, 11 Nov 2026 12:12:44 +0800 Subject: [PATCH 4/5] docs(utils): document the EKF public type aliases The six public typedefs of EKF were the only undocumented public members, which the Doxygen warning gate counts against the repository baseline. Give each alias a one-line bilingual description. No code change. --- src/utils/ekf.hpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/utils/ekf.hpp b/src/utils/ekf.hpp index 4eed272d1..b400247d2 100644 --- a/src/utils/ekf.hpp +++ b/src/utils/ekf.hpp @@ -69,11 +69,24 @@ template class EKF { public: + /// 状态向量类型。 State vector type. using StateVector = Eigen::Matrix; + + /// 状态协方差矩阵类型。 State covariance matrix type. using StateCovariance = Eigen::Matrix; + + /// 状态转移矩阵类型,与状态协方差同形。 State transition matrix type, same shape as + /// the state covariance. using TransitionMatrix = StateCovariance; + + /// 观测向量类型。 Measurement vector type. using MeasurementVector = Eigen::Matrix; + + /// 观测噪声协方差矩阵类型。 Measurement noise covariance matrix type. using MeasurementCovariance = Eigen::Matrix; + + /// 观测矩阵类型,行数为观测维度、列数为状态维度。 Observation matrix type, with one + /// row per measurement and one column per state component. using ObservationMatrix = Eigen::Matrix; /** From 9fa2950ae51d8bfc2ab69e619a198037a8552450 Mon Sep 17 00:00:00 2001 From: molqzone <195118620+molqzone@users.noreply.github.com> Date: Sat, 21 Nov 2026 20:11:15 +0800 Subject: [PATCH 5/5] docs(utils): trim the EKF header comments The class comment ran to five paragraphs, most of them arguing about hook forms, exception/RTTI freedom and bare-metal targets, none of which the other utils headers mention, and every method repeated its brief and @param text on continuation lines. Keep the two-line brief plus single-line @param form used by Inertia and PID, fold the design notes into one sentence each, and move the posterior-residual caveat onto last_residual. Comments only: stripping comments with the preprocessor yields an identical translation unit, and the header still builds warning-free. No code change. --- src/utils/ekf.hpp | 233 +++++++++++++++------------------------------- 1 file changed, 73 insertions(+), 160 deletions(-) diff --git a/src/utils/ekf.hpp b/src/utils/ekf.hpp index b400247d2..d4d834187 100644 --- a/src/utils/ekf.hpp +++ b/src/utils/ekf.hpp @@ -17,47 +17,13 @@ using DefaultScalar = LIBXR_DEFAULT_SCALAR; * @brief 支持非线性状态转移与非线性观测的扩展卡尔曼滤波器。 * Extended Kalman filter with optional nonlinear transition and observation. * - * 状态向量 `x` 与协方差矩阵 `P` 是公有成员,调用方可以在两次预测之间直接改写状态 - * (固定某个自由度、限制角速度、按观测修正某个分量),但改写后的协方差必须仍然描述 - * 改写后的状态,否则后续更新会按旧不确定性加权。 - * The state vector `x` and covariance `P` are public so callers may adjust the state - * between predictions (pin a degree of freedom, clamp a rate, correct one component - * from an observation); after such an edit the covariance must still describe the - * edited state, otherwise later updates weigh the state with stale uncertainty. + * `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, which stays symmetric positive - * semi-definite and suits near-singular process or measurement noise. - * - * 角度类状态通过可自定义钩子处理:`x_add` 决定状态增量如何叠加(例如把 yaw 叠加后 - * 归一化到 [-π, π)),`z_subtract` 决定残差如何相减(例如把角度残差归一化),`f` 与 - * `h` 分别承载非线性状态转移与非线性观测。 - * Angular states use customizable hooks: `x_add` defines how a state increment is - * applied (for example wrapping yaw into [-π, π) after the addition), `z_subtract` - * defines how a residual is formed (for example wrapping an angle residual), and `f` - * and `h` carry the nonlinear transition and observation. - * - * 钩子分两种形式:每次调用传入的 `f`、`h`、`z_subtract` 是模板参数,编译期内联;构造时 - * 传入并长期保存的 `x_add` 是裸函数指针(与 RBTree 的比较器一致)。两种形式都在编译期 - * 绑定,可调用对象零存储开销;滤波器只依赖 Eigen 动态矩阵的常规存储行为,并且不依赖 - * 异常与 RTTI,可在裸金属、RTOS 与 Linux 上使用。 - * The hooks take two forms: the per-call `f`, `h` and `z_subtract` are template - * parameters that inline at compile time, and `x_add`, supplied at construction and - * kept for the object lifetime, is a plain function pointer, matching RBTree's - * comparator. Both forms bind at compile time and add zero storage for the callable; - * the filter relies only on Eigen's usual dynamic-matrix storage and needs neither - * exceptions nor RTTI, so it runs on bare-metal, RTOS and Linux targets alike. - * - * 新息统计:每次更新计算归一化新息平方(NIS)与归一化估计误差平方(NEES),与 - * `nis_threshold` / `nees_threshold` 比较后计数。近期 NIS 失败记录放在编译期固定容量的 - * 环形缓冲里,容量由 `WindowCapacity` 决定,活跃窗口长度用 `SetWindowSize` 调整; - * 统计缓冲区不分配堆内存。 - * Innovation statistics: every update computes the normalized innovation squared (NIS) - * and the normalized estimation error squared (NEES) and compares them with - * `nis_threshold` / `nees_threshold`. Recent NIS failures live in a ring buffer whose - * capacity is fixed at compile time by `WindowCapacity`, with the active window length - * set through `SetWindowSize`; the statistics buffer allocates no heap memory. + * 协方差更新使用 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. @@ -69,57 +35,44 @@ template class EKF { public: - /// 状态向量类型。 State vector type. + /// 状态向量。State vector. using StateVector = Eigen::Matrix; - /// 状态协方差矩阵类型。 State covariance matrix type. + /// 状态协方差。State covariance. using StateCovariance = Eigen::Matrix; - /// 状态转移矩阵类型,与状态协方差同形。 State transition matrix type, same shape as - /// the state covariance. + /// 状态转移矩阵,与状态协方差同形。State transition matrix, same shape as covariance. using TransitionMatrix = StateCovariance; - /// 观测向量类型。 Measurement vector type. + /// 观测向量。Measurement vector. using MeasurementVector = Eigen::Matrix; - /// 观测噪声协方差矩阵类型。 Measurement noise covariance matrix type. + /// 观测噪声协方差。Measurement noise covariance. using MeasurementCovariance = Eigen::Matrix; - /// 观测矩阵类型,行数为观测维度、列数为状态维度。 Observation matrix type, with one - /// row per measurement and one column per state component. + /// 观测矩阵,行数为观测维度。Observation matrix, one row per measurement. using ObservationMatrix = Eigen::Matrix; - /** - * @brief 状态增量叠加函数指针,只接受捕获列表为空的可调用对象。 - * State increment application function pointer; the callable captures nothing. - * - * 构造后长期持有的钩子使用裸函数指针(与 RBTree 的比较器一致):钩子自身不分配堆 - * 内存、可拷贝、可放在裸金属和 RTOS 目标上。需要携带状态的叠加逻辑时,把状态放在 - * 函数内的静态上下文或全局状态里。 - * A hook stored for the object lifetime is a plain function pointer, matching - * RBTree's comparator: no heap allocation, copyable, and usable on bare-metal and - * RTOS targets. Stateful addition logic keeps its state in static or global storage - * inside the function. - */ + /// 状态增量叠加函数指针,可调用对象不得捕获任何变量。 + /// 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 失败环形缓冲容量,等于模板参数 WindowCapacity。 - /// Capacity of the NIS failure ring buffer, equal to the WindowCapacity argument. + /// 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; ///< 最近一次更新的残差 Residual of the last update + 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 application: plain addition. + * Default state increment: plain addition. */ static StateVector PlusState(const StateVector& a, const StateVector& b) { @@ -138,21 +91,18 @@ class EKF /** * @brief 构造空滤波器,状态维度为 0。 - * Construct an empty filter with a zero-sized state. + * Constructs an empty filter with a zero-sized state. */ EKF() = default; /** - * @brief 使用初始状态、初始协方差和状态增量叠加方式构造滤波器。 - * Construct a filter from an initial state, covariance, and state increment - * application. - * - * @param x0 初始状态向量。 Initial state vector. - * @param P0 初始状态协方差,必须为方阵且维度与 x0 一致。 - * Initial state covariance, square and matching x0. - * @param x_add 状态增量叠加方式,默认普通加法。必须是捕获列表为空的可调用对象。 - * State increment application, plain addition by default; the callable - * must capture nothing. + * @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) @@ -163,16 +113,9 @@ class EKF } /** - * @brief 运行期替换状态增量叠加函数,只接受捕获列表为空的可调用对象。 - * Replace the state increment application at runtime; the callable captures - * nothing. - * - * 下一次更新开始使用新的叠加逻辑,已估计的状态与协方差保持不变。 - * The next update uses the new application; the estimated state and covariance are - * kept as they are. - * - * @param x_add 新的状态增量叠加函数,不能为空。 - * New state increment application, must not be null. + * @brief 运行期替换状态增量叠加函数。 + * Replaces the state increment at runtime. + * @param x_add 新的叠加函数,不能为空。New addition function, must not be null. */ void SetStateAdd(StateAddFunction x_add) { @@ -182,11 +125,10 @@ class EKF /** * @brief 使用线性状态转移矩阵预测一步。 - * Predict one step with a linear transition matrix. - * - * @param F 状态转移矩阵。 State transition matrix. - * @param Q 过程噪声协方差。 Process noise covariance. - * @return 预测后的状态向量。 Predicted state vector. + * 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) { @@ -195,15 +137,13 @@ class EKF /** * @brief 使用自定义非线性状态转移预测一步。 - * Predict one step with a custom nonlinear transition. - * - * @param F 状态转移矩阵,同时用于协方差传播。 - * State transition matrix, also used to propagate the covariance. - * @param Q 过程噪声协方差。 Process noise covariance. - * @param f 非线性状态转移函数,按模板参数传入,可内联且不分配内存。 - * Nonlinear transition function, taken as a template parameter so it inlines - * and allocates nothing. - * @return 预测后的状态向量。 Predicted state vector. + * 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, @@ -221,12 +161,11 @@ class EKF /** * @brief 使用线性观测矩阵更新状态。 - * Update the state with a linear observation matrix. - * - * @param z 观测向量。 Measurement vector. - * @param H 观测矩阵。 Observation matrix. - * @param R 观测噪声协方差。 Measurement noise covariance. - * @return 更新后的状态向量。 Updated state vector. + * 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) @@ -237,16 +176,13 @@ class EKF /** * @brief 使用自定义非线性观测函数更新状态。 - * Update the state with a custom nonlinear observation function. - * - * @param z 观测向量。 Measurement vector. - * @param H 观测函数在当前状态处的雅可比。 Jacobian of the observation at the current - * state. - * @param R 观测噪声协方差。 Measurement noise covariance. - * @param h 非线性观测函数,按模板参数传入,可内联且不分配内存。 - * Nonlinear observation function, taken as a template parameter so it - * inlines and allocates nothing. - * @return 更新后的状态向量。 Updated state vector. + * 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, @@ -256,26 +192,16 @@ class EKF } /** - * @brief 使用自定义非线性观测函数和自定义残差方式更新状态。 - * Update the state with a custom nonlinear observation and residual handling. - * - * 残差与 NIS/NEES 均在状态与协方差更新之后计算,因此 `last_residual` 是更新后的 - * 后验残差,不是更新前的预测残差。 - * The residual and the NIS/NEES statistics are computed after the state and - * covariance are updated, so `last_residual` is the post-update residual rather - * than the pre-update innovation. - * - * @param z 观测向量。 Measurement vector. - * @param H 观测函数在当前状态处的雅可比。 Jacobian of the observation at the current - * state. - * @param R 观测噪声协方差。 Measurement noise covariance. - * @param h 非线性观测函数,按模板参数传入,可内联且不分配内存。 - * Nonlinear observation function, taken as a template parameter so it - * inlines and allocates nothing. - * @param z_subtract 残差计算方式,默认普通减法;同样是模板参数。 - * Residual computation, plain subtraction by default; also a - * template parameter. - * @return 更新后的状态向量。 Updated state vector. + * @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, @@ -326,26 +252,22 @@ class EKF } /** - * @brief 设置活跃窗口长度,取值上限为 WINDOW_CAPACITY。 - * Set the active window length, capped at WINDOW_CAPACITY. - * - * @param size 期望的窗口长度,超过容量时按容量处理。 - * Requested window length; values above the capacity are clamped. + * @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; } - /** - * @brief 返回当前活跃窗口长度。 - * Return the current active window length. - */ + /// 返回当前活跃窗口长度。Returns the active window length. size_t WindowSize() const { return window_size_; } /** - * @brief 返回活跃窗口内 NIS 超限的次数。 - * Return the number of NIS failures inside the active window. + * @brief 返回活跃窗口内 NIS 超限次数。 + * Returns NIS failures inside the active window. */ int NisFailureCount() const { @@ -360,22 +282,13 @@ class EKF return failures; } - /** - * @brief 返回 NIS 超限的更新次数。 - * Return the number of updates whose NIS exceeded the threshold. - */ + /// 返回 NIS 超限的总次数。Returns the total NIS failure count. int NisFailCount() const { return nis_count_; } - /** - * @brief 返回 NEES 超限的更新次数。 - * Return the number of updates whose NEES exceeded the threshold. - */ + /// 返回 NEES 超限的总次数。Returns the total NEES failure count. int NeesFailCount() const { return nees_count_; } - /** - * @brief 返回已完成的更新次数。 - * Return the number of completed updates. - */ + /// 返回已完成的更新次数。Returns the number of completed updates. int UpdateCount() const { return total_count_; } private: