Added wheel encoder, compass kalman filter