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

ekf2: separate mag and mag heading control logic (#21212)

Browse Source 
- split mag_3d into new standalone mag fusion and mag fusion allowed to update all states (full mag_3d)
 - new dedicated control logic for mag/mag_3d fusion and standalone mag heading fusion
 - if WMM available use for mag_I and mag_B init
 - mag states reset if external yaw reset (yaw estimator, GPS yaw, etc)
 - mag reset if declination changed (eliminate _mag_yaw_reset_req)
 - mag fusion (but not mag_hdg or mag_3d) can be active during gps_yaw or ev_yaw (if yaw aligned north)

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2023-08-04 10:39:16 -04:00
committed by GitHub
co-authored by bresch
parent 7db667558e
commit d75bb62a65
26 changed files with 1650 additions and 1397 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/CMakeLists.txt
+3 -1
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2015-2018 ECL Development Team. All rights reserved.
# Copyright (c) 2015-2023 ECL Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -49,8 +49,10 @@ list(APPEND EKF_SRCS
gravity_fusion.cpp
height_control.cpp
imu_down_sampler.cpp
mag_3d_control.cpp
mag_control.cpp
mag_fusion.cpp
mag_heading_control.cpp
output_predictor.cpp
vel_pos_fusion.cpp
zero_innovation_heading_update.cpp
src/modules/ekf2/EKF/common.h
+4 -4
View File
@@ -102,9 +102,6 @@ enum MagFuseType : uint8_t {
// Integer definitions for mag_fusion_type
AUTO = 0, ///< The selection of either heading or 3D magnetometer fusion will be automatic
HEADING = 1, ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
MAG_3D = 2, ///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
UNUSED = 3, ///< Not implemented
INDOOR = 4, ///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
NONE = 5 ///< Do not use magnetometer under any circumstance..
};
@@ -335,7 +332,7 @@ struct parameters {
// magnetometer fusion
float mag_heading_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad)
float mag_noise{5.0e-2f}; ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
float mag_noise{5.0e-2f}; ///< measurement noise used for 3-axis magnetometer fusion (Gauss)
float mag_declination_deg{0.0f}; ///< magnetic declination (degrees)
float heading_innov_gate{2.6f}; ///< heading fusion innovation consistency gate size (STD)
float mag_innov_gate{3.0f}; ///< magnetometer fusion innovation consistency gate size (STD)
@@ -601,6 +598,9 @@ union filter_control_status_u {
uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused
uint64_t fake_hgt : 1; ///< 33 - true when fake height measurements are being fused
uint64_t gravity_vector : 1; ///< 34 - true when gravity vector measurements are being fused
uint64_t mag : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
uint64_t ev_yaw_fault : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used
} flags;
uint64_t value;
};
src/modules/ekf2/EKF/control.cpp
+8 -8
View File
@@ -76,7 +76,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
_control_status.flags.tilt_align = true;
// send alignment status message to the console
const char *height_source = nullptr;
const char *height_source = "unknown";
if (_control_status.flags.baro_hgt) {
height_source = "baro";
@@ -89,16 +89,16 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
} else if (_control_status.flags.rng_hgt) {
height_source = "rng";
} else {
height_source = "unknown";
}
if (height_source) {
ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
(unsigned long long)imu_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
}
ECL_DEBUG("tilt aligned, roll: %.3f, pitch %.3f, yaw: %.3f",
(double)matrix::Eulerf(_state.quat_nominal).phi(),
(double)matrix::Eulerf(_state.quat_nominal).theta(),
(double)matrix::Eulerf(_state.quat_nominal).psi()
);
}
}
src/modules/ekf2/EKF/covariance.cpp
+12 -33
View File
@@ -180,7 +180,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
float mag_I_sig;
if (_control_status.flags.mag_3D && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) {
if (_control_status.flags.mag && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) {
mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
} else {
@@ -190,7 +190,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
float mag_B_sig;
if (_control_status.flags.mag_3D && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) {
if (_control_status.flags.mag && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) {
mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);
} else {
@@ -292,7 +292,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
P(row, row) = nextP(row, row);
}
if (_control_status.flags.mag_3D) {
if (_control_status.flags.mag) {
for (unsigned row = 16; row <= 21; row++) {
for (unsigned column = 0 ; column < row; column++) {
P(row, column) = P(column, row) = nextP(column, row);
@@ -471,8 +471,9 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
}
// magnetic field states
if (!_control_status.flags.mag_3D) {
zeroMagCov();
if (!_control_status.flags.mag) {
P.uncorrelateCovarianceSetVariance<3>(16, 0.0f);
P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
} else {
// constrain variances
@@ -525,12 +526,6 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP)
return healthy;
}
void Ekf::resetMagRelatedCovariances()
{
resetQuatCov();
resetMagCov();
}
void Ekf::resetQuatCov()
{
zeroQuatCov();
@@ -542,7 +537,7 @@ void Ekf::resetQuatCov()
initialiseQuatCovariances(rot_vec_var);
// update the yaw angle variance using the variance of the measurement
if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
if (_params.mag_fusion_type != MagFuseType::NONE) {
// using magnetic heading tuning parameter
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
}
@@ -556,31 +551,15 @@ void Ekf::zeroQuatCov()
void Ekf::resetMagCov()
{
// reset the corresponding rows and columns in the covariance matrix and
// set the variances on the magnetic field states to the measurement variance
clearMagCov();
if (_mag_decl_cov_reset) {
ECL_INFO("reset mag covariance");
_mag_decl_cov_reset = false;
}
P.uncorrelateCovarianceSetVariance<3>(16, sq(_params.mag_noise));
P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
if (!_control_status.flags.mag_3D) {
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
// if already in 3-axis fusion mode, the covariances are automatically saved when switching out
// of this mode
saveMagCovData();
}
}
void Ekf::clearMagCov()
{
zeroMagCov();
_mag_decl_cov_reset = false;
}
void Ekf::zeroMagCov()
{
P.uncorrelateCovarianceSetVariance<3>(16, 0.0f);
P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
saveMagCovData();
}
void Ekf::resetGyroBiasZCov()
src/modules/ekf2/EKF/ekf.cpp
+6 -3
View File
@@ -45,9 +45,12 @@
bool Ekf::init(uint64_t timestamp)
{
bool ret = initialise_interface(timestamp);
reset();
return ret;
if (!_initialised) {
_initialised = initialise_interface(timestamp);
reset();
}
return _initialised;
}
void Ekf::reset()
src/modules/ekf2/EKF/ekf.h
+23 -38
View File
@@ -161,9 +161,8 @@ public:
if (_control_status.flags.mag_hdg) {
heading_innov = _aid_src_mag_heading.innovation;
return;
}
if (_control_status.flags.mag_3D) {
} else if (_control_status.flags.mag_3D) {
heading_innov = Vector3f(_aid_src_mag.innovation).max();
return;
}
@@ -188,9 +187,8 @@ public:
if (_control_status.flags.mag_hdg) {
heading_innov_var = _aid_src_mag_heading.innovation_variance;
return;
}
if (_control_status.flags.mag_3D) {
} else if (_control_status.flags.mag_3D) {
heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
return;
}
@@ -215,9 +213,8 @@ public:
if (_control_status.flags.mag_hdg) {
heading_innov_ratio = _aid_src_mag_heading.test_ratio;
return;
}
if (_control_status.flags.mag_3D) {
} else if (_control_status.flags.mag_3D) {
heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
return;
}
@@ -378,11 +375,11 @@ public:
const Vector3f &getMagBias() const { return _state.mag_B; }
Vector3f getMagBiasVariance() const
{
if (_control_status.flags.mag_3D) {
if (_control_status.flags.mag) {
return Vector3f{P(19, 19), P(20, 20), P(21, 21)};
}
return _saved_mag_bf_variance;
return _saved_mag_bf_covmat.diag();
}
float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss
@@ -547,6 +544,9 @@ private:
bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised
float _mag_heading_prev{}; ///< previous value of mag heading (rad)
float _mag_heading_pred_prev{}; ///< previous value of yaw state used by mag heading fusion (rad)
// booleans true when fresh sensor data is available at the fusion time horizon
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
@@ -580,11 +580,10 @@ private:
bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable
bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable
uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected
uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetometer use was inhibited
bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested
float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad)
bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements
uint8_t _nb_mag_heading_reset_available{0};
uint8_t _nb_mag_3d_reset_available{0};
uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
SquareMatrix24f P{}; ///< state covariance matrix
@@ -657,7 +656,6 @@ private:
uint8_t _nb_ev_vel_reset_available{0};
uint8_t _nb_ev_yaw_reset_available{0};
#endif // CONFIG_EKF2_EXTERNAL_VISION
bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
estimator_aid_source1d_s _aid_src_gnss_hgt{};
estimator_aid_source2d_s _aid_src_gnss_pos{};
@@ -706,9 +704,8 @@ private:
uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec)
uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
uint64_t _time_last_mag_check_failing{0};
Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
Matrix2f _saved_mag_ef_ne_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
float _saved_mag_ef_d_variance{}; ///< D magnetic field state variance saved for use at the next initialisation (Gauss**2)
Matrix3f _saved_mag_ef_covmat{}; ///< NED magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
Matrix3f _saved_mag_bf_covmat{}; ///< magnetic field state covariance sub-matrix that has been saved for use at the next initialisation (Gauss**2)
gps_check_fail_status_u _gps_check_fail_status{};
@@ -765,8 +762,6 @@ private:
void updateGpsYaw(const gpsSample &gps_sample);
void startGpsYawFusion(const gpsSample &gps_sample);
#endif // CONFIG_EKF2_GNSS_YAW
void stopGpsYawFusion();
@@ -922,14 +917,14 @@ private:
}
// mag I: states 16, 17, 18
if (!_control_status.flags.mag_3D) {
if (!_control_status.flags.mag) {
K(16) = 0.f;
K(17) = 0.f;
K(18) = 0.f;
}
// mag B: states 19, 20, 21
if (!_control_status.flags.mag_3D) {
if (!_control_status.flags.mag) {
K(19) = 0.f;
K(20) = 0.f;
K(21) = 0.f;
@@ -1022,23 +1017,20 @@ private:
// control fusion of magnetometer observations
void controlMagFusion();
void controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source1d_s &aid_src);
void controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source3d_s &aid_src);
bool resetMagHeading(const Vector3f &mag);
bool resetMagStates(const Vector3f &mag);
bool checkHaglYawResetReq() const;
void resetMagHeading(const Vector3f &mag);
void resetMagStates(const Vector3f &mag, bool reset_heading = true);
bool haglYawResetReq();
void selectMagAuto();
void check3DMagFusionSuitability();
void checkYawAngleObservability();
void checkMagBiasObservability();
bool canUse3DMagFusion() const;
void checkMagDeclRequired();
bool shouldInhibitMag() const;
bool checkMagField(const Vector3f &mag);
static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
void runMagAndMagDeclFusions(const Vector3f &mag);
void run3DMagAndDeclFusions(const Vector3f &mag);
// control fusion of fake position observations to constrain drift
void controlFakePosFusion();
@@ -1069,11 +1061,8 @@ private:
void controlBaroHeightFusion();
void controlGnssHeightFusion(const gpsSample &gps_sample);
void stopMagFusion();
void stopMag3DFusion();
void stopMagHdgFusion();
void startMagHdgFusion();
void startMag3DFusion();
void stopMagFusion();
void stopBaroHgtFusion();
void stopGpsHgtFusion();
@@ -1087,11 +1076,9 @@ private:
// do not call before quaternion states are initialised
void initialiseQuatCovariances(Vector3f &rot_vec_var);
// perform a limited reset of the magnetic field related state covariances
void resetMagRelatedCovariances();
void resetQuatCov();
void zeroQuatCov();
void resetMagCov();
// perform a reset of the wind states and related covariances
@@ -1105,8 +1092,6 @@ private:
// load and save mag field state covariance data for re-use
void loadMagCovData();
void saveMagCovData();
void clearMagCov();
void zeroMagCov();
void resetGyroBiasZCov();
src/modules/ekf2/EKF/ekf_helper.cpp
+11 -22
View File
@@ -1086,33 +1086,24 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
P(3,2) -= yaw_variance*SQ[0]*SQ[3];
}
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
void Ekf::saveMagCovData()
{
// save variances for XYZ body axis field
_saved_mag_bf_variance(0) = P(19, 19);
_saved_mag_bf_variance(1) = P(20, 20);
_saved_mag_bf_variance(2) = P(21, 21);
// save the NED axis covariance sub-matrix
_saved_mag_ef_covmat = P.slice<3, 3>(16, 16);
// save the NE axis covariance sub-matrix
_saved_mag_ef_ne_covmat = P.slice<2, 2>(16, 16);
// save variance for the D earth axis
_saved_mag_ef_d_variance = P(18, 18);
// save the XYZ body covariance sub-matrix
_saved_mag_bf_covmat = P.slice<3, 3>(19, 19);
}
void Ekf::loadMagCovData()
{
// re-instate variances for the XYZ body axis field
P(19, 19) = _saved_mag_bf_variance(0);
P(20, 20) = _saved_mag_bf_variance(1);
P(21, 21) = _saved_mag_bf_variance(2);
// re-instate the NED axis covariance sub-matrix
P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat;
// re-instate the NE axis covariance sub-matrix
P.slice<2, 2>(16, 16) = _saved_mag_ef_ne_covmat;
// re-instate the D earth axis variance
P(18, 18) = _saved_mag_ef_d_variance;
// re-instate the XYZ body axis covariance sub-matrix
P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat;
}
void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
@@ -1133,7 +1124,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
uncorrelateQuatFromOtherStates();
// update the yaw angle variance
if (yaw_variance > FLT_EPSILON) {
if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
increaseQuatYawErrVariance(yaw_variance);
}
@@ -1182,8 +1173,6 @@ bool Ekf::resetYawToEKFGSF()
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
_flt_mag_align_start_time = _time_delayed_us;
_control_status.flags.yaw_align = true;
_information_events.flags.yaw_aligned_to_imu_gps = true;
src/modules/ekf2/EKF/estimator_interface.cpp
+1 -1
View File
@@ -471,7 +471,7 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
_system_flag_buffer->push(system_flags_new);
} else {
ECL_WARN("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
}
}
src/modules/ekf2/EKF/estimator_interface.h
+22 -1
View File
@@ -140,16 +140,34 @@ public:
void set_in_air_status(bool in_air)
{
if (!in_air) {
if (_control_status.flags.in_air) {
ECL_DEBUG("no longer in air");
}
_time_last_on_ground_us = _time_delayed_us;
} else {
if (!_control_status.flags.in_air) {
ECL_DEBUG("in air");
}
_time_last_in_air = _time_delayed_us;
}
_control_status.flags.in_air = in_air;
}
void set_vehicle_at_rest(bool at_rest) { _control_status.flags.vehicle_at_rest = at_rest; }
void set_vehicle_at_rest(bool at_rest)
{
if (!_control_status.flags.vehicle_at_rest && at_rest) {
ECL_DEBUG("at rest");
} else if (_control_status.flags.vehicle_at_rest && !at_rest) {
ECL_DEBUG("no longer at rest");
}
_control_status.flags.vehicle_at_rest = at_rest;
}
// return true if the attitude is usable
bool attitude_valid() const { return _control_status.flags.tilt_align; }
@@ -420,9 +438,12 @@ protected:
// allocate data buffers and initialize interface variables
bool initialise_interface(uint64_t timestamp);
uint64_t _wmm_gps_time_last_checked{0}; // time WMM last checked
uint64_t _wmm_gps_time_last_set{0}; // time WMM last set
float _mag_declination_gps{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad)
float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T)
float _mag_inclination{NAN};
float _mag_strength{NAN};
src/modules/ekf2/EKF/ev_yaw_control.cpp
+5 -1
View File
@@ -49,10 +49,14 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
aid_src.observation_variance = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f));
aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
if (ev_reset) {
_control_status.flags.ev_yaw_fault = false;
}
// determine if we should use EV yaw aiding
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
&& _control_status.flags.tilt_align
&& !_inhibit_ev_yaw_use
&& !_control_status.flags.ev_yaw_fault
&& PX4_ISFINITE(aid_src.observation)
&& PX4_ISFINITE(aid_src.observation_variance);
src/modules/ekf2/EKF/fake_pos_control.cpp
+1
View File
@@ -84,6 +84,7 @@ void Ekf::controlFakePosFusion()
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);
if (is_fusion_failing) {
ECL_WARN("fake position fusion failing, resetting");
resetFakePosFusion();
}
src/modules/ekf2/EKF/gps_checks.cpp
+27 -26
View File
@@ -84,52 +84,53 @@ bool Ekf::collect_gps(const gpsMessage &gps)
_NED_origin_initialised = true;
_earth_rate_NED = calcEarthRateNED((float)math::radians(_pos_ref.getProjectionReferenceLat()));
const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps);
// set the magnetic field data returned by the geo library using the current GPS position
_mag_declination_gps = get_mag_declination_radians(lat, lon);
_mag_inclination_gps = get_mag_inclination_radians(lat, lon);
_mag_strength_gps = get_mag_strength_gauss(lat, lon);
// request a reset of the yaw using the new declination
if ((_params.mag_fusion_type != MagFuseType::NONE)
&& !declination_was_valid) {
_mag_yaw_reset_req = true;
}
// save the horizontal and vertical position uncertainty of the origin
_gps_origin_eph = gps.eph;
_gps_origin_epv = gps.epv;
_information_events.flags.gps_checks_passed = true;
ECL_INFO("GPS checks passed");
}
} else if (!_NED_origin_initialised) {
// a rough 2D fix is still sufficient to lookup declination
if ((gps.fix_type >= 2) && (gps.eph < 1000)) {
if (isTimedOut(_wmm_gps_time_last_checked, 1e6)) {
// a rough 2D fix is sufficient to lookup declination
const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.eph < 1000);
const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps);
if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) {
// If we have good GPS data set the origin's WGS-84 position to the last gps fix
const double lat = gps.lat * 1.0e-7;
const double lon = gps.lon * 1.0e-7;
// set the magnetic field data returned by the geo library using the current GPS position
_mag_declination_gps = get_mag_declination_radians(lat, lon);
_mag_inclination_gps = get_mag_inclination_radians(lat, lon);
_mag_strength_gps = get_mag_strength_gauss(lat, lon);
const float mag_declination_gps = get_mag_declination_radians(lat, lon);
const float mag_inclination_gps = get_mag_inclination_radians(lat, lon);
const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
// request mag yaw reset if there's a mag declination for the first time
if (_params.mag_fusion_type != MagFuseType::NONE) {
if (!declination_was_valid && PX4_ISFINITE(_mag_declination_gps)) {
_mag_yaw_reset_req = true;
if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) {
const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f));
const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f));
if ((_wmm_gps_time_last_set == 0)
|| !PX4_ISFINITE(mag_declination_gps)
|| !PX4_ISFINITE(mag_inclination_gps)
|| !PX4_ISFINITE(mag_strength_gps)
|| mag_declination_changed
|| mag_inclination_changed
) {
_mag_declination_gps = mag_declination_gps;
_mag_inclination_gps = mag_inclination_gps;
_mag_strength_gps = mag_strength_gps;
_wmm_gps_time_last_set = _time_delayed_us;
}
}
_earth_rate_NED = calcEarthRateNED((float)math::radians(lat));
}
_wmm_gps_time_last_checked = _time_delayed_us;
}
// start collecting GPS if there is a 3D fix and the NED origin has been set
src/modules/ekf2/EKF/gps_control.cpp
+3 -12
View File
@@ -41,7 +41,7 @@
void Ekf::controlGpsFusion(const imuSample &imu_delayed)
{
if (!_gps_buffer || !((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) {
if (!_gps_buffer || (_params.gnss_ctrl == 0)) {
stopGpsFusion();
return;
}
@@ -166,7 +166,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
ECL_WARN("GPS emergency yaw reset");
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
// stop using the magnetometer in the main EKF otherwise it' fusion could drag the yaw around
// stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around
// and cause another navigation failure
_control_status.flags.mag_fault = true;
_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
@@ -184,7 +184,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_control_status.flags.ev_yaw) {
_inhibit_ev_yaw_use = true;
_control_status.flags.ev_yaw_fault = true;
}
#endif // CONFIG_EKF2_EXTERNAL_VISION
@@ -369,11 +369,6 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
if (starting_conditions_passing) {
// Try to activate GPS yaw fusion
if (resetYawToGps(gps_sample.yaw)) {
stopEvYawFusion();
stopMagHdgFusion();
stopMag3DFusion();
ECL_INFO("starting GPS yaw fusion");
_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
@@ -429,8 +424,4 @@ void Ekf::stopGpsFusion()
stopGpsHgtFusion();
stopGpsYawFusion();
// We do not need to know the true North anymore
// EV yaw can start again
_inhibit_ev_yaw_use = false;
}
src/modules/ekf2/EKF/mag_3d_control.cpp
+218
View File
@@ -0,0 +1,218 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mag_3d_control.cpp
* Control functions for ekf mag 3D fusion
*/
#include "ekf.h"
void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing,
estimator_aid_source3d_s &aid_src)
{
static constexpr const char *AID_SRC_NAME = "mag";
resetEstimatorAidStatus(aid_src);
const bool wmm_updated = (_wmm_gps_time_last_set > aid_src.time_last_fuse);
// determine if we should use mag fusion
bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
&& _control_status.flags.tilt_align
&& (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
&& (wmm_updated || checkHaglYawResetReq() || isRecent(_time_last_mov_3d_mag_suitable, (uint64_t)3e6))
&& mag_sample.mag.longerThan(0.f)
&& mag_sample.mag.isAllFinite();
const bool starting_conditions_passing = common_starting_conditions_passing
&& continuing_conditions_passing;
// 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
_control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO)
&& _control_status.flags.mag
&& _control_status.flags.mag_aligned_in_flight
&& !_control_status.flags.mag_fault
&& isRecent(aid_src.time_last_fuse, 500'000)
&& getMagBiasVariance().longerThan(0.f) && !getMagBiasVariance().longerThan(sq(0.02f))
&& !_control_status.flags.ev_yaw
&& !_control_status.flags.gps_yaw;
// TODO: allow clearing mag_fault if mag_3d is good?
if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {
ECL_INFO("starting mag 3D fusion");
} else if (!_control_status.flags.mag_3D && _control_status_prev.flags.mag_3D) {
ECL_INFO("stopping mag 3D fusion");
}
// if we are using 3-axis magnetometer fusion, but without external NE aiding,
// then the declination must be fused as an observation to prevent long term heading drift
// fusing declination when gps aiding is available is optional, but recommended to prevent
// problem if the vehicle is static for extended periods of time
const bool mag_decl_user_selected = (_params.mag_declination_source & GeoDeclinationMask::FUSE_DECL);
const bool not_using_ne_aiding = !_control_status.flags.gps;
_control_status.flags.mag_dec = (_control_status.flags.mag && (not_using_ne_aiding || mag_decl_user_selected));
if (_control_status.flags.mag) {
aid_src.timestamp_sample = mag_sample.time_us;
aid_src.fusion_enabled = true;
if (continuing_conditions_passing && _control_status.flags.yaw_align) {
if (mag_sample.reset || checkHaglYawResetReq()) {
ECL_INFO("reset to %s", AID_SRC_NAME);
resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
aid_src.time_last_fuse = _time_delayed_us;
} else {
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
// before fusing magnetometer data to prevent rapid rotation of the earth field
// states for the first few observations.
fuseDeclination(0.02f);
_mag_decl_cov_reset = true;
fuseMag(mag_sample.mag, aid_src, false);
} 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;
fuseMag(mag_sample.mag, aid_src, update_all_states);
if (_control_status.flags.mag_dec) {
fuseDeclination(0.5f);
}
}
}
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
if (is_fusion_failing) {
if (_nb_mag_3d_reset_available > 0) {
// Data seems good, attempt a reset (mag states only unless mag_3D currently active)
ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
aid_src.time_last_fuse = _time_delayed_us;
if (_control_status.flags.in_air) {
_nb_mag_3d_reset_available--;
}
} else if (starting_conditions_passing) {
// Data seems good, but previous reset did not fix the issue
// something else must be wrong, declare the sensor faulty and stop the fusion
//_control_status.flags.mag_fault = true;
ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
stopMagFusion();
} else {
// A reset did not fix the issue but all the starting checks are not passing
// This could be a temporary issue, stop the fusion without declaring the sensor faulty
ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
stopMagFusion();
}
}
} else {
// Stop fusion but do not declare it faulty
ECL_DEBUG("stopping %s fusion, continuing conditions no longer passing", AID_SRC_NAME);
stopMagFusion();
}
} else {
if (starting_conditions_passing) {
_control_status.flags.mag = true;
loadMagCovData();
// activate fusion, reset mag states and initialize variance if first init or in flight reset
if (!_control_status.flags.yaw_align
|| wmm_updated
|| !_mag_decl_cov_reset
|| !_state.mag_I.longerThan(0.f)
|| (P.slice<3, 3>(16, 16).diag().min() < sq(0.0001f)) // mag_I
|| (P.slice<3, 3>(19, 19).diag().min() < sq(0.0001f)) // mag_B
) {
ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME);
bool reset_heading = !_control_status.flags.yaw_align;
resetMagStates(_mag_lpf.getState(), reset_heading);
if (reset_heading) {
_control_status.flags.yaw_align = true;
}
} else {
ECL_INFO("starting %s fusion", AID_SRC_NAME);
fuseMag(mag_sample.mag, aid_src, false);
}
aid_src.time_last_fuse = _time_delayed_us;
_nb_mag_3d_reset_available = 2;
}
}
aid_src.fusion_enabled = _control_status.flags.mag;
}
void Ekf::stopMagFusion()
{
if (_control_status.flags.mag) {
ECL_INFO("stopping mag fusion");
_control_status.flags.mag = false;
_control_status.flags.mag_dec = false;
if (_control_status.flags.mag_3D) {
ECL_INFO("stopping mag 3D fusion");
_control_status.flags.mag_3D = false;
}
_fault_status.flags.bad_mag_x = false;
_fault_status.flags.bad_mag_y = false;
_fault_status.flags.bad_mag_z = false;
_fault_status.flags.bad_mag_decl = false;
saveMagCovData();
}
}
src/modules/ekf2/EKF/mag_control.cpp
+113 -421
View File
@@ -41,199 +41,86 @@
void Ekf::controlMagFusion()
{
bool mag_data_ready = false;
bool mag_data_valid = false;
magSample mag_sample;
if (_mag_buffer) {
mag_data_ready = _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample);
if (mag_data_ready) {
// sensor or calibration has changed, clear any mag bias and reset low pass filter
if (mag_sample.reset || (_mag_counter == 0)) {
// Zero the magnetometer bias states
_state.mag_B.zero();
// Zero the corresponding covariances and set
// variances to the values use for initial alignment
P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
// reset any saved covariance data for re-use when auto-switching between heading and 3-axis fusion
_saved_mag_bf_variance.zero();
_control_status.flags.mag_fault = false;
_mag_lpf.reset(mag_sample.mag);
_mag_counter = 1;
} else {
_mag_lpf.update(mag_sample.mag);
_mag_counter++;
}
// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
// this is useful if there is a lot of interference on the sensor measurement.
if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
&& (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps))
) {
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred);
_control_status.flags.synthetic_mag_z = true;
} else {
_control_status.flags.synthetic_mag_z = false;
}
mag_data_valid = checkMagField(mag_sample.mag);
// compute mag heading innovation (for estimator_aid_src_mag_heading logging)
const Vector3f mag_observation = mag_sample.mag - _state.mag_B;
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
const Vector3f mag_earth_pred = R_to_earth * mag_observation;
resetEstimatorAidStatus(_aid_src_mag_heading);
_aid_src_mag_heading.timestamp_sample = mag_sample.time_us;
_aid_src_mag_heading.observation = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
_aid_src_mag_heading.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _aid_src_mag_heading.observation);
// compute magnetometer innovations (for estimator_aid_src_mag logging)
// rotate magnetometer earth field state into body frame
const Vector3f mag_I_body = _state.quat_nominal.rotateVectorInverse(_state.mag_I);
const Vector3f mag_innov = mag_I_body - mag_observation;
resetEstimatorAidStatus(_aid_src_mag);
_aid_src_mag.timestamp_sample = mag_sample.time_us;
mag_observation.copyTo(_aid_src_mag.observation);
mag_innov.copyTo(_aid_src_mag.innovation);
} else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) {
// No data anymore. Stop until it comes back.
stopMagFusion();
}
}
// If we are on ground, reset the flight alignment flag so that the mag fields will be
// re-initialised next time we achieve flight altitude
if (!_control_status.flags.in_air) {
// reset the flight alignment flag so that the mag fields will be
// re-initialised next time we achieve flight altitude
if (!_control_status_prev.flags.in_air && _control_status.flags.in_air) {
_control_status.flags.mag_aligned_in_flight = false;
}
if (mag_data_ready && mag_data_valid && !_control_status.flags.tilt_align && !_control_status.flags.yaw_align) {
// calculate the initial magnetic field and yaw alignment
// but do not mark the yaw alignement complete as it needs to be
// reset once the leveling phase is done
if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
if ((_mag_counter > 1) && isTimedOut(_aid_src_mag_heading.time_last_fuse, (uint64_t)100'000)) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
// the angle of the projection onto the horizontal gives the yaw angle
const Vector3f mag_earth_pred = updateYawInRotMat(0.f, _R_to_earth) * _mag_lpf.getState();
const float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
// check mag state observability
checkYawAngleObservability();
checkMagBiasObservability();
const float yaw_prev = getEulerYaw(_R_to_earth);
if (fabsf(yaw_new - yaw_prev) > math::radians(1.f)) {
ECL_INFO("mag heading init %.3f -> %.3f rad (declination %.1f)", (double)yaw_prev, (double)yaw_new, (double)getMagDeclination());
// update the rotation matrix using the new yaw value
_R_to_earth = updateYawInRotMat(yaw_new, Dcmf(_state.quat_nominal));
_state.quat_nominal = _R_to_earth;
// reset the output predictor state history to match the EKF initial values
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);
// set the earth magnetic field states using the updated rotation
_state.mag_I = _R_to_earth * _mag_lpf.getState();
_state.mag_B.zero();
_aid_src_mag_heading.time_last_fuse = _time_delayed_us;
_time_last_heading_fuse = _time_delayed_us;
}
}
}
return;
if (_mag_bias_observable || _yaw_angle_observable) {
_time_last_mov_3d_mag_suitable = _time_delayed_us;
}
if (_params.mag_fusion_type >= MagFuseType::NONE
|| _control_status.flags.mag_fault
|| !_control_status.flags.tilt_align) {
if (_params.mag_fusion_type == MagFuseType::NONE) {
stopMagFusion();
stopMagHdgFusion();
return;
}
_mag_yaw_reset_req |= !_control_status.flags.yaw_align;
// stop mag (require a reset before using again) if there was an external yaw reset (yaw estimator, GPS yaw, etc)
if (_mag_decl_cov_reset && (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat)) {
ECL_INFO("yaw reset, stopping mag fusion to force reinitialization");
stopMagFusion();
resetMagCov();
}
if (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) {
magSample mag_sample;
if (shouldInhibitMag() || !mag_data_valid) {
if (uint32_t(_time_delayed_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
// If magnetometer use has been inhibited continuously then stop the fusion
stopMagFusion();
}
if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) {
return;
if (mag_sample.reset || (_mag_counter == 0)) {
// sensor or calibration has changed, reset low pass filter (reset handled by controlMag3DFusion/controlMagHeadingFusion)
_control_status.flags.mag_fault = false;
_state.mag_B.zero();
resetMagCov();
_mag_lpf.reset(mag_sample.mag);
_mag_counter = 1;
} else {
_mag_use_not_inhibit_us = _time_delayed_us;
_mag_lpf.update(mag_sample.mag);
_mag_counter++;
}
const bool mag_enabled_previously = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
const bool starting_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
&& checkMagField(mag_sample.mag)
&& (_mag_counter > 5) // wait until we have more than a few samples through the filter
&& (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame
&& (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset
&& isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL);
// Determine if we should use simple magnetic heading fusion which works better when
// there are large external disturbances or the more accurate 3-axis fusion
switch (_params.mag_fusion_type) {
default:
// FALLTHROUGH
case MagFuseType::AUTO:
selectMagAuto();
break;
// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
// this is useful if there is a lot of interference on the sensor measurement.
if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
&& (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps))
) {
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
* Vector3f(_mag_strength_gps, 0, 0);
case MagFuseType::INDOOR:
mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred);
/* fallthrough */
case MagFuseType::HEADING:
startMagHdgFusion();
break;
_control_status.flags.synthetic_mag_z = true;
case MagFuseType::MAG_3D:
startMag3DFusion();
break;
} else {
_control_status.flags.synthetic_mag_z = false;
}
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
controlMag3DFusion(mag_sample, starting_conditions_passing, _aid_src_mag);
controlMagHeadingFusion(mag_sample, starting_conditions_passing, _aid_src_mag_heading);
if (_mag_yaw_reset_req || !_control_status.flags.yaw_align || mag_sample.reset || !mag_enabled_previously || haglYawResetReq()) {
if ((_mag_counter > 1) && resetMagStates(_mag_lpf.getState())) {
_mag_yaw_reset_req = false;
} else {
// mag reset failed, try again next time
_mag_yaw_reset_req = true;
if (mag_sample.reset) {
return;
}
}
}
}
if (!_control_status.flags.yaw_align) {
// Having the yaw aligned is mandatory to continue
return;
}
checkMagDeclRequired();
runMagAndMagDeclFusions(mag_sample.mag);
} else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) {
// No data anymore. Stop until it comes back.
stopMagHdgFusion();
stopMagFusion();
}
}
bool Ekf::haglYawResetReq()
bool Ekf::checkHaglYawResetReq() const
{
// We need to reset the yaw angle after climbing away from the ground to enable
// recovery from ground level magnetic interference.
@@ -244,30 +131,20 @@ bool Ekf::haglYawResetReq()
static constexpr float mag_anomalies_max_hagl = 1.5f;
const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
return above_mag_anomalies;
#else
return true;
#endif // CONFIG_EKF2_RANGE_FINDER
}
return false;
}
bool Ekf::resetMagStates(const Vector3f &mag)
void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
{
// prevent a reset being performed more than once on the same frame
if ((_flt_mag_align_start_time == _time_delayed_us)
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
return false;
}
bool reset_heading = true;
bool has_realigned_yaw = false;
// reinit mag states
const Vector3f mag_I_before_reset = _state.mag_I;
const Vector3f mag_B_before_reset = _state.mag_B;
const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
// reset covariances to default
resetMagCov();
// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
@@ -276,91 +153,72 @@ bool Ekf::resetMagStates(const Vector3f &mag)
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
* Vector3f(_mag_strength_gps, 0, 0);
// mag_B: reset
if (isYawEmergencyEstimateAvailable()) {
const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth);
const Dcmf R_to_body = R_to_earth.transpose();
// mag_B: reset using WMM and yaw estimator, initialize variances (so resetMagHeading will use mag bias)
// mag_B: reset using WMM and yaw estimator
_state.mag_B = mag - (R_to_body * mag_earth_pred);
P.uncorrelateCovarianceSetVariance<3>(19, R_MAG);
_saved_mag_bf_variance = P.slice<3, 3>(19, 19).diag();
ECL_INFO("using yaw estimator to reset mag bias [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
(double)mag_B_before_reset(0), (double)mag_B_before_reset(1), (double)mag_B_before_reset(2),
(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
);
ECL_INFO("resetMagStates using yaw estimator");
} else if (!reset_heading && _control_status.flags.yaw_align) {
// mag_B: reset using WMM
const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
_state.mag_B = mag - (R_to_body * mag_earth_pred);
} else {
// mag_B: reset
_state.mag_B.zero();
}
// mag_I: reset, skipped if no change in state and variance good
_state.mag_I = mag_earth_pred;
if (reset_heading) {
has_realigned_yaw = resetMagHeading(mag);
resetMagHeading(mag);
}
// mag_I: reset to WMM
_state.mag_I = mag_earth_pred;
// earth field was reset to WMM, skip initial declination fusion
_mag_decl_cov_reset = true;
} else {
// mag_B: reset
_state.mag_B.zero();
// Use the magnetometer measurement to reset the field states
if (reset_heading) {
has_realigned_yaw = resetMagHeading(mag);
resetMagHeading(mag);
}
// mag_I: use the last magnetometer measurements to reset the field states
_state.mag_I = _R_to_earth * mag;
}
resetMagCov();
if (!mag_I_before_reset.longerThan(0.f)) {
ECL_INFO("initializing mag I [%.3f, %.3f, %.3f], mag B [%.3f, %.3f, %.3f]",
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
);
if (has_realigned_yaw) {
if (!mag_I_before_reset.longerThan(0.f)) {
ECL_INFO("initializing mag I [%.3f, %.3f, %.3f], mag B [%.3f, %.3f, %.3f]",
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
} else {
ECL_INFO("resetting mag I [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
(double)mag_I_before_reset(0), (double)mag_I_before_reset(1), (double)mag_I_before_reset(2),
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2)
);
if (mag_B_before_reset.longerThan(0.f) || _state.mag_B.longerThan(0.f)) {
ECL_INFO("resetting mag B [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
(double)mag_B_before_reset(0), (double)mag_B_before_reset(1), (double)mag_B_before_reset(2),
(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
);
} else {
ECL_INFO("resetting mag I [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
(double)mag_I_before_reset(0), (double)mag_I_before_reset(1), (double)mag_I_before_reset(2),
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2)
);
}
_control_status.flags.yaw_align = true;
if (_control_status.flags.in_air) {
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _time_delayed_us;
_control_status.flags.mag_aligned_in_flight = true;
} else {
_flt_mag_align_start_time = 0;
_control_status.flags.mag_aligned_in_flight = false;
}
return true;
}
return false;
}
void Ekf::selectMagAuto()
{
check3DMagFusionSuitability();
canUse3DMagFusion() ? startMag3DFusion() : startMagHdgFusion();
}
void Ekf::check3DMagFusionSuitability()
{
checkYawAngleObservability();
checkMagBiasObservability();
if (_mag_bias_observable || _yaw_angle_observable) {
_time_last_mov_3d_mag_suitable = _time_delayed_us;
// record the start time for the magnetic field alignment
if (_control_status.flags.in_air) {
_control_status.flags.mag_aligned_in_flight = true;
_flt_mag_align_start_time = _time_delayed_us;
}
}
@@ -399,49 +257,10 @@ void Ekf::checkMagBiasObservability()
_time_yaw_started = _time_delayed_us;
}
bool Ekf::canUse3DMagFusion() const
{
// Use of 3D fusion requires an in-air heading alignment but it should not
// be used when the heading and mag biases are not observable for more than 2 seconds
return _control_status.flags.mag_aligned_in_flight
&& ((_time_delayed_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6);
}
void Ekf::checkMagDeclRequired()
{
// if we are using 3-axis magnetometer fusion, but without external NE aiding,
// then the declination must be fused as an observation to prevent long term heading drift
// fusing declination when gps aiding is available is optional, but recommended to prevent
// problem if the vehicle is static for extended periods of time
const bool user_selected = (_params.mag_declination_source & GeoDeclinationMask::FUSE_DECL);
const bool not_using_ne_aiding = !_control_status.flags.gps;
_control_status.flags.mag_dec = (_control_status.flags.mag_3D && (not_using_ne_aiding || user_selected));
}
bool Ekf::shouldInhibitMag() const
{
// If the user has selected auto protection against indoor magnetic field errors, only use the magnetometer
// if a yaw angle relative to true North is required for navigation. If no GPS or other earth frame aiding
// is available, assume that we are operating indoors and the magnetometer should not be used.
// Also inhibit mag fusion when a strong magnetic field interference is detected or the user
// has explicitly stopped magnetometer use.
const bool user_selected = (_params.mag_fusion_type == MagFuseType::INDOOR);
const bool heading_not_required_for_navigation = !_control_status.flags.gps;
return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed;
}
bool Ekf::checkMagField(const Vector3f &mag_sample)
{
_control_status.flags.mag_field_disturbed = false;
if ((_params.mag_fusion_type == MagFuseType::NONE)
|| (_params.mag_fusion_type == MagFuseType::INDOOR && !_control_status.flags.gps)) {
//TODO: review this: gps flag cannnot be set if yaw isn't aligned
return false;
}
if (_params.mag_check == 0) {
// skip all checks
return true;
@@ -506,105 +325,38 @@ bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected,
&& (measured <= expected + gate);
}
void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
void Ekf::resetMagHeading(const Vector3f &mag)
{
if (_control_status.flags.mag_3D) {
run3DMagAndDeclFusions(mag);
} else if (_control_status.flags.mag_hdg) {
// Rotate the measurements into earth frame using the zero yaw angle
Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
Vector3f mag_earth_pred = R_to_earth * (mag - _state.mag_B);
// the angle of the projection onto the horizontal gives the yaw angle
// calculate the yaw innovation and wrap to the interval between +-pi
float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f);
_aid_src_mag_heading.observation = measured_hdg;
_aid_src_mag_heading.observation_variance = obs_var;
_aid_src_mag_heading.innovation = innovation;
_aid_src_mag_heading.fusion_enabled = _control_status.flags.mag_hdg;
fuseYaw(_aid_src_mag_heading);
}
}
void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
{
// sanity check mag_B before they are used to constrain heading drift
const Vector3f mag_bias_var = P.slice<3, 3>(19, 19).diag();
const bool mag_bias_var_good = (mag_bias_var.min() > 0.f) && (mag_bias_var.max() < sq(0.02f));
const bool update_all_states = _control_status.flags.mag_aligned_in_flight && mag_bias_var_good;
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
// before fusing magnetometer data to prevent rapid rotation of the earth field
// states for the first few observations.
fuseDeclination(0.02f);
_mag_decl_cov_reset = true;
fuseMag(mag, _aid_src_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, _aid_src_mag, update_all_states);
if (_control_status.flags.mag_dec) {
fuseDeclination(0.5f);
}
}
}
bool Ekf::resetMagHeading(const Vector3f &mag)
{
// prevent a reset being performed more than once on the same frame
if ((_flt_mag_align_start_time == _time_delayed_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
return false;
}
// use mag bias if variance good (unless configured for HEADING only)
Vector3f mag_bias{0.f, 0.f, 0.f};
const Vector3f mag_bias_var = getMagBiasVariance();
if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= math::max(sq(_params.mag_noise), sq(0.01f)))) {
if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) {
mag_bias = _state.mag_B;
}
const Vector3f mag_init = _mag_lpf.getState() - mag_bias;
// calculate mag heading
// rotate the magnetometer measurements into earth frame using a zero yaw angle
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
const bool heading_required_for_navigation = _control_status.flags.gps;
// the angle of the projection onto the horizontal gives the yaw angle
const Vector3f mag_earth_pred = R_to_earth * (mag - mag_bias);
if ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || ((_params.mag_fusion_type == MagFuseType::INDOOR) && heading_required_for_navigation)) {
// calculate the observed yaw angle and yaw variance
const float declination = getMagDeclination();
float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination;
float yaw_new_variance = math::max(sq(_params.mag_heading_noise), sq(0.01f));
// rotate the magnetometer measurements into earth frame using a zero yaw angle
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
ECL_INFO("reset mag heading %.3f -> %.3f rad (bias:[%.3f, %.3f, %.3f], declination:%.1f)",
(double)getEulerYaw(_R_to_earth), (double)yaw_new,
(double)mag_bias(0), (double)mag_bias(1), (double)mag_bias(2),
(double)declination);
// the angle of the projection onto the horizontal gives the yaw angle
const Vector3f mag_earth_pred = R_to_earth * mag_init;
// update quaternion states and corresponding covarainces
resetQuatStateYaw(yaw_new, yaw_new_variance);
_mag_heading_last_declination = declination;
// calculate the observed yaw angle and yaw variance
float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
float yaw_new_variance = math::max(sq(_params.mag_heading_noise), sq(0.01f));
ECL_INFO("reset mag heading %.3f -> %.3f rad (bias:[%.3f, %.3f, %.3f], declination:%.1f)",
(double)getEulerYaw(_R_to_earth), (double)yaw_new,
(double)mag_bias(0), (double)mag_bias(1), (double)mag_bias(2),
(double)getMagDeclination());
// update quaternion states and corresponding covarainces
resetQuatStateYaw(yaw_new, yaw_new_variance);
return true;
}
return false;
_time_last_heading_fuse = _time_delayed_us;
}
float Ekf::getMagDeclination()
@@ -616,72 +368,12 @@ float Ekf::getMagDeclination()
} else if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
// use parameter value until GPS is available, then use value returned by geo library
if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) {
if (PX4_ISFINITE(_mag_declination_gps)) {
return _mag_declination_gps;
} else {
return math::radians(_params.mag_declination_deg);
}
} else {
// always use the parameter value
return math::radians(_params.mag_declination_deg);
}
// otherwise use the parameter value
return math::radians(_params.mag_declination_deg);
}
void Ekf::stopMagFusion()
{
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
ECL_INFO("stopping all mag fusion");
stopMag3DFusion();
stopMagHdgFusion();
clearMagCov();
}
}
void Ekf::stopMag3DFusion()
{
// save covariance data for re-use if currently doing 3-axis fusion
if (_control_status.flags.mag_3D) {
saveMagCovData();
_control_status.flags.mag_3D = false;
_control_status.flags.mag_dec = false;
_fault_status.flags.bad_mag_x = false;
_fault_status.flags.bad_mag_y = false;
_fault_status.flags.bad_mag_z = false;
_fault_status.flags.bad_mag_decl = false;
}
}
void Ekf::stopMagHdgFusion()
{
if (_control_status.flags.mag_hdg) {
_control_status.flags.mag_hdg = false;
_fault_status.flags.bad_hdg = false;
}
}
void Ekf::startMagHdgFusion()
{
if (!_control_status.flags.mag_hdg) {
stopMag3DFusion();
ECL_INFO("starting mag heading fusion");
_control_status.flags.mag_hdg = true;
}
}
void Ekf::startMag3DFusion()
{
if (!_control_status.flags.mag_3D) {
stopMagHdgFusion();
zeroMagCov();
loadMagCovData();
_control_status.flags.mag_3D = true;
}
}
src/modules/ekf2/EKF/mag_fusion.cpp
+69 -29
View File
@@ -54,10 +54,10 @@
bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states)
{
// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f));
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
const char* numerical_error_covariance_reset_string = "numerical error - covariance reset";
const char *numerical_error_covariance_reset_string = "numerical error - covariance reset";
Vector3f mag_innov;
Vector3f innov_var;
@@ -75,7 +75,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
_fault_status.flags.bad_mag_x = true;
// we need to re-initialise covariances and abort this fusion step
resetMagRelatedCovariances();
if (update_all_states) {
resetQuatCov();
}
resetMagCov();
ECL_ERR("magX %s", numerical_error_covariance_reset_string);
return false;
}
@@ -88,7 +93,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
_fault_status.flags.bad_mag_y = true;
// we need to re-initialise covariances and abort this fusion step
resetMagRelatedCovariances();
if (update_all_states) {
resetQuatCov();
}
resetMagCov();
ECL_ERR("magY %s", numerical_error_covariance_reset_string);
return false;
}
@@ -100,7 +110,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
_fault_status.flags.bad_mag_z = true;
// we need to re-initialise covariances and abort this fusion step
resetMagRelatedCovariances();
if (update_all_states) {
resetQuatCov();
}
resetMagCov();
ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
return false;
}
@@ -118,7 +133,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
aid_src_mag.innovation[i] = mag_innov(i);
}
aid_src_mag.fusion_enabled = _control_status.flags.mag_3D && update_all_states;
aid_src_mag.fusion_enabled = _control_status.flags.mag;
// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
if (_control_status.flags.synthetic_mag_z) {
@@ -159,7 +174,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
_fault_status.flags.bad_mag_y = true;
// we need to re-initialise covariances and abort this fusion step
resetMagRelatedCovariances();
if (update_all_states) {
resetQuatCov();
}
resetMagCov();
ECL_ERR("magY %s", numerical_error_covariance_reset_string);
return false;
}
@@ -183,7 +203,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
_fault_status.flags.bad_mag_z = true;
// we need to re-initialise covariances and abort this fusion step
resetMagRelatedCovariances();
if (update_all_states) {
resetQuatCov();
}
resetMagCov();
ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
return false;
}
@@ -214,7 +239,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
_fault_status.flags.bad_mag_y = !fused[1];
_fault_status.flags.bad_mag_z = !fused[2];
if (fused[0] && fused[1] && fused[2]) {
if (fused[0] && fused[1] && (fused[2] || _control_status.flags.synthetic_mag_z)) {
aid_src_mag.fused = true;
aid_src_mag.time_last_fuse = _time_delayed_us;
@@ -230,7 +255,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
}
// update quaternion states and covariances using the yaw innovation and yaw observation variance
bool Ekf::fuseYaw(estimator_aid_source1d_s& aid_src_status)
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status)
{
Vector24f H_YAW;
computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW);
@@ -238,7 +263,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s& aid_src_status)
return fuseYaw(aid_src_status, H_YAW);
}
bool Ekf::fuseYaw(estimator_aid_source1d_s& aid_src_status, const Vector24f &H_YAW)
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW)
{
// define the innovation gate size
float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
@@ -285,9 +310,9 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s& aid_src_status, const Vector24f &H_Y
// we allow to use it when on the ground because the large innovation could be caused
// by interference or a large initial gyro bias
if (!_control_status.flags.in_air
&& isTimedOut(_time_last_in_air, (uint64_t)5e6)
&& isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6)
) {
&& isTimedOut(_time_last_in_air, (uint64_t)5e6)
&& isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6)
) {
// constrain the innovation to the maximum set by the gate
// we need to delay this forced fusion to avoid starting it
// immediately after touchdown, when the drone is still armed
@@ -339,6 +364,10 @@ void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vec
bool Ekf::fuseDeclination(float decl_sigma)
{
if (!_control_status.flags.mag) {
return false;
}
// observation variance (rad**2)
const float R_DECL = sq(decl_sigma);
@@ -346,6 +375,7 @@ bool Ekf::fuseDeclination(float decl_sigma)
float decl_pred;
float innovation_variance;
// TODO: review getMagDeclination() usage, use mag_I, _mag_declination_gps, or parameter?
sym::ComputeMagDeclinationPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);
const float innovation = wrap_pi(decl_pred - getMagDeclination());
@@ -373,26 +403,29 @@ bool Ekf::fuseDeclination(float decl_sigma)
void Ekf::limitDeclination()
{
const Vector3f mag_I_before = _state.mag_I;
// get a reference value for the earth field declinaton and minimum plausible horizontal field strength
// set to 50% of the horizontal strength from geo tables if location is known
float decl_reference;
float decl_reference = NAN;
float h_field_min = 0.001f;
if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
// use parameter value until GPS is available, then use value returned by geo library
if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) {
decl_reference = _mag_declination_gps;
h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL
&& (PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps) && PX4_ISFINITE(_mag_inclination_gps))
) {
decl_reference = _mag_declination_gps;
} else {
decl_reference = math::radians(_params.mag_declination_deg);
}
// set to 50% of the horizontal strength from geo tables if location is known
h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
} else {
// always use the parameter value
} else if (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) {
// use parameter value if GPS isn't available
decl_reference = math::radians(_params.mag_declination_deg);
}
if (!PX4_ISFINITE(decl_reference)) {
return;
}
// do not allow the horizontal field length to collapse - this will make the declination fusion badly conditioned
// and can result in a reversal of the NE field states which the filter cannot recover from
// apply a circular limit
@@ -406,9 +439,8 @@ void Ekf::limitDeclination()
} else {
// too small to scale radially so set to expected value
const float mag_declination = getMagDeclination();
_state.mag_I(0) = 2.0f * h_field_min * cosf(mag_declination);
_state.mag_I(1) = 2.0f * h_field_min * sinf(mag_declination);
_state.mag_I(0) = 2.0f * h_field_min * cosf(decl_reference);
_state.mag_I(1) = 2.0f * h_field_min * sinf(decl_reference);
}
h_field = h_field_min;
@@ -428,6 +460,14 @@ void Ekf::limitDeclination()
_state.mag_I(0) = h_field * cosf(decl_min);
_state.mag_I(1) = h_field * sinf(decl_min);
}
if ((mag_I_before - _state.mag_I).longerThan(0.01f)) {
ECL_DEBUG("declination limited mag I [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f] (decl: %.3f)",
(double)mag_I_before(0), (double)mag_I_before(1), (double)mag_I_before(2),
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
(double)decl_reference
);
}
}
float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)
src/modules/ekf2/EKF/mag_heading_control.cpp
+190
View File
@@ -0,0 +1,190 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mag_heading_control.cpp
* Control functions for ekf mag heading fusion
*/
#include "ekf.h"
void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing,
estimator_aid_source1d_s &aid_src)
{
static constexpr const char *AID_SRC_NAME = "mag heading";
// use mag bias if variance good
Vector3f mag_bias{0.f, 0.f, 0.f};
const Vector3f mag_bias_var = getMagBiasVariance();
if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) {
mag_bias = _state.mag_B;
}
// calculate mag heading
// Rotate the measurements into earth frame using the zero yaw angle
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
// the angle of the projection onto the horizontal gives the yaw angle
// calculate the yaw innovation and wrap to the interval between +-pi
const Vector3f mag_earth_pred = R_to_earth * (mag_sample.mag - mag_bias);
const float declination = getMagDeclination();
const float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination;
resetEstimatorAidStatus(aid_src);
aid_src.observation = measured_hdg;
aid_src.observation_variance = math::max(sq(_params.mag_heading_noise), sq(0.01f));
if (_control_status.flags.yaw_align) {
// mag heading
aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
} else {
// mag heading delta (logging only)
aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _mag_heading_pred_prev)
- wrap_pi(measured_hdg - _mag_heading_prev));
}
// determine if we should use mag heading aiding
bool continuing_conditions_passing = ((_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.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
&& !_control_status.flags.mag_fault
&& !_control_status.flags.mag_field_disturbed
&& !_control_status.flags.ev_yaw
&& !_control_status.flags.gps_yaw
&& PX4_ISFINITE(aid_src.observation)
&& PX4_ISFINITE(aid_src.observation_variance);
const bool starting_conditions_passing = common_starting_conditions_passing
&& continuing_conditions_passing
&& isTimedOut(aid_src.time_last_fuse, 3e6);
if (_control_status.flags.mag_hdg) {
aid_src.fusion_enabled = true;
aid_src.timestamp_sample = mag_sample.time_us;
if (continuing_conditions_passing && _control_status.flags.yaw_align) {
const bool declination_changed = _control_status_prev.flags.mag_hdg
&& (fabsf(declination - _mag_heading_last_declination) > math::radians(3.f));
if (mag_sample.reset || declination_changed) {
if (declination_changed) {
ECL_INFO("reset to %s, declination changed %.5f->%.5f",
AID_SRC_NAME, (double)_mag_heading_last_declination, (double)declination);
} else {
ECL_INFO("reset to %s", AID_SRC_NAME);
}
resetMagHeading(_mag_lpf.getState());
aid_src.time_last_fuse = _time_delayed_us;
} else {
fuseYaw(aid_src);
}
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
if (is_fusion_failing) {
if (_nb_mag_heading_reset_available > 0) {
// Data seems good, attempt a reset
ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
resetMagHeading(_mag_lpf.getState());
aid_src.time_last_fuse = _time_delayed_us;
if (_control_status.flags.in_air) {
_nb_mag_heading_reset_available--;
}
} else if (starting_conditions_passing) {
// Data seems good, but previous reset did not fix the issue
// something else must be wrong, declare the sensor faulty and stop the fusion
_control_status.flags.mag_fault = true;
ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
stopMagHdgFusion();
} else {
// A reset did not fix the issue but all the starting checks are not passing
// This could be a temporary issue, stop the fusion without declaring the sensor faulty
ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
stopMagHdgFusion();
}
}
} else {
// Stop fusion but do not declare it faulty
ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME);
stopMagHdgFusion();
}
} else {
if (starting_conditions_passing) {
// activate fusion
ECL_INFO("starting %s fusion", AID_SRC_NAME);
if (!_control_status.flags.yaw_align) {
// reset heading
resetMagHeading(_mag_lpf.getState());
_control_status.flags.yaw_align = true;
}
_control_status.flags.mag_hdg = true;
aid_src.time_last_fuse = _time_delayed_us;
_nb_mag_heading_reset_available = 1;
}
}
aid_src.fusion_enabled = _control_status.flags.mag_hdg;
// record corresponding mag heading and yaw state for future mag heading delta heading innovation (logging only)
_mag_heading_prev = measured_hdg;
_mag_heading_pred_prev = getEulerYaw(_state.quat_nominal);
_mag_heading_last_declination = getMagDeclination();
}
void Ekf::stopMagHdgFusion()
{
if (_control_status.flags.mag_hdg) {
ECL_INFO("stopping mag heading fusion");
resetEstimatorAidStatus(_aid_src_mag_heading);
_control_status.flags.mag_hdg = false;
_fault_status.flags.bad_hdg = false;
}
}
src/modules/ekf2/EKF/zero_innovation_heading_update.cpp
+3 -4
View File
@@ -44,13 +44,12 @@ void Ekf::controlZeroInnovationHeadingUpdate()
|| _control_status.flags.ev_yaw || _control_status.flags.gps_yaw;
// fuse zero innovation at a limited rate if the yaw variance is too large
if (_control_status.flags.tilt_align
&& !yaw_aiding
if(!yaw_aiding
&& isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
// Use an observation variance larger than usual but small enough
// to constrain the yaw variance just below the threshold
const float obs_var = 0.25f;
const float obs_var = _control_status.flags.tilt_align ? 0.25f : 0.001f;
estimator_aid_source1d_s aid_src_status{};
aid_src_status.observation = getEulerYaw(_state.quat_nominal);
@@ -61,7 +60,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);
if ((aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
if (!_control_status.flags.tilt_align || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
// The yaw variance is too large, fuse fake measurement
aid_src_status.fusion_enabled = true;
fuseYaw(aid_src_status, H_YAW);
src/modules/ekf2/EKF/zero_velocity_update.cpp
+1 -1
View File
@@ -46,7 +46,7 @@ void Ekf::controlZeroVelocityUpdate()
if (zero_velocity_update_data_ready) {
const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest
&& _control_status_prev.flags.vehicle_at_rest
&& !isVerticalVelocityAidingActive(); // otherwise the filter is "too rigid" to follow a position drift
&& (!isVerticalVelocityAidingActive() || !_control_status.flags.tilt_align); // otherwise the filter is "too rigid" to follow a position drift
if (continuing_conditions_passing) {
Vector3f vel_obs{0, 0, 0};