The problem
Automotive radar reports range and range-rate with noise, occasional missed detections and outliers. Adaptive cruise control and collision warning cannot act directly on those raw values: a jittery range estimate produces jerky braking and false alarms. A Kalman filter is the standard first answer, and building one from the equations up teaches the prediction–correction idea behind every tracker used in ADAS.
Objectives
- Model the lead vehicle's relative motion with a constant-velocity and a constant-acceleration state model.
- Generate radar-like range and range-rate measurements with realistic noise and dropouts.
- Implement the Kalman filter by hand in Python and with trackingKF in MATLAB, and show both agree.
- Tune process and measurement noise and check filter consistency with NEES and NIS tests.
- Compare RMSE of the filtered estimate with raw measurements and a moving-average filter.
What you need
Tools and toolboxes
- MATLAB — Scripting the scenario, the filter and the Monte Carlo runs, and plotting error and consistency statistics.
- Sensor Fusion and Tracking Toolbox — The trackingKF object as a reference implementation to check the hand-written filter against.
- Automated Driving Toolbox (optional) — drivingScenario with drivingRadarDataGenerator to produce more realistic radar detections of a lead car.
- Python with NumPy and matplotlib — A from-scratch Kalman filter written in matrix form, used to show every equation explicitly.
Software
- MATLAB R2023b or later
- Sensor Fusion and Tracking Toolbox
- Python 3 with NumPy, SciPy and matplotlib
- Jupyter or VS Code
Extra parts and data
None beyond the software above.
Key concepts
- State-space model
- The lead vehicle's relative state is written as a vector, for example [range, range-rate], that evolves through a transition matrix F each time step. Unknown accelerations are treated as process noise.
- Predict and update
- The filter first predicts the state and its covariance forward in time, then corrects them with the new measurement. The Kalman gain decides how much the measurement moves the estimate.
- Process noise Q and measurement noise R
- Q expresses how much the target can change its motion between steps; R expresses the sensor's noise. Their ratio sets how smooth versus how responsive the estimate is.
- NEES and NIS
- Normalised estimation error squared and normalised innovation squared compare actual errors with the filter's own claimed covariance. If they sit inside chi-square bounds, the filter is consistent rather than over- or under-confident.
Step-by-step guide
- 1
Define the tracking problem and targets
Write a short requirement: track one lead vehicle between 5 and 120 m ahead, at a 20 Hz radar rate, with range RMSE and range-rate RMSE targets you set from the downstream use (for example, a smooth input to ACC). State the assumed sensor noise (for example 0.5 m range and 0.2 m/s range-rate standard deviation) and a 5 per cent missed-detection rate. List the driving profiles you will test: steady following, gentle braking, hard braking and a lead car accelerating away.
Checkpoint: A one-page requirement sheet with sensor assumptions, test profiles and numerical accuracy targets.
- 2
Generate ground truth and noisy measurements
In MATLAB, script the ego and lead vehicle speed profiles and integrate them to get true relative range and range-rate at 20 Hz. Add Gaussian noise with the stated standard deviations, randomly drop 5 per cent of samples and insert a few large outliers. Optionally build the same scenario in drivingScenario with drivingRadarDataGenerator to compare. Save truth and measurements to CSV with a fixed random seed, so MATLAB and Python use identical data.
Checkpoint: Plots of true and measured range for each profile, with the seed and noise settings recorded in the file header.
- 3
Write the filter equations
Derive the discrete constant-velocity model with state x = [r, á¹™], transition matrix F = [1 dt; 0 1] and process noise from a white-acceleration model scaled by a spectral density q. The measurement matrix H is the identity because radar measures both range and range-rate. Also derive a constant-acceleration version with a three-element state. Write the predict and update equations on paper with matrix dimensions before coding, and identify where a missed detection skips the update.
Checkpoint: A derivation sheet with F, Q, H and R written out for both models, checked by a peer or guide.
- 4
Implement the filter in Python
Code the predict and update steps with NumPy matrices, using the Joseph form of the covariance update for numerical robustness. When a measurement is missing, run only the prediction. Initialise the state from the first measurement and set a generous initial covariance. Log the state, covariance diagonal, innovation and Kalman gain at every step. Keep the code short and readable, because explaining every line is part of the viva.
Checkpoint: The Python filter runs on all four profiles and produces a smoother range trace than the raw measurements.
- 5
Cross-check with trackingKF in MATLAB
Create a trackingKF object with MotionModel set to '1D Constant Velocity', and set its ProcessNoise and MeasurementNoise to match your Python values. Feed the same CSV measurements with predict and correct calls. Compare the two state histories sample by sample. Differences beyond floating-point noise usually reveal a mismatch in how Q is scaled with dt or how the state is ordered, so resolve them before going further.
Checkpoint: Python and MATLAB state estimates agree within a very small tolerance across the whole profile.
- 6
Add outlier gating
Compute the normalised innovation squared for each measurement and reject it if it exceeds a chi-square gate for two degrees of freedom at a chosen probability (for example 99 per cent). Treat rejected measurements as missed. Show what happens to the estimate during a burst of outliers with and without gating. Record how many good measurements the gate wrongly rejects during hard braking, when the model is temporarily wrong.
Checkpoint: A plot showing an outlier pulling the ungated estimate while the gated filter ignores it.
- 7
Tune Q and R
Keep R at the known sensor noise and sweep the process noise intensity q over two decades. For each value, run 100 Monte Carlo trials per profile and record range and range-rate RMSE and the lag after the lead car starts braking. Plot error against q for steady and braking profiles; the best value differs between them, which motivates adaptive or multiple-model filters later. Choose one value and justify it from the requirement.
Checkpoint: A tuning plot of RMSE against q for each profile with the chosen value marked and justified.
- 8
Check consistency with NEES and NIS
Using the Monte Carlo runs, compute average NEES against truth and average NIS from innovations at every time step. Plot them with the two-sided 95 per cent chi-square bounds for the number of runs. A filter whose NEES sits above the bound is over-confident; below it, under-confident. Repeat for the constant-acceleration model. This shows the examiner that the filter's covariance can be trusted by downstream functions.
Checkpoint: NEES and NIS plots with chi-square bounds for both motion models and a written verdict on consistency.
- 9
Compare against baselines
Implement a five-sample moving average and an alpha-beta filter as baselines. Compute RMSE, maximum error and lag for raw measurements, moving average, alpha-beta, constant-velocity KF and constant-acceleration KF on every profile. Present a single table with mean and standard deviation across Monte Carlo runs. Note where the constant-velocity model lags in hard braking and how much the constant-acceleration model helps.
Checkpoint: A results table across all methods and profiles with mean and spread over 100 runs.
- 10
Report and demonstration
Write up the model, derivation, implementation, tuning study, consistency checks and baseline comparison. Include a short animation or plot showing the filtered lead-car range feeding a simple time-gap calculation, to link the work to ACC. Release the scripts with the fixed seeds so results can be reproduced. State the limitations: one target, simulated noise and no data association. Add a short section that explains, in plain terms, how the chosen q would be re-tuned for a different radar.
Checkpoint: A report and code package that reproduces every table and plot from a single script run.
Testing & validation
- Python and MATLAB trackingKF outputs agree on identical data.
- Range and range-rate RMSE below the stated targets on all profiles.
- NEES and NIS inside chi-square bounds for the chosen tuning.
- Outlier gating demonstrated on injected outliers.
- Comparison with raw data, moving average and alpha-beta filter.
Expected results
A documented, consistent Kalman filter for lead-vehicle tracking in MATLAB and Python, with a tuning study, consistency evidence and a quantified improvement over simple smoothing filters.
Take it further
- Track in two dimensions with range, azimuth and range-rate using an extended Kalman filter.
- Add a second target and simple nearest-neighbour association.
- Replace fixed q with an adaptive noise estimate and compare during braking.
- Feed the filtered range into a simple ACC model in Simulink.
Viva and review questions
Why does the filter lag when the lead vehicle brakes hard?
The constant-velocity model predicts no change in speed, so it treats the deceleration as noise until the innovations build up. A larger q or an acceleration state reduces the lag at the cost of more noise.
What does the Kalman gain tell you?
It is the weight given to the new measurement relative to the prediction. A gain near one trusts the sensor; a gain near zero trusts the model.
How did you know your filter was consistent?
Average NEES and NIS over Monte Carlo runs stayed inside chi-square confidence bounds, so the filter's covariance matched its actual errors.
Why use the Joseph form of the covariance update?
It keeps the covariance symmetric and positive semi-definite even with rounding errors or a slightly sub-optimal gain.
Learn it at DIYguru
These DIYguru courses and programmes teach what this project needs.