Repository navigation
Conversation
Adds LibXR::EKF<Scalar, WindowCapacity>, 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.
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.
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).
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.
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.
[Medium impact] The changes reach callers who choose the new state-estimation utility. Fix the unsafe default constructor, incorrect statistics, and repeated heap allocations before merging.
|
| * @brief 构造空滤波器,状态维度为 0。 | ||
| * Constructs an empty filter with a zero-sized state. | ||
| */ | ||
| EKF() = default; |
There was a problem hiding this comment.
Default construction leaves x_add_ unset and I_ empty. If a caller fills the public x and P, the first Update() subtracts a nonempty matrix from I_, then calls an uninitialized function pointer. This can abort or crash.
Initialize the default hook and provide a way to size I_ when the state is set, or remove the default constructor.
Prompt To Fix With AI
This is a comment left during a code review.
Path: src/utils/ekf.hpp
Line: 96
Comment:
**Empty filters cannot update**
Default construction leaves `x_add_` unset and `I_` empty. If a caller fills the public `x` and `P`, the first `Update()` subtracts a nonempty matrix from `I_`, then calls an uninitialized function pointer. This can abort or crash.
Initialize the default hook and provide a way to size `I_` when the state is set, or remove the default constructor.
---
For each issue above, determine whether it is valid and should be fixed. If so, fix it directly.| 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); |
There was a problem hiding this comment.
NIS misses unexpected measurements
last_nis uses the residual and covariance after the measurement has changed the state. With scalar x=0, P=H=R=1, and z=2, the innovation NIS is 2, but this code reports 2/3. That falls below the default 0.711 threshold, so the failure counts miss it.
Compute NIS from the prior residual and the same covariance used to calculate K. Keep the posterior residual separately if needed.
Prompt To Fix With AI
This is a comment left during a code review.
Path: src/utils/ekf.hpp
Line: 221-223
Comment:
**NIS misses unexpected measurements**
`last_nis` uses the residual and covariance after the measurement has changed the state. With scalar `x=0`, `P=H=R=1`, and `z=2`, the innovation NIS is 2, but this code reports 2/3. That falls below the default 0.711 threshold, so the failure counts miss it.
Compute NIS from the prior residual and the same covariance used to calculate `K`. Keep the posterior residual separately if needed.
---
For each issue above, determine whether it is valid and should be fixed. If so, fix it directly.| const StateVector dx = x - x_prior; | ||
| const Scalar nees = (dx.transpose() * P.inverse() * dx)(0, 0); |
There was a problem hiding this comment.
last_nees measures the change from the prior estimate, not the error against the true state. With scalar x=0, P=H=R=1, and z=2, the new estimate is 1 and this code reports NEES=2. If the true state is 1, the actual error is zero, yet the failure count increases.
Accept a reference state and a suitable state-difference hook to compute NEES, or rename this statistic and its threshold so callers do not mistake correction size for estimation error.
Prompt To Fix With AI
This is a comment left during a code review.
Path: src/utils/ekf.hpp
Line: 224-225
Comment:
**NEES measures the wrong error**
`last_nees` measures the change from the prior estimate, not the error against the true state. With scalar `x=0`, `P=H=R=1`, and `z=2`, the new estimate is 1 and this code reports NEES=2. If the true state is 1, the actual error is zero, yet the failure count increases.
Accept a reference state and a suitable state-difference hook to compute NEES, or rename this statistic and its threshold so callers do not mistake correction size for estimation error.
---
For each issue above, determine whether it is valid and should be fixed. If so, fix it directly.| const StateVector x_prior = x; | ||
| const StateCovariance K = P * H.transpose() * (H * P * H.transpose() + R).inverse(); |
There was a problem hiding this comment.
Every nonempty Update() allocates memory because StateVector uses dynamic Eigen storage. Copying x into the local x_prior creates a fresh buffer each time. The other local matrices and returning x by value add more allocations. This breaks the stated no-allocation goal and prevents use in loops that forbid heap allocation.
Use compile-time or bounded matrix sizes and reusable storage. Add a test that forbids Eigen allocations during prediction and update.
Prompt To Fix With AI
This is a comment left during a code review.
Path: src/utils/ekf.hpp
Line: 216-217
Comment:
**Updates still allocate memory**
Every nonempty `Update()` allocates memory because `StateVector` uses dynamic Eigen storage. Copying `x` into the local `x_prior` creates a fresh buffer each time. The other local matrices and returning `x` by value add more allocations. This breaks the stated no-allocation goal and prevents use in loops that forbid heap allocation.
Use compile-time or bounded matrix sizes and reusable storage. Add a test that forbids Eigen allocations during prediction and update.
---
For each issue above, determine whether it is valid and should be fixed. If so, fix it directly.Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!
Adds a nonlinear extended Kalman filter to
src/utilsalongsidePIDand thetransform utilities. Header-only, no allocation, no exceptions or RTTI.
Predict(F, Q)andUpdate(z, H, R)take the linear matrices. The overloadstaking
f,handz_subtractaccept a nonlinear model instead; they aretemplate parameters, so they inline and cost no storage.
SetStateAdd()swapsthe stored state-increment hook at runtime, for angular states.
The covariance update uses the Joseph form, and each update records NIS/NEES
against a threshold. Recent NIS failures go into a ring buffer sized at compile
time by
WindowCapacity, so the statistics never allocate.test/automatic/utils/ekfcovers a constant-velocity model,F P Fᵀ + Q,wrapped angle residuals and wrapped state addition, runtime hook replacement,
the innovation gate, the failure window, and Joseph-form symmetry.
New files only:
src/utils/ekf.hppand its test. The header is guarded byLIBXR_NO_EIGENlike the other Eigen utilities.