Sensor Fusion with KF, EKF, and UKF for CV & CTRV Process Models and Lidar & Radar Measurements Models

This repository contains implementations of Kalman filter, extended Kalman filter, and unscented Kalman filter for the selected process and measurement models.

Process Models:

• CV (constant velocity)
• CTRV (constant turn rate and velocity magnitude)

Measurement Models:

• Radar
• Lidar

The project relies on the Eigen library for vector and matrix operations.

A great effort has been put in designing abstractions of filter, process model, and measurement model. The code heavily relies on C++ templates and avoids dynamic memory allocations, which is crucial for embedded systems, such as, self-driving car's onboard computer.

Configurations

There are several files defining int main(int, char**) function.

• udacity_sim_main.cpp
  • Use it in conjunction with the Udacity Self-Driving Car Engineer Nanodegree Term 2 Simulator for checking how the sensor fusion works on simulated measurement data.
• all_main.cpp
  • Use it to check that all possible configurations of filters, process models, and measurement models compile.
• test.cpp
  • Use it to run unit tests.

Demo

One of the configurations works with the Udacity Self-Driving Car Engineer Nanodegree Term 2 Simulator.

Dependencies

• cmake >= 3.5
• gcc/g++ >= 6.0

Additional:

• For udacity_sim_main.cpp
  • uWebSocketIO with commit hash e94b6e1
  • Udacity Self-Driving Car Engineer Nanodegree Term 2 Simulator

Build

The project can be built by doing the following from the project top directory.

$> mkdir build
$> cd build
$> cmake ..
$> make
$> cd ..

Run

• For udacity_sim_main.cpp run build/sensor_fusion_udacity_sim and then run Udacity Self-Driving Car Engineer Nanodegree Term 2 Simulator.
• For all_main.cpp run build/sensor_fusion_all.
• For unit tests run build/sensor_fusion_test.

Filters

While implementing different variations of Kalman filters, the notation from the book "Thrun, S., Burgard, W. and Fox, D., 2005. Probabilistic robotics. MIT press." was followed.

Kalman Filter

The equations below describe the Kalman filter and are implemented in the KalmanFilter class.
For explanations of what each variable means, please, refer to comments in the code in corresponding files or the book "Thrun, S., Burgard, W. and Fox, D., 2005. Probabilistic robotics. MIT press."

Extended Kalman Filter

The equations below describe the extended Kalman filter and are implemented in the ExtendedKalmanFilter class. For explanations of what each variable means, please, refer to comments in the code in corresponding files or the book "Thrun, S., Burgard, W. and Fox, D., 2005. Probabilistic robotics. MIT press."

Unscented Kalman Filter

The equations below describe the unscented Kalman filter and are implemented in the UnscentedKalmanFilter class. For explanations of what each variable means, please, refer to comments in the code in corresponding files or the book "Thrun, S., Burgard, W. and Fox, D., 2005. Probabilistic robotics. MIT press."

Process Models

The following illustration helps to understand what the state vector dimensions mean.

CV (Constant Velocity)

The CV process model is a process model where the object moves linearly with constant velocity. In this project, CV process model dials with a 2D world. The state vector consists of 4 components---px, py, vx, vy---where p* represents the position and v* represents the velocity. The leftmost column in the following equation represents the additive process noise; a* represents acceleration. The CV process model is implemented as a CVProcessModel class.

CTRV (Constant Turn Rate and Velocity Magnitude)

The CTRV process model is a process model where the object moves with a constant turn rate and velocity, that is, with zero longitudinal and yaw accelerations. CTRV process model dials with a 2D world. The state vector consists of 5 components---px, py, v, yaw, yaw_rate---where p* represents the position, v represents the velocity module, yaw represents the yaw angle, and yaw_rate represents the yaw velocity. The leftmost column in the following equation represents the non-linear process noise; a_a represents longitudinal acceleration, and a_psi is yaw acceleration.
where and The results of solving these integrals depends on the yaw_rate, see CTRVProcessModel.

Measurement Models

Lidar

The Lidar measurement model is a linear measurement model. This project does not deal with the lidar point cloud. It assumes that the lidar point cloud has already been processed and a single measurement vector has been identified for the object under consideration. The measurement vector consists of 2 components---px, py---where p* represents the position. The transformation from the state space to the Lidar measurement space is as follows
where The Lidar measurement model is implemented as a LidarMeasurementModel class.

Radar

The Radar measurement model is a non-linear measurement model. The measurement vector consists of 3 components---range, bearing, range_rate---where the range is a radial distance from the origin, the bearing is an angle between range and X-axis which points into the direction of the heading of the vehicle, where sensors are installed, and range_rate is a radial velocity. The transformation from the state space to the Radar measurement space is as follows
where The Radar measurement model is implemented as a RadarMeasurementModel class.