eskf-vio

Visual-inertial odometry in C. A 15-state error-state Kalman filter at 200 Hz fused with MSCKF visual corrections from a hand-written tracker. On EuRoC Machine Hall, 3808 m of IMU-only drift becomes a 0.38 m absolute trajectory error, and the same parameters hold on all five sequences without retuning.

GitHub repo: eskf-vio
Last update: 2026/09/19

Why this project

The flight controller’s complementary filter gives attitude, with yaw drifting free because nothing absolute observes it, and it cannot give position: the accelerometer double-integrates into metres of error within seconds. MH_01 is a standard drone VIO benchmark: a motion-capture room with millimetre ground truth, camera and IMU on one clock. Every number below is measured against that.

The filter

A quaternion has four components and three degrees of freedom. Propagate it linearly and unit length breaks; normalise after each step and the covariance stops describing the distribution it surrounds. The ESKF splits the state instead. The nominal state carries the nonlinear part and integrates like an ordinary strapdown INS; the error state is the difference from truth, small by construction, in a tangent space: δposition, δvelocity, δθ, δba, δbg. Fifteen numbers.

The nominal quaternion updates exactly:

$$q_{nom,k+1} = q_{nom,k} \otimes q\{\omega\,dt\}$$

and the error transition is the small-angle version:

$$\delta\theta_{k+1} = R\{\omega\,dt\}^\top \delta\theta_k - \delta b_g\,dt$$

That transpose is the first frame convention that will bite you. Injection uses right multiplication, $q_{true} = q_{nom} \otimes q\{\delta\theta\}$, for the same reason: the error lives in the body frame. Compose on the left and the filter converges on every axis but one, and that axis takes a week to find. The injection resets the state, but its effect on the covariance, the Jacobian that transforms P into the new tangent space, is still unimplemented.

Dead reckoning, fake GPS

The first version does no filtering, and drifts 3808 m over 183 seconds with a quadratic shape, a residual acceleration bias near 0.23 m/s². Gyro-only drift is 86° raw and 5.65° debiased, so the bias is a state rather than a calibration. The first run with covariance came out far wider than the errors it was supposed to bound, and I assumed process noise. The cause was the initial condition: eskf_init seeds gyro bias at $10^{-3}$ while also taking the ground-truth bias at $t_0$ as the nominal value, so the number describes a bias handed in near-exact, not a residual. I settled it by predicting the covariance growth from the $F$ and $Q$ I had written down and comparing against what the code printed, ten minutes of arithmetic after a while spent guessing first.

Then a fake position measurement, subsampled ground truth at about 20 Hz: 3808 m becomes 0.03 m, and the gyro bias converges too, which nothing in that pipeline measures. The bias is observable through position alone, as the cross-covariance predicts.

gyro bias est -0.00182 0.02148 0.07683 | true -0.00324 0.02134 0.07829

Norm off by 1.8% and the x axis off by 44% of truth.

The vision front-end

Just under 350 lines: FAST-9 with NMS and grid selection (src/fast.c), pyramidal Lucas-Kanade (src/klt.c), pinhole plus radial-tangential (src/cam.c), the tracker that keeps feature IDs alive (src/frontend.c), and stb_image for the PNGs. I skipped OpenCV because a tracker that fails silently does not fail visibly: it stops producing tracks and the estimator gets blamed.

Frame 0 gives about 10,000 raw FAST corners at threshold 30, useless as a distribution. Sort by score, reject anything within 5 px of an accepted corner, cap each cell of an 8×6 grid at 5 corners: 231 corners across the frame. Without the cap the top 200 all sit in one corner and the triangulation geometry is degenerate. Lucas-Kanade runs four pyramid levels, forward and backward, and a point survives only if the round trip returns within 1 px.

The MSCKF

The camera gives tracks of 2D points, each one a 3D feature I do not want to estimate. Feature positions cannot simply be ignored, because a feature 10 m away constrains position weakly and orientation strongly, one 30 cm away the reverse.

MSCKF keeps the dependency long enough to remove it exactly. Stack the $2k$ residuals of a track observed in $k$ poses and linearise against the only two things they depend on:

$$r \simeq H_x \delta x + H_f \delta p_f + n$$

Project onto the left null space of $H_f$, of dimension $2k-3$ generically, less when the geometry is degenerate. With $A H_f = 0$ the feature term annihilates and what remains constrains only states in the filter, so a standard update applies on $S = A H_x P H_x^\top A^\top + \sigma^2 I$. The feature is never estimated, never stored, never in the state vector.

Each camera frame appends the current pose and the covariance grows by a 6×6 block with the cross terms. The window caps at 10 clones and the oldest is marginalised; a track must die before use, because fusing one still observed double-counts.

Triangulation is plain DLT-style least squares, not epipolar-constrained. It does now carry a cheirality check, because the plane equations are homogeneous in $(x - c_i)$ and the forward ray and its reflection satisfy them equally well, so on the rotation-dominated stretch the solution was landing 3.4 m behind every camera.

Verification

A wrong Jacobian does not crash; it gives a filter that converges to the wrong answer with a confident-looking covariance. The Jacobians ($H_p$, $H_\theta$, $H_f$) are checked against finite differences by perturbing one input 1e-7; the errors print at 1e-8 to 1e-9 against a threshold of 1e-5. Triangulation returns exact synthetic observations exactly (tests/test_tri.c: 1e-15). The null space is tested as a property, $A H_f$ zero at order 1e-16 against a 1e-10 threshold.

