State estimation
Overview
Why the vehicle state estimator is shaped the way it is: the frames, the choice to write a fixed-lag smoother rather than use one, what the sensors cannot observe, how two unsynchronised clocks are lined up, and the bugs that left marks on the code. Written 2026-09-23, when the stack was first built against simulation; nothing here has run on the car yet.
The reader-facing pages are state_estimator (the node) and estimator_offline (the workstation tools). The code is six libraries, in dependency order:
| Library | What it is |
|---|---|
libs/csym | Compile-time symbolic differentiation (a constexpr port of SymForce’s idea). Residuals are written once; their Jacobians are generated by the compiler. |
libs/geodesy | WGS84, geodetic and ECEF conversions, Somigliana normal gravity, earth rate; templated on the scalar so csym can trace it. |
libs/wmm | The World Magnetic Model (WMM-HR 2025), parsed from its COF file at compile time. The magnetometer’s reference field and the declination at a re-anchor. |
libs/factor_graph | Variables, factors, Levenberg-Marquardt, Schur-complement marginalisation, FixedLagSmoother, BatchSmoother. Eigen for the sparse solve. |
libs/imu_preint | Preintegration of the MTi’s Δq and Δv in the rotating ECEF frame, with bias Jacobians and covariance. |
libs/vehicle_estimator | The problem itself: the factors, initialisation, clock mapping, outputs. No zenoh, no capnp. |
The inputs
The MTi-610 is an IMU with no orientation filter. What it gives the estimator is delta_q and delta_v at 100 Hz: rotation and velocity increments the device has already integrated from its internal samples, with coning and sculling compensated. They are consumed directly and never re-integrated from rates. Each increment’s delta_v is expressed in the body frame at the end of its interval (DvFrame::end), which follows the strapdown equations in Xsens’ reference manual and has not been confirmed on a device.
The BD992 is a dual-antenna RTX receiver at 10 Hz: position (GSOF 2), velocity as speed, course and vertical rate (GSOF 8), and yaw and pitch of the antenna baseline (GSOF 27), with accuracy (GSOF 12) and fix type (GSOF 38) at 1 Hz. Wheel speeds and steering angle are expected later.
The car drifts, so sideslip and wheelslip are the normal case. No factor assumes the car moves the way it points.
Frames
States live in ECEF, with earth rate. A local-level frame would have been simpler to write, but a car covers enough ground in a session for the tangent plane’s curvature and the gravity direction to change, and ECEF with the Coriolis and transport terms written out is exact where a local frame is an approximation that has to be re-anchored.
Per keyframe (one per GNSS epoch, 10 Hz) the state is the IMU’s attitude R_e_i, its position and velocity in ECEF, and the gyro and accelerometer biases: 15 degrees of freedom. Beside them is the installation, one set per one-second segment: the IMU’s mounting rotation into the body (3), the lever arm from the IMU to the primary antenna in the IMU frame (3), and the boresight of the antenna baseline relative to the IMU as yaw and pitch offsets (2). See Learned calibration.
The body frame is SAE J670: x forward, y right, z down (FRD). The reference point (the CG, or the rear axle) is configured. Outputs are reported at the reference point, with attitude and velocity in local NED. Sideslip is β = atan2(v_y, v_x) of the body velocity at the reference point, positive to the right, and flagged invalid below 2 m/s, where it has no meaning.
Gravity is WGS84 normal gravity, with its small north component at latitude and height, tilted over North America by NGS’s DEFLEC2022 deflection of the vertical (see Gravity), behind a GravityModel interface so the factors never know which.
Why our own smoother
GTSAM’s IncrementalFixedLagSmoother is the obvious off-the-shelf answer and was the reference while this was written. Writing our own was decided at the start, before any code; three things kept it the right call. It is a large dependency (Boost, optionally TBB, its own build) to carry into a Yocto cross build for one node. Its factors’ Jacobians are mostly hand-written, where csym generates them from the residual at compile time, so a new sensor is a residual and nothing else, and a Jacobian cannot disagree with the function it differentiates. And the estimator needs control over exactly the parts GTSAM hides: what happens to a marginal prior’s linearisation point, and what is stamped as never to be marginalised.
What was written is small. factor_graph assembles the whitened Jacobians into a sparse normal matrix, solves it with Eigen’s SimplicialLDLT under Levenberg-Marquardt, and folds variables that fall out of the window into a LinearPrior by Schur complement. BatchSmoother is the same code with an infinite lag, which is what keeps the offline RTS-style solve open: its tests prove that for a linear-Gaussian chain the fixed-lag smoother with a lag of zero is the Kalman filter, that with a lag of L it is the RTS smoother over the window, and that the batch is the RTS smoother over everything, all to 1e-9.
Robust losses are inside the residuals (pseudo-Huber, transition at 3σ), so the optimiser never sees them. A measurement more than 30σ from the prediction is gated out entirely, unless 5 in a row have been: then the prediction is what is wrong, and the next one goes in anyway. Without that lockout breaker, one bad stretch locked the estimator out of the very data that would have corrected it.
First-estimate Jacobians
When a keyframe is marginalised, what it knew survives as a quadratic in the variables it touched, linearised at their estimates at that moment. That linearisation point is then frozen. Re-linearising a marginal prior at a later estimate invents information the discarded factors never carried, and the smoother becomes confidently wrong about exactly the directions it cannot observe. Yaw before the car has turned is the case that matters. The freeze is the whole of first-estimate Jacobians (FEJ) as this code applies it: the prior’s residual is R·d + e, where d is the local coordinate from the frozen point, and the point never moves.
What cannot be observed
Some of the car’s description is a definition, not a measurement. The reference point cannot be recovered from IMU and GNSS: every point of a rigid body moves consistently, so a different point is a different but equally consistent answer. It is configured, and a wrong one is a wrong sideslip angle that looks right. The IMU-to-body rotation is in the same position as far as IMU and GNSS go – both agree with any mounting – and it is learned only because two statements about the CAR pin it: a parked body is level, and a body running straight does not slide. A yaw error in it adds straight onto β. The lever arm and the boresight are observable while the car turns and accelerates, so they are estimated from measured priors (2 cm and 1°); the lever arm’s height is the weakest axis (about 6 cm of sigma after a figure-of-eight drive), because body roll and pitch are small.
Parked, roll and pitch trade against the accelerometer bias: a tilt and a bias produce the same specific force. The estimator cannot separate them until the car moves, and the parked test checks that the bias stays inside its bound and that the reported sigmas cover the error, not that tilt is exact.
Course over ground is not heading. The direction the car travels differs from the direction it points by the slip angle, which is the thing this estimator exists to measure. So the initialiser never takes heading from course: without a dual-antenna yaw, or a magnetometer it has learned, it waits.
The magnetometer’s vertical hard iron and the soft-iron terms involving z are in the same position as the reference point, for a different reason: learning them needs the car to roll and pitch, and it barely does. They stay near their priors indefinitely, and the reported heading sigma includes them.
Until 2026-09-23 the lever arm and the boresight were single kStatic variables, never marginalised. That is correct for a constant, and it is why they were replaced: see Learned calibration.
Time alignment
The MTi and the BD992 have no wire between them yet. Each stream has its own clock (the MTi’s 10 kHz tick counter, the receiver’s GPS time) and each sample has a host arrival time. Arrival is device time plus a clock offset plus a latency that varies but never drops below a floor, so the minimum of host − device over a 20 s window is the offset plus the floor, and it is stable where any one difference is not. Mapping IMU time into GPS time through both minima leaves one unknown: the difference between the two latency floors. That is imu.time_offset_s, calibrated by hand. In simulation it is the simulated GNSS latency minus the IMU latency; estimator_sim prints it.
The mapping slews rather than steps, at most 2 ms per second, which moves an IMU stamp by 20 µs per sample. That is invisible to the preintegration, yet fast enough to follow the few milliseconds the minimum settles by in a run’s first seconds, and it means a sample is never stamped before the one it follows.
This is the naive alignment. The plan is to bring both sensors’ sync outputs into timed GPIOs on the N100 (or to couple their sync pins), and a PPS clock will replace this behind the same interface. Until then, 10 ms of residual skew at 20 m/s is 0.2 m of error.
Pairing GNSS records
bd992_bridge publishes one topic per GSOF record and fuses nothing. The estimator needs a GPS time on every fix to line it up with the IMU, and neither position (GSOF 2) nor velocity (GSOF 8) carries one.
The first design stamped every GSOF sample in the bridge with the transmission’s sequence number and GPS time, in the zenoh attachment beside the schema fingerprint. It was dropped. An attachment does not survive bag record and bag play, so every recorded drive would have lost the pairing, and the addition would have broken the fixed 8-byte layout the fingerprint check reads.
What the node does instead is map_match’s rule: pair by arrival age. Records arriving within 15 ms of the first form an epoch, and a record type arriving twice closes it early. The epoch’s time is GSOF 1’s (same transmission, microseconds apart), or GSOF 27’s own time of week with the week from the last GSOF 1 or 16. An epoch with no time is dropped and counted, never guessed. The 1 Hz accuracy and fix-type records are used by age (fresh within 2.5 s and 5 s), because batch membership would leave nine epochs in ten without a sigma. The bridge is untouched.
An epoch becomes a keyframe only once the IMU is 0.15 s past it, so one that arrives after its successor still gets its turn.
Bugs that shaped the code
Missing earth-rate terms. The first IMU model rotated gravity and Coriolis correctly but left out the terms from rotating the preintegrated specific force out of the frame it was summed in. Parked, the residual was ⅙ ω×g Δt³, 8e-5 m over a second, and it showed as position drift. The velocity now carries −ω×(R Δv Δt + R Δp) − ω×g Δt² and the position −ω×v Δt² − ω×(R Δp) Δt − ⅓ ω×g Δt³, exact to first order in ωΔt (7e-6 over a 0.1 s keyframe). Both are written once in imu_preint/imu_model.h, shared by the factor and the runtime propagation. A sub-sample slope correction, −Δt/12·(a_k − a_{k−1}), cut the skidpad position error, and its bias Jacobians (including the previous sample’s) were then added.
A segfault from a stale sparsity pattern. LM reused the LDLT’s symbolic analysis across iterations. Marginalisation and gating change the pattern between iterations, and a factorisation reusing a stale ordering indexed past the end of it, silently with assertions off. An ASan build found it. compute() now runs every iteration, and the regression test crashes on the mutant.
Initialisation that threw away its history. A failed initialisation attempt dropped every buffered IMU sample, so the next attempt never had enough to level on and the estimator never started. It now trims to the last second and requires at least 0.4 s of IMU before trying.
A roll error of 39° on a moving restart. Levelling from the mean specific force assumes the car is not accelerating. Re-initialising mid-drift, it was, and roll came out 39° wrong. The initialiser now subtracts the acceleration seen between two GNSS velocities, taking a few degrees of roll uncertainty instead of ten.
Calibration lost on restart. A reset threw away the estimated lever arm and boresight along with the state, and the next minute re-learned them. They are now carried across resets with their covariance.
Out-of-order GNSS. A receiver’s latency jitters, so an epoch sometimes arrives after its successor, and it was dropped as late. Hence the 0.15 s reorder window, and a faster clock slew to follow the latency minimum.
The last hand-derived Jacobians. Until 2026-09-24 the preintegrator’s covariance transition and bias Jacobians were written out by hand, and the slope term’s bias Jacobian through the previous sample’s force was once missing. They now come from one csym function per sample. Moving them found one more thing: a sample’s noise also reaches position through the next sample’s slope term, so the covariance is carried with the previous force in its state. A Monte Carlo over three samples, where that path is 7% of the position variance, now checks it.
Every regression test here was mutation-checked by reverting the fix and watching it fail: the Schur complement, the north gravity term, the Coriolis sign, the frame-rotation term, the sideslip sign, the stale LDLT and the vertical-velocity sign (caught by the unit test only).
Measured accuracy
In simulation, 2026-09-23, with the default sensor model (RTX-grade GNSS, MTi-610 datasheet noise):
| Scenario | Position | Attitude | Sideslip |
|---|---|---|---|
| skidpad, figure of eight, spin | about 3.5 cm | under 0.3° | under 0.3° |
| through the node’s capnp pipeline (skidpad, worst case after 12 s) | under the test’s 0.10 m bound | yaw 0.093° | 0.12° |
The whole-drive batch against the fixed-lag output over the same drive:
| Fixed-lag RMS | Batch RMS | |
|---|---|---|
| yaw | 0.044° | 0.008° |
| sideslip | 0.045° | 0.011° |
Consistency is measured as NEES over twenty figure-of-eight drives with independent noise, averaged per keyframe: 1.4 for position and 1.9 for velocity (3 dimensions each) and 0.8 for yaw (1). All are below their dimension, so the reported sigmas are mildly conservative: a consumer trusting them is not misled, and a little information is left on the table.
Solver cost
Measured 2026-09-24 as CPU time per keyframe over a two-lap track, with the magnetometer and barometer on. Before, every keyframe ran LM to its eight-iteration cap: 8.9 ms. After, two iterations: 1.7 ms. Accuracy and NEES are unchanged to the last printed digit. In order of what it bought:
- Damping started effectively at zero. Marquardt’s
λ·diag(H)throttled the window’s weak common mode against the IMU chain’s 1e11 diagonal, so convergence was linear. - The solve ends on the step inside its tolerance without asking the cost, which cannot resolve it in ECEF. The tolerance is 0.1σ: the step is taken, so what is left is the next one, about 15× smaller.
- The covariance comes from the solve’s last factorisation, before marginalisation, by forward solve alone.
- The symbolic analysis is reused while the pattern is identical, and the marginal prior uses Cholesky when it is well conditioned.
One thing was tried and removed. Assembling straight into a recorded pattern saved nothing measurable once the rest was done, and it was the most intricate of the changes. vehicle_estimator_test_solver asserts the iteration count, the absence of rejected steps and that the covariance reuse happens, because none of these shows in the estimate, only in the time.
Learned calibration
Added 2026-09-23. The installation – mounting, lever arm, boresight – is a prior the car refines, and what it learns is kept between sessions.
A random walk, not a constant. A never-marginalised constant can only become more certain. Over a long session it grows overconfident, soaks up every small model error with growing conviction, and cannot follow an antenna that was knocked. Each segment (1 s) now has its own variables, joined by a walk at 0.05°/√h (mounting and boresight) and 5 mm/√h (lever arm), and they are marginalised with the window like everything else; a prior carried over a restart keeps its full 8×8 covariance. The batch solve then returns the calibration per segment, smoothed: its history over the drive. The refactor left the consistency figures exactly where they were (NEES 1.41, 1.88, 0.80), and making the walk effectively infinite fails the check that the batch carries the lever arm learned late back to the drive’s first segment.
The mounting from two assumptions. Parked, the body is level to within the grade (σ 1.5°), which over stops facing different ways gives roll and pitch. Running straight and true, the body neither slides nor heaves (σ 0.5°), which gives yaw and pitch. The second is false in a drift, so its gate is strict and held for 2 s (above 8 m/s, yaw rate under 1.5°/s, lateral acceleration under 0.5 m/s²), with one factor a second at most because the error is correlated. In simulation: a 2° mounting yaw read as 2.04° of slip on the first straight and 0.04° on the last, with the mounting learned to 0.02°; roll and pitch 2° out came in to 0.13° over seven stops; a whole skidpad drift (3° to 26° of slip) judged nothing straight and left the mounting where it was. With a fast walk, an IMU knocked 1.5° mid-drive was followed to 0.008°; without one the estimate ended 0.52° out, a compromise between the minute before and the minute after.
A trap in testing it. The default MTi mounting is a half turn about x, and every half turn is its own inverse – so is a small yaw error on one – which makes R_b_i and R_b_iᵀ the same rotation. A factor using the mounting backwards passed every test on the default mounting, and on a quarter turn composed with it (still a half turn). The mounting tests and the factor Jacobian tests use an IMU on its side (120° about (1,1,1)); both direction mutations fail there.
Kept per group, matched by hash, never deleted. Rows go into SQLite (see calibration_store) one group at a time, so re-measuring the lever arm does not throw away a well-learned mounting. Each row carries an FNV-1a hash of the configured means it was learned against – the boresight’s covers both antennas – and a model version; sigmas are left out, so a change of confidence keeps what was learned. Nothing is deleted: a row whose hash no longer matches simply stops being found and stays in the history. The golden hashes in the test were computed independently in Python from the byte layout, so a change in how the doubles are hashed on another host or compiler cannot pass unnoticed.
When a row is written. When a group has moved more than 1σ of its last row (Mahalanobis, so metres and radians compare) or a sigma has halved; at most every 15 minutes; never in the first two minutes after a start; for the mounting, only once something has taught it; and once more at shutdown. The policy runs on the estimator’s GPS time, so a replay decides as the drive did. A stored covariance is widened by 4 when loaded, floored at 0.02° or 2 mm, and capped at the config’s sigma. Over five simulated sessions against one file: the second started with the mounting 0.027° out instead of 2°; a re-measured lever arm orphaned only the lever-arm and boresight rows; a file that was not a database was refused untouched; and an IMU knocked 1.5° between sessions was flagged at 6.6σ. The same held live over the bus with bag play, and a --replay without --calibration-db left the file alone.
Power loss. WAL with synchronous=FULL: a torn write is impossible, and a reported write is durable, provided the storage honours a flush. Rows written at one moment go in one transaction. A database that fails quick_check at open is moved aside and begun again rather than refused, because refusing it would leave the car on its config until someone came to look; the checks run with checkpoint-on-close off, since closing would otherwise write the log into the damaged file before it was set aside. That last trap was found by the recovery test, not by reasoning about it.
Magnetometer, barometer and the start without GNSS
Added 2026-09-24. Both sensors are on the MTi already and arrive in the same packets as the increments. The assembler joins them by packet counter, one sample (10 ms) behind.
Why the honest value is narrow. A learned gyro loses under 0.1° of yaw a minute, and a car’s magnetometer is good to about 3°. While GNSS is good the magnetometer is noise. It earns its place in three places: at a start with no GNSS, parked through an outage (nothing else observes heading when the car does not move), and when the gyro bias wanders over minutes. The tests assert those three and assert that it costs nothing elsewhere; they do not claim more. The barometer is similar: with RTX the GNSS height is better, and the smoother weights it so. Through an outage or a degraded fix the barometer holds height that an IMU’s vertical channel cannot.
Keyframes had to leave the GNSS clock first. A factor needs a keyframe to attach to, and keyframes existed only at GNSS epochs, so an outage had nowhere to put a magnetometer reading. It also had a growing IMU buffer and a frozen covariance. Inertial keyframes every 0.1 s fixed all three. They wait max_gnss_wait so a late epoch is never pre-empted, and they sit on a GPS-time grid so returning epochs land on them rather than a hair after (two keyframes 1 ms apart made the IMU factor’s covariance not positive definite).
The gate is Mahalanobis on the whole prediction. The first gate compared a reading’s strength and dip against WMM with fixed tolerances. It was either so tight that a large, unlearned hard iron failed every reading and was never learned, or so loose that steel alongside was accepted. The prediction’s covariance, attitude plus the 9×9 calibration plus the sensor, is wide while the calibration is unknown and narrows as it is learned. That is exactly the gate’s shape.
One graph for the cold start, one special step. States are ECEF, and without a position “level” and “north” mean nothing there. So the start without GNSS anchors at a placeholder and runs the outage machinery unchanged. Gravity and earth rate at the wrong latitude are absorbed by the biases and re-learned within seconds. The one special step is the re-anchor at the first fix: R_e_n(fix)·Rz(declination)·R_e_n(anchor)ᵀ applied on the left of the attitude, which leaves its right-perturbation covariance unchanged. No position is kept between power cycles, which is why there is no WMM declination until the fix, and why the heading before it is MAGNETIC and flagged so.
Two findings only the node path showed. With no receiver there is no GPS time either: TimeMapper maps IMU time through both sensors’ offsets, so every sample was discarded and the “cold start” happened after the fix. The IMU now runs on host time until GNSS is heard. The anchored attitude and biases then carry over into a new graph on GPS time, since no increment can be preintegrated across the change of clock. Second, attitude sigmas read exactly zero while anchored. The cause was a 10 m placeholder position prior against an IMU chain holding each keyframe to about 1e11 of information: the scaled pivot, about 1e-13, is at the backward error of the factorisation itself. The covariance was refused, and zero sigmas passed the attitude bound. The anchor is now held to 1 cm (it is the frame’s origin), and nothing is claimed valid without a covariance. The same precision limit returned once a long outage took position sigma past a few metres: in a five-minute outage, 814 of 2,987 keyframes had no covariance and a good attitude stopped being claimed valid. Since 2026-09-24 the covariance falls back to a QR of the Jacobian there, which keeps half the exponent; a ten-minute outage now keeps a covariance on every keyframe. The fallback costs about 7 ms, and only while the LDLT cannot answer.
Kept and not kept. The magnetometer’s hard and soft iron and the barometer’s airflow are properties of the installation and are stored like the mounting. A stored magnetometer is trusted at a cold start only if its row records as much learning against the antennas as this session would need. The barometer’s offset is weather and is never stored.
Gravity
Added 2026-09-24. The plumb line leans off the ellipsoid normal by the deflection of the vertical: over the contiguous US 6.6” RMS, 20” at the 99th percentile, 48” at worst. As an acceleration that is 3e-4 m/s² RMS, the level of an MTi-610’s bias instability. The accelerometer bias absorbs it only while it stays put.
Verbatim, loaded at run time. NGS publishes DEFLEC2022 as a 1-arcminute grid over North America, 233 MB per component. Both files are kept exactly as NGS distributes them, in Git LFS under models/. They are installed beside the binaries and memory-mapped at start, so only the pages under the car are ever read.
A compile-time 3’ extract of the contiguous US was built first (2.5 MB, checked by static_assert at build time, as wmm is) and replaced by this before it shipped. The verbatim files keep the model’s full resolution. The 3’ cut cost 0.67” RMS and 3” at the 99th percentile, and 10’ would have cost 9”. They also keep NGS’s exact bytes, which can be checked against its published hashes, and the model can be updated without a rebuild. The price: 467 MB in LFS (quota), and an install step the image must honour. The build refuses to install LFS pointer files, and the node degrades, not stops, when the model is missing.
Three things were checked, not assumed.
- The interpolation: NGS specifies a 4 × 4 bicubic, and of the kernels that name covers, only Catmull-Rom reproduces NGS’s published test values to their last digit. Bilinear misses by up to 3”, other bicubics by 1.5”.
- The signs: across the Guam grid, xi and eta match minus the geoid’s north and east slopes with correlation 1.000. That is Vening Meinesz, so gravity gains -g xi north and -g eta east.
- The test coordinates themselves: typed to four decimals, they missed Mount Whitney by 0.01”, because on that slope 0.00005 deg is 0.01”.
What it does in the estimator. Ignoring a 29.4” lean in the Colorado foothills leaves the estimated attitude leaning by it. Modelled, the mean tilt error is identical to a world with no deflection. A three-minute outage on a straight road there did not show the gravity at all: an unaided MTi’s heading error on a straight dominates, and shuffles by metres with any change to the arithmetic. That scenario is not in the tests, because a comparison that passes by luck is worse than none.
Deferred
The reference point stays a definition until steering and wheel speeds give a vehicle model to estimate it against. A time-offset state (estimating imu.time_offset_s instead of calibrating it) and the PPS clock are next for timing. EGM2008’s gravity anomaly (magnitude, not direction) would slot in behind GravityModel beside DEFLEC2022. Wheel speeds (per-wheel slip ratio from the reference-point velocity plus ω × the wheel’s position) and steering angle are a factor each; the smoother never learns sensor types.
Unverified on hardware
As of 2026-09-24, none of these has been checked against a device, and none can be caught in simulation because the simulator shares the reading:
- the MTi-610’s axis convention, and the frame
delta_vis expressed in - GSOF 27’s variance units (taken as rad²) and pitch sign (taken as nose-up positive)
- the receiver emitting long-form GSOF 27 with variances on our unit
- the latency floors, and so the right
imu.time_offset_s - the magnetometer’s axes against the increments, and what its clipping flag means in practice
- the barometer’s airflow sign where the IMU is mounted, and whether cabin pressure (a window, the HVAC fan) is a step the robust loss absorbs
Before trusting a real drive: parked, gravity should land on body +z; a left turn should decrease yaw in step with GSOF 27; the status topic’s lever arm should settle near the tape-measured one.