ekf2: add unaided_yaw for more resilient yaw control

This estimate doesn't converge to the true yaw but can be used as a
more consistent but drifting heading source.
It can be used by a setpoint generator to adjust its heading setpoint
while the true yaw estimate is converging in order to keep a constant
course over ground.
This commit is contained in:
bresch
2023-08-18 16:31:43 +02:00
committed by Mathieu Bresciani
parent 329a2d0e98
commit 0aa4afdbce
5 changed files with 13 additions and 0 deletions
+1
View File
@@ -1490,6 +1490,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
_ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);
lpos.heading = Eulerf(_ekf.getQuaternion()).psi();
lpos.unaided_heading = _ekf.getUnaidedYaw();
lpos.delta_heading = Eulerf(delta_q_reset).psi();
lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();