Self-Driving Car Engineer Nanodegree Program
This repository utilizes a parametric kalman filter framework to fuse radar and lidar sensor. They are implemented towards Udacity's Self-Driving Car's Term 2 Simulator.
The Kalman Filter architecture consists of two main building blocks,
- Individual filters
- Fusion of filters
The fusion of filters is governed by a controller that adds multiple filters as needed. The fusion controller holds references to a multivariate gaussian mean and sigma, which the individual filters operate on.
Currently supported filters
- Linear - Based on Bayes filter algorithm
- Extended - Based on Jacobian approximation for non-linear distributions
- Unscented - Based on sigma-points as approximation of non-linear distributions
Additionally, the filters are implemented in the context of sensor fusion with different kinematic models. The table below shows the kinematic models available
Kalman Filter | Radar | Lidar |
---|---|---|
Linear | - | Constant Vx, Vy |
Extended | Constant Vx, Vy | - |
Unscented | Constant turnrate & velocity | Constant turnrate & velocity |
The graph below shows the inheritance structure of the various filters
FusionKF FKF(CONSTANT_TURNRATE_VELOCITY); // Sensor fusion controller using CTRV
FKF.AddLaserUKF(); // adds Lidar Unscented Kalman Filter to fusion controller
FKF.AddRadarUKF(); // adds Radar Unscented Kalman Filter to fusion controller
FKF.ProcessMeasurements(Input) // Measurement inputs
INPUT: values provided by the simulator to the c++ program
["sensor_measurement"] => the measurement that the simulator observed (either lidar or radar)
- For lidar: inputs are position in x and y axis
- For radar: inputs are rho, theta, and rho dot
OUTPUT: values provided by the c++ program to the simulator
For Constant Vx and Vy kinematic model
- ["estimate_x"] <= kalman filter estimated position x
- ["estimate_y"] <= kalman filter estimated position y
- ["estimate_vx"] <= kalman filter estimated velocity along x axis
- ["estimate_vy"] <= kalman filter estimated velocity along y axis
For Constant turnrate and velocity kinematic model
- ["estimate_x"] <= kalman filter estimated position x
- ["estimate_y"] <= kalman filter estimated position y
- ["estimate_v"] <= kalman filter estimated velocity
- ["estimate_yaw"] <= kalman filter estimated yaw
- ["estimate_yaw_dot"] <= kalman filter estimated yaw rate
Both kinematic model produces Root-Mean-Squared-Error below
- ["rmse_x"]
- ["rmse_y"]
- ["rmse_vx"]
- ["rmse_vy"]
See Udacity's Extended Kalman Filter Project page on instructions to install the Simulator. Once the necessary prerequisites are completed, the main program can be built and run by doing the following from the project top directory.
- mkdir build
- cd build
- cmake ..
- make
- ./UnscentedKF
Kalman filter library is maintained here