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

ekf2: declination fusion don't use uninitialized parameter (EKF2_MAG_DECL)

Browse Source
This commit is contained in:
Daniel Agar
2024-02-21 09:45:44 -05:00
committed by GitHub
parent 643d3e3bf3
commit 506c60c471
2 changed files with 159 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_fusion.cpp
+40 -21
View File
@@ -227,35 +227,52 @@ bool Ekf::fuseDeclination(float decl_sigma)
return false;
}
// observation variance (rad**2)
const float R_DECL = sq(decl_sigma);
float decl_measurement = NAN;
VectorState H;
float decl_pred;
float innovation_variance;
if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
&& PX4_ISFINITE(_mag_declination_gps)
) {
decl_measurement = _mag_declination_gps;
// TODO: review getMagDeclination() usage, use mag_I, _mag_declination_gps, or parameter?
sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
const float innovation = wrap_pi(decl_pred - getMagDeclination());
if (innovation_variance < R_DECL) {
// variance calculation is badly conditioned
return false;
} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
) {
decl_measurement = math::radians(_params.mag_declination_deg);
}
// Calculate the Kalman gains
VectorState Kfusion = P * H / innovation_variance;
if (PX4_ISFINITE(decl_measurement)) {
const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation);
// observation variance (rad**2)
const float R_DECL = sq(decl_sigma);
_fault_status.flags.bad_mag_decl = !is_fused;
VectorState H;
float decl_pred;
float innovation_variance;
if (is_fused) {
limitDeclination();
sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
const float innovation = wrap_pi(decl_pred - decl_measurement);
if (innovation_variance < R_DECL) {
// variance calculation is badly conditioned
return false;
}
// Calculate the Kalman gains
VectorState Kfusion = P * H / innovation_variance;
const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation);
_fault_status.flags.bad_mag_decl = !is_fused;
if (is_fused) {
limitDeclination();
}
return is_fused;
}
return is_fused;
return false;
}
void Ekf::limitDeclination()
@@ -274,7 +291,9 @@ void Ekf::limitDeclination()
// set to 50% of the horizontal strength from geo tables if location is known
h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
} else if (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) {
} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
) {
// use parameter value if GPS isn't available
decl_reference = math::radians(_params.mag_declination_deg);
}