Implementation of kalman filter in matlab