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

ekf2: treat inhibited states as "consider states" and remove conservative covariance matrix fixes (#22597)

Browse Source 
When a state stops to be estimated it becomes a "consider state". Its value and variance are frozen but its covariance with other states continue to evolve normally.

 - removes conservative accel bias variance limiting
 - force symmetry is skipped after fusion of NED vel/pos (a direct measurement)

---------

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2024-01-24 14:14:09 -05:00
committed by GitHub
co-authored by bresch
parent 2c81c9fdea
commit 51155f7a29
13 changed files with 874 additions and 1039 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_3d_control.cpp
+2 -24
View File
@@ -159,15 +159,13 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
_control_status.flags.mag = true;
loadMagCovData();
// activate fusion, reset mag states and initialize variance if first init or in flight reset
if (!_control_status.flags.yaw_align
|| wmm_updated
|| !_mag_decl_cov_reset
|| !_state.mag_I.longerThan(0.f)
|| (getStateVariance<State::mag_I>().min() < sq(0.0001f))
|| (getStateVariance<State::mag_B>().min() < sq(0.0001f))
|| (getStateVariance<State::mag_I>().min() < kMagVarianceMin)
|| (getStateVariance<State::mag_B>().min() < kMagVarianceMin)
) {
ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME);
@@ -209,25 +207,5 @@ void Ekf::stopMagFusion()
_fault_status.flags.bad_mag_z = false;
_fault_status.flags.bad_mag_decl = false;
saveMagCovData();
}
}
void Ekf::saveMagCovData()
{
// save the NED axis covariance sub-matrix
_saved_mag_ef_covmat = getStateCovariance<State::mag_I>();
// save the XYZ body covariance sub-matrix
_saved_mag_bf_covmat = getStateCovariance<State::mag_B>();
}
void Ekf::loadMagCovData()
{
// re-instate the NED axis covariance sub-matrix
resetStateCovariance<State::mag_I>(_saved_mag_ef_covmat);
// re-instate the XYZ body axis covariance sub-matrix
resetStateCovariance<State::mag_B>(_saved_mag_bf_covmat);
}