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: - A lidar sensor that measures a tracked object's position in cartesian-coordinates
(x, y)- A radar sensor that measures a tracked object's position and relative velocity (the velocity within line of sight) in polar coordinates
(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. - In essence we want to get: the position of the system in cartesian coordinates, the velocity magnitude, the yaw angle in radians, and yaw rate in radians per second
(x, y, v, yaw, yawrate)- We are assuming a constant turn/yaw rate and velocity magnitude model (CRTV) for this particular system
This unscented kalman filter does just that.
$ git clone https://github.com/mithi/fusion-ukf/
Go inside the
buildfolder and compile:
$ cd build $ CC=gcc-6 cmake .. && make
To execute inside the
buildfolder use the following format:
$ ./unscentedKF /PATH/TO/INPUT/FILE /PATH/TO/OUTPUT/FILE $ ./unscentedKF ../data/data-3.txt ../data/out-3.txt
Where: (mx, my) - measurements by the lidar (mrho, mphi, mdrho) - measurements by the radar in polar coordinates (t) - timestamp in unix/epoch time the measurements were taken (rx, ry, rvx, rvy, ryaw, 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
- The program outputs the predictions in the following format on the output file path you specified:timestamp pxstate pystate vstate yawanglestate yawratestate sensortype NIS pxmeasured pymeasured pxgroundtruth pygroundtruth vxgroundtruth vyground_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