fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 14:17:35 +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-04-29 12:15:19 -04: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_control.cpp
+37 -7
View File
@@ -39,6 +39,8 @@
#include "ekf.h"
#include <mathlib/mathlib.h>
#include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
void Ekf::controlMagFusion()
{
static constexpr const char *AID_SRC_NAME = "mag";
@@ -68,8 +70,6 @@ void Ekf::controlMagFusion()
if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) {
resetEstimatorAidStatus(aid_src);
if (mag_sample.reset || (_mag_counter == 0)) {
// sensor or calibration has changed, reset low pass filter
_control_status.flags.mag_fault = false;
@@ -101,6 +101,37 @@ void Ekf::controlMagFusion()
_control_status.flags.synthetic_mag_z = false;
}
resetEstimatorAidStatus(aid_src);
aid_src.timestamp_sample = mag_sample.time_us;
// 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;
sym::ComputeMagInnovInnovVarAndHx(_state.vector(), P, mag_sample.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.observation[i] = mag_sample.mag(i);
aid_src.observation_variance[i] = R_MAG;
aid_src.innovation[i] = mag_innov(i);
aid_src.innovation_variance[i] = innov_var(i);
}
const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
// determine if we should use mag fusion
bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
&& _control_status.flags.tilt_align
@@ -136,7 +167,7 @@ void Ekf::controlMagFusion()
_control_status.flags.mag_hdg = ((_params.mag_fusion_type == MagFuseType::HEADING)
|| (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D))
&& _control_status.flags.tilt_align
&& _control_status.flags.mag
&& ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
|| (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
&& !_control_status.flags.mag_fault
@@ -161,7 +192,6 @@ void Ekf::controlMagFusion()
&& (not_using_ne_aiding || !_control_status.flags.mag_aligned_in_flight);
if (_control_status.flags.mag) {
aid_src.timestamp_sample = mag_sample.time_us;
if (continuing_conditions_passing && _control_status.flags.yaw_align) {
@@ -178,7 +208,7 @@ void Ekf::controlMagFusion()
// states for the first few observations.
fuseDeclination(0.02f);
_mag_decl_cov_reset = true;
fuseMag(mag_sample.mag, aid_src, false);
fuseMag(mag_sample.mag, H, aid_src, false);
} else {
// The normal sequence is to fuse the magnetometer data first before fusing
@@ -186,7 +216,7 @@ void Ekf::controlMagFusion()
// 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, aid_src, update_all_states, update_tilt);
fuseMag(mag_sample.mag, H, aid_src, update_all_states, update_tilt);
if (_control_status.flags.mag_dec) {
fuseDeclination(0.5f);
@@ -253,7 +283,7 @@ void Ekf::controlMagFusion()
} else {
ECL_INFO("starting %s fusion", AID_SRC_NAME);
fuseMag(mag_sample.mag, aid_src, false);
fuseMag(mag_sample.mag, H, aid_src, false);
}
aid_src.time_last_fuse = _time_delayed_us;
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;
}
src/modules/ekf2/EKF/ekf.h
+1 -1
View File
@@ -758,7 +758,7 @@ private:
#if defined(CONFIG_EKF2_MAGNETOMETER)
// ekf sequential fusion of magnetometer measurements
bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true, bool update_tilt = true);
bool fuseMag(const Vector3f &mag, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states = true, bool update_tilt = true);
// fuse magnetometer declination measurement
// argument passed in is the declination uncertainty in radians