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

ekf2: access state covariance using helper functions

Browse Source
This commit is contained in:
bresch
2023-09-26 06:41:04 +02:00
committed by Mathieu Bresciani
parent 619616b9f0
commit 99197919d7
11 changed files with 36 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_3d_control.cpp
+6 -8
View File
@@ -167,8 +167,8 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
|| wmm_updated
|| !_mag_decl_cov_reset
|| !_state.mag_I.longerThan(0.f)
|| (P.slice<3, 3>(16, 16).diag().min() < sq(0.0001f)) // mag_I
|| (P.slice<3, 3>(19, 19).diag().min() < sq(0.0001f)) // mag_B
|| (getStateVariance<State::mag_I>().min() < sq(0.0001f))
|| (getStateVariance<State::mag_B>().min() < sq(0.0001f))
) {
ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME);
@@ -220,19 +220,17 @@ void Ekf::stopMagFusion()
void Ekf::saveMagCovData()
{
// save the NED axis covariance sub-matrix
_saved_mag_ef_covmat = P.slice<State::mag_I.dof, State::mag_I.dof>(State::mag_I.idx, State::mag_I.idx);
_saved_mag_ef_covmat = getStateCovariance<State::mag_I>();
// save the XYZ body covariance sub-matrix
_saved_mag_bf_covmat = P.slice<State::mag_B.dof, State::mag_B.dof>(State::mag_B.idx, State::mag_B.idx);
_saved_mag_bf_covmat = getStateCovariance<State::mag_B>();
}
void Ekf::loadMagCovData()
{
// re-instate the NED axis covariance sub-matrix
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, 0.f);
P.slice<State::mag_I.dof, State::mag_I.dof>(State::mag_I.idx, State::mag_I.idx) = _saved_mag_ef_covmat;
resetStateCovariance<State::mag_I>(_saved_mag_ef_covmat);
// re-instate the XYZ body axis covariance sub-matrix
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, 0.f);
P.slice<State::mag_B.dof, State::mag_B.dof>(State::mag_B.idx, State::mag_B.idx) = _saved_mag_bf_covmat;
resetStateCovariance<State::mag_B>(_saved_mag_bf_covmat);
}