Browse Source

AP_NavEKF3: Reset all co-variances when performing a yaw reset

c415-sdk
Paul Riseborough 5 years ago committed by Peter Barker
parent
commit
b469a80633
  1. 2
      libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

2
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@ -1548,6 +1548,8 @@ void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance) @@ -1548,6 +1548,8 @@ void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance)
// update the yaw angle variance using the variance of the EKF-GSF estimate
angleErrVarVec.z = yawVariance;
zeroRows(P,0,3);
zeroCols(P,0,3);
initialiseQuatCovariances(angleErrVarVec);
// record the yaw reset event

Loading…
Cancel
Save