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

ekf2: mag control always populate estimator aid src

Browse Source
This commit is contained in:
Daniel Agar
2024-05-17 11:19:04 +02:00
committed by Mathieu Bresciani
parent 95ae5a657d
commit bfc39cf341
3 changed files with 75 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
+37 -90
View File
@@ -43,7 +43,6 @@
#include "ekf.h"
#include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
#include <ekf_derivation/generated/compute_mag_y_innov_var_and_h.h>
#include <ekf_derivation/generated/compute_mag_z_innov_var_and_h.h>
@@ -51,39 +50,17 @@
#include <mathlib/mathlib.h>
bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states, bool update_tilt)
bool Ekf::fuseMag(const Vector3f &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));
// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains
Vector3f mag_innov;
Vector3f innov_var;
// Observation jacobian and Kalman gain vectors
VectorState H;
const auto state_vector = _state.vector();
sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
if (_control_status.flags.synthetic_mag_z) {
mag_innov(2) = 0.0f;
}
for (int i = 0; i < 3; i++) {
aid_src_mag.observation[i] = mag(i) - _state.mag_B(i);
aid_src_mag.observation_variance[i] = R_MAG;
aid_src_mag.innovation[i] = mag_innov(i);
aid_src_mag.innovation_variance[i] = innov_var(i);
}
const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
setEstimatorAidStatusTestRatio(aid_src_mag, innov_gate);
if (update_all_states) {
_fault_status.flags.bad_mag_x = (aid_src_mag.innovation_variance[0] < aid_src_mag.observation_variance[0]);
_fault_status.flags.bad_mag_y = (aid_src_mag.innovation_variance[1] < aid_src_mag.observation_variance[1]);
_fault_status.flags.bad_mag_z = (aid_src_mag.innovation_variance[2] < aid_src_mag.observation_variance[2]);
_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;
@@ -92,28 +69,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
}
// Perform an innovation consistency check and report the result
_innov_check_fail_status.flags.reject_mag_x = (aid_src_mag.test_ratio[0] > 1.f);
_innov_check_fail_status.flags.reject_mag_y = (aid_src_mag.test_ratio[1] > 1.f);
_innov_check_fail_status.flags.reject_mag_z = (aid_src_mag.test_ratio[2] > 1.f);
const char *numerical_error_covariance_reset_string = "numerical error - covariance reset";
// check innovation variances for being badly conditioned
if (innov_var.min() < R_MAG) {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
// we need to re-initialise covariances and abort this fusion step
if (update_all_states) {
resetQuatCov(_params.mag_heading_noise);
}
resetMagCov();
ECL_ERR("mag %s", numerical_error_covariance_reset_string);
return false;
}
_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 (aid_src_mag.innovation_rejected) {
if (aid_src.innovation_rejected) {
return false;
}
@@ -127,25 +88,10 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
} 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)
sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H);
sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H);
// recalculate innovation using the updated state
aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
if (aid_src_mag.innovation_variance[index] < R_MAG) {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.flags.bad_mag_y = true;
// we need to re-initialise covariances and abort this fusion step
if (update_all_states) {
resetQuatCov(_params.mag_heading_noise);
}
resetMagCov();
ECL_ERR("magY %s", numerical_error_covariance_reset_string);
return false;
}
aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
} else if (index == 2) {
// we do not fuse synthesized magnetomter measurements when doing 3D fusion
@@ -155,28 +101,33 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
}
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H);
sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H);
// recalculate innovation using the updated state
aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
if (aid_src_mag.innovation_variance[index] < R_MAG) {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.flags.bad_mag_z = true;
// we need to re-initialise covariances and abort this fusion step
if (update_all_states) {
resetQuatCov(_params.mag_heading_noise);
}
resetMagCov();
ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
return false;
}
aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
}
VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index];
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
if (update_all_states) {
resetQuatCov(_params.mag_heading_noise);
}
resetMagCov();
return false;
}
VectorState Kfusion = P * H / aid_src.innovation_variance[index];
if (!update_all_states) {
// zero non-mag Kalman gains if not updating all states
@@ -192,16 +143,13 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
}
if (!update_tilt) {
Kfusion(State::quat_nominal.idx) = 0.f;
Kfusion(State::quat_nominal.idx + 0) = 0.f;
Kfusion(State::quat_nominal.idx + 1) = 0.f;
}
if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index])) {
if (measurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index])) {
fused[index] = true;
limitDeclination();
} else {
fused[index] = false;
}
}
@@ -212,8 +160,8 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
}
if (fused[0] && fused[1] && fused[2]) {
aid_src_mag.fused = true;
aid_src_mag.time_last_fuse = _time_delayed_us;
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
if (update_all_states) {
_time_last_heading_fuse = _time_delayed_us;
@@ -222,7 +170,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
return true;
}
aid_src_mag.fused = false;
return false;
}