diff --git a/EKF/control.cpp b/EKF/control.cpp index 69754f2800..ad51c36f7f 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1356,6 +1356,13 @@ void Ekf::controlMagFusion() // check for new magnetometer data that has fallen behind the fusion time horizon // If we are using external vision data for heading then no magnetometer fusion is used if (!_control_status.flags.ev_yaw && _mag_data_ready) { + // perform a yaw reset if requested by other functions + if (_mag_yaw_reset_req) { + if (!_mag_use_inhibit ) { + resetMagHeading(_mag_sample_delayed.mag); + } + _mag_yaw_reset_req = false; + } // 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 diff --git a/EKF/ekf.h b/EKF/ekf.h index e9c9fdf835..06c1693c75 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -339,6 +339,7 @@ private: bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetomer inhibit has been active for long enough to require a yaw reset when conditons improve. float _last_static_yaw{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad) bool _vehicle_at_rest_prev{false}; ///< true when the vehicle was at rest the previous time the status was checked + bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetomer data has been requested float P[_k_num_states][_k_num_states] {}; ///< state covariance matrix diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 98636196c6..1abb02eb7f 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -545,6 +545,11 @@ bool Ekf::realignYawGPS() // Reset heading and magnetic field states bool Ekf::resetMagHeading(Vector3f &mag_init) { + // don't reseet twice on the same frame + if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) { + return true; + } + if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) { // do not use the magnetomer and deactivate magnetic field states zeroRows(P, 16, 21); @@ -701,51 +706,50 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) delta_ang_error(1) = scalar * q_error(2); delta_ang_error(2) = scalar * q_error(3); + // update quaternion states + _state.quat_nominal = quat_after_reset; - // update the quaternion state estimates and corresponding covariances only if the change in angle has been large + // record the state change + _state_reset_status.quat_change = q_error; + + // update transformation matrix from body to world frame using the current estimate + _R_to_earth = quat_to_invrotmat(_state.quat_nominal); + + // reset the rotation from the EV to EKF frame of reference if it is being used + if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_yaw) { + resetExtVisRotMat(); + } + + // update the yaw angle variance using the variance of the measurement + if (!_control_status.flags.ev_yaw) { + // using error estimate from external vision data + angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)); + + } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { + // using magnetic heading tuning parameter + angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); + } + + // update the covariances only if the change in angle has been large to avoid unnecessary + // manipulation of the covariance matrix that can temporarily reduce filter performance if (delta_ang_error.norm() > math::radians(15.0f)) { - // update quaternion states - _state.quat_nominal = quat_after_reset; - - // record the state change - _state_reset_status.quat_change = q_error; - - // update transformation matrix from body to world frame using the current estimate - _R_to_earth = quat_to_invrotmat(_state.quat_nominal); - - // reset the rotation from the EV to EKF frame of reference if it is being used - if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_yaw) { - resetExtVisRotMat(); - } - - // update the yaw angle variance using the variance of the measurement - if (!_control_status.flags.ev_yaw) { - // using error estimate from external vision data - angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)); - - } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { - // using magnetic heading tuning parameter - angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); - } - // reset the quaternion covariances using the rotation vector variances initialiseQuatCovariances(angle_err_var_vec); - - // add the reset amount to the output observer buffered data - for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { - // Note q1 *= q2 is equivalent to q1 = q2 * q1 - _output_buffer[i].quat_nominal *= _state_reset_status.quat_change; - } - - // apply the change in attitude quaternion to our newest quaternion estimate - // which was already taken out from the output buffer - _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; - - // capture the reset event - _state_reset_status.quat_counter++; - } + // add the reset amount to the output observer buffered data + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + // Note q1 *= q2 is equivalent to q1 = q2 * q1 + _output_buffer[i].quat_nominal *= _state_reset_status.quat_change; + } + + // apply the change in attitude quaternion to our newest quaternion estimate + // which was already taken out from the output buffer + _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; + + // capture the reset event + _state_reset_status.quat_counter++; + return true; } diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 871b4d1efa..b46096392f 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -79,6 +79,8 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) _last_gps_origin_time_us = _time_last_imu; // set the magnetic declination returned by the geo library using the current GPS position _mag_declination_gps = math::radians(get_mag_declination(lat, lon)); + // request a reset of the yaw using the new declination + _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;