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

EKF2: only fuse zero innovation if yaw var is large

Browse Source
This commit is contained in:
bresch
2022-12-29 12:56:36 -05:00
committed by Daniel Agar
parent 40e3503c39
commit c75a9058a5
4 changed files with 31 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_fusion.cpp
+16 -7
View File
@@ -226,16 +226,15 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
// update quaternion states and covariances using the yaw innovation and yaw observation variance
bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s& aid_src_status)
{
aid_src_status.innovation = innovation;
Vector24f H_YAW;
computeYawInnovVarAndH(variance, aid_src_status.innovation_variance, H_YAW);
if (shouldUse321RotationSequence(_R_to_earth)) {
sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &aid_src_status.innovation_variance, &H_YAW);
return fuseYaw(innovation, variance, aid_src_status, H_YAW);
}
} else {
sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &aid_src_status.innovation_variance, &H_YAW);
}
bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s& aid_src_status, const Vector24f &H_YAW)
{
aid_src_status.innovation = innovation;
float heading_innov_var_inv = 0.f;
@@ -330,6 +329,16 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
return false;
}
void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const
{
if (shouldUse321RotationSequence(_R_to_earth)) {
sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
} else {
sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
}
}
bool Ekf::fuseDeclination(float decl_sigma)
{
// observation variance (rad**2)