Added part of the EKF equations