changed equation for P in update step to be as given in Shoudongs note - more consistent when I am going to implement EKF