OpenVINS MSCKF: the transformation graph

A sliding-window filter over IMU poses: the IMU propagates a 15-state nav error and its covariance; each image clones the pose into the window; a feature track that ends is triangulated, its feature marginalized by a null-space projection, and fused into every clone it was seen from; the oldest clone is marginalized to bound the window.

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.

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 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 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…”
S9	marginalize	the state → the state without its oldest clone (a principal submatrix of P), after fusing every track that touches it	FilterState, TrackDB → FilterState, TrackDB	mourikis2007, §III-E, window management	implemented	sdk/include/branes/sdk/msckf/stages/s9_marginalization.hpp	tests/tools/stage_bench_s9_s10.cpp “S9 policies remove the clone they name…”
S8	zero_velocity	IMU and image both at rest → the v = 0 and b_g = ω̄ pseudo-measurements; the frame adds no clone	FilterState, ImuSample, TrackDB → FilterState	geneva2020, zero-velocity update	implemented (optional)	sdk/include/branes/sdk/msckf/stages/s8_zero_velocity.hpp	tests/sdk/zero_velocity.cpp “S8 finds rest under vibration…”
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, intrinsics and time offset, estimated with the rest	FilterState, CameraCalib → FilterState	geneva2020, online calibration	partial (optional)	sdk/include/branes/sdk/msckf/stages/s10_online_calibration.hpp	tests/tools/stage_bench_s9_s10.cpp “S10 bench: every fixture passes for every variant…”

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)

• Pre: a successful seed
• Post: P₀ symmetric positive-definite, in the error convention every later node assumes
• Test: tests/sdk/msckf_backend.cpp “the backend initializes attitude from a stationary window…”
• Decision — the prior P₀: isotropic σ = 0.1 on every core state over the tilt–accelerometer-bias correlation an IMU window implies (#609). no reason recorded
• Deviates from the paper: the prior is an isotropic σ = 0.1 with no tilt-bias correlation, whatever the init method (#609)

propagate (S2, IMU propagation)

• Pre: Δt > 0 and bounded; the state initialized
• 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: ω 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)

augment (S3, State augmentation)

• Pre: the state propagated to the image time
• Post: P grows by 6 with an exact block copy; P PSD
• Test: tests/sdk/msckf_stages.cpp “S2_propagation and S3_augmentation…”

triangulate (S5, Triangulation)

• Pre: at least two observations from live clones; enough parallax
• Post: positive depth in every camera; bounded reprojection error; conditioning reported
• Test: tests/sdk/msckf_stages.cpp “S5 and S6a-S6e run one feature track…”

msckf_update (S6, MSCKF update)

• Pre: the landmark triangulated against the current clones
• Post: Nᵀ H_f = 0; P⁺ ⪯ P⁻; no information along the unobservable directions; δx finite
• Test: tests/sdk/msckf_stages.cpp “S6 composite is bit-identical to CameraUpdater::update…”
• Test: tests/sdk/msckf_camera_update.cpp “S6d gate line: the chi-square quantile by default…”
• Test: tests/tools/stage_bench_s6.cpp “S6 update: no information along the unobservable directions…”
• Precision: T6, T6e (msckf/stage_types.hpp) — S6e writes P: double on EuRoC (#532); the carried square-root factor holds in posit32
• Decision — the S6d gate line: the χ²₁₋α(dof) quantile, α = 0.05 over 5 per dof (before #481). #481: it admits 1 − α of consistent inliers at every dof; the per-dof line admitted most 2σ-biased short tracks
• Decision — S6c compression: none (the identity) over QR to n + 1 rows (geneva2020). one feature per update leaves 2m − 3 rows, fewer than the state dimension, so QR would not shrink it
• Deviates from the paper: no first-estimates Jacobians on H (FEJ, #606); the yaw leak is S6 moving the mean (95–99%, #480)
• Deviates from the paper: S6c compression is the identity (one feature per update leaves 2m − 3 rows)

marginalize (S9, Marginalization)

• Pre: every track touching the clone has been fused
• Post: P is the principal submatrix; P PSD; no observation refers to the dropped clone
• Test: tests/sdk/msckf_stages.cpp “S9_marginalization keeps the principal submatrix…”

zero_velocity (S8, Zero-velocity update)

• Pre: both the IMU and the image tests say rest
• Post: never fires while moving faster than 0.05 m/s on EuRoC (a measurement, not a guarantee)
• Test: tests/sdk/zero_velocity.cpp “S8 pseudo-measurements pull v to zero…”

online_calib (S10, Online calibration)

• Pre: the extrinsic prior σ’s stated; enabled before any clone exists
• Post: the extrinsic rotation ∈ SO(3); the calibration covariance PSD; estimating moves the extrinsic toward the truth
• Test: tests/sdk/msckf_stages.cpp “S9_marginalization keeps the principal submatrix; S10 adds calibration states…”
• Test: tests/tools/stage_bench_s9_s10.cpp “S10: estimating the extrinsic moves it toward the truth…”
• Deviates from the paper: extrinsics only (VioConfig::estimate_extrinsics, off by default); no intrinsics; no time offset (#332)

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 (θ, p, v, b_g, b_a: 15), clones (θ, p: 6 each), optional camera↔IMU extrinsics (6 per camera), the joint covariance P, and the error-state convention every Jacobian assumes	sdk/include/branes/sdk/msckf/state.hpp State	✓ P symmetric; ✓ P positive semidefinite; ✓ R and every clone rotation ∈ SO(3); ✗ the layout and the error convention agree with every node’s Jacobians
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 P-writing stages must stay double (#532)
• FilterState deviates from the paper: error convention R = R̂·Exp(δθ) (body-frame right perturbation); OpenVINS uses a left quaternion error. Legitimate if consistent everywhere.

Schedule

The driver is sdk/include/branes/sdk/msckf_backend.hpp.

on imu (200 Hz):
  initialize  [when not initialized]  — accumulate; static or gravity-align when a window allows
  seed_state  [when a seed is ready]
  online_calib  [when a seed is ready and extrinsics are estimated]  — appends the calibration block; S6 updates it
  propagate  [when initialized]
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)	sdk/include/branes/sdk/eval/nav_consistency.hpp nav_block_nees	failing — the body-frame filter is over-confident on EuRoC (#212, #605)
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 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 (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”
• 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 “the layout and the error convention agree with every node’s Jacobians”
• edge ‘FilterState’: deviates from the paper: error convention R = R̂·Exp(δθ) (body-frame right perturbation); OpenVINS uses a left quaternion error. Legitimate if consistent everywhere.
• 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’: 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 ‘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)
• 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 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)