Week 9 — Sensor Integration and State Estimation: IMU, Bayes and Kalman Filters

← Course 1 syllabus

Overview

A robot never knows its true state — it infers it from noisy sensors. This week builds the state-estimation layer: ingest an IMU/sensor stream in the Week 8 runtime, model the noise, and recursively estimate state with a Bayes filter, realized concretely as a Kalman filter (and its extended form for nonlinear motion). The key idea is recursion: rather than re-process all history each step, you maintain a belief (mean + covariance) and update it as each measurement arrives — the foundation of every localization and tracking system.

This is where Courses 5 and 6 converge in application. Course 6 already covered the signal side — sampling the analog sensor, anti-aliasing, and digitally filtering the raw waveform — so here we assume clean, sampled measurements and focus on the estimation side, which is Course 5’s probability made recursive. The result is a state estimate good enough to feed the planner and the safety runtime.

Readings

  • PR (Probabilistic Robotics): recursive state estimation, the Bayes filter, the Kalman and extended Kalman filters, motion models, and sensor models. Extract: the predict/update cycle and the Gaussian assumptions.
  • BT (review): expectation, variance, and covariance. Extract: the covariance algebra the Kalman filter runs on.
  • Embedded AI: smart sensors, IMU preprocessing, and sensor fusion. Extract: practical IMU quirks (bias, drift, scale).
  • (ADC, sampling, anti-aliasing, and digital low-pass filtering of the raw signal: assumed from Course 6; probability foundations from Course 5.)

Key Concepts

The recursive Bayes filter

Maintain belief \(bel(x_t)=p(x_t\mid z_{1:t},u_{1:t})\). Each step: predict using the motion model \(p(x_t\mid x_{t-1},u_t)\) (belief spreads, uncertainty grows), then update using the measurement model \(p(z_t\mid x_t)\) (belief sharpens). This predict/update recursion is the universal structure; the Kalman filter is its exact solution under linear-Gaussian assumptions.

The Kalman filter

Under linear dynamics and Gaussian noise, the belief stays Gaussian and the filter is closed-form: a predict step (\(\hat x^- = F\hat x + Bu\), \(P^- = FPF^\top + Q\)) and an update step weighting prediction vs measurement by the Kalman gain \(K = P^-H^\top(HP^-H^\top+R)^{-1}\). \(Q\) (process noise) and \(R\) (measurement noise) encode trust; the gain automatically balances them. The EKF linearizes nonlinear motion/measurement models via Jacobians for the AV case.

IMU realities

An IMU gives accelerometer + gyro at high rate, but with bias, drift, scale error, and noise. Naive integration of acceleration to position drifts unboundedly (double integration of bias). Fusion with a slower absolute reference (the filter’s job) bounds the drift. The signal-conditioning — sampling rate, anti-alias filtering, and a digital low-pass on the raw stream — is Course 6 material applied here as the front end to the estimator.

Tuning and consistency

A filter is consistent if its reported covariance matches the actual error statistics. Mistuned \(Q/R\) gives an over- or under-confident estimate. Normalized innovation squared (NIS) and reliability checks (Course 5 calibration ideas) diagnose this.

Theory Exercises

  1. Derive the Kalman predict and update equations from the linear-Gaussian Bayes filter; identify where \(Q\) and \(R\) enter.
  2. Show the Kalman gain reduces to a weighted average of prediction and measurement, and its limits as \(R\to0\) and \(R\to\infty\).
  3. Linearize a nonlinear unicycle motion model for the EKF; write the Jacobian.
  4. Show why integrating IMU acceleration with constant bias produces position error growing as \(t^2\).
  5. Define filter consistency and describe how NIS detects a mistuned filter.

Implementation

In the Week 8 runtime, add a sensors/ ingestion path (real BNO055/ICM-20948 IMU or a simulated stream with injected bias/noise) and an estimation/ Kalman/EKF that fuses IMU with a position reference. Assume the raw signal is already sampled and pre-filtered per Course 6. Log true vs estimated state in simulation where ground truth exists.

Benchmark

Estimation error (RMSE) and filter consistency (NIS) vs a naive integrate-the-IMU baseline, under varying noise/bias. Per-update compute time in the real-time loop (it must fit the Week 8 budget). Ablate \(Q/R\) tuning to show under/over-confidence.

Expected baselines: the filter bounds drift that naive integration lets diverge; a well-tuned filter is consistent (NIS in range); per-update cost fits comfortably in the 100 Hz budget. Mistuned \(Q/R\) shows clear inconsistency.

Connections

The state estimate feeds the Week 6 planner and the Week 10 safety runtime (which needs a trustworthy ego state). It runs inside the Week 8 loop on Week 8’s timestamped data. Course 6 owns the signal front end; Course 5 owns the probability; this week composes them into recursive estimation.