Implementation of kalman filter