Update ekf algorithm