Fixed another bug in the EKF