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

ekf2: add mag fusion timestamps

Browse Source
This commit is contained in:
Daniel Agar
2022-03-21 14:11:37 -04:00
parent dd28c3e019
commit dedecce39f
3 changed files with 58 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+8 -4
View File
@@ -386,6 +386,8 @@ private:
uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of vertical position measurements was performed (uSec)
uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec)
uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec)
uint64_t _time_last_heading_fuse{0};
uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec)
uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec)
@@ -394,6 +396,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
@@ -581,10 +585,10 @@ private:
void predictCovariance();
// ekf sequential fusion of magnetometer measurements
void fuseMag(const Vector3f &mag);
bool fuseMag(const Vector3f &mag, bool update_all_states = true);
// fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer)
void fuseHeading(float measured_hdg = NAN, float obs_var = NAN);
bool fuseHeading(float measured_hdg = NAN, float obs_var = NAN);
// fuse the yaw angle defined as the first rotation in a 321 Tait-Bryan rotation sequence
// yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad)
@@ -615,7 +619,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();
@@ -1025,7 +1029,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
src/modules/ekf2/EKF/mag_control.cpp
+9 -3
View File
@@ -356,12 +356,18 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
// the angle of the projection onto the horizontal gives the yaw angle
float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
fuseHeading(measured_hdg, sq(_params.mag_heading_noise));
if (fuseHeading(measured_hdg, sq(_params.mag_heading_noise))) {
_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
@@ -369,13 +375,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);
src/modules/ekf2/EKF/mag_fusion.cpp
+41 -22
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,12 +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));
if (_mag_test_ratio(index) > 1.0f) {
all_innovation_checks_passed = false;
_innov_check_fail_status.value |= (1 << (index + 3));
bool rejected = (_mag_test_ratio(index) > 1.f);
} else {
_innov_check_fail_status.value &= ~(1 << (index + 3));
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 (rejected) {
all_innovation_checks_passed = false;
}
}
@@ -188,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;
@@ -276,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);
@@ -356,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);
@@ -419,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;
}
bool Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
@@ -715,13 +730,15 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
// apply the state corrections
fuse(Kfusion, _heading_innov);
_time_last_heading_fuse = _time_last_imu;
return true;
}
return false;
}
void Ekf::fuseHeading(float measured_hdg, float obs_var)
bool Ekf::fuseHeading(float measured_hdg, float obs_var)
{
// observation variance
float R_YAW = PX4_ISFINITE(obs_var) ? obs_var : 0.01f;
@@ -769,14 +786,14 @@ void Ekf::fuseHeading(float measured_hdg, float obs_var)
}
if (shouldUse321RotationSequence(_R_to_earth)) {
fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
return fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
} else {
fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov);
return fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov);
}
}
void Ekf::fuseDeclination(float decl_sigma)
bool Ekf::fuseDeclination(float decl_sigma)
{
// assign intermediate state variables
const float &magN = _state.mag_I(0);
@@ -791,7 +808,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);
@@ -810,7 +827,7 @@ void Ekf::fuseDeclination(float decl_sigma)
HK9 = HK4/innovation_variance;
} else {
// variance calculation is badly conditioned
return;
return false;
}
// Calculate the observation Jacobian
@@ -843,6 +860,8 @@ void Ekf::fuseDeclination(float decl_sigma)
if (is_fused) {
limitDeclination();
}
return is_fused;
}
void Ekf::limitDeclination()