EV_yaw: reset ev_to_ekf to identity when yaw is reset to EV

Otherwise, the old rotation matrix is used and not updated anymore
because the EKF is fusing EV yaw data.
This commit is contained in:
bresch
2021-06-22 10:25:55 +02:00
committed by Mathieu Bresciani
parent 804c3563fb
commit 611ace0ed6
2 changed files with 42 additions and 0 deletions
+1
View File
@@ -532,6 +532,7 @@ bool Ekf::resetYawToEv()
const float yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
resetQuatStateYaw(yaw_new, yaw_new_variance, true);
_R_ev_to_ekf.setIdentity();
return true;
}