Debugged Kalman half way.