Right-Invariant MSCKF: the transformation graph

The OpenVINS MSCKF with the nav error taken on SE₂(3), right-invariant (R = Exp(δθ)·R̂, v = Exp(δθ)·v̂ + ν, p = Exp(δθ)·p̂ + ρ). Propagation’s Φ then doesn’t depend on the estimate, so the 4-D unobservable subspace is preserved by construction and no update can gain information along it: the property FEJ approximates in the body-frame filter.

A delta on OpenVINS MSCKF: it changes the nodes and data structures listed under Changes from the base, and keeps the rest.

Sources

• mourikis2007 — A. I. Mourikis and S. I. Roumeliotis, “A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation”, ICRA 2007.
• li2013 — M. Li and A. I. Mourikis, “High-precision, consistent EKF-based visual-inertial odometry”, IJRR 32(6), 2013 (MSCKF 2.0, first-estimates Jacobians).
• geneva2020 — P. Geneva, K. Eckenhoff, W. Lee, Y. Yang and G. Huang, “OpenVINS: A Research Platform for Visual-Inertial Estimation”, ICRA 2020.
• huang2010 — G. P. Huang, A. I. Mourikis and S. I. Roumeliotis, “Observability-based Rules for Designing Consistent EKF SLAM Estimators”, IJRR 29(5), 2010.
• barrau2017 — A. Barrau and S. Bonnabel, “The Invariant Extended Kalman Filter as a Stable Observer”, IEEE TAC 62(4), 2017.
• hartley2020 — R. Hartley, M. Ghaffari, R. M. Eustice and J. W. Grizzle, “Contact-aided invariant extended Kalman filtering for robot state estimation”, IJRR 39(4), 2020 (the right-invariant IMU error dynamics on SE₂(3)).

Transformations

Each has the shape (state, inputs, params) → (state′, diagnostics).

Stage	Node	Does	Reads → writes	Paper	Status	Code	Bench
S0	sensor_model	pixel → normalized image point (undistort, unproject)	ImageFrame, CameraCalib → NormalizedObs	geneva2020, camera models	implemented	sdk/include/branes/sdk/msckf/stages/s0_sensor_model.hpp	tests/tools/stage_bench_s0_s1.cpp “S0 bench: every fixture passes in every type…”
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 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	—
S9	marginalize	the state → the state without its oldest clone, after fusing every track that touches it	FilterState, TrackDB → FilterState, TrackDB	mourikis2007, §III-E	implemented	sdk/include/branes/sdk/msckf/invariant_vio_backend.hpp	—
S8	zero_velocity	at rest → v = 0 in the invariant error (H_θ = −[v̂]×, H_ν = I, r = −v̂) and b_g = ω̄; the frame adds no clone	FilterState, ImuSample, TrackDB → FilterState	geneva2020, zero-velocity update	implemented (optional)	sdk/include/branes/sdk/msckf/msckf_invariant_backend.hpp	—
S7	slam_update	long-lived features kept in the state as landmarks and updated every frame	FilterState, TrackDB → FilterState	geneva2020, SLAM features	missing (optional)	—	—
S10	online_calib	the state ⊕ camera↔IMU extrinsics; MsckfInvariantBackend::enable_calibration exists, the EuRoC adapter doesn’t call it	FilterState, CameraCalib → FilterState	geneva2020, online calibration	missing (optional)	—	—

Contracts

sensor_model (S0, Sensor model)

• Pre: intrinsics valid; the pixel inside the image
• Post: project ∘ unproject is the identity; the analytic Jacobian matches finite differences
• Test: tests/sdk/msckf_stages.cpp “S0_sensor_model normalizes a pixel…”

track (S4, Visual front end)

