Skip to content

feat(utils): add extended Kalman filter - #372

Open
molqzone wants to merge 5 commits into
xrobot-org:devfrom
molqzone:feat/ekf
Open

molqzone wants to merge 5 commits into
xrobot-org:devfrom
molqzone:feat/ekf

Conversation

@molqzone

Copy link
Copy Markdown
Contributor

Adds a nonlinear extended Kalman filter to src/utils alongside PID and the
transform utilities. Header-only, no allocation, no exceptions or RTTI.

Predict(F, Q) and Update(z, H, R) take the linear matrices. The overloads
taking f, h and z_subtract accept a nonlinear model instead; they are
template parameters, so they inline and cost no storage. SetStateAdd() swaps
the 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/ekf covers 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.hpp and its test. The header is guarded by
LIBXR_NO_EIGEN like the other Eigen utilities.

molqzone and others added 5 commits November 21, 2026 21:38
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.
@greptile-apps

greptile-apps Bot commented Nov 21, 2026 •

Copy link
Copy Markdown

RetriggerView in GreptileConfidence Score: 1/5

[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.

Fix All in Claude CodeFindings

  1. P1 Empty filters cannot update ▶
  2. P1 NIS misses unexpected measurements ▶
  3. P1 NEES measures the wrong error ▶
  4. P1 Updates still allocate memory ▶
Fix with agent prompt
### Issue 1
src/utils/ekf.hpp:96
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.

### Issue 2
src/utils/ekf.hpp:221-223
`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.

### Issue 3
src/utils/ekf.hpp:224-225
`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.

### Issue 4
src/utils/ekf.hpp:216-217
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.

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!

---

For each issue above, determine whether it is valid and should be fixed. If so, fix it directly.

Summary

Adds a header-only EKF with linear and nonlinear models, custom state addition, Joseph-form covariance updates, and fixed-capacity failure history.

  • The new EKF predicts states and updates them from measurements.
  • The filter can wrap angle errors and updated states.
  • The filter tracks recent and total innovation failures.

Reviews (1) · Last reviewed commit: "docs(utils): trim the EKF header comment..." · Reviewed by Greptile

Comment thread src/utils/ekf.hpp
* @brief 构造空滤波器,状态维度为 0。
* Constructs an empty filter with a zero-sized state.
*/
EKF() = default;

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P1 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.

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.

Fix in Claude Code Fix in Codex

Comment thread src/utils/ekf.hpp
Comment on lines +221 to +223
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);

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P1 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.

Fix in Claude Code Fix in Codex

Comment thread src/utils/ekf.hpp
Comment on lines +224 to +225
const StateVector dx = x - x_prior;
const Scalar nees = (dx.transpose() * P.inverse() * dx)(0, 0);

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P1 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.

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.

Fix in Claude Code Fix in Codex

Comment thread src/utils/ekf.hpp
Comment on lines +216 to +217
const StateVector x_prior = x;
const StateCovariance K = P * H.transpose() * (H * P * H.transpose() + R).inverse();

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P1 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.

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!

Fix in Claude Code Fix in Codex

This branch has not been deployed

No deployments
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant