Updated Kalman filter

Smal changes