Modified ekf to work inside of the PID controller.