Added kalman filter stuff
Added first version of following robot simulator. Works with crude PID