In this project we utilized a kalman filter to estimate the state of a moving car with noisy lidar and radar measurements in a simulator. A Kalman Filter to process only laser measurements was also conducted here. For project instructions, check this.
Why use Kalman Filter: sensor measurement & physical model are not perfect
How: assuming sensor measurements & physical model are Gaussian distributions
- Predict states based on physical models
- Update states based on sensor measurements
KF / Extended KF / Unscented KF
- Physical Model
- Linear: KF
- Non-linear: EKF / UKF
- Linearization approximation
- One point using Taylor expansion: EKF
- Multi-points using Sigma points method: UKF
- Linearization approximation
- Physical Model
- initializing Kalman filter variables
- predicting where our object is going to be after a time step Δt
- updating where our object is based on sensor measurements
Then the prediction and update steps repeat themselves in a loop.
To measure how well our Kalman filter performs, root mean squared error comparing the Kalman filter results with the provided ground truth was calculated.
These three steps (initialize, predict, update) plus calculating RMSE encapsulate the entire extended Kalman filter project.
Files in the src Folder
- main.cpp - communicates with the Simulator receiving data measurements, calls a function to run the Kalman filter, calls a function to calculate RMSE
- FusionEKF.cpp - initializes the filter, calls the predict function, calls the update function
- kalman_filter.cpp- defines the predict function, the update function for lidar, and the update function for radar
- tools.cpp- function to calculate RMSE and the Jacobian matrix
Here is a brief overview of what happens when you run the code files:
- Main.cpp reads in the data and sends a sensor measurement to FusionEKF.cpp
- FusionEKF.cpp takes the sensor data and initializes variables and updates variables. The Kalman filter equations are not in this file. FusionEKF.cpp has a variable called ekf_, which is an instance of a KalmanFilter class. The ekf_ will hold the matrix and vector values. You will also use the ekf_instance to call the predict and update equations.
- The KalmanFilter class is defined in kalman_filter.cpp and kalman_filter.h. You will only need to modify 'kalman_filter.cpp', which contains functions for the prediction and update steps.
There are many intuitive materials taking about the Kalman Filter, thus it’s not necessary to add one more. Here are some key points to myself.
The process flow can be shown as
- x is the mean state vector. For an extended Kalman filter, the mean state vector contains information about the object's position and velocity that you are tracking. It is called the "mean" state vector because position and velocity are represented by a Gaussian distribution with mean x.
- P is the state covariance matrix, which contains information about the uncertainty of the object's position and velocity. You can think of it as containing standard deviations.
- k represents time steps. So x_k refers to the object's position and velocity vector at time k.
- The notation k+1∣k refers to the prediction step. At time k+1, you receive a sensor measurement. Before taking into account the sensor measurement to update your belief about the object's position and velocity, you predict where you think the object will be at time k+1. You can predict the position of the object at k+1 based on its position and velocity at time k. Hence x_(k+1∣k) means that you have predicted where the object will be at k+1 but have NOT YET taken the sensor measurement into account.
- x_(k+1) means that you have now predicted where the object will be at time k+1 and then used the sensor measurement to update the object's position and velocity.
- x - state vector
- P - uncertainty covariance matrix of state x (process covariance)
- z - measurement vector
- R - uncertainty covariance matrix of sensor that produces z (measurement covariance)
- F - update matrix - used to get predicted x - based on time elapsed and assumed dynamic model being tracked
- H - extraction matrix - used to extract the hypothetical measurement if state x is correct and the sensor is perfect
- Q - noise covariance matrix - adds uncertainty to the process covariance
- S - 'innovation' covariance that combines process covariance and measurement covariance
- y - difference between the actual measurement and the predicted measurement
- K - Kalman gain - contains information on how much weight to place on the current prediction and current observed measurement that will result the final fused updated state vector and process covariance matrix computed from P (process covariance), H (extraction), R (measurement covariance)
- Lesson 6.4 Estimation Problem Refresh
- Lesson 7.8 File Structure
- Kalman Filter Design
- Sensor Fusion and Object Tracking using an Extended Kalman Filter Algorithm
- Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation
- Kalman Filter Matrices
- Kalman filter
- Kalman Filter, Extended Kalman Filter, Unscented Kalman Filter