From fa10d114e4fc586e3565666a7f8d39f4dc9200a5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 30 Sep 2020 19:24:16 +1000 Subject: [PATCH] AP_NavEKF3: Fix yaw drift after yaw reset at +-90 deg pitch Use existing covariance prediction code to set quaternion state covariances. Assumes tilt error is 3 deg 1-sigma. --- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index e68cc2b532..11b65e3d8f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -1078,16 +1078,21 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) bool quatCovResetOnly = false; if (rotVarVecPtr != nullptr) { - // handle special case where we are initialising the quaternion covarianfces using an earth frame - // vector defining the varaince of the angular alignment uncertainty - // use the exisiting gyro error propagation mechanism to define an equivalent gyro error variance + // Handle special case where we are initialising the quaternion covariances using an earth frame + // vector defining the variance of the angular alignment uncertainty. Convert he varaince vector + // to a matrix and rotate into body frame. Use the exisiting gyro error propagation mechanism to + // propagate the body frame angular uncertainty variances. Vector3f rotVarVec = *rotVarVecPtr; + Matrix3f R_ef = Matrix3f ( + rotVarVec.x, 0.0f, 0.0f, + 0.0f, rotVarVec.y, 0.0f, + 0.0f, 0.0f, rotVarVec.z); Matrix3f Tnb; stateStruct.quat.inverse().rotation_matrix(Tnb); - rotVarVec = rotVarVec * Tnb; - daxVar = rotVarVec.x; - dayVar = rotVarVec.y; - dazVar = rotVarVec.z; + Matrix3f R_bf = Tnb * R_ef * Tnb.transposed(); + daxVar = R_bf.a.x; + dayVar = R_bf.b.y; + dazVar = R_bf.c.z; quatCovResetOnly = true; zeroRows(P,0,3); zeroCols(P,0,3);