diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 0a6e23f074..39aa79ba3d 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -91,6 +91,7 @@ enum class VelocityFrame : uint8_t { BODY_FRAME_FRD = 2 }; +#if defined(CONFIG_EKF2_MAGNETOMETER) enum GeoDeclinationMask : uint8_t { // Bit locations for mag_declination_source USE_GEO_DECL = (1<<0), ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value @@ -104,6 +105,7 @@ enum MagFuseType : uint8_t { 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 NONE = 5 ///< Do not use magnetometer under any circumstance.. }; +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_TERRAIN) enum TerrainFusionMask : uint8_t { @@ -284,7 +286,6 @@ struct parameters { int32_t sensor_interval_max_ms{10}; ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec) // measurement time delays - float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec) float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) @@ -295,8 +296,6 @@ struct parameters { // process noise float gyro_bias_p_noise{1.0e-3f}; ///< process noise for IMU rate gyro bias prediction (rad/sec**2) float accel_bias_p_noise{1.0e-2f}; ///< process noise for IMU accelerometer bias prediction (m/sec**3) - float mage_p_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec) - float magb_p_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec) #if defined(CONFIG_EKF2_WIND) const float initial_wind_uncertainty{1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec) @@ -322,17 +321,31 @@ struct parameters { float gnd_effect_deadzone{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m) float gnd_effect_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m) - // magnetometer fusion + float heading_innov_gate{2.6f}; ///< heading fusion innovation consistency gate size (STD) float mag_heading_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad) + +#if defined(CONFIG_EKF2_MAGNETOMETER) + float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) + + float mage_p_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec) + float magb_p_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec) + + // magnetometer fusion 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) int32_t mag_declination_source{7}; ///< bitmask used to control the handling of declination data int32_t mag_fusion_type{0}; ///< integer used to specify the type of magnetometer fusion used float mag_acc_gate{0.5f}; ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2) float mag_yaw_rate_gate{0.20f}; ///< yaw rate threshold used by mode select logic (rad/sec) + // compute synthetic magnetomter Z value if possible + int32_t synthesize_mag_z{0}; + int32_t mag_check{0}; + float mag_check_strength_tolerance_gs{0.2f}; + float mag_check_inclination_tolerance_deg{20.f}; +#endif // CONFIG_EKF2_MAGNETOMETER + #if defined(CONFIG_EKF2_GNSS_YAW) // GNSS heading fusion float gps_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad) @@ -483,12 +496,6 @@ struct parameters { const float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD) #endif // CONFIG_EKF2_AUXVEL - // compute synthetic magnetomter Z value if possible - int32_t synthesize_mag_z{0}; - int32_t mag_check{0}; - float mag_check_strength_tolerance_gs{0.2f}; - float mag_check_inclination_tolerance_deg{20.f}; - // Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s) const unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index bf359a6cea..50c15c8262 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -177,24 +177,6 @@ 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 = 0.0f; - - if (_control_status.flags.mag && P.trace(State::mag_I.idx) < 0.1f) { -#if defined(CONFIG_EKF2_MAGNETOMETER) - mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f); -#endif // CONFIG_EKF2_MAGNETOMETER - } - - // 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 = 0.0f; - - if (_control_status.flags.mag && P.trace(State::mag_B.idx) < 0.1f) { -#if defined(CONFIG_EKF2_MAGNETOMETER) - mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f); -#endif // CONFIG_EKF2_MAGNETOMETER - } - // assign IMU noise variances // inputs to the system are 3 delta angles and 3 delta velocities float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f); @@ -255,18 +237,31 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) } if (_control_status.flags.mag) { - const float mag_I_process_noise = sq(mag_I_sig); - for (unsigned index = 0; index < State::mag_I.dof; index++) { - unsigned i = State::mag_I.idx + index; - nextP(i, i) += mag_I_process_noise; +#if defined(CONFIG_EKF2_MAGNETOMETER) + // 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 + if (P.trace(State::mag_I.idx) < 0.1f) { + + float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f); + float mag_I_process_noise = sq(mag_I_sig); + + for (unsigned index = 0; index < State::mag_I.dof; index++) { + unsigned i = State::mag_I.idx + index; + nextP(i, i) += mag_I_process_noise; + } } - const float mag_B_process_noise = sq(mag_B_sig); - for (unsigned index = 0; index < State::mag_B.dof; index++) { - unsigned i = State::mag_B.idx + index; - nextP(i, i) += mag_B_process_noise; - } + // Don't continue to grow the body 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 + if (P.trace(State::mag_B.idx) < 0.1f) { + float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f); + float mag_B_process_noise = sq(mag_B_sig); + + for (unsigned index = 0; index < State::mag_B.dof; index++) { + unsigned i = State::mag_B.idx + index; + nextP(i, i) += mag_B_process_noise; + } + } +#endif // CONFIG_EKF2_MAGNETOMETER } else { // keep previous covariance for (unsigned i = 0; i < State::mag_I.dof; i++) { @@ -335,8 +330,6 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) const float vel_var_max = 1e6f; const float pos_var_max = 1e6f; const float gyro_bias_var_max = 1.0f; - const float mag_I_var_max = 1.0f; - const float mag_B_var_max = 1.0f; constrainStateVar(State::quat_nominal, 0.f, quat_var_max); constrainStateVar(State::vel, 1e-6f, vel_var_max); @@ -471,17 +464,22 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // magnetic field states if (!_control_status.flags.mag) { - P.uncorrelateCovarianceSetVariance<3>(16, 0.0f); - P.uncorrelateCovarianceSetVariance<3>(19, 0.0f); + P.uncorrelateCovarianceSetVariance(State::mag_I.idx, 0.0f); + P.uncorrelateCovarianceSetVariance(State::mag_B.idx, 0.0f); } else { +#if defined(CONFIG_EKF2_MAGNETOMETER) + const float mag_I_var_max = 1.f; constrainStateVar(State::mag_I, 0.f, mag_I_var_max); + + const float mag_B_var_max = 1.f; constrainStateVar(State::mag_B, 0.f, mag_B_var_max); if (force_symmetry) { P.makeRowColSymmetric(State::mag_I.idx); P.makeRowColSymmetric(State::mag_B.idx); } +#endif // CONFIG_EKF2_MAGNETOMETER } // wind velocity states diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index f0f7f98088..276f574c09 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -57,14 +57,18 @@ void Ekf::reset() { ECL_INFO("reset"); + _state.quat_nominal.setIdentity(); _state.vel.setZero(); _state.pos.setZero(); _state.gyro_bias.setZero(); _state.accel_bias.setZero(); + +#if defined(CONFIG_EKF2_MAGNETOMETER) _state.mag_I.setZero(); _state.mag_B.setZero(); +#endif // CONFIG_EKF2_MAGNETOMETER + _state.wind_vel.setZero(); - _state.quat_nominal.setIdentity(); #if defined(CONFIG_EKF2_RANGE_FINDER) _range_sensor.setPitchOffset(_params.rng_sens_pitch); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 1f927f6675..faffaba236 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -239,8 +239,8 @@ void Ekf::constrainStates() const float accel_bias_limit = getAccelBiasLimit(); _state.accel_bias = matrix::constrain(_state.accel_bias, -accel_bias_limit, accel_bias_limit); - _state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f); #if defined(CONFIG_EKF2_MAGNETOMETER) + _state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f); _state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit()); #endif // CONFIG_EKF2_MAGNETOMETER @@ -401,6 +401,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons _pos_ref.initReference(latitude, longitude, _time_delayed_us); _gps_alt_ref = altitude; +#if defined(CONFIG_EKF2_MAGNETOMETER) const float mag_declination_gps = get_mag_declination_radians(latitude, longitude); const float mag_inclination_gps = get_mag_inclination_radians(latitude, longitude); const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude); @@ -412,6 +413,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons _wmm_gps_time_last_set = _time_delayed_us; } +#endif // CONFIG_EKF2_MAGNETOMETER // We don't know the uncertainty of the origin _gpos_origin_eph = 0.f; @@ -809,8 +811,12 @@ void Ekf::fuse(const VectorState &K, float innovation) _state.pos -= K.slice(State::pos.idx, 0) * innovation; _state.gyro_bias -= K.slice(State::gyro_bias.idx, 0) * innovation; _state.accel_bias -= K.slice(State::accel_bias.idx, 0) * innovation; + +#if defined(CONFIG_EKF2_MAGNETOMETER) _state.mag_I -= K.slice(State::mag_I.idx, 0) * innovation; _state.mag_B -= K.slice(State::mag_B.idx, 0) * innovation; +#endif // CONFIG_EKF2_MAGNETOMETER + _state.wind_vel -= K.slice(State::wind_vel.idx, 0) * innovation; } diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 04879dfd92..3063ba077f 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -571,10 +571,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) } #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_MAGNETOMETER) // mag mode if (_params.mag_fusion_type != MagFuseType::NONE) { max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms); } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) // using range finder diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index f09745ec98..a9e7f06f1a 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -452,12 +452,15 @@ protected: uint64_t _wmm_gps_time_last_checked{0}; // time WMM last checked uint64_t _wmm_gps_time_last_set{0}; // time WMM last set + +#if defined(CONFIG_EKF2_MAGNETOMETER) 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}; +#endif // CONFIG_EKF2_MAGNETOMETER // this is the current status of the filter control modes filter_control_status_u _control_status{}; diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index 0b45b1bd78..62f63467b5 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -41,7 +41,10 @@ #include "ekf.h" -#include +#if defined(CONFIG_EKF2_MAGNETOMETER) +# include +#endif // CONFIG_EKF2_MAGNETOMETER + #include // GPS pre-flight check bit locations @@ -100,6 +103,8 @@ bool Ekf::collect_gps(const gpsMessage &gps) // 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; + +#if defined(CONFIG_EKF2_MAGNETOMETER) const double lon = gps.lon * 1.0e-7; // set the magnetic field data returned by the geo library using the current GPS position @@ -126,6 +131,7 @@ bool Ekf::collect_gps(const gpsMessage &gps) _wmm_gps_time_last_set = _time_delayed_us; } } +#endif // CONFIG_EKF2_MAGNETOMETER _earth_rate_NED = calcEarthRateNED((float)math::radians(lat)); }