This is an extended Kalman Filter implementation in C++ for fusing lidar and radar sensor measurements. A Kalman filter can be used anywhere where you have uncertain information about some dynamic system, and you want to make an educated guess about what the system is going to do next.
In this case, we have two 'noisy' sensors:
(rho, phi, drho)
We want to predict our position, and how fast we are going in what direction at any point in time:
(x, y, vx, vy)
This extended kalman filter does just that.
This project was tested on a Macbook Pro with Mac0S Sierra 10.12.4 with the following:
$ xcode-select --install
$ ruby -e "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/master/install)"
$ brew install homebrew/dupes/make --with-default-names
$ brew install cmake
$ gcc-6 --version $ make --version $ cmake --version
$ git clone https://github.com/mithi/Fusion-EKF-CPP/
buildfolder and compile:
$ cd build $ CC=gcc-6 cmake .. && make
buildfolder use the following format:
$ ./extendedKF /PATH/TO/INPUT/FILE /PATH/TO/OUTPUT/FILE $ ./ExtendedKF ../data/data-1.txt ../data/out-1.txt
L(for lidar) m_x m_y t r_x r_y r_vx r_vy R(for radar) m_rho m_phi m_drho t r_px r_py r_vx r_vy Where: (m_x, m_y) - measurements by the lidar (m_rho, m_phi, m_drho) - measurements by the radar in polar coordinates (t) - timestamp in unix/epoch time the measurements were taken (r_x, r_y, r_vx, r_vy) - the real ground truth state of the system Example: R 8.60363 0.0290616 -2.99903 1477010443399637 8.6 0.25 -3.00029 0 L 8.45 0.25 1477010443349642 8.45 0.25 -3.00027 0
p_x p_y p_vx p_vy m_x m_y r_px r_py r_vx r_vy Where: (p_x, p_y, p_vx, p_vy) - the predicted state of the system by FusionEKF (m_x, m_y) - the position value as measured by the sensor converted to cartesian coordinates (r_x, r_y, r_vx, r_vy) - the real ground truth state of the system Example: 4.53271 0.279 -0.842172 53.1339 4.29136 0.215312 2.28434 0.226323 43.2222 2.65959 0.931181 23.2469 4.29136 0.215312 2.28434 0.226323
radar_Rare hard-coded in
line 16-21of src/fusionekf.cpp. Change and recompile as necessary.
Pis hard-coded in
line 36-39of src/fusionekf.cpp. Change and recompile as necessary.
ayused to update the process covariance matrix
Qis hard-coded in
line 17-18of headers/fusionekf.h. Change and recompile as necessary.
docsfolder for important findings and explanations