Figure 12: Control loop for waypoint navigation using the EKF for heading estimation.