fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 17:08:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Add function enabling yaw variance to be increased

Browse Source
This commit is contained in:
Paul Riseborough
2019-01-31 09:53:57 -05:00
committed by Daniel Agar
parent bf1f3a224e
commit bce1b96d17
3 changed files with 44 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.h
+4
View File
@@ -668,4 +668,8 @@ private:
// check that the range finder data is continuous // check that the range finder data is continuous
void checkRangeDataContinuity(); void checkRangeDataContinuity();
// Increase the yaw error variance of the quaternions
// Argument is additional yaw variance in rad**2
void increaseQuatYawErrVariance(float yaw_variance);
}; };
EKF/ekf_helper.cpp
+32
View File
@@ -1647,3 +1647,35 @@ void Ekf::get_ekf2ev_quaternion(float *quat)
quat[i] = quat_ekf2ev(i); quat[i] = quat_ekf2ev(i);
} }
} }
// Increase the yaw error variance of the quaternions
// Argument is additional yaw variance in rad**2
void Ekf::increaseQuatYawErrVariance(float yaw_variance)
{
// See DeriveYawResetEquations.m for derivation
// Intermediate variables
float SG[3];
SG[0] = sq(_state.quat_nominal(0)) - sq(_state.quat_nominal(1)) - sq(_state.quat_nominal(2)) + sq(_state.quat_nominal(3));
SG[1] = 2*_state.quat_nominal(0)*_state.quat_nominal(2) - 2*_state.quat_nominal(1)*_state.quat_nominal(3);
SG[2] = 2*_state.quat_nominal(0)*_state.quat_nominal(1) + 2*_state.quat_nominal(2)*_state.quat_nominal(3);
float SQ[4];
SQ[0] = 0.5f * ((_state.quat_nominal(1)*SG[0]) - (_state.quat_nominal(0)*SG[2]) + (_state.quat_nominal(3)*SG[1]));
SQ[1] = 0.5f * ((_state.quat_nominal(0)*SG[1]) - (_state.quat_nominal(2)*SG[0]) + (_state.quat_nominal(3)*SG[2]));
SQ[2] = 0.5f * ((_state.quat_nominal(3)*SG[0]) - (_state.quat_nominal(1)*SG[1]) + (_state.quat_nominal(2)*SG[2]));
SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2]) + (_state.quat_nominal(2)*SG[1]));
// Add covariances for additonal yaw uncertainty to existing covariances.
// This assumes that the additional yaw error is uncorrrelated to existing errors
P[0][0] += yaw_variance*sq(SQ[2]);
P[0][1] += yaw_variance*SQ[1]*SQ[2];
P[1][1] += yaw_variance*sq(SQ[1]);
P[0][2] += yaw_variance*SQ[0]*SQ[2];
P[1][2] += yaw_variance*SQ[0]*SQ[1];
P[2][2] += yaw_variance*sq(SQ[0]);
P[0][3] += yaw_variance*SQ[2]*SQ[3];
P[1][3] += yaw_variance*SQ[1]*SQ[3];
P[2][3] += yaw_variance*SQ[0]*SQ[3];
P[3][3] += yaw_variance*sq(SQ[3]);
}
EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c
+8 -8
View File
@@ -1,21 +1,21 @@
/* /*
C code fragment for function that enables the yaw uncertainty to be increased following a yaw reset. C code fragment for function that enables the yaw uncertainty to be increased following a yaw reset.
The variables q0 -> q3 are the attitude quaternions The variables _state.quat_nominal(0) -> _state.quat_nominal(3) are the attitude quaternions
The variable daYawVar is the variance of the yaw angle uncertainty in rad**2 The variable daYawVar is the variance of the yaw angle uncertainty in rad**2
See DeriveYawResetEquations.m for the derivation See DeriveYawResetEquations.m for the derivation
*/ */
// Intermediate variables // Intermediate variables
float SG[3]; float SG[3];
SG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); SG[0] = sq(_state.quat_nominal(0)) - sq(_state.quat_nominal(1)) - sq(_state.quat_nominal(2)) + sq(_state.quat_nominal(3));
SG[1] = 2*q0*q2 - 2*q1*q3; SG[1] = 2*_state.quat_nominal(0)*_state.quat_nominal(2) - 2*_state.quat_nominal(1)*_state.quat_nominal(3);
SG[2] = 2*q0*q1 + 2*q2*q3; SG[2] = 2*_state.quat_nominal(0)*_state.quat_nominal(1) + 2*_state.quat_nominal(2)*_state.quat_nominal(3);
float SQ[4]; float SQ[4];
SQ[0] = 0.5f * ((q1*SG[0]) - (q0*SG[2]) + (q3*SG[1])); SQ[0] = 0.5f * ((_state.quat_nominal(1)*SG[0]) - (_state.quat_nominal(0)*SG[2]) + (_state.quat_nominal(3)*SG[1]));
SQ[1] = 0.5f * ((q0*SG[1]) - (q2*SG[0]) + (q3*SG[2])); SQ[1] = 0.5f * ((_state.quat_nominal(0)*SG[1]) - (_state.quat_nominal(2)*SG[0]) + (_state.quat_nominal(3)*SG[2]));
SQ[2] = 0.5f * ((q3*SG[0]) - (q1*SG[1]) + (q2*SG[2])); SQ[2] = 0.5f * ((_state.quat_nominal(3)*SG[0]) - (_state.quat_nominal(1)*SG[1]) + (_state.quat_nominal(2)*SG[2]));
SQ[3] = 0.5f * ((q0*SG[0]) + (q1*SG[2]) + (q2*SG[1])); SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2]) + (_state.quat_nominal(2)*SG[1]));
// Variance of yaw angle uncertainty (rad**2) // Variance of yaw angle uncertainty (rad**2)
const float daYawVar = TBD; const float daYawVar = TBD;