An Extended Kalman Filter fusing IMU, wheel odometry and GNSS for a planar rover — which also estimates the gyro bias and encoder scale factor that make dead reckoning drift in the first place.
Over 20 independent missions: 0.37 m position RMSE (0.08% drift) against 52 m for dead reckoning and 5.8 m for raw GNSS, and 0.9 m of drift through a 45-second GNSS blackout.
Results are measured against a simulated rover trajectory, not a real vehicle.
This is not a shortcut, it is the only way the question can be asked. Filter accuracy is the distance between an estimate and the truth — and on a real rover the truth is precisely what you do not have, which is why you are running a filter at all. Getting it requires motion capture or an RTK base station. Simulation provides it exactly and for free.
The honest cost: a simulator contains only the error sources its author modelled. Real IMUs have temperature-dependent bias and scale-factor error, real GNSS has multipath and correlated errors, real wheels slip in ways no Gaussian captures. Treat these numbers as a clean-room upper bound.
20 independent missions, each with its own trajectory, its own gyro bias, and its own noise. Figures are median across runs.
| estimator | RMSE (m) | p95 (m) | max (m) | final (m) | drift (%) | heading RMSE (deg) | after 45s outage (m) |
|---|---|---|---|---|---|---|---|
| EKF | 0.37 | 0.63 | 1.87 | 0.19 | 0.08 | 3.9 | 0.91 |
| GNSS only | 5.75 | 15.56 | 21.73 | 2.00 | 0.94 | 94.9 | 20.68 |
| dead reckoning | 52.14 | 100.18 | 104.81 | 91.51 | 47.54 | 86.0 | 35.07 |
Calibration states recovered (median absolute error): gyro bias to 0.0026 rad/s, encoder scale to 0.0025 — removing 90% of a 2.5% scale error that the rover was never told about.
Filter consistency: NIS = 2.02 against a theoretical 2.0 for a 2-DOF measurement. This matters more than the accuracy numbers. A filter can be accurate today and still be over-confident — understating its own uncertainty, which makes it reject good measurements and diverge later. The normalised innovation squared test catches that; raw error cannot.
The outage starts two minutes in, after the filter has calibrated itself.
| outage | distance driven | EKF error | dead reckoning error | EKF drift |
|---|---|---|---|---|
| 15 s | 11.5 m | 0.37 m | 33.6 m | 3.2% |
| 30 s | 24.0 m | 0.98 m | 50.3 m | 4.1% |
| 60 s | 38.9 m | 1.97 m | 40.3 m | 5.1% |
| 120 s | 86.6 m | 8.29 m | 91.3 m | 9.6% |
| 240 s | 171.2 m | 17.30 m | 114.5 m | 10.1% |
Degradation is graceful rather than catastrophic: error grows with distance, not explosively, and stays near 3-10% of distance driven. That is the practical payoff of estimating bias and scale — dead reckoning from calibrated sensors is far better than dead reckoning from raw ones.
The three sensors fail in complementary ways, which is the entire argument:
| sensor | strength | failure mode |
|---|---|---|
| Rate gyro | smooth, high-rate heading change | bias — heading error grows linearly, position error quadratically |
| Wheel encoders | accurate short-range distance | scale error — systematic, grows with distance, never averages out |
| GNSS | never drifts, absolute | noisy (1.4 m), slow (5 Hz), disappears under cover |
Neither odometry nor GNSS is usable alone at both timescales. Odometry is locally excellent and globally hopeless; GNSS is the reverse.
The interesting part is observability. A gyro cannot detect its own bias and encoders cannot detect their own scale error — from their own measurements, both errors are invisible. But an absolute position fix contradicts a drifting heading and a mis-scaled distance in ways the filter can attribute to the right cause. Adding them to the state turns two unknown error sources into two estimated quantities.
The scale factor converges from an uninformed 1.0 to the true 1.025 within about 60 seconds and stays locked. During the shaded outage the bias estimate goes flat — with no GNSS there is nothing to observe it with, so the filter correctly stops updating it rather than inventing information.
IMU 100Hz ──predict──► ┌──────────────────────────────────┐
│ state: x, y, θ, v, b_gyro, s_enc │
encoder 50Hz ──update──► │ covariance: 6×6 │ ──► pose
└──────────────────────────────────┘
GNSS 5Hz ──update──► gated on innovation
State: [px, py, theta, v, b_gyro, s_enc] — pose and speed, plus the two
error sources estimated alongside them.
Prediction uses the IMU as a control input (strapdown mechanisation): the gyro says how heading changed, the accelerometer how speed changed, both corrupted by the biases in the state. Position integrates at the midpoint heading, which is markedly more accurate than the start-of-step heading on curves.
Encoder update models the measurement as s · v, not v. Writing it that
way is what makes the scale factor estimable instead of silently corrupting the
speed estimate.
GNSS update is gated on normalised innovation squared, so a multipath jump
is rejected rather than absorbed. (On simulated data the gate is near-free
insurance rather than a demonstrated win — see docs/RESULTS.md.) Covariance uses the Joseph form, which
stays symmetric and positive definite far longer in floating point than the
shorter (I − KH)P.
The Jacobian term everyone drops. Heading at the midpoint depends on the
gyro bias, so the bias propagates into position through
∂x/∂b = ½·v·sin(θ)·dt². It is a second-order term and easy to omit — and
omitting it makes the filter over-confident about position, which shows up as
inflated NIS long before it shows up as visible error.
Angle wrapping is not cosmetic. An estimate of +179° against a truth of
−179° is 2° of error, not 358°. Every heading comparison in this package goes
through one wrap_angle function, and interpolating a heading series is done
through its sine and cosine so a wrap does not produce a spurious sweep around
the circle.
Outage error can be lower than steady-state error. Counter-intuitive but real: during normal operation the filter is continuously pulled around by 1.4 m GNSS noise, while during a short outage it propagates smoothly on calibrated odometry. For outages under roughly 30 seconds the smooth estimate is closer to truth than the GNSS-corrected one. This inverts once accumulated drift exceeds the GNSS noise floor.
git clone https://github.com/Ashok-Kumar17/rover-state-estimation.git
cd rover-state-estimation
pip install -e ".[dev]"
python -m rse demo # one mission, printed
python -m rse evaluate --out results --figures # the table above (~30 s)
python -m rse outage --out results --figures # the outage sweep (~60 s)
pytestfrom rse import generate, simulate_sensors, run_all, evaluate
traj = generate(seed=1)
suite = simulate_sensors(traj, seed=2)
estimates = run_all(traj, suite)
for name, est in estimates.items():
print(name, round(evaluate(traj, est).position_rmse, 3), "m RMSE")from rse import RoverEKF
ekf = RoverEKF()
ekf.predict(gyro_z=0.02, accel_x=0.1, dt=0.01) # IMU sample
ekf.update_encoder(v_meas=1.2) # wheel speed
ekf.update_gnss(x_meas=10.4, y_meas=3.1) # position fix
print(ekf.position, ekf.position_std)src/rse/
├── trajectory.py ground-truth motion, angle wrapping
├── sensors.py IMU / encoder / GNSS models with realistic errors
├── ekf.py the filter: predict, updates, innovation gating
├── fusion.py runners for the EKF and both baselines
├── metrics.py accuracy, outage drift, NIS consistency
├── pipeline.py Monte Carlo experiment and outage sweep
├── plots.py trajectory, error, convergence figures
└── cli.py python -m rse
tests/ 80 tests
- Simulated data, as described above.
- Planar only. No roll, pitch, or terrain slope. A real rover on rough ground needs a full 3D attitude filter.
- No loop closure. This is pure filtering, not SLAM — with GNSS gone indefinitely, error grows without bound.
- Gaussian noise assumption. Real GNSS multipath is heavy-tailed; the innovation gate helps but does not make the filter robust in a statistical sense.
- EKF, not UKF. The linearisation is fine at these rates and noise levels, but an Unscented filter would handle a poorer initial heading estimate better.
MIT — see LICENSE.