feat(tools): obs_inspect R-IEKF vs standard yaw-leak comparison#426
Merged
Conversation
Extends the observability diagnostic to measure the right-invariant (R-IEKF) leak alongside the standard one on the SAME real clone windows, driving the shipped build_invariant_measurement (invariant_update.hpp). For each perturbed window (perturbed by the filter's own per-clone sigma) it now reports both the standard yaw leak and the invariant yaw leak. On V2_03_difficult the standard yaw leak is mean 0.39 / max 619 at the filter's own claimed clone uncertainty, while the R-IEKF leak on the SAME perturbations is mean 2.3e-16 / max 3.0e-14 — eliminated. The invariant gauge directions are estimate-independent constants (rho = e_k, phi = g), so the stacked invariant H annihilates the gauge regardless of the clone-window drift that makes the standard filter fabricate yaw information. This is the real-data evidence (complementing the synthetic invariant_vio_backend end-to-end test, yaw NEES 993 -> <15) that the right-invariant parameterization is the fix for the #212 yaw-gauge over-confidence. Adds obs_build_H_invariant / obs_build_N_invariant to the header and a gate test case asserting the invariant leak stays ~0 under the same perturbation that leaks the standard one. Relates to #212, #337, #347, #348 Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
WalkthroughAdds two header-only templated helpers ( ChangesR-IEKF Invariant Observability Leak Diagnostics
Estimated code review effort🎯 3 (Moderate) | ⏱️ ~20 minutes Possibly related issues
Possibly related PRs
🚥 Pre-merge checks | ✅ 4 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (4 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches📝 Generate docstrings
🧪 Generate unit tests (beta)
Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 4
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@tests/tools/observability_inspect.cpp`:
- Around line 115-118: The comment block above the std::mt19937_64 rng
initialization contains a wording error in the last line that says "yaw
observable by construction" which contradicts the gauge invariance being tested.
The test verifies that yaw remains UNOBSERVABLE (in the invariant null-space),
not observable, which is why the leak must stay ~0. Change the phrase from "yaw
observable by construction" to correctly state that yaw is UNOBSERVABLE by
construction to accurately reflect the gauge preservation property being
validated.
- Around line 108-113: The test does not validate that the invariant Jacobian
H_invariant is non-degenerate before checking the leak results. If
obs_build_H_invariant returns all zeros due to hitting a degenerate path, the
subsequent obs_leak assertions pass vacuously without actually validating the
invariant property. Add a non-degeneracy assertion on the result of
obs_build_H_invariant before the obs_leak checks at both locations (around line
111 and line 128). The assertion should verify that H_invariant contains
meaningful non-zero values rather than being all zeros, ensuring the invariant
property is genuinely validated.
In `@tools/README.md`:
- Around line 358-368: The README documentation overstates the invariant outputs
being measured and uses reversed observability terminology. At the section
discussing invariant leak measurements, remove the reference to
"invariant...translation leaks" since only invariant yaw accumulation is
actually reported by the implementation, not translation. Additionally, at the
end of this section where it states the R-IEKF "is observable-by-construction,"
this wording inverts the gauge claim being made—revise it to correctly describe
that the invariant gauge directions are estimate-independent constants (as
mentioned earlier in the same passage) rather than claiming
observability-by-construction.
In `@tools/src/obs_inspect.cpp`:
- Around line 271-276: The code at the invariant accumulation loop in
obs_inspect.cpp is currently accumulating only inv.second (yaw) via dyaw_inv
while discarding inv.first (translation) from the bt::obs_leak result. Clarify
the intent by either adding an explanatory comment above the dyaw_inv
accumulation line documenting why the translation component is intentionally
excluded (referencing that both parameterizations share the same translation
gauge), or if translation diagnostics should be tracked, add a corresponding
dtr_inv variable that accumulates inv.first similar to how dyaw_inv accumulates
inv.second, then divide dtr_inv by args.draws after the loop.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Path: .coderabbit.yaml
Review profile: ASSERTIVE
Plan: Pro
Run ID: a066cf4f-0f8a-489c-8804-1a034761af5d
📒 Files selected for processing (4)
tests/tools/observability_inspect.cpptools/README.mdtools/include/branes/tools/observability_inspect.hpptools/src/obs_inspect.cpp
Comment on lines
+108
to
+113
| const auto Ninv = bt::obs_build_N_invariant<T>(truth, p_f, g); | ||
|
|
||
| // Consistent point: the shipped invariant Jacobian annihilates the invariant gauge. | ||
| const auto [tr0, yaw0] = bt::obs_leak<T>(bt::obs_build_H_invariant<T>(truth, p_f, ext), Ninv); | ||
| REQUIRE_THAT(tr0, WithinAbs(0.0, 1e-9)); | ||
| REQUIRE_THAT(yaw0, WithinAbs(0.0, 1e-9)); |
There was a problem hiding this comment.
Prevent a vacuous pass when invariant Jacobian construction degenerates.
At Lines 111 and 128, the test only checks obs_leak(...) == ~0. If obs_build_H_invariant hits its degenerate path and returns zeros, these assertions pass without validating the invariant property. Add a non-degeneracy assertion on H_invariant before leak checks.
Proposed patch
- const auto [tr0, yaw0] = bt::obs_leak<T>(bt::obs_build_H_invariant<T>(truth, p_f, ext), Ninv);
+ const auto H0 = bt::obs_build_H_invariant<T>(truth, p_f, ext);
+ REQUIRE(H0.rows() > 0);
+ REQUIRE(H0.cwiseAbs().maxCoeff() > 1e-12);
+ const auto [tr0, yaw0] = bt::obs_leak<T>(H0, Ninv);
@@
- const auto [tr_p, yaw_p] = bt::obs_leak<T>(bt::obs_build_H_invariant<T>(e, p_f, ext), Ninv);
+ const auto Hp = bt::obs_build_H_invariant<T>(e, p_f, ext);
+ REQUIRE(Hp.rows() > 0);
+ REQUIRE(Hp.cwiseAbs().maxCoeff() > 1e-12);
+ const auto [tr_p, yaw_p] = bt::obs_leak<T>(Hp, Ninv);Also applies to: 128-130
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@tests/tools/observability_inspect.cpp` around lines 108 - 113, The test does
not validate that the invariant Jacobian H_invariant is non-degenerate before
checking the leak results. If obs_build_H_invariant returns all zeros due to
hitting a degenerate path, the subsequent obs_leak assertions pass vacuously
without actually validating the invariant property. Add a non-degeneracy
assertion on the result of obs_build_H_invariant before the obs_leak checks at
both locations (around line 111 and line 128). The assertion should verify that
H_invariant contains meaningful non-zero values rather than being all zeros,
ensuring the invariant property is genuinely validated.
Comment on lines
+115
to
+118
| // Same clone-window perturbation that makes the STANDARD yaw leak (previous | ||
| // test). The invariant gauge directions are constants (rho = e_k, phi = g), so | ||
| // the leak must STAY ~0 — yaw observable by construction. This is the fix. | ||
| std::mt19937_64 rng(0x0B5E11ull); |
There was a problem hiding this comment.
Line 117 wording inverts the gauge claim.
“yaw observable by construction” conflicts with the invariant null-space argument being tested here; this path is about preserving the yaw unobservable gauge (leak stays ~0). Please reword to avoid future confusion.
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@tests/tools/observability_inspect.cpp` around lines 115 - 118, The comment
block above the std::mt19937_64 rng initialization contains a wording error in
the last line that says "yaw observable by construction" which contradicts the
gauge invariance being tested. The test verifies that yaw remains UNOBSERVABLE
(in the invariant null-space), not observable, which is why the leak must stay
~0. Change the phrase from "yaw observable by construction" to correctly state
that yaw is UNOBSERVABLE by construction to accurately reflect the gauge
preservation property being validated.
Comment on lines
+358
to
+368
| consistent/real/invariant yaw & translation leaks. On V2_03 the consistent leak is | ||
| ~1e-15 (the shipped Jacobian is correct), the translation leak is **structurally 0** | ||
| (the ±Hf cancellation protects it), and the **standard yaw leak is nonzero on 100% | ||
| of updates and grows with the clone window's attitude drift** — localizing the #212 | ||
| over-confidence to the yaw gauge on real data. | ||
|
|
||
| It also measures the **right-invariant (R-IEKF) leak on the same perturbed clone | ||
| windows** (driving the shipped `build_invariant_measurement`): because the | ||
| invariant gauge directions are estimate-independent constants (ρ = e_k, φ = ĝ), the | ||
| invariant yaw leak stays ~0 where the standard one rises — the real-data evidence | ||
| that R-IEKF is observable-by-construction and is the fix the diagnosis points to. |
There was a problem hiding this comment.
README overstates invariant outputs and uses inverted observability wording.
At Line 358, docs currently imply invariant yaw and translation leaks are reported, but the implementation context shows invariant yaw accumulation/reporting only. At Line 368, “observable-by-construction” reverses the gauge claim in this section.
Proposed patch
-consistent/real/invariant yaw & translation leaks. On V2_03 the consistent leak is
+consistent/real yaw & translation leaks, plus invariant yaw leak. On V2_03 the consistent leak is
@@
-invariant yaw leak stays ~0 where the standard one rises — the real-data evidence
-that R-IEKF is observable-by-construction and is the fix the diagnosis points to.
+invariant yaw leak stays ~0 where the standard one rises — the real-data evidence
+that R-IEKF preserves the unobservable gauge by construction and is the fix the diagnosis points to.🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@tools/README.md` around lines 358 - 368, The README documentation overstates
the invariant outputs being measured and uses reversed observability
terminology. At the section discussing invariant leak measurements, remove the
reference to "invariant...translation leaks" since only invariant yaw
accumulation is actually reported by the implementation, not translation.
Additionally, at the end of this section where it states the R-IEKF "is
observable-by-construction," this wording inverts the gauge claim being
made—revise it to correctly describe that the invariant gauge directions are
estimate-independent constants (as mentioned earlier in the same passage) rather
than claiming observability-by-construction.
Comment on lines
+271
to
+276
| const auto inv = bt::obs_leak<T>(bt::obs_build_H_invariant<T>(pe, p_f, cal.extrinsics), Ninv); | ||
| dyaw_inv += static_cast<double>(inv.second); | ||
| } | ||
| dyaw /= args.draws; | ||
| dtr /= args.draws; | ||
| dyaw_inv /= args.draws; |
There was a problem hiding this comment.
🧹 Nitpick | 🔵 Trivial | 💤 Low value
Invariant translation leak discarded — intentional?
Line 272 accumulates only inv.second (yaw); inv.first (translation) is dropped. Both parameterizations share the same translation gauge, so this is likely intentional for the yaw-dominance focus. If future diagnostics need translation comparisons, consider storing it.
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@tools/src/obs_inspect.cpp` around lines 271 - 276, The code at the invariant
accumulation loop in obs_inspect.cpp is currently accumulating only inv.second
(yaw) via dyaw_inv while discarding inv.first (translation) from the
bt::obs_leak result. Clarify the intent by either adding an explanatory comment
above the dyaw_inv accumulation line documenting why the translation component
is intentionally excluded (referencing that both parameterizations share the
same translation gauge), or if translation diagnostics should be tracked, add a
corresponding dtr_inv variable that accumulates inv.first similar to how
dyaw_inv accumulates inv.second, then divide dtr_inv by args.draws after the
loop.
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
1 participant
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.
Summary
Extends
obs_inspectto measure the right-invariant (R-IEKF) yaw leak alongside the standard one on the same real clone windows — the real-data validation that R-IEKF fixes the #212 yaw-gauge over-confidence. Driven by the shippedbuild_invariant_measurement(no load-bearing change).The measurement
For each MSCKF update, after perturbing the clone window by the filter's own per-clone σ (from P), it computes the yaw leak ‖H·N‖ under both parameterizations on identical geometry.
V2_03_difficult (2,918 updates):
The invariant gauge directions are estimate-independent constants (ρ = e_k, φ = ĝ), so the stacked invariant H annihilates the gauge regardless of the clone-window drift that makes the standard filter fabricate yaw information. R-IEKF eliminates the leak (~10¹⁵×) — the real-data complement to the synthetic
invariant_vio_backendend-to-end test (yaw NEES 993 → <15).Changes
tools/include/branes/tools/observability_inspect.hpp—obs_build_H_invariant(drives shippedbuild_invariant_measurement) +obs_build_N_invariant(constant invariant gauge).tools/src/obs_inspect.cpp— per-update R-IEKF leak + summary verdict.tests/tools/observability_inspect.cpp— gate: invariant leak stays ~0 under the same perturbation that leaks the standard one.tools/README.md— updated.Test Results
Test plan
Relates to #212, #337, #347, #348
🤖 Generated with Claude Code
Summary by CodeRabbit
Release Notes
New Features
Tests
Documentation