Last working commit with cleaned up code and old localization method.
Started Kalman filter implementation. Prediction based on Odometry works.
Implemented complete Kalman filter. Working. Some tuning on paramters needed.