Skip to content

Filters: KF, EKF, UKF || Process Models: CV, CTRV || Measurement Models: Radar, Lidar

License

Notifications You must be signed in to change notification settings

ser94mor/sensor-fusion

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 

History

10 Commits
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

Repository files navigation

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:

Measurement Models:

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.

Demo

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

Dependencies

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

Additional:

Build

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

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

Run

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. Algorithm_Kalman_filter
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. Algorithm_Extended_Kalman_filter 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. Algorithm_Unscented_Kalman_filter 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.
graph

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. CV_process_model 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. CTRV_process_model
where CTRV_process_model_alpha and CTRV_process_model_beta 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
Lidar_measurement_model where Lidar_measurement_model_H 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
Radar_measurement_model where Radar_measurement_model_h The Radar measurement model is implemented as a RadarMeasurementModel class.