Another ekf fix