Kalman filter fixed and first test done