diff --git a/docs/benchmarks/euroc-baseline.mdx b/docs/benchmarks/euroc-baseline.mdx index bd407a4b..582949cd 100644 --- a/docs/benchmarks/euroc-baseline.mdx +++ b/docs/benchmarks/euroc-baseline.mdx @@ -19,7 +19,7 @@ The numbers come from `scripts/euroc-baseline.json`, which - **Data:** the 11 EuRoC MAV sequences, cam0 and imu0, ground truth from `state_groundtruth_estimate0`. -- **cortex** (commit 37bd455, 2026-11-07; every sequence changed from the first baseline at 348c774, see below): `vio_pipeline --source euroc` with +- **cortex** (commit f1551cc, 2026-11-09, with S2 integrating each IMU interval from both of its samples, #616; every sequence changed from the first baseline at 348c774, see below): `vio_pipeline --source euroc` with its defaults, monocular, the MSCKF with double stages and a full covariance. Every run starts at the first frame. - **OpenVINS v2.7:** the unmodified upstream binary, run outside every Branes @@ -52,17 +52,17 @@ The numbers come from `scripts/euroc-baseline.json`, which | Sequence | cortex, whole run | cortex, window | OpenVINS mono | OpenVINS stereo | cortex ÷ OV mono | |---|---:|---:|---:|---:|---:| -| MH_01_easy | 0.322 | 0.332 | 0.096 | 0.072 | 3.5× | -| MH_02_easy | 0.420 | 0.369 | 0.168 | 0.117 | 2.2× | -| MH_03_medium | 0.471 | 0.439 | 0.147 | 0.096 | 3.0× | -| MH_04_difficult | 0.376 | 0.396 | 0.147 | 0.127 | 2.7× | -| MH_05_difficult | 0.690 | 0.645 | 0.458 | 0.281 | 1.4× | -| V1_01_easy | 0.217 | 0.213 | 0.069 | 0.063 | 3.1× | -| V1_02_medium | 0.183 | 0.183 | 0.058 | 0.048 | 3.1× | -| V1_03_difficult | 0.188 | 0.189 | 0.067 | 0.059 | 2.8× | -| V2_01_easy | 0.224 | 0.215 | 0.127 | 0.060 | 1.7× | -| V2_02_medium | 0.331 | 0.330 | 0.073 | 0.044 | 4.5× | -| V2_03_difficult | 0.178 | 0.181 | 0.181 | 0.087 | 1.0× | +| MH_01_easy | 0.331 | 0.333 | 0.096 | 0.072 | 3.5× | +| MH_02_easy | 0.456 | 0.393 | 0.168 | 0.117 | 2.3× | +| MH_03_medium | 0.500 | 0.472 | 0.147 | 0.096 | 3.2× | +| MH_04_difficult | 0.375 | 0.379 | 0.147 | 0.127 | 2.6× | +| MH_05_difficult | 0.671 | 0.629 | 0.458 | 0.281 | 1.4× | +| V1_01_easy | 0.200 | 0.197 | 0.069 | 0.063 | 2.8× | +| V1_02_medium | 0.163 | 0.162 | 0.058 | 0.048 | 2.8× | +| V1_03_difficult | 0.158 | 0.161 | 0.067 | 0.059 | 2.4× | +| V2_01_easy | 0.223 | 0.215 | 0.127 | 0.060 | 1.7× | +| V2_02_medium | 0.338 | 0.334 | 0.073 | 0.044 | 4.6× | +| V2_03_difficult | 0.173 | 0.175 | 0.181 | 0.087 | 1.0× | The ratio compares cortex's window with OpenVINS mono; both are monocular. @@ -72,22 +72,22 @@ Over the window: | Sequence | cortex | OpenVINS mono | OpenVINS stereo | |---|---:|---:|---:| -| MH_01_easy | 0.377 m / 1.62° | 0.137 m / 1.55° | 0.141 m / 1.67° | -| MH_02_easy | 0.461 m / 1.12° | 0.173 m / 1.07° | 0.132 m / 0.86° | -| MH_03_medium | 0.388 m / 0.65° | 0.178 m / 0.61° | 0.159 m / 0.62° | -| MH_04_difficult | 0.583 m / 1.19° | 0.206 m / 0.83° | 0.213 m / 0.50° | -| MH_05_difficult | 0.705 m / 0.81° | 0.506 m / 1.06° | 0.300 m / 0.85° | -| V1_01_easy | 0.342 m / 4.03° | 0.262 m / 2.84° | 0.229 m / 2.83° | -| V1_02_medium | 0.233 m / 2.52° | 0.110 m / 0.57° | 0.106 m / 0.73° | -| V1_03_difficult | 0.272 m / 2.63° | 0.125 m / 1.24° | 0.125 m / 1.02° | -| V2_01_easy | 0.448 m / 2.43° | 0.220 m / 1.06° | 0.102 m / 0.89° | -| V2_02_medium | 0.295 m / 4.55° | 0.090 m / 1.55° | 0.061 m / 1.27° | -| V2_03_difficult | 0.249 m / 2.21° | 0.201 m / 1.56° | 0.108 m / 1.38° | +| MH_01_easy | 0.370 m / 1.57° | 0.137 m / 1.55° | 0.141 m / 1.67° | +| MH_02_easy | 0.478 m / 1.35° | 0.173 m / 1.07° | 0.132 m / 0.86° | +| MH_03_medium | 0.386 m / 0.75° | 0.178 m / 0.61° | 0.159 m / 0.62° | +| MH_04_difficult | 0.548 m / 1.03° | 0.206 m / 0.83° | 0.213 m / 0.50° | +| MH_05_difficult | 0.701 m / 0.95° | 0.506 m / 1.06° | 0.300 m / 0.85° | +| V1_01_easy | 0.330 m / 4.04° | 0.262 m / 2.84° | 0.229 m / 2.83° | +| V1_02_medium | 0.209 m / 2.31° | 0.110 m / 0.57° | 0.106 m / 0.73° | +| V1_03_difficult | 0.235 m / 2.56° | 0.125 m / 1.24° | 0.125 m / 1.02° | +| V2_01_easy | 0.447 m / 2.41° | 0.220 m / 1.06° | 0.102 m / 0.89° | +| V2_02_medium | 0.301 m / 4.09° | 0.090 m / 1.55° | 0.061 m / 1.27° | +| V2_03_difficult | 0.225 m / 1.74° | 0.201 m / 1.56° | 0.108 m / 1.38° | ## What the baseline says - **The typical gap is 2–3.5×** in 4-DoF ATE: the median over the 11 - sequences is 2.8×. V2_03, MH_05 and V2_01 are within 2×. + sequences is 2.6× (2.8× before #616). V2_03, MH_05 and V2_01 are within 2×. - **Initialization costs the most.** - MH_03 diverged in the first baseline (348c774: 29,804 m). Rotor vibration kept its still start out of static init, and the gravity-align fallback @@ -136,8 +136,10 @@ Over the window: 0.396 m); the median ratio stays 2.8×. MH_05's rotation RPE over the whole run goes from 1.07° to 1.89°. - **Rotation drift is the next gap.** In the Vicon rooms cortex's rotation - RPE is 1.4–4.4× OpenVINS mono's (V1_02: 2.52° against 0.57°). That is - consistent with the body-frame MSCKF's yaw observability leak (#212, #480). + RPE is 1.1–4.1× OpenVINS mono's (V1_02: 2.31° against 0.57°; before #616 + 1.4–4.4×, V1_02 2.52°). That is consistent with the body-frame MSCKF's yaw + observability leak (#212, #480). The R-IEKF's is 0.88° at the median + ([below](#the-r-iekf-on-the-shipped-real-data-path)). - **Starting at the first frame matters for OpenVINS too.** Run monocular from the first frame, it diverges on MH_04 (4-DoF ATE 9,889 m) and is otherwise within 0.035 m of its start-offset score. Its start offsets hide @@ -254,7 +256,57 @@ turns that into a pass/fail check that any backend can be run through: collapse. - Every verdict below is the same at any bound between 0.13 and 0.9. -### Results with one truth +### Results with one truth, and S2 integrating both samples (#616) + +Recorded at f1551cc for both backends, median over the three seeds. #616 made S2 +integrate each IMU interval from both of its samples (RK4 on the line between +them) instead of holding one, and made Φ that mean's exact Jacobian +([Aligning Sensor Samples](../first-principles/sample-alignment.mdx)). The +matched-run NIS is now **1.00 on every sequence for both backends** (1.1–1.9 on +the fast ones before): + +| Gate condition | MSCKF | R-IEKF | +|---|---|---| +| 1. No divergence (66 matched runs) | **fail:** 2 diverge (MH_03 1 px seed 2, 26.8 m; MH_04 1 px seed 3, 8.8 km) | **fail:** the same 2 (MH_03 2.3 m; MH_04 9.0 km) | +| 2. Matched beats default (median, 1 px / 0.5 px) | **pass:** 0.45× vs 0.52×; 0.33× vs 0.53× | **pass:** 0.40× vs 0.41×; 0.18× vs 0.36× | +| 3. No yaw collapse (median yaw σ min ÷ anchor ≥ 0.5) | **fail:** 0.179 | **pass:** 0.984 | +| Median NIS, matched runs (range) | 1.00 (1.00–1.11) | 1.00 (1.00–1.02) | +| Roll/pitch NEES, matched runs | 1.1–24 | 0.9–6.8 | + +At 0.5 px, matched `R`, per sequence (window ATE ratio to OpenVINS mono, yaw σ +min ÷ anchor, NIS): + +| Sequence | MSCKF ratio | MSCKF yaw σ | MSCKF NIS | R-IEKF ratio | R-IEKF yaw σ | R-IEKF NIS | +|---|---:|---:|---:|---:|---:|---:| +| MH_01_easy | 0.4× | 0.09 | 1.00 | 0.4× | 0.84 | 1.00 | +| MH_02_easy | 0.4× | 0.17 | 1.00 | 0.2× | 0.90 | 1.00 | +| MH_03_medium | 0.3× | 0.17 | 1.00 | 0.2× | 0.89 | 1.00 | +| MH_04_difficult | 0.2× | 0.16 | 1.00 | 0.2× | 0.91 | 1.00 | +| MH_05_difficult | 0.1× | 0.21 | 1.00 | 0.0× | 0.93 | 1.00 | +| V1_01_easy | 0.4× | 0.23 | 1.00 | 0.3× | 0.88 | 1.00 | +| V1_02_medium | 0.5× | 0.28 | 1.00 | 0.2× | 0.94 | 1.00 | +| V1_03_difficult | 0.4× | 0.23 | 1.00 | 0.2× | 0.94 | 1.00 | +| V2_01_easy | 0.2× | 0.23 | 1.00 | 0.1× | 0.95 | 1.00 | +| V2_02_medium | 0.4× | 0.21 | 1.00 | 0.1× | 0.94 | 1.00 | +| V2_03_difficult | 0.1× | 0.21 | 1.00 | 0.1× | 0.97 | 1.00 | + +- **The fast sequences recovered.** At 0.5 px the MSCKF's V2_03, V2_02 and V1_03 + went from 0.9×, 2.1× and 1.7× OpenVINS mono to 0.1×, 0.4× and 0.4×; the + R-IEKF's from 1.4×, 2.4× and 2.0× to 0.1×, 0.1× and 0.2×. Condition 2, which + both failed at 1 px, now passes for both. +- **The MSCKF's yaw still collapses**, to 9–28% of its anchor σ (0.107 → 0.179 + at the median). The observability ledger books none of it to S2 any more: on + MH_04 it puts 5e-21 rad⁻² of the spurious yaw information on S2 and 100% on + S6's mean shift, against 0.6–5.3% on S2 before. What is left is #480's S6 + leak, which the body-frame error state can't avoid. +- **Both backends diverge on the same two noise draws.** On MH_04 (1 px, seed + 3) both start from the same dynamic alignment, at the same instant (the + R-IEKF runs the MSCKF's initializer since #608), and both reach a roll/pitch + NEES of 34,000–40,000 and about 9 km: the seed, not either filter. MH_03 + (1 px, seed 2) starts by gravity alignment; the MSCKF reaches 26.8 m, the + R-IEKF 2.3 m. Both point at the tilt and bias seed of #609. + +### Results with one truth, before #616 Recorded at c6b87ec (#613) for both backends, median over the three seeds. With the noise model true, the median NIS of the matched runs is **1.03 for both** @@ -303,9 +355,7 @@ that it belongs to the estimator: 1.1–1.9, against 1.01–1.04 on the Machine Hall. The cause is #616: S2 holds the sample that ends each interval (the R-IEKF the one that starts it), a 2.5 ms IMU–camera skew. Averaging the two samples gives NIS 1.00 everywhere and cuts - V2_03's ATE 10–15×, and `tests/tools/gt_spline.cpp` holds S2 to a closure bound - taken from what the camera resolves, which the shipped S2 misses 28× (a - `[!shouldfail]` tripwire until #616). + V2_03's ATE 10–15×. Fixed in #616 (above). - **Conditions 1 and 2 depend on the noise draw at three seeds.** The first recording drew the IMU noise and the track noise from the same random stream (a seeding bug found in review); drawing them independently moved which runs @@ -406,7 +456,8 @@ half its NIS. The gate stays as recorded until #613 measures the ground truth's residual floor, folds it into matched `R`, runs every backend with the same gate line and re-records both. Condition 3 does not depend on the gate's line in the same way, but it is re-measured there too. #613 did this by removing the floor -rather than modelling it: see [Results with one truth](#results-with-one-truth). +rather than modelling it: see [Results with one truth, before #616](#results-with-one-truth-before-616), and +[after it](#results-with-one-truth-and-s2-integrating-both-samples-616). This is the fourth result in the VIO work that came from the test setup rather than the estimator ([the storyline](../first-principles/storyline.mdx)). @@ -473,6 +524,40 @@ Per sequence, window 4-DoF ATE (m) and window rotation RPE (°): - **Roll/pitch is still wrong at the start:** V1_01 has a roll/pitch NEES near 3000 in every configuration, the tilt and accelerometer-bias seed of #609. +### After #616: S2 integrates both samples + +#616 changed S2 in both backends (each IMU interval integrated from both of its +samples by RK4, and Φ the exact Jacobian of that mean). On the real KLT front +end, window 4-DoF ATE (m) and window rotation RPE (°), before → after: + +| Sequence | MSCKF ATE | R-IEKF ATE | OpenVINS mono | MSCKF ÷ mono | R-IEKF ÷ mono | MSCKF rot | R-IEKF rot | +|---|---:|---:|---:|---:|---:|---:|---:| +| MH_01_easy | 0.332 → 0.333 | 0.315 → 0.312 | 0.096 | 3.5× | 3.3× | 1.62 → 1.57 | 0.80 → 0.82 | +| MH_02_easy | 0.369 → 0.393 | 0.384 → 0.370 | 0.168 | 2.3× | 2.2× | 1.12 → 1.35 | 1.51 → 1.37 | +| MH_03_medium | 0.439 → 0.472 | 0.392 → 0.402 | 0.147 | 3.2× | 2.7× | 0.65 → 0.75 | 0.67 → 0.66 | +| MH_04_difficult | 0.396 → 0.379 | 0.377 → 0.359 | 0.147 | 2.6× | 2.4× | 1.19 → 1.03 | 0.42 → 0.46 | +| MH_05_difficult | 0.645 → 0.629 | 0.619 → 0.612 | 0.458 | 1.4× | 1.3× | 0.81 → 0.95 | 0.70 → 0.68 | +| V1_01_easy | 0.213 → 0.197 | 0.181 → 0.182 | 0.069 | 2.8× | 2.6× | 4.03 → 4.04 | 3.81 → 3.74 | +| V1_02_medium | 0.183 → 0.162 | 0.159 → 0.146 | 0.058 | 2.8× | 2.5× | 2.52 → 2.31 | 0.89 → 0.81 | +| V1_03_difficult | 0.189 → 0.161 | 0.240 → 0.131 | 0.067 | 2.4× | 2.0× | 2.63 → 2.56 | 0.90 → 0.88 | +| V2_01_easy | 0.215 → 0.215 | 0.221 → 0.219 | 0.127 | 1.7× | 1.7× | 2.43 → 2.41 | 1.26 → 1.33 | +| V2_02_medium | 0.330 → 0.334 | 0.274 → 0.261 | 0.073 | 4.6× | 3.6× | 4.55 → 4.09 | 1.35 → 1.37 | +| V2_03_difficult | 0.181 → 0.175 | 0.210 → 0.177 | 0.181 | 1.0× | 1.0× | 2.21 → 1.74 | 1.81 → 1.80 | +| **median** | | | | **2.83× → 2.57×** | **2.62× → 2.44×** | **2.21 → 1.74** | **0.90 → 0.88** | + +- **On real data the gain is real but small.** On the spline truth #616 cut the + fast sequences' ATE 4–24×; here the medians move 9% (MSCKF) and 7% (R-IEKF). + The R-IEKF's V1_03 halves (0.240 → 0.131 m) and V2_03 gains 16%; the MSCKF's + rotation drift falls 21% at the median (V2_03 2.21° → 1.74°). The MSCKF's MH_02 + and MH_03 are 7% worse. The R-IEKF is now ahead of the MSCKF on 9 of 11 + sequences (7 before). +- **The real-data NIS did not move.** Both backends stay at 0.93–2.55 on the + Machine Hall, V1_01 and V2_01, and at 6.7–15.5 on V1_02, V1_03, V2_02 and + V2_03. On the spline truth the same sequences went from 1.1–1.9 to 1.00, so + the half-sample skew is not what makes the filters over-confident on the real + IMU and the real tracks. What does is still open: the front end's outliers, R, the + ADIS16448's own filter delay, the camera–IMU time offset (#332). + ```sh CORTEX_VIO_ARGS="--invariant" scripts/euroc-baseline.sh --no-build # all pieces CORTEX_VIO_ARGS="--invariant --riekf-legacy gate,init,zupt" scripts/euroc-baseline.sh # before #608 diff --git a/docs/first-principles/overview.mdx b/docs/first-principles/overview.mdx index 4ec96b3c..88bb4761 100644 --- a/docs/first-principles/overview.mdx +++ b/docs/first-principles/overview.mdx @@ -19,8 +19,12 @@ here goes wrong in any MSCKF-style pipeline, and a unit-test suite that passes d measurement that decided its cause, and how it was fixed. - [A symptom → cause table and twenty pitfalls](signatures.mdx), so the next person recognizes a - failure on sight. + [A symptom → cause table and twenty-two pitfalls](signatures.mdx), so the next person + recognizes a failure on sight. + + + [A failure mode of every fusion pipeline](sample-alignment.mdx): integrating a sample over an + interval it doesn't represent, how it hid here for five months, and a checklist. [Every functional test a VIO pipeline must pass](regression-suite.mdx), with its pass @@ -30,10 +34,12 @@ here goes wrong in any MSCKF-style pipeline, and a unit-test suite that passes d ## The main lesson -Over about six months, **three results that looked solid turned out to be artifacts of the +Over about six months, **five results that looked solid turned out to be artifacts of the test setup**: a compiler "bug" that was an unsequenced random draw, a filter "inconsistency" that -was a velocity step in the synthetic world, and a "consistent" control that fed the filter exact -sensors. Each time, the filter was blamed first. The storyline states every revision next to the +was a velocity step in the synthetic world, a "consistent" control that fed the filter exact +sensors, a consistency verdict whose "true noise" left out the ground truth's own error, and an +S2 closure test whose threshold was fitted to the very defect it should have caught. Each time, +the filter or the test was trusted first. The storyline states every revision next to the claim it revises, because how a wrong conclusion was reached is as useful as the right one. The other lessons, in one line each: @@ -44,6 +50,10 @@ The other lessons, in one line each: times too small. Report NEES and NIS beside ATE, always. - **Measure before fixing; ship candidates default-off.** Several textbook fixes made things worse once they were measured end to end. +- **Know what time each sample stands for.** Holding an IMU sample over an interval it doesn't + represent skewed the IMU 2.5 ms against the camera in both backends, and it passed for + ground-truth error and over-confidence until a continuous truth exposed it + ([Aligning Sensor Samples](sample-alignment.mdx)). ## Vocabulary diff --git a/docs/first-principles/regression-suite.mdx b/docs/first-principles/regression-suite.mdx index 8ffddfa5..2aad5ba2 100644 --- a/docs/first-principles/regression-suite.mdx +++ b/docs/first-principles/regression-suite.mdx @@ -47,7 +47,8 @@ cortex test case that implements it (Catch2 case names, under `tests/`). | B3 | S1: SfM under realistic noise | The window builds at 1–2 px and rejects about 0.05 px of pseudo-parallax | "S1 SfM sweep: … 2 px of noise never builds" | characterization | | B4 | S1: known-answer scale with a lever arm | Exact data recovers the scale to rounding (now 0.78% off) | init-window known-answer fixture, 2% tolerance | weak | | B5 | S1: bootstrap pair selection | The window builds at 20 Hz where adjacent pairs fail | "S1 SfM bench: the alternatives on the same windows" | locked | -| B6 | S2: propagation preserves the gauge | Φ·N = N each step; the invariant Φ doesn't depend on the estimate | "R-IEKF Phase A: the invariant propagation Phi is state-independent"; the S2 higher-order-Φ sweep | locked | +| B6 | S2: propagation preserves the gauge | Φ·N = N each step, in both error states; the invariant Φ doesn't depend on the estimate; the body-frame Φ is the exact Jacobian of the mean it propagates (finite differences, 1e-8) | "R-IEKF Phase A: the invariant propagation Phi is state-independent"; "S2 bench sweep: the mean's own Jacobian leaks no yaw, a truncated Phi does"; "S2 interval: Phi is the finite-difference Jacobian of the RK4 mean" | locked (body-frame since ) | +| B6a | S2: the mean closes on a continuous truth | Integrating a fast spline's own noise-free IMU (1.3 rad/s roll and pitch) over a 0.55 s clone window stays within what the camera resolves (a tenth of 0.5 px: 1.09e-4 rad, 0.22 mm at 2 m) and converges at second order; holding one sample over the interval misses it 28× at first order | `gt_spline.cpp`: "S2 closure, the shipped S2 within what the camera resolves (#616)" and its companions | locked | | B7 | S2: process noise | Predicted covariance matches a Monte-Carlo within 1% per block, 20 m from the origin; cross-correlations within 0.06 | "… predicted covariance matches a Monte-Carlo far from the origin" | locked | | B8 | S2: propagation-only NEES at the shipped Q | About dof (6.4 against 6), monotone in the Q scale | the S2 probe | locked | | B9 | S3/S9: augmentation and marginalization | Exact block copy and principal submatrix; residual about 0 | the S3 and S9 benches | locked | @@ -93,7 +94,7 @@ cortex test case that implements it (Catch2 case names, under `tests/`). | E6 | Mixed-precision presets on real data | No divergence for any shipped stage-type preset; ATE and NEES per preset | `vio_pipeline` EuRoC runs | manual | | E7 | Sanitizers on a real replay | ASan and UBSan clean end to end | run by hand | **missing** in CI | | E8 | Latency | Real-time factor 1.0× or better (now 0.10×, p50 506 ms); synthetic budget 75 ms double, 50 ms float | `vio_latency.cpp` | gated, informational | -| E9 | **Consistency gate:** told the true noise, the filter improves | One truth (a B-spline through the ground truth) feeds the synthesized IMU, the ideal tracks (1 and 0.5 px, noise in normalized coordinates) and the score; seeds 1–3, all 11 sequences, the same shipped S6d gate for every backend: no run with R matched worse than 10× OpenVINS mono; matched R beats the default R on the median; median (smallest world-yaw σ ÷ yaw σ at the gauge anchor) at least 0.5 | `scripts/euroc-consistency.sh --backend msckf` or `riekf` (results in `scripts/euroc-consistency.json`); `gt_spline.cpp` locks the truth (S2 closure, noise densities); `pose_consistency.cpp` the NEES split | **failing**, manual. Median NIS 1.03 for both. MSCKF: fails all three (2 of 66 diverge; yaw σ ratio 0.107, the yaw leak); R-IEKF: fails 1 (3 of 66) and 2 at 1 px (S2's sample-hold skew, ), passes 3 (0.985). Conditions 1 and 2 still move with the noise draw at three seeds (, ) | +| E9 | **Consistency gate:** told the true noise, the filter improves | One truth (a B-spline through the ground truth) feeds the synthesized IMU, the ideal tracks (1 and 0.5 px, noise in normalized coordinates) and the score; seeds 1–3, all 11 sequences, the same shipped S6d gate for every backend: no run with R matched worse than 10× OpenVINS mono; matched R beats the default R on the median; median (smallest world-yaw σ ÷ yaw σ at the gauge anchor) at least 0.5 | `scripts/euroc-consistency.sh --backend msckf` or `riekf` (results in `scripts/euroc-consistency.json`); `gt_spline.cpp` locks the truth (S2 closure, noise densities); `pose_consistency.cpp` the NEES split | **failing**, manual. NIS 1.00 on every sequence for both since . MSCKF: fails 1 (2 of 66 diverge) and 3 (yaw σ ratio 0.179, the S6 yaw leak), passes 2; R-IEKF: fails 1 only (the same 2 runs), passes 2 and 3 (0.984). Both divergences start from the shared initializer's seed (). Conditions 1 and 2 still move with the noise draw at three seeds (, ) | ## F. Engineering hygiene diff --git a/docs/first-principles/sample-alignment.mdx b/docs/first-principles/sample-alignment.mdx new file mode 100644 index 00000000..c206204c --- /dev/null +++ b/docs/first-principles/sample-alignment.mdx @@ -0,0 +1,220 @@ +--- +title: Aligning Sensor Samples +description: How integrating a sensor sample over an interval it does not represent becomes an unmodelled time offset, why that error hides as ground-truth error, over-confidence or a calibration problem, how cortex was caught by it for five months (#616), and a checklist for any sensor-fusion pipeline. +--- + +import { Aside } from '@astrojs/starlight/components'; +import Issue from '@site/components/Issue.astro'; + +A sensor-fusion pipeline combines streams that sample the world at different times: an IMU at +200 Hz, a camera at 20 Hz, perhaps a GNSS receiver at 10 Hz. **Every sample stands for a time.** +A computation that uses a sample at any other time adds a skew between the streams. That skew is +an unmodelled time offset, and its error grows with how fast the signal changes. + +cortex's IMU propagation did exactly this from its first version until +. Both VIO backends held one IMU sample constant over each 5 ms interval, and +they held different samples. This page explains the failure mode, how it stayed hidden, how it +was caught, and what to do in any pipeline that integrates one sensor between the samples of +another. + +## What a sample stands for + +Before integrating a stream, write down the time each sample represents. It is not always the +timestamp. + +| Sample | What it stands for | Use it as | +|---|---|---| +| An instantaneous rate (most MEMS gyros as delivered, a synthetic IMU) | the rate at its timestamp | one end of the interval it bounds; interpolate between samples | +| An increment, Δθ or Δv (many tactical IMUs) | the integral over the interval that ends at its timestamp | that interval's increment, with a coning correction; never as a rate held over the next interval | +| A filtered or decimated sample (the ADIS16448 on EuRoC's rig) | the rate a filter group delay earlier | an instantaneous rate shifted by that delay, which calibration has to find | +| A global-shutter image | the middle of the exposure, offset from the trigger stamp | a measurement at that time, through the camera–IMU time offset t_d | +| A rolling-shutter image | a different time on each row | per-row times, or a model of the readout | + +When two devices are synchronized in hardware, as on EuRoC, the clock is right but the meaning of +each sample can still be off by a fraction of its interval. + +## How a hold becomes a skew + +An IMU interval [t₍ₖ₋₁₎, tₖ] of length Δt has a sample at each end. Propagation has to choose +an input over the interval, and each choice implies a time: + +| Input over the interval | Equivalent to | Who did it in cortex | +|---|---|---| +| The end sample, held | the IMU read Δt/2 **early** | `MsckfBackend`, to | +| The start sample, held | the IMU read Δt/2 **late** | `InvariantVioBackend`, to | +| The line between the two samples | no skew to first order | both backends since ; OpenVINS | + +At 200 Hz, Δt/2 is 2.5 ms. Holding the end sample integrates ∫ω dt as ω(t_k)·Δt, which is +ω(t_mid)·Δt + ω̇·Δt²/2. Summed over a window, the rotation error is the change in rate over the +window times the skew: **δθ ≈ Δω · Δt/2**. EuRoC's fast sequences roll and pitch at up to +1.3 rad/s (the 95th percentile). On a test flight at that rate the rate changes by about 1.2 rad/s +across a 0.55 s clone window, and the error reaches 3.1 mrad: 1.4 px at cam0's focal length, 28× +what the camera resolves. The camera then measures the attitude far more precisely than +propagation delivers it. + +The integrator adds a second, separate error. A single Euler step rotates the specific force with +the attitude at the *start* of the interval. With the body turning at 1.3 rad/s, that rotation is +2.5 ms stale on average too, and it shows up as a first-order drift in velocity and position. +Centring the input doesn't fix it; integrating along the interval does. + +## Why it hides + +A half-sample skew rarely looks like itself. In cortex it appeared as four other things: + +- **Ground-truth error.** EuRoC's Vicon ground truth disagrees with the gyro by 0.25–0.45 px over + 0.25 s in the Vicon rooms. The skew sits at the same size, so a consistency test against the real + IMU can't tell the two apart (). +- **A calibration problem.** A constant skew between IMU and camera is exactly what the + camera–IMU time offset t_d models. An online t_d estimator () would have learned + a backend-specific 2.5 ms, and the defect would have looked calibrated away. +- **Over-confidence.** NIS was 1.02–1.04 on the slow Machine Hall sequences but 1.1–1.9 on V1_03, + V2_02 and V2_03, which turn 3.5–4× faster. The over-confidence investigation of + had a dozen candidates, and a speed-dependent NIS fits several of them. +- **Nothing at all.** The error scales with angular *acceleration*. The synthetic test world only + yaws, gently, so it never excited the error, and every unit test of S2 passed. + + + +## How cortex was caught + +| When | What happened | Issue | +|---|---|---| +| 2026-05 | The MSCKF backend was built in a day from the standard error-state EKF derivation. The one-sample Euler step is the first thing those texts derive. The driver passes each interval the sample that ends it. | | +| 2026-06 | The S0–S10 stage contracts state that OpenVINS integrates "with a zeroth-order hold or RK4". The S2 probe went after the covariance suspects of #212 (the diagonal Q_d, the structure of Φ) and cleared S2. The mean was never checked against a continuous truth at the real rate. | , | +| 2026-06 | The same S2 probe measured the mean against ground-truth injection and recorded "integrator order O(Δt): the error halves with Δt, 59 → 30 → 15 → 7 mm". First order is the defect itself. It went into a metrics table with no bound, so nothing failed. | | +| 2026-10 | "A more accurate Φ makes the yaw leak worse" (#471) came to read as "a higher-order S2 doesn't help". The lesson was about the covariance, not the mean. | | +| 2026-11 | The R-IEKF backend got its own driver, which holds the *previous* sample. The two backends now disagreed by 5 ms. | | +| 2026-11 | The consistency gate was rebuilt on one smooth truth: a B-spline through the ground truth feeds the synthesized IMU, the ideal tracks and the score, so R and Q are exact. NIS was 1.00 on the slow sequences and 1.1–1.9 on the fast ones. | | +| 2026-11 | Probes changed one thing at a time (below). Only centring the input removed the excess. | | + +The probes ran on the MSCKF on #613's exact truth, at 0.5 px with matched R: + +| S2 variant | V2_03 4-DoF ATE | V2_03 NIS | MH_01 NIS | +|---|---|---|---| +| shipped before #616: end sample held, start attitude | 0.19–0.24 m | 1.77–1.80 | 1.03 | +| specific force rotated at the midpoint attitude | 0.18–0.21 m | 1.76–1.80 | 1.03 | +| 5 substeps on the same held sample | 0.19–0.23 m | 1.79–1.80 | — | +| **the average of the interval's two samples** | **0.013–0.018 m** | **1.00** | **1.00** | +| IMU synthesized at 1 kHz, shipped S2 | 0.03 m | 1.02 | — | + +Substeps didn't help, so the integrator's order wasn't the cause. The attitude the specific force +is rotated with didn't help either. The cause was the input: a sample held over an interval it +doesn't represent. + +### The test that blessed the defect + +While building #613's gate, I wrote an S2 closure test: integrate the spline's own noise-free IMU +and compare with the spline. It measured S2's error, saw that the error halved when the step +halved, and called it "discretization, not a disagreement". I set its threshold just above what +S2 achieved. The test was reporting a first-order defect at 28× what the camera resolves, and I +signed it off, because I took its tolerance from the code's current residual rather than from what +the next stage has to resolve. + +This was the fifth result in cortex's VIO work that came from the test setup rather than the +estimator ([Chapter 5](storyline.mdx#chapter-5-when-the-test-lies)). It produced two rules in the +[paper-to-pipeline method](../methodology/overview.mdx) (): + +- **Every option the paper offers is a recorded decision with a reason.** "Zeroth-order hold or + RK4" named the choice, but nobody made it, and two drivers made it differently by default. +- **Every tolerance comes from what the consuming stage must resolve.** The closure bound is a + tenth of the 0.5 px track noise over a clone window: 1.09e-4 rad, and 0.22 mm at a 2 m depth. + +## What fixed it + +Both backends now integrate each interval from both of its samples, with one convention stated in +`sdk/include/branes/sdk/msckf/stages/s2_propagation.hpp`: + +- **The input** is the line between the two samples. +- **The mean** is RK4 on that input (`rk4_strapdown` in `propagator.hpp`). With a rate linear in + time, the attitude has a closed form: the rate's integral plus the coning term. Velocity and + position are then quadratures, evaluated at the start, middle and end of the interval. +- **The covariance** propagates with Φ, the exact Jacobian of that mean. The first-order Φ had + been chosen (#471) because it agreed best with the old Euler mean. Against the RK4 mean it leaked + 7× more yaw per step, so Φ had to follow the mean. Now it preserves the unobservable directions + to roundoff. +- **An image between two samples:** propagate to the image time holding the last sample, since the + next one hasn't arrived. When it does, integrate the rest of the interval from the rate + interpolated at the image time. The residual skew lives only on that partial interval, and is + zero on EuRoC, where image and IMU stamps coincide. + +The closure test (`tests/tools/gt_spline.cpp`) separates the two errors, on a flight that rolls +and pitches at about 1.3 rad/s, against the camera-derived bound: + +| Scheme | Rotation ÷ bound (200 Hz) | Position ÷ bound | Order | +|---|---|---|---| +| End sample held, one Euler step (MSCKF before #616) | 28.6 | 41 | first | +| Start sample held, one Euler step (R-IEKF before #616) | 28.0 | 6.9 | first | +| The two samples' average, one Euler step | 0.28 | 24 | first, in position | +| The two samples' average, RK4 | 0.28 | 0.16 | second | +| **The line between the samples, RK4 (shipped)** | **0.27** | **0.10** | **second** | + +Centring the input fixes the rotation. Integrating along the interval fixes the position. The +order is second, not fourth: RK4 integrates the line exactly enough, but the line follows the true +rate only to second order between samples, and that is the error that remains, a quarter of the +bound. + +## What it changed + +On the consistency gate's spline truth, where R and Q are exact (recorded at f1551cc, +[EuRoC Baseline](../benchmarks/euroc-baseline.mdx#results-with-one-truth-and-s2-integrating-both-samples-616)): + +| | MSCKF before → after | R-IEKF before → after | +|---|---|---| +| NIS on V1_03, V2_02, V2_03 (0.5 px) | 1.23–1.80 → **1.00** | 1.30–1.87 → **1.00** | +| V2_03 window ATE ÷ OpenVINS mono (0.5 px) | 0.9× → **0.1×** | 1.4× → **0.1×** | +| Matched R beats the default R (condition 2) | fail → **pass** | fail at 1 px → **pass** | +| S2's share of the spurious yaw information, real EuRoC MH_05 / V2_03 (observability ledger, #480) | 0.9% / 5.3% → **0.0%** | — (no leak by construction) | + +On the real EuRoC IMU and the real KLT tracks the gain is much smaller: the median +window ATE goes from 2.83× to 2.57× OpenVINS mono for the MSCKF and from 2.62× to +2.44× for the R-IEKF, and the R-IEKF's V1_03 halves. **The real-data NIS doesn't +move:** it stays at 6.7–15.5 on the same fast sequences where the spline truth went +to 1.00. So the half-sample skew was real and is gone, but it isn't what makes the +filters over-confident on real data. That remains open, and the candidates are the +ones this page warns about: the ADIS16448's own filter delay, the camera–IMU time +offset, and the front end's outliers. The last item of the checklist below is not +hypothetical. + +## A checklist for any fusion pipeline + +1. **Write down what time each sample stands for**: instantaneous, an increment over the past + interval, delayed by a filter, the middle of an exposure. Put it in the stage contract. +2. **State one integration convention, and use it everywhere.** Say what input the integrator sees + over an interval, and what happens to an event between two samples. Two implementations of the + same stage must not choose differently by default. +3. **Interpolate between samples; never hold one over an interval it doesn't represent.** An + increment sample is already its interval's integral: use it as one. +4. **Integrate along the interval.** Rotate the specific force with the attitude at each point of + the interval, not the attitude at its start. +5. **Re-derive the covariance from the new mean.** A transition matrix chosen because it agreed + with the old integrator disagrees with the new one, and the disagreement is an information + leak. Take Φ as the exact Jacobian of the mean you ship, and check it against finite differences. +6. **Test against a continuous truth at the real rate and the real dynamics.** Integrate a smooth + trajectory's own noise-free samples and close on the trajectory. Take the bound from what the + next measurement resolves, check the convergence order, and pin the alternative you rejected as + failing. +7. **Excite the error.** It scales with angular acceleration times the skew, so test on the + fastest segments, not only on a gentle synthetic world. +8. **Split the consistency metrics by motion speed.** An NIS that rises with speed points at + timing before it points at noise. +9. **Sweep the skew.** Shift one stream by fractions of its interval and watch NIS. The minimum is + the effective offset the pipeline is running with. +10. **Fix the convention before estimating t_d.** An online time-offset state absorbs any constant + skew, including a software one, and then the calibration carries the bug. +11. **Don't assume synthetic gains carry over.** A real IMU's internal filtering adds its own + delay, and the dataset's calibration has its own time shift; measure the change on real data. + +## Where it lives + +| What | Where | +|---|---| +| The convention | `sdk/include/branes/sdk/msckf/stages/s2_propagation.hpp` | +| `ImuInterval`, `interpolate_sample`, `rk4_strapdown` | `sdk/include/branes/sdk/msckf/propagator.hpp` | +| Both drivers | `MsckfBackend::process_imu` / `process_camera`, `InvariantVioBackend::process_imu` / `propagate_to` | +| The closure tests | `tests/tools/gt_spline.cpp` | +| The decisions and their reasons | the `propagate` node of the [OpenVINS MSCKF spec](../methodology/openvins-msckf.mdx) | +| The stage page | [S2 — IMU Propagation](../pipeline/stages/s2-imu-propagation.mdx) | diff --git a/docs/first-principles/signatures.mdx b/docs/first-principles/signatures.mdx index d0e8f1fe..01534a76 100644 --- a/docs/first-principles/signatures.mdx +++ b/docs/first-principles/signatures.mdx @@ -1,6 +1,6 @@ --- title: Diagnostic Signatures and Pitfalls -description: A symptom-to-cause table for VIO failures, each with the measurement that confirms it, and twenty pitfalls distilled from getting cortex's pipeline to work. +description: A symptom-to-cause table for VIO failures, each with the measurement that confirms it, and twenty-two pitfalls distilled from getting cortex's pipeline to work. --- import Issue from '@site/components/Issue.astro'; @@ -32,6 +32,7 @@ from its look-alikes; the [storyline](storyline.mdx) has the full account. | Over-confident only on a slow start | Bad triangulation of low-parallax windows | Oracle-landmark substitution; per-track depth error against parallax | | | A "consistent" control with NEES ≪ dof | Noise-free inputs: the control measures nothing | Inject noise matched to the filter's model | | | Position variance under-predicted far from the origin | Process noise not mapped through the [p̂]× lever arm (invariant error) | Monte-Carlo of the propagated error about 20 m from the origin | | +| NIS ≈ 1 on slow sequences, 1.1–1.9 on fast ones, with R and Q exact | An IMU sample held over an interval it doesn't represent: a half-sample IMU–camera skew | Close S2 on a continuous truth at the real rate; average the interval's two samples and watch NIS return to 1.00 | | | Most new tracks die on their first frame | Detector margin narrower than the KLT pyramid footprint | Fraction of tracks lost on their first frame | | ## Pitfalls @@ -50,9 +51,10 @@ from its look-alikes; the [storyline](storyline.mdx) has the full account. innovations. 7. **A body-frame EKF leaks yaw.** The leak comes from the update moving the linearization point under a basis the filter's Jacobians keep (95–99% on EuRoC); S2's Φ, linearized against the mean's - Euler step, adds the rest, and - S3 and S9 add none (). A more accurate Φ makes it worse; the cure is a different - error state. + Euler step, added the rest until made it the RK4 mean's exact Jacobian (now + 0.0%), and S3 and S9 add none (). Φ must be the Jacobian of the mean it + propagates: a better truncation of the continuous Φ leaked more against the Euler mean. The cure + for S6's share is a different error state. 8. **Partial FEJ is worse than none.** 9. **A fix that cures the symptom can worsen the disease.** Inflating R and gating on parallax both did. @@ -78,3 +80,9 @@ from its look-alikes; the [storyline](storyline.mdx) has the full account. 20. **Measure first, and ship candidates default-off.** A/B test on a slow (V1_01), a medium (MH_05) and an aggressive (V2_03) sequence: slow sequences expose gauge problems, aggressive ones expose R. +21. **Know what time each sample stands for, and integrate between samples.** Holding one IMU sample + over the interval it ends or starts is a half-sample time offset against the camera. It hid for + five months as ground-truth error and over-confidence + ([Aligning Sensor Samples](sample-alignment.mdx), ). +22. **Take every tolerance from what the next stage resolves, never from the current residual.** A + closure test fitted to what S2 achieved blessed the skew in item 21. diff --git a/docs/first-principles/storyline.mdx b/docs/first-principles/storyline.mdx index 320a0c04..a2284c56 100644 --- a/docs/first-principles/storyline.mdx +++ b/docs/first-principles/storyline.mdx @@ -204,7 +204,7 @@ What we learned: ## Chapter 5. When the test lies -Three times, a "filter defect" turned out to be a defect in the test. +Four times, a "filter defect" turned out to be a defect in the test. **The compiler was not the cause** (). The R-IEKF end-to-end test passed on GCC (3.44 m) and failed on Clang (58.48 m), with each compiler deterministic and the sanitizers clean. @@ -280,12 +280,41 @@ there. So the cause is the textbook EKF inconsistency. An update corrects the mean, the next Jacobians are evaluated at the corrected mean, and the basis they annihilate is no longer the one the earlier Jacobians carried. S2's share, Φ linearized against the mean's Euler step (a higher-order Φ leaks more, not -less), grows with rotation rate (V2_03); it isn't the driver. Three remedies act on that mechanism: +less), grows with rotation rate (V2_03); it isn't the driver. Since Φ is the exact +Jacobian of the RK4 mean, and S2's share is 0.0% on MH_05 and V2_03. Three remedies act on that mechanism: - First-estimates Jacobians, which were tried and refuted on ATE (, ). - Projecting each H off the carried basis, the observability-constrained EKF, which hasn't been tried. - An error state whose basis doesn't depend on the estimate: the R-IEKF, the chosen path (). +**The truth wasn't true, and then the test blessed the defect** (, +, ). The consistency gate feeds a filter ideal tracks made from +the ground truth and tells it the true noise. Its first verdict was that the R-IEKF passed and the +MSCKF failed. + +- **The truth:** with the real IMU, the "true noise" wasn't true. EuRoC's ground truth disagrees + with the gyro by 0.25–0.45 px over 0.25 s in the Vicon rooms, so matched R and Q were wrong by an + unmodelled, time-correlated error, and the χ² gate starved both filters. +- **The fix:** one truth for everything (). A B-spline through the ground truth + feeds the synthesized IMU, the tracks and the score. Median NIS became 1.03 for both backends. +- **What that exposed:** NIS was 1.00 on the slow sequences and 1.1–1.9 on the three fast ones. + Both backends held one IMU sample over each 5 ms interval, the MSCKF the end sample and the + R-IEKF the start sample: a ±2.5 ms IMU–camera skew. Averaging the two samples gave NIS 1.00 and + cut V2_03's ATE 10–15× (). +- **The test that blessed it:** the S2 closure test written for #613 measured the first-order + error, and its threshold was set just above it. Its bound now comes from what the camera resolves, + a tenth of 0.5 px over a clone window. The S2 probe had also recorded the first-order error in + June, as a metric with no bound. + +- **The fix and its limit:** both backends now integrate each interval from both samples by + RK4, with Φ that mean's exact Jacobian. On the spline truth NIS is 1.00 on every sequence and + the fast sequences' ATE drops 4–24×. On the real IMU and tracks the median ATE improves only + 7–9%, and the real NIS doesn't move (6.7–15.5 on the fast sequences), so the skew isn't what + makes the filters over-confident on real data. + +The whole account, with a checklist for any fusion pipeline, is +[Aligning Sensor Samples](sample-alignment.mdx). + **Smaller ones:** - The synthetic camera's tangential distortion p2 was 1000× EuRoC's (): a diff --git a/docs/methodology/euroc-consistency-gate.mdx b/docs/methodology/euroc-consistency-gate.mdx index 8b97a0a2..1eaf9561 100644 --- a/docs/methodology/euroc-consistency-gate.mdx +++ b/docs/methodology/euroc-consistency-gate.mdx @@ -44,8 +44,8 @@ Each has the shape *(state, inputs, params) → (state′, diagnostics)*. **synth_imu** (G2, Synthesize the IMU) - *Pre:* the IMU timestamps lie inside the spline's span -- *Post:* noise-free and integrated from both samples of each interval, it closes on the spline's rotation within a tenth of 0.5 px over a clone window; the noise statistics match the densities -- *Test:* `tests/tools/gt_spline.cpp` “gt spline: S2 closure, averaging each interval's two samples, holds the rotation bound…” +- *Post:* noise-free and integrated by the shipped S2 from both samples of each interval, it closes on the spline within a tenth of 0.5 px over a clone window; the noise statistics match the densities +- *Test:* `tests/tools/gt_spline.cpp` “gt spline: S2 closure, the shipped S2 within what the camera resolves (#616)…” - *Test:* `tests/tools/gt_spline.cpp` “gt spline: the synthesized noise has the configured densities…” **synth_tracks** (G3, Synthesize the tracks) @@ -81,7 +81,7 @@ Each has the shape *(state, inputs, params) → (state′, diagnostics)*. | **NoiseModel** | input | the gyro and accelerometer noise densities and bias random walks (VioConfig's), the track pixel σ S, and the seed | `sdk/include/branes/sdk/vio_backend.hpp` `VioConfig` | ✗ the filter's Q and the synthesis use the same densities; matched R equals S | | **CameraCalib** | input | EuRoC cam0 intrinsics, distortion and camera↔IMU extrinsic | `tools/include/branes/tools/euroc_cam0.hpp` `euroc_cam0` | ✓ R_imu_cam ∈ SO(3) | | **TruthSpline** | data | a uniform cumulative cubic B-spline on SO(3) × ℝ³ with control poses at ground-truth samples every 0.05 s; pose, angular velocity and acceleration at any time inside its span | `tools/include/branes/tools/gt_spline.hpp` `PoseSpline` | ✓ C² in position and rotation; ω and a match finite differences of its pose; ✓ stays within a stated bound of the ground truth (it approximates, it is not fitted) | -| **ImuStream** | data | at the real IMU timestamps, ω_m = ω + b_g + n_g, a_m = Rᵀ(p̈ − g) + b_a + n_a, with b_g, b_a random walks from zero | `tools/include/branes/tools/gt_spline.hpp` `synthesize_imu` | ✓ noise-free, integrated from both samples of each interval, it closes on the spline's rotation within a tenth of 0.5 px over a clone window (rotation only: the shipped S2 step's position still drifts at first order, and the shipped end-sample hold fails, see s2_closure, #616); ✓ the noise matches NoiseModel's densities (per-axis variance, per-sample whiteness) | +| **ImuStream** | data | at the real IMU timestamps, ω_m = ω + b_g + n_g, a_m = Rᵀ(p̈ − g) + b_a + n_a, with b_g, b_a random walks from zero | `tools/include/branes/tools/gt_spline.hpp` `synthesize_imu` | ✓ noise-free, integrated by the shipped S2 from both samples of each interval, it closes on the spline within a tenth of 0.5 px over a clone window, in rotation and position (see s2_closure, #616); ✓ the noise matches NoiseModel's densities (per-axis variance, per-sample whiteness) | | **TrackStream** | data | per image time, pixel observations of fixed landmarks seen from the truth's camera pose, plus N(0, S²) per axis | `tools/include/branes/tools/ideal_tracks.hpp` `IdealTracks` | ✓ noise-free, each observation is the exact projection of its landmark; ✓ the added noise is N(0, (S/fx)²) per axis in normalized coordinates and nothing else | | **Estimate** | data | the backend's trajectory (TUM) and per-frame pose covariance in its own error convention | `tools/src/vio_pipeline.cpp` `write_tum` | ✗ finite; one pose per tracked frame | | **Score** | data | window 4-DoF ATE ÷ OpenVINS mono, the pose NEES split, the world-yaw σ over the run, NIS | `tools/include/branes/tools/pose_consistency.hpp` `PoseConsistencySummary` | ✗ scored against TruthSpline, not the raw ground truth; ✗ a run with no trajectory counts as diverged; one that cannot be scored is unavailable | @@ -108,15 +108,15 @@ on grid complete: | Invariant | Check | Status | |---|---|---| -| the median normalized NIS over the matched runs is ≈ 1 (the bench's noise model is true) | `scripts/euroc-consistency.sh` | holds — 1.03 for both backends at c6b87ec (1.65–4.3 with the real IMU and the raw ground truth) | -| every sequence's matched runs have normalized NIS within the χ² band for their dof | `scripts/euroc-consistency.sh` | failing — 1.01–1.04 on the Machine Hall and V1_01, V2_01; 1.1–1.9 on V1_02, V1_03, V2_02, V2_03 for both backends: S2's sample-hold skew, an estimator defect (#616) | -| the shipped S2, fed the synthesized IMU noise-free, closes on the truth within what the camera resolves (a tenth of 0.5 px over a 0.55 s clone window: 1.1e-4 rad, 0.22 mm at 2 m), at second order (bound 1.09e-4 rad rotation, 0.22 mm position at 2 m, over a 0.55 s clone window, from what **run_filter** resolves: the backend's update resolves a clone pose only to the track noise, 0.5 px at fx = 458.654 (1.09e-3 rad); propagation error must stay a tenth of that over the clone window so it never competes with the measurement, and the same angle at a 2 m landmark depth is 0.22 mm) | `tests/tools/gt_spline.cpp` “gt spline: S2 closure, the shipped S2 within what the camera resolves…” | failing — the shipped S2 holds each interval's end sample: 3.1e-3 rad (28× the bound) at first order on a 1.3 rad/s roll/pitch flight; averaging the two samples meets the rotation bound at second order, the position still needs the integrator's fix (#616). The test is a [!shouldfail] tripwire until #616 | +| the median normalized NIS over the matched runs is ≈ 1 (the bench's noise model is true) | `scripts/euroc-consistency.sh` | holds — 1.00 for both backends at f1551cc (1.03 at c6b87ec before #616; 1.65–4.3 with the real IMU and the raw ground truth) | +| every sequence's matched runs have normalized NIS within the χ² band for their dof | `scripts/euroc-consistency.sh` | holds — 1.00 on every sequence for both backends at f1551cc. Before #616 it was 1.1–1.9 on V1_02, V1_03, V2_02, V2_03: S2's sample-hold skew, an estimator defect | +| the shipped S2, fed the synthesized IMU noise-free, closes on the truth within what the camera resolves (a tenth of 0.5 px over a 0.55 s clone window: 1.1e-4 rad, 0.22 mm at 2 m), at second order (bound 1.09e-4 rad rotation, 0.22 mm position at 2 m, over a 0.55 s clone window, from what **run_filter** resolves: the backend's update resolves a clone pose only to the track noise, 0.5 px at fx = 458.654 (1.09e-3 rad); propagation error must stay a tenth of that over the clone window so it never competes with the measurement, and the same angle at a 2 m landmark depth is 0.22 mm) | `tests/tools/gt_spline.cpp` “gt spline: S2 closure, the shipped S2 within what the camera resolves…” | holds — since #616, S2 integrates each interval from both samples by RK4: 0.27 of the rotation bound and 0.10 of the position bound at 200 Hz on a 1.3 rad/s roll/pitch flight, converging at second order. Before it, the end-sample hold missed the rotation bound 28× at first order; the companion tests pin that, and that centring the input fixes the rotation while the integrator fixes the position | | every backend runs with the same S6d gate line | `scripts/euroc-consistency.sh` | holds — since #608 both use the χ² quantile; before it the R-IEKF used 16 per dof | -| each condition's result changes with the estimator, not with the gate's line | — | holds — each remaining failure rerun with the gate at 5 per dof: one divergence per backend vanishes (with a true noise model, the estimator's transient over-confidence made the 5% gate fatal), the others persist; condition 2's fast-sequence loss is #616's S2 skew. Conditions 1 and 2 still depend on the noise draw at three seeds (#613) | +| each condition's result changes with the estimator, not with the gate's line | — | holds — at c6b87ec each remaining failure was rerun with the gate at 5 per dof: one divergence per backend vanished, the others persisted, and condition 2's fast-sequence loss was #616's S2 skew. At f1551cc condition 2 passes for both; both backends diverge on the same two noise draws, from the shared initializer's seed (#609); the MSCKF's condition 3 is S6's yaw leak. Conditions 1 and 2 still depend on the noise draw at three seeds (#613) | ## Gap report -What `pipeline_graph.py gaps` reports for this spec (14 items): +What `pipeline_graph.py gaps` reports for this spec (12 items): - edge 'NoiseModel': no validator for "the filter's Q and the synthesis use the same densities; matched R equals S" - edge 'Estimate': no validator for "finite; one pose per tracked frame" @@ -129,6 +129,4 @@ What `pipeline_graph.py gaps` reports for this spec (14 items): - node 'score': no stage bench - node 'verdict': no stage bench - node 'verdict': no unit tests -- system invariant 'nis_band_per_sequence': failing — 1.01–1.04 on the Machine Hall and V1_01, V2_01; 1.1–1.9 on V1_02, V1_03, V2_02, V2_03 for both backends: S2's sample-hold skew, an estimator defect (#616) -- system invariant 's2_closure': failing — the shipped S2 holds each interval's end sample: 3.1e-3 rad (28× the bound) at first order on a 1.3 rad/s roll/pitch flight; averaging the two samples meets the rotation bound at second order, the position still needs the integrator's fix (#616). The test is a [!shouldfail] tripwire until #616 - system invariant 'verdict_attributable': no check diff --git a/docs/methodology/openvins-msckf.mdx b/docs/methodology/openvins-msckf.mdx index c69d6078..49a6bc5a 100644 --- a/docs/methodology/openvins-msckf.mdx +++ b/docs/methodology/openvins-msckf.mdx @@ -28,7 +28,7 @@ Each has the shape *(state, inputs, params) → (state′, diagnostics)*. | S4 | **track** | image → feature tracks (FAST + KLT), and the tracks that ended | NormalizedObs, TrackDB → TrackDB | geneva2020, visual tracking | implemented | `sdk/include/branes/sdk/msckf/stages/s4_frontend.hpp` | `tests/tools/stage_bench_s4_s5.cpp` “S4 bench: every fixture passes for every variant…” | | S1a | **initialize** | an IMU window at rest (static), or a moving window of frames (dynamic VI alignment) → the nav seed | ImuSample, TrackDB → NavSeed | geneva2020, initialization | implemented | `sdk/include/branes/sdk/imu_init.hpp` | `tests/tools/stage_bench_s0_s1.cpp` “S1 bench: static paths pass every fixture…” | | S1b | **seed_state** | the nav seed → a fresh filter state, its mean and its prior P₀ in the filter's error convention | NavSeed → FilterState | geneva2020, initialization | implemented | `sdk/include/branes/sdk/msckf/stages/s1_initialization.hpp` | `tests/tools/stage_bench_s0_s1.cpp` “S1 bench: static paths pass every fixture…” | -| S2 | **propagate** | one IMU sample → the propagated mean, P ← Φ P Φᵀ + Q_d | FilterState, ImuSample → FilterState | li2013, §III, the IMU model; Φ at first estimates | implemented | `sdk/include/branes/sdk/msckf/stages/s2_propagation.hpp` | `tests/tools/stage_bench_s2_s3.cpp` “S2 bench: every fixture passes for every variant…” | +| S2 | **propagate** | one IMU interval (the samples at both of its ends) → the propagated mean, P ← Φ P Φᵀ + Q_d | FilterState, ImuSample → FilterState | li2013, §III, the IMU model; Φ at first estimates | implemented | `sdk/include/branes/sdk/msckf/stages/s2_propagation.hpp` | `tests/tools/stage_bench_s2_s3.cpp` “S2 bench: every fixture passes for every variant…” | | S3 | **augment** | the state → the state plus a clone of the pose at the image time, P′ = G P Gᵀ | FilterState → FilterState | mourikis2007, §III-B, stochastic cloning | implemented | `sdk/include/branes/sdk/msckf/stages/s3_augmentation.hpp` | `tests/tools/stage_bench_s2_s3.cpp` “S3 bench: every fixture passes for every variant…” | | S5 | **triangulate** | an ended track and the clone window → the feature position | TrackDB, FilterState → Landmark | mourikis2007, §III-C, multi-view least squares, then Gauss-Newton | implemented | `sdk/include/branes/sdk/msckf/stages/s5_triangulation.hpp` | `tests/tools/stage_bench_s4_s5.cpp` “S5 bench: every fixture passes for every variant…” | | S6 | **msckf_update** | track + landmark → Jacobians H_f, H_x (S6a) → left null-space projection (S6b) → QR compression (S6c) → χ² gate (S6d) → EKF update and retraction (S6e) | FilterState, TrackDB, Landmark → FilterState, UpdateDiag | mourikis2007, §III-C–D; H at first estimates (li2013) | implemented | `sdk/include/branes/sdk/msckf/stages/s6_msckf_update.hpp` | `tests/tools/stage_bench_s6.cpp` “S6 benches: every fixture passes for every variant…” | @@ -75,12 +75,12 @@ Each has the shape *(state, inputs, params) → (state′, diagnostics)*. - *Post:* R ∈ SO(3); P symmetric PSD; Φ preserves the 4-D unobservable subspace - *Test:* `tests/sdk/msckf_stages.cpp` “S2_propagation and S3_augmentation…” - *Precision:* T2 (msckf/stage_types.hpp) — posit<32,2> passes the bench; P must be written in double on EuRoC (#532) -- *Decision — the IMU input over each interval:* the sample that ends it, held constant (#186) over the previous sample held, both samples, interpolated and integrated by RK4 (geneva2020). **no reason recorded** -- *Decision — the mean's integrator:* one Euler step, the specific force rotated with the attitude at the start of the interval over midpoint, RK4. **no reason recorded** -- *Decision — the transition Φ:* first order, I + A·dt over closed form, a higher-order series. #471: a higher-order Φ leaks more yaw in the body-frame error state +- *Decision — the IMU input over each interval:* ω and a interpolated linearly between the samples at t_{k−1} and t_k (#616) over the sample that ends the interval, held (#186 to #616), the sample that starts it, held. a sample is the rate at its timestamp: holding either end over the interval integrates the IMU half a sample (2.5 ms) early or late against the camera, NIS 1.1–1.9 on the fast sequences (#616, docs/first-principles/sample-alignment.mdx) +- *Decision — the mean's integrator:* RK4 on the interpolated input: the attitude in closed form (the rate's integral plus the coning term), velocity and position by the RK4 quadrature (geneva2020) over one Euler step on a held sample (#186 to #616), one Euler step on the two samples' average, RK4 on the two samples' average, held. measured against the S2 closure bound (a tenth of 0.5 px over a clone window, tests/tools/gt_spline.cpp) on a 1.3 rad/s flight: an Euler step on the average meets it in rotation but misses position 24×, because it rotates the specific force with the start attitude; RK4 on the average or on the line meets both at second order (0.16 and 0.10 of the position bound). The line is OpenVINS's choice and the closer of the two +- *Decision — an image time between two IMU samples:* the last sample held up to the image time; the rest of the interval integrated from the rate interpolated at the image time once the next sample arrives over hold the frame until the next sample arrives, then interpolate exactly (one IMU sample of latency). no added latency; the residual skew lives only on the partial interval before the image, zero on EuRoC, whose image and IMU stamps coincide (#616) +- *Decision — the transition Φ:* the exact Jacobian of the shipped RK4 mean (#616) over first order, I + A·dt (#186 to #616), I + A·dt at the interval's mean input, a higher-order series of A·dt. a yaw leak is Φ disagreeing with the mean it propagates. #471 found the first-order Φ the least leaky because it sat closest to the one-sample Euler mean; against the RK4 mean it leaks 7× more than before (4.4e-6 per step at 2.5 ms, the S2 sweep). The RK4 mean has the preintegration form, which commutes with a rotation about gravity, so its exact Jacobian preserves the unobservable subspace to roundoff (tests/sdk/imu_interval.cpp, the S2 bench) - *Decision — the discrete process noise Q_d:* diagonal, σ²·dt per state over G·Q·Gᵀ with the θ–v–p correlations (geneva2020). #212's S2 probe found the full Q_d did not change the over-confidence (docs/pipeline/stages/s2-imu-propagation.mdx) - *Deviates from the paper:* no first-estimates Jacobians on Φ (FEJ, #606) -- *Deviates from the paper:* the sample that ends each interval is held over it: a 2.5 ms IMU–camera skew, NIS 1.1–1.9 on the fast sequences (#616) **augment** (S3, State augmentation) @@ -179,12 +179,12 @@ on image (20 Hz): | NIS ≈ dim(measurement) per update | `tests/sdk/consistency.cpp` “NEES/NIS flag over- and under-confidence…” | unknown | | innovations zero-mean and white | `tests/sdk/innovation_whiteness.cpp` “i.i.d. N(0,1) innovations read as zero-mean and white…” | unknown | | no information gained along global yaw and translation (the 4-D unobservable space) | `tests/tools/observability_ledger.cpp` “Observability ledger: on the synthetic world the leak is yaw…” | failing — the yaw leak (#480); fixed structurally by FEJ (#606) or the R-IEKF (#608) | -| told the true noise, the filter improves and its yaw σ never collapses (EuRoC ideal tracks) | `scripts/euroc-consistency.sh` | failing — with one truth (#613, median NIS 1.03): fails 1 (2 of 66, MH_03 at 1 px), fails 2 (1.09× vs 0.97×; 0.84× vs 0.69×), fails 3 (yaw σ ratio 0.107: the yaw leak, #480) | +| told the true noise, the filter improves and its yaw σ never collapses (EuRoC ideal tracks) | `scripts/euroc-consistency.sh` | failing — with one truth and S2 integrating both samples (#616, median NIS 1.00): fails 1 (2 of 66 at 1 px, MH_03 and MH_04, the same draws as the R-IEKF: the shared tilt and bias seed, #609), passes 2 (0.45× vs 0.52×; 0.33× vs 0.53×), fails 3 (yaw σ ratio 0.179: the S6 yaw leak, #480) | | 4-DoF ATE and rotation RPE within a factor of OpenVINS mono on the 11 EuRoC sequences | `scripts/euroc-baseline.sh` | failing — median 2.8× OpenVINS mono (#595) | ## Gap report -What `pipeline_graph.py gaps` reports for this spec (26 items): +What `pipeline_graph.py gaps` reports for this spec (23 items): - edge 'ImageFrame': no validator for "t increasing and inside the IMU span" - edge 'ImageFrame': no validator for "the image matches the calibration's resolution" @@ -199,16 +199,13 @@ What `pipeline_graph.py gaps` reports for this spec (26 items): - node 'initialize': deviates from the paper: the dynamic path is excitation-limited on EuRoC and has open SfM defects (#254, #499–#501) - node 'seed_state': deviates from the paper: the prior is an isotropic σ = 0.1 with no tilt-bias correlation, whatever the init method (#609) - node 'propagate': deviates from the paper: no first-estimates Jacobians on Φ (FEJ, #606) -- node 'propagate': deviates from the paper: the sample that ends each interval is held over it: a 2.5 ms IMU–camera skew, NIS 1.1–1.9 on the fast sequences (#616) - node 'msckf_update': deviates from the paper: no first-estimates Jacobians on H (FEJ, #606); the yaw leak is S6 moving the mean (95–99%, #480) - node 'msckf_update': deviates from the paper: S6c compression is the identity (one feature per update leaves 2m − 3 rows) - node 'slam_update' (S7): not implemented - node 'online_calib' (S10): partially implemented - node 'online_calib': deviates from the paper: extrinsics only (VioConfig::estimate_extrinsics, off by default); no intrinsics; no time offset (#332) - node 'seed_state': decision 'the prior P₀' has no recorded reason (chosen: isotropic σ = 0.1 on every core state) -- node 'propagate': decision 'the IMU input over each interval' has no recorded reason (chosen: the sample that ends it, held constant (#186)) -- node 'propagate': decision 'the mean's integrator' has no recorded reason (chosen: one Euler step, the specific force rotated with the attitude at the start of the interval) - system invariant 'nees': failing — the body-frame filter is over-confident on EuRoC (#212, #605) - system invariant 'gauge': failing — the yaw leak (#480); fixed structurally by FEJ (#606) or the R-IEKF (#608) -- system invariant 'consistency_gate': failing — with one truth (#613, median NIS 1.03): fails 1 (2 of 66, MH_03 at 1 px), fails 2 (1.09× vs 0.97×; 0.84× vs 0.69×), fails 3 (yaw σ ratio 0.107: the yaw leak, #480) +- system invariant 'consistency_gate': failing — with one truth and S2 integrating both samples (#616, median NIS 1.00): fails 1 (2 of 66 at 1 px, MH_03 and MH_04, the same draws as the R-IEKF: the shared tilt and bias seed, #609), passes 2 (0.45× vs 0.52×; 0.33× vs 0.53×), fails 3 (yaw σ ratio 0.179: the S6 yaw leak, #480) - system invariant 'accuracy': failing — median 2.8× OpenVINS mono (#595) diff --git a/docs/methodology/riekf-msckf.mdx b/docs/methodology/riekf-msckf.mdx index db854c42..67afaaef 100644 --- a/docs/methodology/riekf-msckf.mdx +++ b/docs/methodology/riekf-msckf.mdx @@ -32,7 +32,7 @@ Each has the shape *(state, inputs, params) → (state′, diagnostics)*. | S4 | **track** | image → feature tracks (FAST + KLT), and the tracks that ended | NormalizedObs, TrackDB → TrackDB | geneva2020, visual tracking | implemented | `sdk/include/branes/sdk/msckf/stages/s4_frontend.hpp` | `tests/tools/stage_bench_s4_s5.cpp` “S4 bench: every fixture passes for every variant…” | | S1a | **initialize** | an IMU window at rest (static), or a moving window of frames (dynamic VI alignment) → the nav seed | ImuSample, TrackDB → NavSeed | geneva2020, initialization | implemented | `sdk/include/branes/sdk/imu_init.hpp` | `tests/tools/stage_bench_s0_s1.cpp` “S1 bench: static paths pass every fixture…” | | S1b | **seed_state** | the seed's mean as it is (velocity included); the base's body-frame prior mapped into the right-invariant error, P₀ = J·P_body·Jᵀ with δθ_world = R̂·δθ_body, ν = δv + [v̂]×δθ_world, ρ = δp + [p̂]×δθ_world | NavSeed → FilterState | barrau2017, the invariant error | implemented | `tools/include/branes/tools/invariant_backend_adapter.hpp` | — | -| S2 | **propagate** | one IMU sample → the propagated SE₂(3) mean; P ← Φ P Φᵀ + L Lᵀ with Φ independent of the estimate | FilterState, ImuSample → FilterState | hartley2020, the right-invariant IMU error dynamics | implemented | `sdk/include/branes/sdk/msckf/invariant_propagator.hpp` | `tests/tools/stage_bench_s2_s3.cpp` “S2 bench: the invariant error maps the body-frame gauge…” | +| S2 | **propagate** | one IMU interval (the samples at both of its ends) → the propagated SE₂(3) mean; P ← Φ P Φᵀ + L Lᵀ with Φ independent of the estimate | FilterState, ImuSample → FilterState | hartley2020, the right-invariant IMU error dynamics | implemented | `sdk/include/branes/sdk/msckf/invariant_propagator.hpp` | `tests/tools/stage_bench_s2_s3.cpp` “S2 bench: the invariant error maps the body-frame gauge…” | | S3 | **augment** | the state → the state plus a clone sharing the nav's δθ and ρ, P′ = G P Gᵀ | FilterState → FilterState | mourikis2007, §III-B, stochastic cloning, in the invariant error | implemented | `sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp` | — | | S5 | **triangulate** | an ended track and the invariant clones → the feature position, linear then Gauss-Newton | TrackDB, FilterState → Landmark | mourikis2007, §III-C | implemented | `sdk/include/branes/sdk/msckf/invariant_update.hpp` | — | | S6 | **msckf_update** | track + landmark → the invariant H (nav columns zero) → null-space projection → χ² gate → EKF update, right-invariant retraction | FilterState, TrackDB, Landmark → FilterState, UpdateDiag | barrau2017, the invariant measurement update | implemented | `sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp` | — | @@ -79,6 +79,8 @@ Each has the shape *(state, inputs, params) → (state′, diagnostics)*. - *Post:* R ∈ SO(3); P symmetric PSD; Φ preserves the gauge null space at any state - *Test:* `tests/sdk/invariant_propagator.cpp` “invariant propagation: Phi preserves the gauge null space at any state…” - *Test:* `tests/sdk/invariant_propagator.cpp` “invariant propagation: the predicted covariance matches a Monte-Carlo…” +- *Decision — the IMU input over each interval and the mean's integrator:* the base's: both samples interpolated linearly, integrated by RK4 (#616) over the sample that starts the interval, held (#608 to #616). the mean is the same strapdown physics in either error convention, and one convention for both backends keeps a time-offset state (#332) from learning a backend-specific skew +- *Decision — Φ and L over the interval:* unchanged: Φ's nav block does not depend on the input, and its bias lever arms are taken at the start of the interval over lever arms at the interval's midpoint. the lever-arm terms move by v·Δt over 5 ms, far below what the update resolves; the gauge test holds at any state **augment** (S3, Invariant augmentation) @@ -144,7 +146,7 @@ The driver is `tools/include/branes/tools/invariant_backend_adapter.hpp`. on imu (200 Hz): initialize [when not initialized] — the base's initializer seed_state [when a seed is ready] - propagate [when initialized] — on the held previous sample + propagate [when initialized] — the base's interval convention: both samples, RK4 (#616) on image (20 Hz): sensor_model track @@ -167,7 +169,7 @@ on image (20 Hz): | NIS ≈ dim(measurement) per update | `sdk/include/branes/sdk/msckf/invariant_vio_backend.hpp` `nis_consistency` | failing — real KLT tracks: normalized NIS 0.97–2.55 on MH, V1_01 and V2_01; 6.8–15.4 on V1_02, V1_03, V2_02 and V2_03 (#608) | | innovations zero-mean and white | `sdk/include/branes/sdk/msckf/invariant_vio_backend.hpp` `innovation_whiteness` | unknown — reported since #608 | | no information gained along global yaw and translation (the 4-D unobservable space) | `tests/sdk/msckf_invariant_backend.cpp` “invariant backend: the marginalized joint update annihilates the 4-DoF gauge…” | holds | -| told the true noise, the filter improves and its yaw σ never collapses (EuRoC ideal tracks) | `scripts/euroc-consistency.sh` | failing — with one truth (#613, median NIS 1.03): fails 1 (3 of 66 at 1 px; MH_04's vanishes with a looser gate), fails 2 at 1 px (1.04× vs 0.84×; S2's sample-hold skew on the fast sequences, #616), passes 3 (0.985) | +| told the true noise, the filter improves and its yaw σ never collapses (EuRoC ideal tracks) | `scripts/euroc-consistency.sh` | failing — with one truth and S2 integrating both samples (#616, median NIS 1.00): fails 1 (2 of 66 at 1 px, MH_03 and MH_04, the same draws as the MSCKF: the shared tilt and bias seed, #609), passes 2 (0.40× vs 0.41×; 0.18× vs 0.36×), passes 3 (0.984) | | 4-DoF ATE and rotation RPE within a factor of OpenVINS mono on the 11 EuRoC sequences | `scripts/euroc-baseline.sh` | failing — median 2.62× OpenVINS mono (the MSCKF 2.83×; 7 of 11 sequences better); rotation RPE 0.90° vs 2.21° (#608) | ## Changes from the base @@ -207,5 +209,5 @@ What `pipeline_graph.py gaps` reports for this spec (22 items): - node 'slam_update' (S7): not implemented - node 'online_calib' (S10): not implemented - system invariant 'nis': failing — real KLT tracks: normalized NIS 0.97–2.55 on MH, V1_01 and V2_01; 6.8–15.4 on V1_02, V1_03, V2_02 and V2_03 (#608) -- system invariant 'consistency_gate': failing — with one truth (#613, median NIS 1.03): fails 1 (3 of 66 at 1 px; MH_04's vanishes with a looser gate), fails 2 at 1 px (1.04× vs 0.84×; S2's sample-hold skew on the fast sequences, #616), passes 3 (0.985) +- system invariant 'consistency_gate': failing — with one truth and S2 integrating both samples (#616, median NIS 1.00): fails 1 (2 of 66 at 1 px, MH_03 and MH_04, the same draws as the MSCKF: the shared tilt and bias seed, #609), passes 2 (0.40× vs 0.41×; 0.18× vs 0.36×), passes 3 (0.984) - system invariant 'accuracy': failing — median 2.62× OpenVINS mono (the MSCKF 2.83×; 7 of 11 sequences better); rotation RPE 0.90° vs 2.21° (#608) diff --git a/docs/methodology/specs/euroc-consistency-gate.yaml b/docs/methodology/specs/euroc-consistency-gate.yaml index 00a97fc4..58596928 100644 --- a/docs/methodology/specs/euroc-consistency-gate.yaml +++ b/docs/methodology/specs/euroc-consistency-gate.yaml @@ -92,8 +92,8 @@ edges: code: tools/include/branes/tools/gt_spline.hpp symbol: synthesize_imu invariants: - - text: "noise-free, integrated from both samples of each interval, it closes on the spline's rotation within a tenth of 0.5 px over a clone window (rotation only: the shipped S2 step's position still drifts at first order, and the shipped end-sample hold fails, see s2_closure, #616)" - check: {file: tests/tools/gt_spline.cpp, case: "gt spline: S2 closure, averaging each interval's two samples, holds the rotation bound"} + - text: "noise-free, integrated by the shipped S2 from both samples of each interval, it closes on the spline within a tenth of 0.5 px over a clone window, in rotation and position (see s2_closure, #616)" + check: {file: tests/tools/gt_spline.cpp, case: "gt spline: S2 closure, the shipped S2 within what the camera resolves (#616)"} - text: "the noise matches NoiseModel's densities (per-axis variance, per-sample whiteness)" check: {file: tests/tools/gt_spline.cpp, case: "gt spline: the synthesized noise has the configured densities"} @@ -158,11 +158,11 @@ nodes: pre: - "the IMU timestamps lie inside the spline's span" post: - - "noise-free and integrated from both samples of each interval, it closes on the spline's rotation within a tenth of 0.5 px over a clone window" + - "noise-free and integrated by the shipped S2 from both samples of each interval, it closes on the spline within a tenth of 0.5 px over a clone window" - "the noise statistics match the densities" code: tools/include/branes/tools/gt_spline.hpp tests: - - {file: tests/tools/gt_spline.cpp, case: "gt spline: S2 closure, averaging each interval's two samples, holds the rotation bound"} + - {file: tests/tools/gt_spline.cpp, case: "gt spline: S2 closure, the shipped S2 within what the camera resolves (#616)"} - {file: tests/tools/gt_spline.cpp, case: "gt spline: the synthesized noise has the configured densities"} status: implemented @@ -251,12 +251,12 @@ system_invariants: text: "the median normalized NIS over the matched runs is ≈ 1 (the bench's noise model is true)" check: {file: scripts/euroc-consistency.sh} status: holds - note: "1.03 for both backends at c6b87ec (1.65–4.3 with the real IMU and the raw ground truth)" + note: "1.00 for both backends at f1551cc (1.03 at c6b87ec before #616; 1.65–4.3 with the real IMU and the raw ground truth)" - id: nis_band_per_sequence text: "every sequence's matched runs have normalized NIS within the χ² band for their dof" check: {file: scripts/euroc-consistency.sh} - status: failing - note: "1.01–1.04 on the Machine Hall and V1_01, V2_01; 1.1–1.9 on V1_02, V1_03, V2_02, V2_03 for both backends: S2's sample-hold skew, an estimator defect (#616)" + status: holds + note: "1.00 on every sequence for both backends at f1551cc. Before #616 it was 1.1–1.9 on V1_02, V1_03, V2_02, V2_03: S2's sample-hold skew, an estimator defect" - id: s2_closure text: "the shipped S2, fed the synthesized IMU noise-free, closes on the truth within what the camera resolves (a tenth of 0.5 px over a 0.55 s clone window: 1.1e-4 rad, 0.22 mm at 2 m), at second order" check: {file: tests/tools/gt_spline.cpp, case: "gt spline: S2 closure, the shipped S2 within what the camera resolves"} @@ -264,8 +264,8 @@ system_invariants: value: "1.09e-4 rad rotation, 0.22 mm position at 2 m, over a 0.55 s clone window" consumer: run_filter derivation: "the backend's update resolves a clone pose only to the track noise, 0.5 px at fx = 458.654 (1.09e-3 rad); propagation error must stay a tenth of that over the clone window so it never competes with the measurement, and the same angle at a 2 m landmark depth is 0.22 mm" - status: failing - note: "the shipped S2 holds each interval's end sample: 3.1e-3 rad (28× the bound) at first order on a 1.3 rad/s roll/pitch flight; averaging the two samples meets the rotation bound at second order, the position still needs the integrator's fix (#616). The test is a [!shouldfail] tripwire until #616" + status: holds + note: "since #616, S2 integrates each interval from both samples by RK4: 0.27 of the rotation bound and 0.10 of the position bound at 200 Hz on a 1.3 rad/s roll/pitch flight, converging at second order. Before it, the end-sample hold missed the rotation bound 28× at first order; the companion tests pin that, and that centring the input fixes the rotation while the integrator fixes the position" - id: same_gate text: every backend runs with the same S6d gate line check: {file: scripts/euroc-consistency.sh} @@ -274,4 +274,4 @@ system_invariants: - id: verdict_attributable text: "each condition's result changes with the estimator, not with the gate's line" status: holds - note: "each remaining failure rerun with the gate at 5 per dof: one divergence per backend vanishes (with a true noise model, the estimator's transient over-confidence made the 5% gate fatal), the others persist; condition 2's fast-sequence loss is #616's S2 skew. Conditions 1 and 2 still depend on the noise draw at three seeds (#613)" + note: "at c6b87ec each remaining failure was rerun with the gate at 5 per dof: one divergence per backend vanished, the others persisted, and condition 2's fast-sequence loss was #616's S2 skew. At f1551cc condition 2 passes for both; both backends diverge on the same two noise draws, from the shared initializer's seed (#609); the MSCKF's condition 3 is S6's yaw leak. Conditions 1 and 2 still depend on the noise draw at three seeds (#613)" diff --git a/docs/methodology/specs/openvins-msckf.yaml b/docs/methodology/specs/openvins-msckf.yaml index 9e8b01fe..34fa96f0 100644 --- a/docs/methodology/specs/openvins-msckf.yaml +++ b/docs/methodology/specs/openvins-msckf.yaml @@ -240,7 +240,7 @@ nodes: - id: propagate stage: S2 title: IMU propagation - does: one IMU sample → the propagated mean, P ← Φ P Φᵀ + Q_d + does: one IMU interval (the samples at both of its ends) → the propagated mean, P ← Φ P Φᵀ + Q_d reads: [FilterState, ImuSample] writes: [FilterState] source: {ref: li2013, where: "§III, the IMU model; Φ at first estimates"} @@ -259,22 +259,27 @@ nodes: precision: {policy: "T2 (msckf/stage_types.hpp)", note: "posit<32,2> passes the bench; P must be written in double on EuRoC (#532)"} decisions: - question: the IMU input over each interval - chosen: "the sample that ends it, held constant (#186)" - alternatives: ["the previous sample held", "both samples, interpolated and integrated by RK4 (geneva2020)"] + chosen: "ω and a interpolated linearly between the samples at t_{k−1} and t_k (#616)" + alternatives: ["the sample that ends the interval, held (#186 to #616)", "the sample that starts it, held"] + reason: "a sample is the rate at its timestamp: holding either end over the interval integrates the IMU half a sample (2.5 ms) early or late against the camera, NIS 1.1–1.9 on the fast sequences (#616, docs/first-principles/sample-alignment.mdx)" - question: the mean's integrator - chosen: one Euler step, the specific force rotated with the attitude at the start of the interval - alternatives: [midpoint, RK4] + chosen: "RK4 on the interpolated input: the attitude in closed form (the rate's integral plus the coning term), velocity and position by the RK4 quadrature (geneva2020)" + alternatives: ["one Euler step on a held sample (#186 to #616)", "one Euler step on the two samples' average", "RK4 on the two samples' average, held"] + reason: "measured against the S2 closure bound (a tenth of 0.5 px over a clone window, tests/tools/gt_spline.cpp) on a 1.3 rad/s flight: an Euler step on the average meets it in rotation but misses position 24×, because it rotates the specific force with the start attitude; RK4 on the average or on the line meets both at second order (0.16 and 0.10 of the position bound). The line is OpenVINS's choice and the closer of the two" + - question: an image time between two IMU samples + chosen: "the last sample held up to the image time; the rest of the interval integrated from the rate interpolated at the image time once the next sample arrives" + alternatives: ["hold the frame until the next sample arrives, then interpolate exactly (one IMU sample of latency)"] + reason: "no added latency; the residual skew lives only on the partial interval before the image, zero on EuRoC, whose image and IMU stamps coincide (#616)" - question: the transition Φ - chosen: first order, I + A·dt - alternatives: ["closed form", "a higher-order series"] - reason: "#471: a higher-order Φ leaks more yaw in the body-frame error state" + chosen: "the exact Jacobian of the shipped RK4 mean (#616)" + alternatives: ["first order, I + A·dt (#186 to #616)", "I + A·dt at the interval's mean input", "a higher-order series of A·dt"] + reason: "a yaw leak is Φ disagreeing with the mean it propagates. #471 found the first-order Φ the least leaky because it sat closest to the one-sample Euler mean; against the RK4 mean it leaks 7× more than before (4.4e-6 per step at 2.5 ms, the S2 sweep). The RK4 mean has the preintegration form, which commutes with a rotation about gravity, so its exact Jacobian preserves the unobservable subspace to roundoff (tests/sdk/imu_interval.cpp, the S2 bench)" - question: the discrete process noise Q_d chosen: "diagonal, σ²·dt per state" alternatives: ["G·Q·Gᵀ with the θ–v–p correlations (geneva2020)"] reason: "#212's S2 probe found the full Q_d did not change the over-confidence (docs/pipeline/stages/s2-imu-propagation.mdx)" deviations: - "no first-estimates Jacobians on Φ (FEJ, #606)" - - "the sample that ends each interval is held over it: a 2.5 ms IMU–camera skew, NIS 1.1–1.9 on the fast sequences (#616)" - id: augment stage: S3 @@ -440,7 +445,7 @@ schedule: - {call: track} - {call: initialize, when: not initialized, note: try the dynamic VI alignment} - {call: seed_state, when: a seed is ready, note: then return} - - {call: propagate, note: to the image time, on the held IMU sample} + - {call: propagate, note: to the image time, the last sample held (the rest of the interval follows on the next sample)} - {call: zero_velocity, when: at rest, note: then return; the frame adds no clone} - {call: msckf_update, foreach: track touching the oldest clone, when: window full} - {call: marginalize, when: window full} @@ -470,7 +475,7 @@ system_invariants: text: told the true noise, the filter improves and its yaw σ never collapses (EuRoC ideal tracks) check: {file: scripts/euroc-consistency.sh} status: failing - note: "with one truth (#613, median NIS 1.03): fails 1 (2 of 66, MH_03 at 1 px), fails 2 (1.09× vs 0.97×; 0.84× vs 0.69×), fails 3 (yaw σ ratio 0.107: the yaw leak, #480)" + note: "with one truth and S2 integrating both samples (#616, median NIS 1.00): fails 1 (2 of 66 at 1 px, MH_03 and MH_04, the same draws as the R-IEKF: the shared tilt and bias seed, #609), passes 2 (0.45× vs 0.52×; 0.33× vs 0.53×), fails 3 (yaw σ ratio 0.179: the S6 yaw leak, #480)" - id: accuracy text: 4-DoF ATE and rotation RPE within a factor of OpenVINS mono on the 11 EuRoC sequences check: {file: scripts/euroc-baseline.sh} diff --git a/docs/methodology/specs/riekf-msckf.yaml b/docs/methodology/specs/riekf-msckf.yaml index fc170c68..cdb24ed7 100644 --- a/docs/methodology/specs/riekf-msckf.yaml +++ b/docs/methodology/specs/riekf-msckf.yaml @@ -83,7 +83,7 @@ nodes: op: replace stage: S2 title: Invariant IMU propagation - does: one IMU sample → the propagated SE₂(3) mean; P ← Φ P Φᵀ + L Lᵀ with Φ independent of the estimate + does: one IMU interval (the samples at both of its ends) → the propagated SE₂(3) mean; P ← Φ P Φᵀ + L Lᵀ with Φ independent of the estimate reads: [FilterState, ImuSample] writes: [FilterState] source: {ref: hartley2020, where: the right-invariant IMU error dynamics} @@ -98,6 +98,15 @@ nodes: - {file: tests/sdk/invariant_propagator.cpp, case: "invariant propagation: Phi preserves the gauge null space at any state"} - {file: tests/sdk/invariant_propagator.cpp, case: "invariant propagation: the predicted covariance matches a Monte-Carlo"} status: implemented + decisions: + - question: the IMU input over each interval and the mean's integrator + chosen: "the base's: both samples interpolated linearly, integrated by RK4 (#616)" + alternatives: ["the sample that starts the interval, held (#608 to #616)"] + reason: "the mean is the same strapdown physics in either error convention, and one convention for both backends keeps a time-offset state (#332) from learning a backend-specific skew" + - question: Φ and L over the interval + chosen: "unchanged: Φ's nav block does not depend on the input, and its bias lever arms are taken at the start of the interval" + alternatives: ["lever arms at the interval's midpoint"] + reason: "the lever-arm terms move by v·Δt over 5 ms, far below what the update resolves; the gauge test holds at any state" - id: augment op: replace @@ -221,7 +230,7 @@ schedule: steps: - {call: initialize, when: not initialized, note: the base's initializer, run by a bootstrap MsckfBackend} - {call: seed_state, when: a seed is ready} - - {call: propagate, when: initialized, note: on the held previous sample} + - {call: propagate, when: initialized, note: "the base's interval convention: both samples, RK4 (#616)"} - event: image rate_hz: 20 steps: @@ -229,7 +238,7 @@ schedule: - {call: track} - {call: initialize, when: not initialized, note: try the dynamic VI alignment} - {call: seed_state, when: a seed is ready, note: then return} - - {call: propagate, note: to the image time, on the held IMU sample} + - {call: propagate, note: to the image time, the last sample held, as in the base} - {call: zero_velocity, when: at rest, note: then return; the frame adds no clone} - {call: msckf_update, foreach: track touching the oldest clone, when: window full} - {call: marginalize, when: window full} @@ -261,7 +270,7 @@ system_invariants: text: told the true noise, the filter improves and its yaw σ never collapses (EuRoC ideal tracks) check: {file: scripts/euroc-consistency.sh} status: failing - note: "with one truth (#613, median NIS 1.03): fails 1 (3 of 66 at 1 px; MH_04's vanishes with a looser gate), fails 2 at 1 px (1.04× vs 0.84×; S2's sample-hold skew on the fast sequences, #616), passes 3 (0.985)" + note: "with one truth and S2 integrating both samples (#616, median NIS 1.00): fails 1 (2 of 66 at 1 px, MH_03 and MH_04, the same draws as the MSCKF: the shared tilt and bias seed, #609), passes 2 (0.40× vs 0.41×; 0.18× vs 0.36×), passes 3 (0.984)" - id: accuracy text: 4-DoF ATE and rotation RPE within a factor of OpenVINS mono on the 11 EuRoC sequences check: {file: scripts/euroc-baseline.sh} diff --git a/docs/pipeline/canonical-pipeline.md b/docs/pipeline/canonical-pipeline.md index 607e9ea2..4d577d14 100644 --- a/docs/pipeline/canonical-pipeline.md +++ b/docs/pipeline/canonical-pipeline.md @@ -246,8 +246,15 @@ yaw / accel-bias / scale variance; an over-tight init `P` is a first-frame over- **Signature.** `propagate(x⁻, P⁻, imu[t_k, t_{k+1}]) → (x⁻_{k+1}, P⁻_{k+1})`. -**What it does — mean.** OpenVINS uses a **zeroth-order hold** (measurement constant over the -sample) or RK4 for the mean; debias, then strapdown-integrate `R, v, p`. +**What it does — mean.** OpenVINS offers a **zeroth-order hold** (one measurement constant over +the interval) or RK4 on the input interpolated between the interval's two samples; debias, then +strapdown-integrate `R, v, p`. **cortex's decision (#616):** RK4 on the interpolated input, in both +backends. A sample is the rate at its timestamp, so holding either end of the interval integrates +the IMU half a sample (2.5 ms at 200 Hz) early or late against the camera. Until #616 the MSCKF +held the end sample and the R-IEKF the start sample, which inflated NIS to 1.1–1.9 on the fast +EuRoC sequences. An image between two samples is reached by holding the last sample; the next +sample integrates the rest of the interval from the rate interpolated at the image time. The +failure mode, and how it hid, is on [Aligning Sensor Samples](../first-principles/sample-alignment.mdx). **What it does — covariance.** `P⁻_{k+1} = Φ P⁺_k Φᵀ + Q_d`, with the canonical block structure (rows/cols `[θ, p, v, b_g, b_a]`): @@ -286,7 +293,10 @@ none, as the troubleshooting confirmed. the null space of the linearized propagation; with FEJ, `Φ` does not inject information along yaw. Probe by propagating the analytic nullspace vectors and checking they stay in the null space. - **GT-injection identity:** with ground-truth IMU and zero noise, propagation reproduces the GT - trajectory within integration tolerance over the interval. + trajectory over a clone window within what the camera resolves there: a tenth of the 0.5 px track + noise, 1.09e-4 rad and 0.22 mm at a 2 m depth, converging at second order + (`tests/tools/gt_spline.cpp`, #616). The tolerance comes from the consumer, not from what the + integrator achieves: a tolerance fitted to the residual blessed the half-sample skew (#616). **Probes.** `P` min-eigenvalue (PSD monitor); `R` orthonormality residual `‖RᵀR − I‖`; `Q_d` symmetry/PSD; nullspace-drift metric (`‖projection of P⁻¹ onto yaw direction‖`); per-step diff --git a/docs/pipeline/stages/s2-imu-propagation.mdx b/docs/pipeline/stages/s2-imu-propagation.mdx index 6ddad959..68844a64 100644 --- a/docs/pipeline/stages/s2-imu-propagation.mdx +++ b/docs/pipeline/stages/s2-imu-propagation.mdx @@ -1,6 +1,6 @@ --- title: S2 — IMU Propagation -description: Predicting the mean and covariance forward between images — the Φ/Q_d covariance propagation, and the measured verdict that its diagonal-Q simplification is a minor, cleared #212 candidate. +description: Predicting the mean and covariance forward between images — the Φ/Q_d covariance propagation and its cleared diagonal-Q candidate, and the half-sample skew in the mean that the probe measured but did not catch (#616). --- import { Aside } from '@astrojs/starlight/components'; @@ -14,8 +14,10 @@ for the [#212 over-confidence](../engineering-status.mdx) — and the probe ## What it does -- **Mean** — debias the IMU sample, then strapdown-integrate `R, v, p` (zeroth-order - hold or RK4). +- **Mean** — integrate each IMU interval from **both** of its samples: debias, interpolate + the input linearly between the samples at t₍ₖ₋₁₎ and tₖ, and integrate `R, v, p` by RK4 + (#616). An image between two samples is reached by holding the last sample; the next sample + integrates the rest of its interval from the rate interpolated at the image time. - **Covariance** — `P⁻_{k+1} = Φ P⁺_k Φᵀ + Q_d`. The transition matrix `Φ` carries the **off-diagonal skew blocks** coupling orientation error into velocity and position error — gravity leakage living inside the covariance. The discrete noise @@ -38,8 +40,8 @@ to measure it against a canonical reference. |---|---| | **Signature** | `propagate(x⁻, P⁻, imu[t_k, t_{k+1}]) → (x⁻_{k+1}, P⁻_{k+1})` | | **Pre** | `Δt > 0`, monotonic timestamps; measurements debiased; `P⁺_k` symmetric PSD | -| **Post / invariants** | `R ∈ SO(3)` (orthonormal, det 1) · `P⁻` symmetric PSD · `Q_d` PSD with `θ–v–p` cross-correlations · the 4-D unobservable subspace (global position + yaw) stays in the propagation null space · GT-injection: zero-noise propagation reproduces the GT trajectory within integration tolerance | -| **cortex** | `msckf/propagator.hpp` (Φ assembly, Q, FEJ) | +| **Post / invariants** | `R ∈ SO(3)` (orthonormal, det 1) · `P⁻` symmetric PSD · `Q_d` PSD with `θ–v–p` cross-correlations · the 4-D unobservable subspace (global position + yaw) stays in the propagation null space · GT-injection: zero-noise propagation closes on a continuous truth over a clone window within what the camera resolves (1.09e-4 rad, 0.22 mm at 2 m), at second order | +| **cortex** | `msckf/stages/s2_propagation.hpp` (the convention), `msckf/propagator.hpp` (`ImuInterval`, `rk4_strapdown`, Φ, Q) | ## Metrics @@ -55,7 +57,16 @@ on the synthetic world. | `R` orthonormality `‖RᵀR − I‖` @0.5 s | 2.2e-16 — **pass** | | `P` min-eigenvalue @0.5 s | > 0 — **PSD** | | global-position nullspace leak | 0 — **preserved** | -| GT-injection integrator order | O(Δt) (error halves with Δt: 59→30→15→7 mm @ 50→800 Hz) | +| GT-injection integrator order | O(Δt) (error halves with Δt: 59→30→15→7 mm @ 50→800 Hz) — **this was #616's defect**, see below | + + ### The diagonal-Q deficit — real, but small @@ -120,7 +131,40 @@ This is the real-data check on the synthetic finding below: on EuRoC the envelop covers the inter-frame drift, confirming propagation is essentially honest on its own — the #212 over-confidence enters elsewhere. -