Fixing bugs in Kalman estimator - Kalman update now functional