An unscented Kalman Filter implementation for fusing lidar and radar sensor measurements.
This is an unscented 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:
(x, y)
(rho, phi, drho)
We want to predict a tracked object's position, how fast it's going in what direction, and how fast it is turning (yaw rate) at any point in time.
(x, y, v, yaw, yawrate)
This unscented kalman filter does just that.
$ git clone https://github.com/mithi/fusion-ukf/
build
folder and compile:$ cd build
$ CC=gcc-6 cmake .. && make
build
folder use the following format:$ ./unscentedKF /PATH/TO/INPUT/FILE /PATH/TO/OUTPUT/FILE
$ ./unscentedKF ../data/data-3.txt ../data/out-3.txt
L(for lidar) m_x m_y t r_x r_y r_vx r_vy, r_yaw, r_yawrate
R(for radar) m_rho m_phi m_drho t r_px r_py r_vx r_vy, r_yaw, r_yawrate
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, r_yaw, r_yawrate) - the real ground truth state of the system
Example:
L 3.122427e-01 5.803398e-01 1477010443000000 6.000000e-01 6.000000e-01 5.199937e+00 0 0 6.911322e-03
R 1.014892e+00 5.543292e-01 4.892807e+00 1477010443050000 8.599968e-01 6.000449e-01 5.199747e+00 1.796856e-03 3.455661e-04 1.382155e-02
time_stamp px_state py_state v_state yaw_angle_state yaw_rate_state sensor_type NIS px_measured py_measured px_ground_truth py_ground_truth vx_ground_truth vy_ground_truth
1477010443000000 0.312243 0.58034 0 0 0 lidar 2.32384e-319 0.312243 0.58034 0.6 0.6 0 0
1477010443050000 0.735335 0.629467 7.20389 9.78669e-18 5.42626e-17 radar 74.6701 0.862916 0.534212 0.859997 0.600045 0.000345533 4.77611e-06
...
//process noise standard deviations
const double STD_SPEED_NOISE = 0.9; // longitudinal acceleration in m/s^2
const double STD_YAWRATE_NOISE = 0.6; // yaw acceleration in rad/s^2