Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Paul Riseborough 2020-09-30 19:24:16 +10:00 committed by Andrew Tridgell
parent 73d5ca5ad3
commit fa10d114e4
1 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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);