• Pre: consecutive frames; features detected inside the trackable band
• Post: tracks inside the image; forward-backward error bounded; ended tracks released in id order
• Test: tests/sdk/msckf_stages.cpp “S4_frontend ingests observations…”
• Deviates from the paper: no RANSAC outlier rejection in the front end; the S6d gate carries it
• Deviates from the paper: the shipped default churns (>50% of new tracks lost on their first frame); the border margin is opt-in (#474, #574)

initialize (S1a, Initialization)

• Pre: an IMU window that is at rest, or frames with enough excitation and parallax
• Post: gravity-aligned attitude; the method is recorded
• Test: tests/sdk/imu_init.cpp “static init recovers gravity…”
• Test: tests/sdk/msckf_dynamic_init.cpp “the backend bootstraps a moving start via dynamic VI init…”
• Deviates from the paper: the dynamic path is excitation-limited on EuRoC and has open SfM defects (#254, #499–#501)

seed_state (S1b, Seed the filter in the invariant error)

• Pre: a successful seed from the base’s initialize
• Post: the mean equals the seed; P₀ = J·P_body·Jᵀ, positive-definite; the IMU sample in effect at the seed time is held, so propagation resumes at the seed time
• Test: tests/tools/invariant_backend_adapter.cpp “invariant adapter: the body-to-invariant map matches the exact error…”
• Test: tests/tools/invariant_backend_adapter.cpp “invariant adapter: initializes through the shipped MSCKF with its seed…”
• Deviates from the paper: the base’s prior is itself an isotropic σ = 0.1 with no tilt-bias correlation (#609); this node maps it faithfully

propagate (S2, Invariant IMU propagation)

• Pre: Δt > 0 and bounded
• 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)

• Pre: the state propagated to the image time
• Post: P grows by 6 with an exact block copy; P PSD
• Test: tests/sdk/invariant_joint_step_fd.cpp “DIAG FD: augmentation G under rotation…”
• Test: tests/sdk/msckf_invariant_backend.cpp “invariant backend: propagate + augment + update runs and stays PSD…”

triangulate (S5, Invariant triangulation)

• Pre: at least two observations from live clones
• Post: positive depth in every camera; a low-parallax linear solve repaired by refinement
• Test: tests/sdk/invariant_update.cpp “invariant triangulation: Gauss-Newton refinement repairs…”

msckf_update (S6, Invariant MSCKF update)

• Pre: the landmark triangulated against the current clones
• Post: H annihilates the 4-DoF gauge at the actual clone poses; P⁺ ⪯ P⁻; a non-finite δx is rolled back
• Test: tests/sdk/invariant_update.cpp “invariant update: H annihilates the 4-DoF gauge…”
• Test: tests/sdk/msckf_invariant_backend.cpp “invariant backend: the marginalized joint update annihilates the 4-DoF gauge…”
• Test: tests/sdk/invariant_joint_step_fd.cpp “DIAG FD: measurement-update linearization (proj.r vs -H*e)…”
• Test: tests/sdk/invariant_zero_velocity.cpp “invariant backend: every update reports its NIS, gated or not…”
• Test: tests/sdk/invariant_zero_velocity.cpp “invariant backend: the chi-square gate needs alpha in (0,1)…”

marginalize (S9, Marginalization (invariant backend))

• Pre: every track touching the clone has been fused
• Post: P is the principal submatrix; no observation refers to the dropped clone
• Test: tests/sdk/invariant_vio_backend.cpp “invariant VIO backend: end-to-end integration over a seed sweep…”

zero_velocity (S8, Invariant zero-velocity update)

• Pre: both the IMU and the image tests say rest (the base’s detector)
• Post: the posterior equals the Kalman update with H from finite differences of the retraction
• Test: tests/sdk/invariant_zero_velocity.cpp “invariant S8: the zero-velocity posterior matches the retraction’s Jacobian…”
• Test: tests/sdk/invariant_zero_velocity.cpp “invariant S8: the gyro-bias update is the additive Kalman update…”
• Test: tests/tools/invariant_backend_adapter.cpp “invariant adapter: S8 holds a frame at rest out of the window…”

Data structures

Edge	Kind	Contents	Type	Invariants (✓ has a validator)
ImuSample	input	t, angular rate ω (rad/s), specific force a (m/s²)	sdk/include/branes/sdk/vio_backend.hpp ImuMeasurement	✓ timestamps strictly increasing; ✓ Δt within the sensor-rate bounds; ✓ samples finite
ImageFrame	input	t, 8-bit image, camera id	sdk/include/branes/sdk/vio_estimator.hpp feed_image	✗ t increasing and inside the IMU span; ✗ the image matches the calibration’s resolution
CameraCalib	input	intrinsics (fx, fy, cx, cy), radtan distortion, camera↔IMU extrinsic (R_imu_cam, p_imu_cam)	math/include/branes/math/cameras/pinhole_radtan.hpp PinholeRadtanCamera	✓ R_imu_cam ∈ SO(3); ✓ intrinsics positive, principal point inside the image
NormalizedObs	data	feature id, camera id, normalized image point (x, y)	sdk/include/branes/sdk/msckf/stages/s0_sensor_model.hpp Result	✗ finite, and the ray is in front of the camera (z > 0)
TrackDB	data	feature id → [(clone time, normalized observation)], plus the tracks that ended this frame	sdk/include/branes/sdk/msckf/stages/s4_frontend.hpp TrackTable	✗ every observation’s clone time is a live clone, or the observation is purged; ✓ points inside the image; ✓ track length at most the clone window
NavSeed	data	R, v, p, b_g, b_a, the time, and the prior P₀ in the filter’s error convention	sdk/include/branes/sdk/msckf/stages/s1_initialization.hpp seed_from_imu	✓ gravity sign and magnitude; ✓ P₀ symmetric positive-definite; ✗ P₀ states what the init window does and does not determine (tilt-bias correlation)
FilterState	state	nav on SE₂(3) with additive biases (θ, ν, ρ, b_g, b_a: 15), clones (θ, ρ: 6 each) in the same right-invariant error, the joint covariance P	sdk/include/branes/sdk/msckf/invariant_propagator.hpp InvariantNavState	✓ P symmetric; ✓ P positive semidefinite; ✓ R and every clone rotation ∈ SO(3); ✗ every Jacobian and retraction uses R = Exp(δθ)·R̂, v = Exp(δθ)·v̂ + ν, p = Exp(δθ)·p̂ + ρ
Landmark	data	feature position p_f (world), the triangulation’s parallax and conditioning	sdk/include/branes/sdk/msckf/stages/s5_triangulation.hpp Result	✓ positive depth in every observing camera; ✓ normal-matrix condition number reported
UpdateDiag	output	NIS, dof, the gate verdict, and where a rejected track stopped	sdk/include/branes/sdk/msckf/stages/s6_msckf_update.hpp Result	✓ NIS ~ χ²(dof) over windows (bound χ²₁₋α(dof) per update, α = 0.05, from what msckf_update resolves: the S6d gate rejects above it, so a consistent filter loses α of its inliers and no more)

• ImageFrame precision: integer pixels; no arithmetic-type choice
• NormalizedObs precision: the narrowest edge that keeps sub-pixel accuracy; float holds (#532)
• NavSeed deviates from the paper: P₀ is an isotropic σ = 0.1 on every core state, with no tilt-bias correlation (#609)
• FilterState precision: stored in double; the Joseph form on EuRoC (the square-root form diverges there)

Schedule

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]  — the base's interval convention: both samples, RK4 (#616)
on image (20 Hz):
  sensor_model
  track
  initialize  [when not initialized]  — try the dynamic VI alignment
  seed_state  [when a seed is ready]  — then return
  propagate  — to the image time
  zero_velocity  [when at rest]  — then return; the frame adds no clone
  msckf_update  for each track touching the oldest clone  [when window full]
  marginalize  [when window full]
  augment
  triangulate  for each ended track (ascending id)
  msckf_update  for each ended track (ascending id)

System invariants

Invariant	Check	Status
NEES ≈ dim(state) against ground truth (consistency)	tools/include/branes/tools/pose_consistency.hpp PoseConsistency	unknown — consistent on ideal EuRoC tracks (yaw ANEES 1.26); on real KLT tracks not yet measured (#365)
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 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

• replace edge FilterState: the error convention changes; every consumer is revisited below
• replace node seed_state: #608
• replace node propagate
• replace node augment
• replace node triangulate
• replace node msckf_update: #608: the base’s χ²₁₋α(dof) gate (#481), and the NIS and innovation sum of every update
• replace node marginalize: a principal submatrix is the same in any error convention; the track bookkeeping is the invariant backend’s own
• replace node zero_velocity: #608
• replace node online_calib
• kept base node slam_update on changed edge FilterState: not implemented in either filter

Gap report

What pipeline_graph.py gaps reports for this spec (22 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”
• edge ‘NormalizedObs’: no validator for “finite, and the ray is in front of the camera (z > 0)”
• edge ‘TrackDB’: no validator for “every observation’s clone time is a live clone, or the observation is purged”
• edge ‘NavSeed’: no validator for “P₀ states what the init window does and does not determine (tilt-bias correlation)”
• edge ‘NavSeed’: deviates from the paper: P₀ is an isotropic σ = 0.1 on every core state, with no tilt-bias correlation (#609)
• edge ‘FilterState’: no validator for “every Jacobian and retraction uses R = Exp(δθ)·R̂, v = Exp(δθ)·v̂ + ν, p = Exp(δθ)·p̂ + ρ”
• node ‘track’: deviates from the paper: no RANSAC outlier rejection in the front end; the S6d gate carries it
• node ‘track’: deviates from the paper: the shipped default churns (>50% of new tracks lost on their first frame); the border margin is opt-in (#474, #574)
• 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’: no stage bench
• node ‘seed_state’: deviates from the paper: the base’s prior is itself an isotropic σ = 0.1 with no tilt-bias correlation (#609); this node maps it faithfully
• node ‘augment’: no stage bench
• node ‘triangulate’: no stage bench
• node ‘msckf_update’: no stage bench
• node ‘marginalize’: no stage bench
• node ‘zero_velocity’: no stage bench
• 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 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)