Addition of Kalman steps
New update equations for Kalman filter
Minor improvements