tools/xcheck.py recomputes the projection and the update from scratch in numpy, with SVD where the C uses Gram-Schmidt, so the two share no method: on 120 real tracks the null-space dimension matches $2k-3$ at every length. Eleven test binaries, 99 checks, make test. Rejection on the full run is 0.4%, 233 of 62 640; the 24% reported earlier was the observation Jacobian returning invalid, because triangulation was putting the point behind the cameras.

The result

MH_01, 20 Hz camera, initialised from ground truth at $t_0$, no external input after that. Ground truth ends 181.9 seconds in while the sensors run to 183, so every number is scored over the first 181.9 seconds:

Over the window the error is 0.376 m RMS, peak 1.181 m, last sample 1.2 m against a predicted 0.1 m, attitude 1-sigma 0.372°, 62 407 tracks accepted and 233 rejected. The predicted column is the x-axis sigma; against the predicted sigma over all three axes the error runs 1.9 to 7.3 times it.

Machine Hall 01 to 05 also run, full rate, every parameter unchanged:

The error rises with the difficulty EuRoC assigns each sequence, and every parameter had only seen MH_01 before these runs, so the result transfers.

The vision rate has an optimum below full rate. Decimating to 10 Hz gives 0.236 m against 0.376 m at 20 Hz, and the curve turns up afterwards (0.706 m at 5 Hz, 1.184 at 3.33, 11.41 at 2). Halving the rate buys 1.6× accuracy and 1.7× less CPU. A fixed ten-clone window spans more time at a lower rate, so the window gained an optional duration cap; the advantage survives with the baseline matched. Below 5 Hz the tracker fails while the forward-backward check stays at 0.2 px, because that check tests the round trip, not the track.

The window at 40 s is a scale degeneracy. For about 24 seconds the platform is stationary while still rotating, and the camera baseline across the clone window collapses from 0.220 m to 0.00023 m. No parallax means no metric depth and no measurement carries scale: the error reaches 1.23 m while the filter reports 0.10 m, and the innovations are 1000× smaller than the covariance admits. A zero-velocity update gated on the accelerometer, | |a| - g | < 0.2 m/s² for one second, brings that peak to 0.072 m. Inflating the covariance along the unobservable direction made the window ratio worse at every strength tried.

The end window was measurement starvation and a pose misalignment, behind both a triangulation with no cheirality constraint.

The overconfidence is not the Jacobians. First-estimate Jacobians are the usual explanation and their usual symptom is absent: the covariance projected onto the four directions the camera cannot observe grows 42,700× on translation x over the run. The measurement noise was a hard-coded guess 5× too large; three independent measurements put it at 0.61 to 0.66 px. What is left is time-correlated measurement error, because halving the vision rate at constant observations per frame improves both the error and the ratio.

The error that grows late is yaw, not tilt. This page used to claim a 12.9° tilt worth 2.2 m/s² of spurious horizontal acceleration; both were wrong. The number came from the angle between the filter’s and the truth’s body-z axes, which is tilt only if body z is vertical. In EuRoC the accelerometer at rest sits at (+0.963, -0.054, -0.264) in body coordinates, so body z is 105–114° from gravity and the metric was tracking yaw. Split at the gravity axis, tilt never passes 0.64°. Two instrument faults sat underneath: ground truth ends 1.07 seconds before the sensors, so the last 22 frames were scored against a frozen sample, and the reference quaternions are not unit norm, so every attitude error below half a degree clamped to zero.

Payoff

Solà gives you the equations. What Solà cannot give you is that every equation hides a convention: rate vector body or world, quaternion body-to-world or world-to-body, error state left- or right-multiplied, gyro Jacobian with $R$ or $R^\top$, gravity added or subtracted. None of it is derivable from the text, which is consistent under either choice: the choice is made once and assumed for a hundred pages. Get one wrong and nothing crashes: the filter runs, the covariance stays symmetric, the trajectory looks plausible, the answer is quietly wrong.

What comes next

First-estimate Jacobians come next even though their usual symptom is absent, because they come with a clean test: the shipped noise parameters are the fixed point of the filter’s own calibration rule, and a hand-fit set that beat them on four of five metrics should stop winning once the Jacobians are right. A per-track common-mode term in R is the cheapest test of the correlated-error explanation. Relative pose error and a reference implementation on the same sequences are missing before 0.376 m can be placed against anything published.

Then the port: ESP32-P4, OV9281 mono global shutter, unchanged filter, no Jetson or Pi. The logger writes camera and IMU to one clock in EuRoC format, then Kalibr, then handheld data through the existing pipeline offline, then online. On one thread the filter runs at 4.62× real time, 72.5% of it the update path. Then MAVLink ODOMETRY (331) out, and then the learned inertial network as one more measurement, to stretch the blind window far enough that the camera can drop to 3–5 Hz.

An MSCKF in C, verified against ground truth, 0.376 m ATE on a public sequence and holding across five without retuning, with a covariance 1.9 to 7.3× too tight and a yaw error that grows late uncorrected. Not competitive with OpenVINS, which reaches sub-metre here. Not deployable.