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

ekf2: fix function to increase yaw variance

Browse Source
This commit is contained in:
bresch
2023-08-07 15:35:27 +02:00
committed by Daniel Agar
parent 01fc4c3cf1
commit de702a2e63
4 changed files with 206 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
+5 -38
View File
@@ -41,6 +41,7 @@
#include "ekf.h"
#include "python/ekf_derivation/generated/quat_var_to_rot_var.h"
#include "python/ekf_derivation/generated/yaw_var_to_lower_triangular_quat_cov.h"
#include <mathlib/mathlib.h>
#include <cstdlib>
@@ -1008,46 +1009,12 @@ void Ekf::updateGroundEffect()
}
}
// 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 which produces code fragments in C_code4.txt file
// The auto-code was cleaned up and had terms multiplied by zero removed to give the following:
// 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]));
// Limit yaw variance increase to prevent a badly conditioned covariance matrix
yaw_variance = fminf(yaw_variance, 1.0e-2f);
// 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]);
P(1,0) += yaw_variance*SQ[1]*SQ[2];
P(2,0) += yaw_variance*SQ[0]*SQ[2];
P(2,1) += yaw_variance*SQ[0]*SQ[1];
P(3,0) -= yaw_variance*SQ[2]*SQ[3];
P(3,1) -= yaw_variance*SQ[1]*SQ[3];
P(3,2) -= yaw_variance*SQ[0]*SQ[3];
matrix::SquareMatrix<float, 4> q_cov;
sym::YawVarToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), yaw_variance, &q_cov);
q_cov.copyLowerToUpperTriangle();
P.slice<4, 4>(0, 0) += q_cov;
}
void Ekf::saveMagCovData()