From 5beebfe3a500fcdcab0f37191ebfd94b7e32870f Mon Sep 17 00:00:00 2001 From: Theodore Omtzigt Date: Sun, 21 Jun 2026 21:33:17 -0400 Subject: [PATCH] =?UTF-8?q?feat(tools):=20obs=5Finspect=20=E2=80=94=20R-IE?= =?UTF-8?q?KF=20vs=20standard=20yaw-leak=20comparison?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- tests/tools/observability_inspect.cpp | 31 ++++++++++ tools/README.md | 17 ++++-- .../branes/tools/observability_inspect.hpp | 57 +++++++++++++++++++ tools/src/obs_inspect.cpp | 37 +++++++++--- 4 files changed, 127 insertions(+), 15 deletions(-) diff --git a/tests/tools/observability_inspect.cpp b/tests/tools/observability_inspect.cpp index 88a79e5..00c5cfa 100644 --- a/tests/tools/observability_inspect.cpp +++ b/tests/tools/observability_inspect.cpp @@ -98,3 +98,34 @@ TEST_CASE("observability inspector: perturbing the window leaks yaw but not tran REQUIRE(yaw_small > 1e-6); REQUIRE(yaw_big > yaw_small); } + +TEST_CASE("observability inspector: the R-IEKF parameterization does not leak yaw under perturbation", + "[tools][obs_inspect]") { + const ms::CameraExtrinsics ext{}; // identity extrinsic + const Vec3 g{{0.0, 0.0, 1.0}}; + const Vec3 p_f{{0.2, -0.1, 6.0}}; + const auto truth = window(5); + const auto Ninv = bt::obs_build_N_invariant(truth, p_f, g); + + // Consistent point: the shipped invariant Jacobian annihilates the invariant gauge. + const auto [tr0, yaw0] = bt::obs_leak(bt::obs_build_H_invariant(truth, p_f, ext), Ninv); + REQUIRE_THAT(tr0, WithinAbs(0.0, 1e-9)); + REQUIRE_THAT(yaw0, WithinAbs(0.0, 1e-9)); + + // 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); + auto e = truth; + for (auto& c : e) { + c.R = + c.R * + SO3::exp(Vec3{{0.05 * bt::obs_urand(rng), 0.05 * bt::obs_urand(rng), 0.05 * bt::obs_urand(rng)}}); + c.p = Vec3{{c.p[0] + 0.05 * bt::obs_urand(rng), + c.p[1] + 0.05 * bt::obs_urand(rng), + c.p[2] + 0.05 * bt::obs_urand(rng)}}; + } + const auto [tr_p, yaw_p] = bt::obs_leak(bt::obs_build_H_invariant(e, p_f, ext), Ninv); + REQUIRE_THAT(tr_p, WithinAbs(0.0, 1e-9)); + REQUIRE_THAT(yaw_p, WithinAbs(0.0, 1e-9)); // flat — unlike the standard filter +} diff --git a/tools/README.md b/tools/README.md index 3f8f230..582fd6e 100644 --- a/tools/README.md +++ b/tools/README.md @@ -355,12 +355,17 @@ per-clone σ from P, N at the unperturbed gauge). ``` Output `observability.jsonl`: per-update window size, mean clone σ, and the -consistent/real 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 **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. The fix is the right-invariant -(R-IEKF) parameterization, whose gauge directions are estimate-independent. +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. ## Layout diff --git a/tools/include/branes/tools/observability_inspect.hpp b/tools/include/branes/tools/observability_inspect.hpp index 0ae4aa2..e905ebd 100644 --- a/tools/include/branes/tools/observability_inspect.hpp +++ b/tools/include/branes/tools/observability_inspect.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -106,6 +107,62 @@ template return N; } +/// The RIGHT-INVARIANT (R-IEKF) stacked Jacobian over the clone window, driving the +/// SHIPPED build_invariant_measurement (invariant_update.hpp) — the world-frame +/// twist parameterization ξ_c = (φ, ρ). Same column layout as obs_build_H so it +/// composes with obs_build_N_invariant / obs_leak: [ per clone φ(3) ρ(3) | feature +/// δp(3) ]. This is the candidate fix's Jacobian, measured on the same real geometry. +template +[[nodiscard]] sdk::msckf::DynMat obs_build_H_invariant(const std::vector>& cl, + const math::lie::detail::Vec& p_f, + const sdk::msckf::CameraExtrinsics& ext) { + const std::size_t m = cl.size(); + std::vector> ic; + ic.reserve(m); + for (const auto& c : cl) + ic.push_back({c.R, c.p}); + std::vector> obs; + obs.reserve(m); + for (std::size_t c = 0; c < m; ++c) + obs.push_back({c, 0, {}}); // xy only enters the residual, not H + const std::vector> cal{{ext.R_imu_cam, ext.p_imu_cam}}; + const auto M = sdk::msckf::build_invariant_measurement(ic, obs, p_f, cal, /*estimate_calib=*/false); + sdk::msckf::DynMat H(2 * m, 6 * m + 3); + if (!M.ok) + return H; // degenerate window ⇒ zeros (leak 0) + const std::size_t n = M.n; // 6*m (no calib columns) + for (std::size_t row = 0; row < 2 * m; ++row) { + for (std::size_t cc = 0; cc < 6 * m; ++cc) + H(row, cc) = M.H_x[row * n + cc]; // [φ_c ρ_c] per clone + for (std::size_t b = 0; b < 3; ++b) + H(row, 6 * m + b) = M.H_f[row * 3 + b]; // feature + } + return H; +} + +/// The invariant 4-DoF gauge. Crucially the CLONE directions are CONSTANTS (no +/// estimate): translation ρ = e_k, yaw φ = ĝ for EVERY clone — so a perturbed +/// window cannot break the annihilation. Only the (marginalized) feature carries +/// the estimate. Cols 0–2 translation, col 3 yaw. +template +[[nodiscard]] sdk::msckf::DynMat obs_build_N_invariant(const std::vector>& cl, + const math::lie::detail::Vec& p_f, + const math::lie::detail::Vec& g) { + const std::size_t m = cl.size(); + sdk::msckf::DynMat N(6 * m + 3, 4); + for (std::size_t c = 0; c < m; ++c) + for (std::size_t k = 0; k < 3; ++k) { + N(6 * c + 3 + k, k) = T{1}; // translation: clone ρ = e_k (constant) + N(6 * c + k, 3) = g[k]; // yaw: clone φ = ĝ (constant, estimate-independent) + } + for (std::size_t k = 0; k < 3; ++k) + N(6 * m + k, k) = T{1}; // translation: feature δp + const math::lie::detail::Vec dpf = math::lie::detail::hat(g) * p_f; + for (std::size_t k = 0; k < 3; ++k) + N(6 * m + k, 3) = dpf[k]; // yaw: feature [g]× p_f + return N; +} + /// Per-gauge-direction leak: column norms of H·N. `.first` = translation (RSS of /// cols 0–2), `.second` = yaw (norm of col 3). template diff --git a/tools/src/obs_inspect.cpp b/tools/src/obs_inspect.cpp index 2a4e954..71ba6e7 100644 --- a/tools/src/obs_inspect.cpp +++ b/tools/src/obs_inspect.cpp @@ -28,6 +28,11 @@ // uncertainty the filter actually reports on this window: given that // uncertainty, how much yaw/translation information does the shipped H // spuriously inject? +// * R-IEKF leak -- the SAME perturbed windows run through the shipped +// right-invariant Jacobian (build_invariant_measurement) + the invariant gauge, +// whose clone directions are estimate-independent constants. On V2_03 this is +// ~1e-16 where the standard leak is ~0.4 -- the candidate fix, validated on +// real data. // // Output: per-update JSONL (window size, mean clone sigma, consistent/real yaw & // translation leak, NIS) + a summary localizing the leak to yaw vs translation on @@ -203,6 +208,7 @@ int main(int argc, char** argv) { std::uint64_t n_updates = 0, n_measured = 0; double sum_cons_yaw = 0.0, sum_cons_tr = 0.0; double sum_real_yaw = 0.0, sum_real_tr = 0.0, max_real_yaw = 0.0; + double sum_inv_yaw = 0.0, max_inv_yaw = 0.0; // R-IEKF leak at the SAME perturbations est.backend().set_update_observer([&](const ms::State& s, const ms::FeatureTrack& track, @@ -242,8 +248,12 @@ int main(int argc, char** argv) { const auto [cons_tr, cons_yaw] = bt::obs_leak(bt::obs_build_H(updater, cl, p_f), N); // (2) Real leak: perturb the clone window by the filter's OWN per-clone - // sigma, N at the unperturbed gauge, averaged over seeded draws. - double dyaw = 0.0, dtr = 0.0; + // sigma, N at the unperturbed gauge, averaged over seeded draws. For each + // perturbed window also measure the RIGHT-INVARIANT (R-IEKF) leak — the + // candidate fix — on the SAME geometry: its gauge directions are constants, + // so it should stay ~0 where the standard one leaks. + const ms::DynMat Ninv = bt::obs_build_N_invariant(cl, p_f, g); + double dyaw = 0.0, dtr = 0.0, dyaw_inv = 0.0; std::mt19937_64 rng(0x0B5E11ull ^ (n_updates * 0x9E3779B97F4A7C15ull)); for (int d = 0; d < args.draws; ++d) { std::vector> pe = cl; @@ -258,9 +268,12 @@ int main(int argc, char** argv) { const auto [tr, yaw] = bt::obs_leak(bt::obs_build_H(updater, pe, p_f), N); dyaw += static_cast(yaw); dtr += static_cast(tr); + const auto inv = bt::obs_leak(bt::obs_build_H_invariant(pe, p_f, cal.extrinsics), Ninv); + dyaw_inv += static_cast(inv.second); } dyaw /= args.draws; dtr /= args.draws; + dyaw_inv /= args.draws; T mean_sth = T{0}, mean_sp = T{0}; for (std::size_t i = 0; i < m; ++i) { @@ -275,8 +288,11 @@ int main(int argc, char** argv) { sum_cons_tr += static_cast(cons_tr); sum_real_yaw += dyaw; sum_real_tr += dtr; + sum_inv_yaw += dyaw_inv; if (dyaw > max_real_yaw) max_real_yaw = dyaw; + if (dyaw_inv > max_inv_yaw) + max_inv_yaw = dyaw_inv; json j{{"index", n_updates - 1}, {"t", frame_t}, @@ -288,7 +304,8 @@ int main(int argc, char** argv) { {"leak_yaw_consistent", static_cast(cons_yaw)}, {"leak_trans_consistent", static_cast(cons_tr)}, {"leak_yaw_real", dyaw}, - {"leak_trans_real", dtr}}; + {"leak_trans_real", dtr}, + {"leak_yaw_invariant", dyaw_inv}}; os << j.dump() << '\n'; }); @@ -317,16 +334,18 @@ int main(int argc, char** argv) { os.flush(); const double nm = n_measured ? static_cast(n_measured) : 1.0; - const double ry = sum_real_yaw / nm, rt = sum_real_tr / nm; + const double ry = sum_real_yaw / nm, rt = sum_real_tr / nm, iy = sum_inv_yaw / nm; std::cout << "obs_inspect: processed " << processed << " frames, " << n_updates << " updates (" << n_measured << " measured)\n" - << " consistent leak (sanity, want ~0): yaw " << sum_cons_yaw / nm << " trans " << sum_cons_tr / nm + << " consistent leak (sanity, want ~0): yaw " << sum_cons_yaw / nm << " trans " << sum_cons_tr / nm << "\n" - << " real leak @ filter's own clone sigma: yaw " << ry << " trans " << rt << "\n" - << " max real yaw leak: " << max_real_yaw << "\n" + << " STANDARD leak @ filter's own clone sigma: yaw " << ry << " trans " << rt << " (max yaw " + << max_real_yaw << ")\n" + << " R-IEKF leak @ the SAME perturbations: yaw " << iy << " (max yaw " << max_inv_yaw << ")\n" << " -> " - << (ry > 4.0 * rt + 1e-12 ? "YAW dominates: over-confidence enters the yaw gauge" - : "no clear yaw dominance") + << (ry > 4.0 * rt + 1e-12 ? "YAW dominates the standard filter; " : "no clear standard yaw dominance; ") + << (iy < ry / 10.0 + 1e-12 ? "R-IEKF flattens it (observable by construction)" + : "R-IEKF does NOT flatten it") << "\n records: " << args.out << "/observability.jsonl\n"; return 0; }