This code implements an extended Kalman Filter in C++ to analyze RADAR and LIDAR Data from a simulation in order to estimate the correct position of a bycicle.
To start off, I reviewed the course material and implemented the correct matrix sizes to initialized the Kalman filter. The RADAR and LIDAR measurements consists of different variables in different dimensions, that's why they have to be initialized seperately.
The first tests showed, that the model was off.
From the behavior, i saw, that the measurement data in direction of the curve was off, and that probably the filter didn't update the matrices after initialization. The bug in the code was localized well and shifted to a later stage, where it seemed to divergate from the path.
This point occured when the x coordinate switched to negative values. To reverse engineer the error, i tried to run the filter once without RADAR and once without LIDAR data. It was clear that the RADAR data algorithm caused the problem. The divergent behavior occured once the calculated X value went below 0.
As it turned out, it could easily be solved by guaranteeing an angle between -pi and pi with a simple if statement. The final code ran the track without any issues in both directions.
if (y_(1) > M_PI) { y_(1) -= 2 * M_PI; }
