Skip to content

Commit

Permalink
ekf2: mag fusion don't update all states or tilt by default
Browse files Browse the repository at this point in the history 
 - cleanup some of the legacy mag flags
  • Loading branch information
@dagar
dagar committed Apr 29, 2024
1 parent 7ded2b5 commit d6ac943
Show file tree
Hide file tree
Showing 3 changed files with 30 additions and 40 deletions.
2 changes: 1 addition & 1 deletion src/modules/ekf2/EKF/ekf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -757,7 +757,7 @@ class Ekf final : public EstimatorInterface

#if defined(CONFIG_EKF2_MAGNETOMETER)
// ekf sequential fusion of magnetometer measurements
bool fuseMag(const Vector3f &mag, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states = true, bool update_tilt = true);
bool fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states = false, bool update_tilt = false);

// fuse magnetometer declination measurement
// argument passed in is the declination uncertainty in radians
Expand Down
32 changes: 24 additions & 8 deletions src/modules/ekf2/EKF/mag_control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,11 @@ void Ekf::controlMagFusion()
_control_status.flags.synthetic_mag_z = false;
}

// reset flags
_fault_status.flags.bad_mag_x = false;
_fault_status.flags.bad_mag_y = false;
_fault_status.flags.bad_mag_z = false;


resetEstimatorAidStatus(aid_src);
aid_src.timestamp_sample = mag_sample.time_us;
Expand Down Expand Up @@ -131,6 +136,10 @@ void Ekf::controlMagFusion()
const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
setEstimatorAidStatusTestRatio(aid_src, innov_gate);

// Perform an innovation consistency check and report the result
_innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f);
_innov_check_fail_status.flags.reject_mag_y = (aid_src.test_ratio[1] > 1.f);
_innov_check_fail_status.flags.reject_mag_z = (aid_src.test_ratio[2] > 1.f);

// determine if we should use mag fusion
bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
Expand Down Expand Up @@ -208,15 +217,22 @@ void Ekf::controlMagFusion()
// states for the first few observations.
fuseDeclination(0.02f);
_mag_decl_cov_reset = true;
fuseMag(mag_sample.mag, H, aid_src, false);
fuseMag(mag_sample.mag, R_MAG, H, aid_src);

} else {
// The normal sequence is to fuse the magnetometer data first before fusing
// declination angle at a higher uncertainty to allow some learning of
// declination angle over time.
const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg;
const bool update_tilt = _control_status.flags.mag_3D;
fuseMag(mag_sample.mag, H, aid_src, update_all_states, update_tilt);
fuseMag(mag_sample.mag, R_MAG, H, aid_src, update_all_states, update_tilt);

// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
if (update_all_states && update_tilt) {
_fault_status.flags.bad_mag_x = (aid_src.innovation_variance[0] < aid_src.observation_variance[0]);
_fault_status.flags.bad_mag_y = (aid_src.innovation_variance[1] < aid_src.observation_variance[1]);
_fault_status.flags.bad_mag_z = (aid_src.innovation_variance[2] < aid_src.observation_variance[2]);
}

if (_control_status.flags.mag_dec) {
fuseDeclination(0.5f);
Expand Down Expand Up @@ -276,19 +292,19 @@ void Ekf::controlMagFusion()
bool reset_heading = !_control_status.flags.yaw_align;

resetMagStates(_mag_lpf.getState(), reset_heading);
aid_src.time_last_fuse = _time_delayed_us;
_nb_mag_3d_reset_available = 2;

if (reset_heading) {
_control_status.flags.yaw_align = true;
}

} else {
ECL_INFO("starting %s fusion", AID_SRC_NAME);
fuseMag(mag_sample.mag, H, aid_src, false);
if (fuseMag(mag_sample.mag, R_MAG, H, aid_src)) {
ECL_INFO("starting %s fusion", AID_SRC_NAME);
_nb_mag_3d_reset_available = 2;
}
}

aid_src.time_last_fuse = _time_delayed_us;

_nb_mag_3d_reset_available = 2;
}
}

Expand Down
36 changes: 5 additions & 31 deletions src/modules/ekf2/EKF/mag_fusion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,41 +50,22 @@

#include <mathlib/mathlib.h>

bool Ekf::fuseMag(const Vector3f &mag, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states, bool update_tilt)
bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states, bool update_tilt)
{
// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));

const auto state_vector = _state.vector();

if (update_all_states) {
_fault_status.flags.bad_mag_x = (aid_src.innovation_variance[0] < aid_src.observation_variance[0]);
_fault_status.flags.bad_mag_y = (aid_src.innovation_variance[1] < aid_src.observation_variance[1]);
_fault_status.flags.bad_mag_z = (aid_src.innovation_variance[2] < aid_src.observation_variance[2]);

} else {
_fault_status.flags.bad_mag_x = false;
_fault_status.flags.bad_mag_y = false;
_fault_status.flags.bad_mag_z = false;
}

// Perform an innovation consistency check and report the result
_innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f);
_innov_check_fail_status.flags.reject_mag_y = (aid_src.test_ratio[1] > 1.f);
_innov_check_fail_status.flags.reject_mag_z = (aid_src.test_ratio[2] > 1.f);

// if any axis fails, abort the mag fusion
// if any axis failed, abort the mag fusion
if (aid_src.innovation_rejected) {
return false;
}

const auto state_vector = _state.vector();

bool fused[3] {false, false, false};

// update the states and covariance using sequential fusion of the magnetometer components
for (uint8_t index = 0; index <= 2; index++) {
// Calculate Kalman gains and observation jacobians
if (index == 0) {
// everything was already computed above
// everything was already computed

} else if (index == 1) {
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
Expand All @@ -108,13 +89,6 @@ bool Ekf::fuseMag(const Vector3f &mag, VectorState &H, estimator_aid_source3d_s
}

if (aid_src.innovation_variance[index] < R_MAG) {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
if (update_all_states) {
_fault_status.flags.bad_mag_x = (aid_src.innovation_variance[0] < aid_src.observation_variance[0]);
_fault_status.flags.bad_mag_y = (aid_src.innovation_variance[1] < aid_src.observation_variance[1]);
_fault_status.flags.bad_mag_z = (aid_src.innovation_variance[2] < aid_src.observation_variance[2]);
}

ECL_ERR("mag numerical error covariance reset");

// we need to re-initialise covariances and abort this fusion step
Expand Down

0 comments on commit d6ac943

Please sign in to comment.