dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: add mag fusion timestamps

This commit is contained in:
Daniel Agar 2022-03-21 14:11:37 -04:00
parent e3e067d640
commit f254b55523
3 changed files with 50 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/ekf2/EKF/ekf.h
View File

@ -420,6 +420,8 @@ private:
uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
uint64_t _time_last_gps_yaw_fuse{0}; ///< time the last fusion of GPS yaw measurements were performed (uSec)
uint64_t _time_last_gps_yaw_data{0}; ///< time the last GPS yaw measurement was available (uSec)
uint64_t _time_last_mag_heading_fuse{0};
uint64_t _time_last_mag_3d_fuse{0};
uint64_t _time_last_healthy_rng_data{0};
uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
@ -605,9 +607,9 @@ private:
void predictCovariance();
// ekf sequential fusion of magnetometer measurements
void fuseMag(const Vector3f &mag);
bool fuseMag(const Vector3f &mag, bool update_all_states = true);
// update quaternion states and covariances using a yaw innovation and yaw observation variance
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
// innovation : prediction - measurement
// variance : observaton variance
bool fuseYaw(const float innovation, const float variance);
@ -621,7 +623,7 @@ private:
// fuse magnetometer declination measurement
// argument passed in is the declination uncertainty in radians
void fuseDeclination(float decl_sigma);
bool fuseDeclination(float decl_sigma);
// apply sensible limits to the declination and length of the NE mag field states estimates
void limitDeclination();
@ -1035,7 +1037,7 @@ private:
// yaw : Euler yaw angle (rad)
// yaw_variance : yaw error variance (rad^2)
// update_buffer : true if the state change should be also applied to the output observer buffer
void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer);
void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer = true);
// Declarations used to control use of the EKF-GSF yaw estimator

13
src/modules/ekf2/EKF/mag_control.cpp
View File

@ -345,12 +345,19 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f);
fuseYaw(innovation, obs_var);
if (fuseYaw(innovation, obs_var)) {
_time_last_mag_heading_fuse = _time_last_imu;
}
}
}
void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
{
// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
// before they are used to constrain heading drift
const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
if (!_mag_decl_cov_reset) {
// After any magnetic field covariance reset event the earth field state
// covariances need to be corrected to incorporate knowledge of the declination
@ -358,13 +365,13 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
// states for the first few observations.
fuseDeclination(0.02f);
_mag_decl_cov_reset = true;
fuseMag(mag);
fuseMag(mag, update_all_states);
} 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.
fuseMag(mag);
fuseMag(mag, update_all_states);
if (_control_status.flags.mag_dec) {
fuseDeclination(0.5f);

59
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -45,7 +45,7 @@
#include <mathlib/mathlib.h>
void Ekf::fuseMag(const Vector3f &mag)
bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
{
// assign intermediate variables
const float q0 = _state.quat_nominal(0);
@ -96,7 +96,7 @@ void Ekf::fuseMag(const Vector3f &mag)
// we need to re-initialise covariances and abort this fusion step
resetMagRelatedCovariances();
ECL_ERR("magX %s", numerical_error_covariance_reset_string);
return;
return false;
}
_fault_status.flags.bad_mag_x = false;
@ -138,7 +138,7 @@ void Ekf::fuseMag(const Vector3f &mag)
// we need to re-initialise covariances and abort this fusion step
resetMagRelatedCovariances();
ECL_ERR("magY %s", numerical_error_covariance_reset_string);
return;
return false;
}
_fault_status.flags.bad_mag_y = false;
@ -150,7 +150,7 @@ void Ekf::fuseMag(const Vector3f &mag)
// we need to re-initialise covariances and abort this fusion step
resetMagRelatedCovariances();
ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
return;
return false;
}
_fault_status.flags.bad_mag_z = false;
@ -174,20 +174,24 @@ void Ekf::fuseMag(const Vector3f &mag)
for (uint8_t index = 0; index <= 2; index++) {
_mag_test_ratio(index) = sq(_mag_innov(index)) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var(index));
const bool innov_check_fail = (_mag_test_ratio(index) > 1.0f);
bool rejected = (_mag_test_ratio(index) > 1.f);
if (innov_check_fail) {
all_innovation_checks_passed = false;
switch (index) {
case 0:
_innov_check_fail_status.flags.reject_mag_x = rejected;
break;
case 1:
_innov_check_fail_status.flags.reject_mag_y = rejected;
break;
case 2:
_innov_check_fail_status.flags.reject_mag_z = rejected;
break;
}
if (index == 0) {
_innov_check_fail_status.flags.reject_mag_x = innov_check_fail;
} else if (index == 1) {
_innov_check_fail_status.flags.reject_mag_y = innov_check_fail;
} else {
_innov_check_fail_status.flags.reject_mag_z = innov_check_fail;
if (rejected) {
all_innovation_checks_passed = false;
}
}
@ -196,13 +200,9 @@ void Ekf::fuseMag(const Vector3f &mag)
// if any axis fails, abort the mag fusion
if (!all_innovation_checks_passed) {
return;
return false;
}
// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
// before they are used to constrain heading drift
const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
// Observation jacobian and Kalman gain vectors
SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion;
Vector24f Kfusion;
@ -284,7 +284,7 @@ void Ekf::fuseMag(const Vector3f &mag)
// we need to re-initialise covariances and abort this fusion step
resetMagRelatedCovariances();
ECL_ERR("magY %s", numerical_error_covariance_reset_string);
return;
return false;
}
const float HKY24 = 1.0F/_mag_innov_var(1);
@ -364,7 +364,7 @@ void Ekf::fuseMag(const Vector3f &mag)
// we need to re-initialise covariances and abort this fusion step
resetMagRelatedCovariances();
ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
return;
return false;
}
const float HKZ24 = 1.0F/_mag_innov_var(2);
@ -427,6 +427,13 @@ void Ekf::fuseMag(const Vector3f &mag)
limitDeclination();
}
}
if (!_fault_status.flags.bad_mag_x && !_fault_status.flags.bad_mag_y && !_fault_status.flags.bad_mag_z) {
_time_last_mag_3d_fuse = _time_last_imu;
return true;
}
return false;
}
// update quaternion states and covariances using the yaw innovation and yaw observation variance
@ -678,7 +685,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance)
return false;
}
void Ekf::fuseDeclination(float decl_sigma)
bool Ekf::fuseDeclination(float decl_sigma)
{
// assign intermediate state variables
const float magN = _state.mag_I(0);
@ -693,7 +700,7 @@ void Ekf::fuseDeclination(float decl_sigma)
// Calculate intermediate variables
if (fabsf(magN) < sq(N_field_min)) {
// calculation is badly conditioned close to +-90 deg declination
return;
return false;
}
const float HK0 = ecl::powf(magN, -2);
@ -712,7 +719,7 @@ void Ekf::fuseDeclination(float decl_sigma)
HK9 = HK4/innovation_variance;
} else {
// variance calculation is badly conditioned
return;
return false;
}
// Calculate the observation Jacobian
@ -745,6 +752,8 @@ void Ekf::fuseDeclination(float decl_sigma)
if (is_fused) {
limitDeclination();
}
return is_fused;
}
void Ekf::limitDeclination()