An extended Kalman Filter implementation in Python for fusing lidar and radar sensor measurements
This is an extended Kalman Filter implementation in C++ for fusing lidar and radar sensor measurements. A Kalman filter can be used anywhere 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 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.
[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_dho) - 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