diff --git a/EKF/control.cpp b/EKF/control.cpp index f386e1a04c..5185239f77 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -483,16 +483,14 @@ void Ekf::controlGpsFusion() do_reset = do_reset || (_time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max); if (do_reset) { - // Reset states to the last GPS measurement - if (_control_status.flags.fixed_wing) { - // if flying a fixed wing aircraft, do a complete reset that includes yaw, velocity and position + // use GPS velocity data to check and correct yaw angle if a FW vehicle + if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { + // if flying a fixed wing aircraft, do a complete reset that includes yaw realignYawGPS(); - resetVelocity(); - resetPosition(); - } else { - resetVelocity(); - resetPosition(); } + resetVelocity(); + resetPosition(); + _velpos_reset_request = false; ECL_WARN("EKF GPS fusion timeout - reset to GPS"); // Reset the timeout counters @@ -1200,12 +1198,16 @@ void Ekf::controlMagFusion() if (!_flt_mag_align_complete) { // If we are flying a vehicle that flies forward, eg plane, then we can use the GPS course to check and correct the heading if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { - _control_status.flags.yaw_align = realignYawGPS(); - _flt_mag_align_complete = _control_status.flags.yaw_align; + _flt_mag_align_complete = realignYawGPS(); + if (_velpos_reset_request) { + resetVelocity(); + resetPosition(); + _velpos_reset_request = false; + } } else { - _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); - _flt_mag_align_complete = _control_status.flags.yaw_align; + _flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag); } + _control_status.flags.yaw_align = _control_status.flags.yaw_align || _flt_mag_align_complete; } else { // reset the mag field covariances zeroRows(P, 16, 21); @@ -1242,6 +1244,10 @@ void Ekf::controlMagFusion() _control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) && _control_status.flags.fixed_wing; + // For the first 5 seconds after switching to 3-axis fusion we allow the magnetic field state estimates to stabilise + // before they are used to constrain heading drift + _flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)5e6); + if (!_control_status.flags.update_mag_states_only && _control_status_prev.flags.update_mag_states_only) { // When re-commencing use of magnetometer to correct vehicle states // set the field state variance to the observation variance and zero @@ -1261,7 +1267,7 @@ void Ekf::controlMagFusion() } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) { // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states if (!_control_status.flags.mag_3D) { - _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag) || _control_status.flags.yaw_align; } // always use 3-axis mag fusion diff --git a/EKF/ekf.h b/EKF/ekf.h index 2309748ff4..272f85e29e 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -377,9 +377,12 @@ private: // Variables used to control activation of post takeoff functionality float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) - bool _flt_mag_align_complete{true}; ///< true when the in-flight mag field alignment has been completed + bool _flt_mag_align_complete{false}; ///< true when the in-flight mag field alignment has been completed + bool _flt_mag_align_converging{false}; ///< true when the in-flight mag field post alignment convergence is being performd + uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec) uint64_t _time_last_movement{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec) float _saved_mag_variance[6] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2) + bool _velpos_reset_request{false}; ///< true when a large yaw error has been fixed and a velocity and position state reset is required gps_check_fail_status_u _gps_check_fail_status{}; @@ -593,6 +596,13 @@ private: // zero the specified range of columns in the state covariance matrix void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); + // zero the specified range of off diagonals in the state covariance matrix + void zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); + + // zero the specified range of off diagonals in the state covariance matrix + // set the diagonals to the supplied value + void setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last, float variance); + // calculate the measurement variance for the optical flow sensor float calcOptFlowMeasVar(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 65ddc59ade..6a928c9f11 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -56,8 +56,12 @@ bool Ekf::resetVelocity() // this reset is only called if we have new gps data at the fusion time horizon _state.vel = _gps_sample_delayed.vel; + // use GPS accuracy to reset variances + setDiag(P,4,6,sq(_gps_sample_delayed.sacc)); + } else if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) { _state.vel.setZero(); + zeroOffDiag(P,4,6); } else { return false; @@ -106,15 +110,22 @@ bool Ekf::resetPosition() _state.pos(0) = _gps_sample_delayed.pos(0); _state.pos(1) = _gps_sample_delayed.pos(1); + // use GPS accuracy to reset variances + setDiag(P,7,8,sq(_gps_sample_delayed.hacc)); + } else if (_control_status.flags.opt_flow) { _state.pos(0) = 0.0f; _state.pos(1) = 0.0f; + zeroOffDiag(P,7,8); } else if (_control_status.flags.ev_pos) { // this reset is only called if we have new ev data at the fusion time horizon _state.pos(0) = _ev_sample_delayed.posNED(0); _state.pos(1) = _ev_sample_delayed.posNED(1); + // use EV accuracy to reset variances + setDiag(P,7,8,sq(_ev_sample_delayed.posErr)); + } else { return false; } @@ -411,6 +422,9 @@ bool Ekf::realignYawGPS() // calculate new filter quaternion states using corected yaw angle _state.quat_nominal = Quatf(euler321); + // If heading was bad, then we alos need to reset the velocity and position states + _velpos_reset_request = badMagYaw; + // update transformation matrix from body to world frame using the current state estimate _R_to_earth = quat_to_invrotmat(_state.quat_nominal); @@ -434,6 +448,8 @@ bool Ekf::realignYawGPS() P[index][index] = sq(_params.mag_noise); } + // record the start time for the magnetic field alignment + _flt_mag_align_start_time = _imu_sample_delayed.time_us; // calculate the amount that the quaternion has changed by _state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal; @@ -453,14 +469,26 @@ bool Ekf::realignYawGPS() // capture the reset event _state_reset_status.quat_counter++; - // the alignment using GPS has been successful - _control_status.flags.yaw_align = true; return true; } else { - // attempt a normal alignment using the magnetometer - return resetMagHeading(_mag_sample_delayed.mag); + // align mag states only + // calculate initial earth magnetic field states + _state.mag_I = _R_to_earth * _mag_sample_delayed.mag; + + // reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance + zeroRows(P, 16, 21); + zeroCols(P, 16, 21); + + for (uint8_t index = 16; index <= 21; index ++) { + P[index][index] = sq(_params.mag_noise); + } + + // record the start time for the magnetic field alignment + _flt_mag_align_start_time = _imu_sample_delayed.time_us; + + return true; } } else { @@ -475,6 +503,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) { // save a copy of the quaternion state for later use in calculating the amount of reset change Quatf quat_before_reset = _state.quat_nominal; + Quatf quat_after_reset = _state.quat_nominal; // calculate the variance for the rotation estimate expressed as a rotation vector // this will be used later to reset the quaternion state covariances @@ -515,26 +544,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // calculate initial quaternion states for the ekf // we don't change the output attitude to avoid jumps - _state.quat_nominal = Quatf(euler321); - - // calculate the amount that the quaternion has changed by - _state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal; - - // add the reset amount to the output observer buffered data - outputSample output_states; - unsigned output_length = _output_buffer.get_length(); - for (unsigned i = 0; i < output_length; i++) { - output_states = _output_buffer.get_from_index(i); - output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal; - _output_buffer.push_to_index(i, output_states); - } - - // 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++; + quat_after_reset = Quatf(euler321); } else { // use a 312 sequence @@ -601,32 +611,12 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // calculate initial quaternion states for the ekf // we don't change the output attitude to avoid jumps - _state.quat_nominal = Quatf(R_to_earth); + quat_after_reset = Quatf(R_to_earth); } - // 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) && !(_params.fusion_mode & MASK_USE_EVYAW)) { - resetExtVisRotMat(); - } - - // update the yaw angle variance using the variance of the measurement - if (_params.fusion_mode & MASK_USE_EVYAW) { - // 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); - - // calculate initial earth magnetic field states - _state.mag_I = _R_to_earth * mag_init; + // set the earth magnetic field states using the updated rotation + Dcmf _R_to_earth_after = quat_to_invrotmat(quat_after_reset); + _state.mag_I = _R_to_earth_after * mag_init; // reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance zeroRows(P, 16, 21); @@ -636,25 +626,76 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) P[index][index] = sq(_params.mag_noise); } + // record the time for the magnetic field alignment event + _flt_mag_align_start_time = _imu_sample_delayed.time_us; + // calculate the amount that the quaternion has changed by - _state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal; + Quatf q_error = quat_before_reset.inversed() * quat_after_reset; + q_error.normalize(); - // add the reset amount to the output observer buffered data - outputSample output_states = {}; - unsigned output_length = _output_buffer.get_length(); + // convert the quaternion delta to a delta angle + Vector3f delta_ang_error; + float scalar; - for (unsigned i = 0; i < output_length; i++) { - output_states = _output_buffer.get_from_index(i); - output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal; - _output_buffer.push_to_index(i, output_states); + if (q_error(0) >= 0.0f) { + scalar = -2.0f; + + } else { + scalar = 2.0f; } - // 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; + delta_ang_error(0) = scalar * q_error(1); + delta_ang_error(1) = scalar * q_error(2); + delta_ang_error(2) = scalar * q_error(3); - // capture the reset event - _state_reset_status.quat_counter++; + + // update the quaternion state estimates and corresponding covariances only if the change in angle has been large + 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) && !(_params.fusion_mode & MASK_USE_EVYAW)) { + resetExtVisRotMat(); + } + + // update the yaw angle variance using the variance of the measurement + if (_params.fusion_mode & MASK_USE_EVYAW) { + // 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 + outputSample output_states = {}; + unsigned output_length = _output_buffer.get_length(); + + for (unsigned i = 0; i < output_length; i++) { + output_states = _output_buffer.get_from_index(i); + output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal; + _output_buffer.push_to_index(i, output_states); + } + + // 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; } @@ -1177,6 +1218,39 @@ void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first } } +void Ekf::zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last) +{ + // save diagonal elements + uint8_t row; + float variances[_k_num_states]; + for (row = first; row <= last; row++) { + variances[row] = cov_mat[row][row]; + } + + // zero rows and columns + zeroRows(cov_mat, first, last); + zeroCols(cov_mat, first, last); + + // restore diagonals + for (row = first; row <= last; row++) { + cov_mat[row][row] = variances[row]; + } +} + +void Ekf::setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last, float variance) +{ + // zero rows and columns + zeroRows(cov_mat, first, last); + zeroCols(cov_mat, first, last); + + // set diagonals + uint8_t row; + for (row = first; row <= last; row++) { + cov_mat[row][row] = variance; + } + +} + bool Ekf::global_position_is_valid() { // return true if we are not doing unconstrained free inertial navigation and the origin is set diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 2294d1a5b0..632bda0c67 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -163,6 +163,8 @@ void Ekf::fuseMag() return; } + bool update_all_states = !_control_status.flags.update_mag_states_only && !_flt_mag_align_converging; + // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) { @@ -187,7 +189,7 @@ void Ekf::fuseMag() SK_MX[3] = 2.0f*q0*q2 - 2.0f*q1*q3; SK_MX[4] = 2.0f*q0*q3 + 2.0f*q1*q2; - if (!_control_status.flags.update_mag_states_only) { + if (update_all_states) { Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] - P[0][2]*SH_MAG[1] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[4] - P[0][18]*SK_MX[3]); Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] - P[1][2]*SH_MAG[1] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[4] - P[1][18]*SK_MX[3]); Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[4] - P[2][18]*SK_MX[3]); @@ -241,7 +243,7 @@ void Ekf::fuseMag() SK_MY[3] = 2.0f*q0*q3 - 2.0f*q1*q2; SK_MY[4] = 2.0f*q0*q1 + 2.0f*q2*q3; - if (!_control_status.flags.update_mag_states_only) { + if (update_all_states) { Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]); Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]); Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]); @@ -295,7 +297,7 @@ void Ekf::fuseMag() SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3; SK_MZ[4] = 2.0f*q0*q2 + 2.0f*q1*q3; - if (!_control_status.flags.update_mag_states_only) { + if (update_all_states) { Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] - P[0][1]*SH_MAG[2] + P[0][3]*SH_MAG[0] + P[0][2]*SK_MZ[2] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[4] - P[0][17]*SK_MZ[3]); Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[1][3]*SH_MAG[0] + P[1][2]*SK_MZ[2] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[4] - P[1][17]*SK_MZ[3]); Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] - P[2][1]*SH_MAG[2] + P[2][3]*SH_MAG[0] + P[2][2]*SK_MZ[2] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[4] - P[2][17]*SK_MZ[3]); @@ -467,7 +469,12 @@ void Ekf::fuseHeading() Dcmf R_to_earth(euler321); // rotate the magnetometer measurements into earth frame using a zero yaw angle - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + if (_control_status.flags.mag_3D) { + // don't apply bias corrections if we are learning them + mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + } else { + mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); + } // the angle of the projection onto the horizontal gives the yaw angle measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; @@ -554,7 +561,12 @@ void Ekf::fuseHeading() R_to_earth(2,2) = cp*cr; // rotate the magnetometer measurements into earth frame using a zero yaw angle - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + if (_control_status.flags.mag_3D) { + // don't apply bias corrections if we are learning them + mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + } else { + mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); + } // the angle of the projection onto the horizontal gives the yaw angle measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;