willhnguyen / CarND-Unscented-Kalman-Filter-Project

Self-Driving Car Nanodegree Program Starter Code for the Unscented Kalman Filter Project

Unscented Kalman Filter Project

Self-Driving Car Engineer Nanodegree Program

In this project, students are asked to implement the unsented Kalman Filter to estimate the state of a moving object of interest with noisy LiDAR and RADAR measurements.

Dependencies

• cmake >= 3.5
  • All OSes: click here for installation instructions
• make >= 4.1 (Linux, Mac), 3.81 (Windows)
  • Linux: make is installed by default on most Linux distros
  • Mac: install Xcode command line tools to get make
  • Windows: Click here for installation instructions
• gcc/g++ >= 5.4
  • Linux: gcc / g++ is installed by default on most Linux distros
  • Mac: same deal as make - install Xcode command line tools
  • Windows: recommend using MinGW

Build and Run Instructions

To build the program, run the following sequence of command line instructions.

$ cd /path/to/cloned/repo
$ mkdir build
$ cd build
$ cmake ..
$ make

To see the program in action, make sure to download the Term 2 simulator, run it, and choose the first simulation option. Then, execute the program by executing the UnscentedKF program in the build folder.

$ ./UnscentedKF

Project Information

The Kalman filter is an algorithm that predicts an object's movement by using sensor data. Because sensor data is noisy and has a degree of uncertainty, the Kalman filter is used to provide a more accurate prediction of the object's actual movement. For more information on the Kalman filter, please see my README for the Extended Kalman Filter Project.

Unscented Kalman Filter

The unscented Kalman filter works with non-linear sensors similar to the extended Kalman filter. However, the unscented Kalman filter assumes that the distributions are always Gaussian. To proceed with that assumption, it chooses points around the mean (called sigma points) that it uses to further predict the object's location.

The amount of sigma points chosen is dependent on the size of the state vector. Sigma points include the current state (the mean) and points one unit away in each state parameter.

Sigma points are chosen to be one covariant unit away from the mean in either direction of a particular state parameter. The term is a tunable parameter that determines how far the chosen sigma points are from the mean point. It is usually kept at . The following equation is how to calculate all sigma points—each column of the matrix should be represent a single sigma point.

If necessary, an augmented state vector that includes the noise term should be generated prior to sigma point selection. This is to ensure next state calculations are calculated properly. The CTRV model used requires this augmented state vector.

Once the sigma states are obtained, they are used in the prediction step and the measurement update step. In both steps, the state vector is updated by calculating the weighted average of each sigma point. The weights for each point are calculated using the following equation where a represents the number of augmented state parameters.

The measurement update equation also requires new equations to accomodate for the usage of sigma points. The biggest part of this is the cross-correlation matrix T which affects how the Kalman gain matrix is calculated. All other equations should follow the standard Kalman filter algorithm's equations.

The CTRV Model

For this project, the constant turn rate and velocity model (CTRV) is used. In the previous project, the constant velocity model is used. For CTRV, the state includes the object's angle of movement and its turn rate .

The prediction step will take the turn rate into account. This requires the augmented state vector which include the terms. However, the output vector should not include the terms.

If is 0, then the following equation is used. Otherwise, a divide-by-zero error may occur.

Project Implemenation Details

Following the provided starter code, the src/ukf.cpp and src/tools.cpp files were modified as necessary. For this project, src/ufk.h was also modified to add an extra class variable.

The updated src/ukf.cpp file includes variable initialization and code that controls how RADAR and LiDAR data are processed using the unscented Kalman filter algorithm.

The updated src/tools.cpp file includes a root mean square error calculator.