From 0df8443f2f8eb744bd71da054b4443080a00adab Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 21 Mar 2022 16:35:06 -0400 Subject: [PATCH] ekf2: simplify mag auto (3d select) --- src/modules/ekf2/EKF/ekf.cpp | 20 +----- src/modules/ekf2/EKF/ekf.h | 3 - src/modules/ekf2/EKF/estimator_interface.h | 3 +- src/modules/ekf2/EKF/mag_control.cpp | 80 +++++++++++++--------- 4 files changed, 52 insertions(+), 54 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 17d67084ae..e2262b4846 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -276,11 +276,6 @@ void Ekf::predictState() const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias; const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel; - // calculate a filtered horizontal acceleration with a 1 sec time constant - // this are used for manoeuvre detection elsewhere - const float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt; - _accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy(); - // save the previous value of velocity so we can use trapzoidal integration const Vector3f vel_last = _state.vel; @@ -331,15 +326,6 @@ void Ekf::calculateOutputStates(const imuSample &imu) // Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon const Vector3f delta_angle(imu.delta_ang - _state.delta_ang_bias * dt_scale_correction + _delta_angle_corr); - // calculate a yaw change about the earth frame vertical - const float spin_del_ang_D = delta_angle.dot(Vector3f(_R_to_earth_now.row(2))); - _yaw_delta_ef += spin_del_ang_D; - - // Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic - // Note fixed coefficients are used to save operations. The exact time constant is not important. - _yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu.delta_ang_dt; - - _output_new.time_us = imu.time_us; _output_vert_new.time_us = imu.time_us; @@ -352,13 +338,13 @@ void Ekf::calculateOutputStates(const imuSample &imu) _output_new.quat_nominal.normalize(); // calculate the rotation matrix from body to earth frame - _R_to_earth_now = Dcmf(_output_new.quat_nominal); + const Dcmf R_to_earth_now{_output_new.quat_nominal}; // correct delta velocity for bias offsets const Vector3f delta_vel_body{imu.delta_vel - _state.delta_vel_bias * dt_scale_correction}; // rotate the delta velocity to earth frame - Vector3f delta_vel_earth{_R_to_earth_now * delta_vel_body}; + Vector3f delta_vel_earth{R_to_earth_now * delta_vel_body}; // correct for measured acceleration due to gravity delta_vel_earth(2) += CONSTANTS_ONE_G * imu.delta_vel_dt; @@ -394,7 +380,7 @@ void Ekf::calculateOutputStates(const imuSample &imu) const Vector3f vel_imu_rel_body = ang_rate % _params.imu_pos_body; // rotate the relative velocity into earth frame - _vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body; + _vel_imu_rel_body_ned = R_to_earth_now * vel_imu_rel_body; } // store the INS states in a ring buffer with the same length and time coordinates as the IMU data buffer diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 7a01b7b303..c7773d02a5 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -416,7 +416,6 @@ private: // used by magnetometer fusion mode selection Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) - float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad) float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec) 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 @@ -851,8 +850,6 @@ private: void runYawReset(); - void selectMagAuto(); - void check3DMagFusionSuitability(); void checkYawAngleObservability(); void checkMagBiasObservability(); bool canUse3DMagFusion() const; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index c22065be75..a44893f9aa 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -195,7 +195,7 @@ public: Vector3f getPosition() const { // rotate the position of the IMU relative to the boy origin into earth frame - const Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body; + const Vector3f pos_offset_earth = Dcmf(_output_new.quat_nominal) * _params.imu_pos_body; // subtract from the EKF position (which is at the IMU) to get position at the body origin return _output_new.pos - pos_offset_earth; } @@ -308,7 +308,6 @@ protected: outputSample _output_new{}; // filter output on the non-delayed time horizon outputVert _output_vert_new{}; // vertical filter output on the non-delayed time horizon imuSample _newest_high_rate_imu_sample{}; // imu sample capturing the newest imu data - Matrix3f _R_to_earth_now{}; // rotation matrix from body to earth frame at current time Vector3f _vel_imu_rel_body_ned{}; // velocity of IMU relative to body origin in NED earth frame Vector3f _vel_deriv{}; // velocity derivative at the IMU in NED earth frame (m/s/s) diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 12a0581e56..b3667c57c9 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -76,6 +76,13 @@ void Ekf::controlMagFusion() _num_bad_flight_yaw_events = 0; } + checkYawAngleObservability(); + checkMagBiasObservability(); + + if (_mag_bias_observable || _yaw_angle_observable) { + _time_last_mov_3d_mag_suitable = _imu_sample_delayed.time_us; + } + // When operating without a magnetometer and no other source of yaw aiding is active, // yaw fusion is run selectively to enable yaw gyro bias learning when stationary on // ground and to prevent uncontrolled yaw variance growth @@ -109,7 +116,17 @@ void Ekf::controlMagFusion() // FALLTHROUGH case MAG_FUSE_TYPE_AUTO: - selectMagAuto(); + // 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 + if (_control_status.flags.mag_aligned_in_flight + && ((_imu_sample_delayed.time_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6) + ) { + startMag3DFusion(); + + } else { + startMagHdgFusion(); + } + break; case MAG_FUSE_TYPE_INDOOR: @@ -206,36 +223,44 @@ void Ekf::runYawReset() } } -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 = _imu_sample_delayed.time_us; - } -} - void Ekf::checkYawAngleObservability() { + // calculate a filtered horizontal acceleration with a 1 sec time constant + + // Calculate an earth frame delta velocity + const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias; + const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel; + + const float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt; + _accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy(); + // Check if there has been enough change in horizontal velocity to make yaw observable // Apply hysteresis to check to avoid rapid toggling - _yaw_angle_observable = _yaw_angle_observable - ? _accel_lpf_NE.norm() > _params.mag_acc_gate - : _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate; + if (_control_status.flags.gps) { + if (_yaw_angle_observable) { + _yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate; - _yaw_angle_observable = _yaw_angle_observable - && (_control_status.flags.gps || _control_status.flags.ev_pos); // Do we have to add ev_vel here? + } else { + _yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate * 2.f; + } + + } else { + _yaw_angle_observable = false; + } } void Ekf::checkMagBiasObservability() { + // calculate a yaw change about the earth frame vertical + const Vector3f delta_angle = _imu_sample_delayed.delta_ang - _state.delta_ang_bias; + + const float spin_del_ang_D = delta_angle.dot(Vector3f(_R_to_earth.row(2))); + float yaw_delta_ef = spin_del_ang_D; + + // Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic + // Note fixed coefficients are used to save operations. The exact time constant is not important. + _yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / _imu_sample_delayed.delta_ang_dt; + // check if there is enough yaw rotation to make the mag bias states observable if (!_mag_bias_observable && (fabsf(_yaw_rate_lpf_ef) > _params.mag_yaw_rate_gate)) { // initial yaw motion is detected @@ -245,21 +270,12 @@ void Ekf::checkMagBiasObservability() // require sustained yaw motion of 50% the initial yaw rate threshold const float yaw_dt = 1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started); const float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * yaw_dt; - _mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req; + _mag_bias_observable = fabsf(yaw_delta_ef) > min_yaw_change_req; } - _yaw_delta_ef = 0.0f; _time_yaw_started = _imu_sample_delayed.time_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 - && ((_imu_sample_delayed.time_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,