Queue 1: fix ESKF vertical-channel divergence - #285
Open
jbrodovsky wants to merge 4 commits into
Open
Conversation
Placeholder so the stacked PR has a diff. Replaced by the implementation. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Merging queue position 0 with --delete-branch auto-closed the PR stacked on it (#270), and GitHub then refused to reopen it because the head branch had been force-pushed by the stack rebase. The PR had to be recreated as #285. Record the order that avoids it: merge without deleting, retarget the child PR to main, rebase, force-push, and only then delete the merged branch. Also note that rebase-merge rewrites SHAs even on a fast-forward, which is why the rebase needs --onto against a recorded tip. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
…nsistent
Three defects, all confirmed by reading the code and each measured
independently on core/tests/test_data.csv with full GNSS aiding.
1. Position error was radians in F and H but metres on injection.
error_state_transition_jacobian produces a radian position-error rate
(f[(0,3)] = dt / r_n), gps_position_jacobian is the identity against a
radian-valued measurement, and linearize::apply_eskf_correction adds
straight into state.latitude. But inject_error_state divided by the
principal radii, treating delta_p as metres, so the horizontal correction
actually applied was ~1/6.4e6 of what the gain computed.
The horizontal channel was therefore open loop. The lat/lon innovation
never nulled and the filter drove that persistent residual into velocity,
tilt and accelerometer bias instead.
Horizontal rms: 1734.41 m -> 24.31 m.
2. F mixed two attitude-error conventions.
The velocity/attitude block used -[C_b^n f^b]_x dt, the GLOBAL form, while
attitude propagation (-[omega^b]_x), gyro-bias coupling (bare identity)
and injection (q_nom (x) dq, right-multiplied) are all LOCAL. The two
differ by a rotation, so with non-zero heading the tilt-to-velocity
feedback landed on the wrong axes. Now -C_b^n [f^b]_x dt throughout.
Altitude peak: 627.51 m -> 379.57 m, and this is what let
test_eskf_closed_loop_on_real_data pass again.
3. Covariance jitter was scale-blind.
A flat 1e-9 was added to all 15 diagonal entries after both predict and
update. In rad^2 that is ~(200 m)^2 of bogus horizontal position variance
per step against R_horiz ~ 5.5e-13; for velocity in m^2/s^2 it is a
rounding error. It also silently overwrote any deliberately small
covariance -- freezing a state at 1e-12 variance had no effect at all,
which is how it was found. Replaced with a relative jitter scaled by each
state's own variance, floored by its process noise.
Tests. Three that would have caught this, each verified to fail when its
fix is reverted:
- linearize: finite-difference check of F's velocity/attitude block against
numerical differentiation of the nominal propagation, with a non-identity
C_b^n (the conventions agree at identity attitude, which is why the
existing full-state Jacobian tests missed it). Reverted, the analytic
block reads -0 where the true coupling is 0.084.
- kalman: injection of a known error state must move latitude by the exact
radian amount, at a latitude where a stray 1/(r_e cos lat) cannot cancel.
- kalman: a 1e-12 relative input perturbation must not move altitude by a
millimetre. #266 reported a 1e-14 perturbation growing to 1.5e8 m.
Thresholds. test_eskf_closed_loop_on_real_data is re-enabled, and the golden
numbers in test_eskf_stability_high_dynamics (1905.0/2494.0) and
test_filter_comparison (1905.0) are replaced with physical bounds. Those
numbers were pinned so tightly to the broken behaviour that macOS and Windows
failed them at 2121.97 m on floating-point code generation alone; the ESKF now
runs at 24 m rms and is held to the same standard as the EKF.
Not fixed here: the vertical channel. Altitude rms is 129.54 m against the
UKF's 2.82 m on the same data, vertical velocity error is 44 m/s against
0.44 m/s, and the bias states still reach 9 m/s^2 and 6 rad/s. The two
remaining ESKF tests stay #[ignore]d and the altitude thresholds in the
re-enabled test are labelled provisional. Diagnosis and leads are in #286.
Refs #266, #286
Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
jbrodovsky
marked this pull request as ready for review
September 4, 2026 20:45
Owner
Author
Status: three of four defects fixed; the vertical channel is notMarking this ready for review. It does not close #266 outright -- #266's acceptance criteria What changed, measured independently
The horizontal channel went from 77x worse than the UKF to matching it. Defect 1 is the whole of TestsThree new tests, each verified to fail when its own fix is reverted:
Integration suite: 13 passing (was 12), 3 ignored (was 4). 240 unit + 63 doc tests pass, clippy Golden numbers removed
What is leftAltitude rms 129.54 m vs the UKF's 2.82 m; vertical velocity error 44 m/s vs 0.44 m/s; bias states #286 carries the instrumentation traces, what has been ruled out (the frame flag, the H matrix, the |
Contributor
There was a problem hiding this comment.
🟡 Changes recommended
Several integration-test assertion messages are incorrect/misleading (UKF/EKF labeled as ESKF) and the PR’s stated acceptance criteria still conflict with remaining ignored ESKF tests due to unresolved vertical divergence.
Once you've addressed the issues Copilot identified, you can request another Copilot review.
Pull request overview
This PR targets the ESKF’s vertical-channel divergence instability (#266) by correcting unit conventions and attitude-error conventions in the error-state linearization and injection, and by improving covariance regularization. It also updates the v1 queue documentation and adds/adjusts tests to prevent regressions and reduce “golden number” thresholds.
Changes:
- Fix ESKF state/error-state consistency: inject lat/lon error in radians (not metres) and use a body-frame attitude-error convention in the
Fvelocity/attitude block. - Replace absolute covariance “jitter” with per-state relative regularization scaled by each state’s variance / process-noise floor.
- Add regression tests (finite-difference Jacobian convention check; tiny-perturbation sensitivity check) and revise integration-test bounds/messages.
File summaries
| File | Description |
|---|---|
| docs/V1_QUEUE.md | Documents safer spine-PR merge/rebase ordering to avoid GitHub auto-closing/reopen issues in stacked PRs. |
| docs/queue/01-eskf-fix.md | Adds queue entry capturing motivation and acceptance criteria for the ESKF fix work. |
| core/tests/integration_tests.rs | Re-enables/adjusts ESKF assertions and thresholds; updates ignore reasons; (also introduces several incorrect assert messages). |
| core/src/linearize.rs | Fixes F velocity/attitude block to match local/body-frame attitude-error convention; adds finite-difference regression tests. |
| core/src/kalman.rs | Fixes ESKF position injection units (radians for lat/lon) and introduces variance-scaled covariance regularization; adds ESKF regression tests. |
Review details
Suppressed comments (9)
core/tests/integration_tests.rs:668
- This is the UKF test (
test_ukf_closed_loop_on_real_data), but the assertion message says "ESKF".
"ESKF maximum horizontal error should be less than {:.2}m, got {:.2}m",
core/tests/integration_tests.rs:674
- This is the UKF test, but the altitude assertion message says "ESKF" and mentions a provisional vertical bound / #286, which doesn't apply here.
"ESKF RMS altitude error should be less than {:.2}m (provisional, see #286), got {:.2}m",
core/tests/integration_tests.rs:680
- This is the UKF test, but the altitude assertion message says "ESKF" and includes the provisional/#286 note.
"ESKF maximum altitude error should be less than {:.2}m (provisional, see #286), got {:.2}m",
core/tests/integration_tests.rs:997
- This is the EKF test, but the assertion message says "ESKF".
"ESKF maximum horizontal error should be less than {:.2}m, got {:.2}m",
core/tests/integration_tests.rs:1003
- This is the EKF test, but the altitude assertion message says "ESKF" and mentions provisional bounds/#286, which doesn't apply here.
"ESKF RMS altitude error should be less than {:.2}m (provisional, see #286), got {:.2}m",
core/tests/integration_tests.rs:1009
- This is the EKF test, but the altitude assertion message says "ESKF" and includes the provisional/#286 note.
"ESKF maximum altitude error should be less than {:.2}m (provisional, see #286), got {:.2}m",
core/tests/integration_tests.rs:1140
- This is the degraded-GNSS EKF test, but the altitude assertion message says "ESKF" and mentions provisional bounds/#286.
"ESKF RMS altitude error should be less than {:.2}m (provisional, see #286), got {:.2}m",
core/tests/integration_tests.rs:1146
- This is the degraded-GNSS EKF test, but the altitude assertion message says "ESKF" and includes the provisional/#286 note.
"ESKF maximum altitude error should be less than {:.2}m (provisional, see #286), got {:.2}m",
core/tests/integration_tests.rs:1134
- This is the degraded-GNSS EKF test, but the assertion message says "ESKF".
"ESKF maximum horizontal error should be less than {:.2}m, got {:.2}m",
- Files reviewed: 5/5 changed files
- Comments generated: 4
- Review effort level: Lite
💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.
| /// This test simulates degraded GNSS conditions with reduced update rate (5s intervals). | ||
| #[test] | ||
| #[ignore = "ESKF with degraded GNSS has altitude divergence and health monitor aborts due to out of range"] | ||
| #[ignore = "ESKF vertical channel still diverges under degraded GNSS; horizontal loop fixed in #266, vertical tracked by #286"] |
| assert!( | ||
| stats.rms_horizontal_error < rms_horizontal_limit, | ||
| "RMS horizontal error with degraded GNSS should be less than {:.2}m, got {:.2}m", | ||
| "ESKF RMS horizontal error should be less than {:.2}m, got {:.2}m", |
| assert!( | ||
| stats.rms_horizontal_error < rms_horizontal_limit, | ||
| "RMS horizontal error with degraded GNSS should be less than {:.2}m, got {:.2}m", | ||
| "ESKF RMS horizontal error should be less than {:.2}m, got {:.2}m", |
| assert!( | ||
| stats.rms_horizontal_error < rms_horizontal_limit, | ||
| "RMS horizontal error with degraded GNSS should be less than {:.2}m, got {:.2}m", | ||
| "ESKF RMS horizontal error should be less than {:.2}m, got {:.2}m", |
Owner
Author
CI: every ESKF and RBPF failure is cleared
The three test failures that were red on
The single remaining failure on the two red jobs is Fixing #284 would make this the first fully green CI on the repo in a while, but it is unrelated to |
…k in F
Two further errors in error_state_transition_jacobian, both found by
numerically differentiating forward() and comparing, rather than by reading.
1. The attitude block was assigned over the identity instead of subtracted
from it.
f[(6 + i, 6 + j)] = -omega_skew[(i, j)] * dt; // was
f[(6 + i, 6 + j)] -= omega_skew[(i, j)] * dt; // now
F is initialised to the identity and the block is I - [omega^b]_x dt. A
skew-symmetric matrix has a zero diagonal, so the assignment silently set
f[(6,6)], f[(7,7)] and f[(8,8)] to zero. The attitude error covariance was
therefore annihilated on every propagation step: the filter behaved as
though attitude were perfectly known after each predict, never corrected
tilt, and left gyro bias -- observable only through tilt -- unconstrained.
The full-state state_transition_jacobian already used += on the same block
(linearize.rs:233), so the error-state copy was the outlier.
2. The whole d(vdot)/d(p) block was missing.
earth::gravity is g0(lat) - 3.08e-6 h, so both derivatives exist:
dg/dh = -3.08e-6 s^-2 and dg/dlat ~ 0.0519 m/s^2/rad at 45 degrees. Finite
differences show both clearly and flat across four orders of magnitude of
step size. The comment that stood here argued the gravity-gradient term
should be dropped because in ENU it yields growing eigenvalues. That
instability is real, but it belongs to the system, not the linearisation:
omitting a term the nominal propagation actually applies makes F disagree
with forward(), so the covariance stops describing the true error dynamics
and the filter cannot attribute an altitude innovation to vertical-velocity
error. Added with the frame sign taken from state.is_enu, plus a
gravity_latitude_gradient helper deriving the Somigliana derivative.
Tests. Two more, both of which fail if their fix is reverted:
- every diagonal entry of F must be ~1, with a non-zero angular rate so the
annihilation is visible (it is not, at omega = 0)
- F must agree with a finite difference of forward() across the position,
velocity and attitude columns, at a tolerance above the legitimate O(dt^2)
omissions and far below any first-order structural error
Bias columns are excluded from the second test for now: forward() stores
attitude through Rotation3::from_matrix, an iterative orthonormalising
projection whose own derivative is not unity, and differencing through it
reports d(theta)/d(b_g) as ~1.5x dt instead of dt. Recorded in #286.
Effect on the vertical channel is small -- altitude rms 129.54 -> 119.30 m
against the UKF's 2.82 m -- so neither of these is the dominant cause. What
they do establish, together with the measurement that the ESKF at the UKF's
own Q and P0 scores 286.61 m against the UKF's 2.82 m, is that the remaining
fault is structural rather than tuning. Horizontal is now 23.82 m rms, at
parity with the UKF's 23.62 m.
Refs #286
Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Open
3 tasks
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
v1/01-eskf-fixmainWhy
Three confirmed defects: position error is radians in
FandHbut metres ininject_error_state; the velocity-attitude block ofFuses a global attitude-error conventionwhile attitude propagation, gyro-bias coupling and injection all use local; and a scalar
1e-9covariance jitter is added to all 15 diagonal entries regardless of units.
Acceptance criteria
test_data.csvwith full GNSS aiding#[ignore]d ESKF tests re-enablederror_state_transition_jacobianwith non-identityC_bnapply_eskf_correctionandinject_error_stateunified into one functionapply_eskf_correction)test_eskf_stability_high_dynamicsandtest_filter_comparisonpass on macOS and WindowsCloses #266
Supersedes #270, which GitHub auto-closed when queue position 0's branch was deleted on merge and
refuses to reopen because this branch was force-pushed afterwards. Same branch, same work.
Branches off
main(queue position 0 is merged).Queue:
docs/V1_QUEUE.md| Board: https://github.com/users/jbrodovsky/projects/7