updated the KF part to be 2D, still some issues with covariance
growing with out limits.
Visualization missing.
No planner for the angle/time