diff --git a/libraries/AP_Quaternion/AP_Quaternion.cpp b/libraries/AP_Quaternion/AP_Quaternion.cpp index dc2080d516..d86c7268b3 100644 --- a/libraries/AP_Quaternion/AP_Quaternion.cpp +++ b/libraries/AP_Quaternion/AP_Quaternion.cpp @@ -116,7 +116,13 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel) // our quaternion is bad! Reset based on roll/pitch/yaw // and hope for the best ... renorm_blowup_count++; + if (_compass) { + _compass->null_offsets_disable(); + } quaternion_from_euler(q, roll, pitch, yaw); + if (_compass) { + _compass->null_offsets_enable(); + } return; } SEq_1 *= norm; @@ -213,10 +219,10 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V J_64 = twob_xSEq_2; // compute the gradient (matrix multiplication) - SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1 - J_41 * f_4 - J_51 * f_5 + J_61 * f_6; + SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1 - J_41 * f_4 - J_51 * f_5 + J_61 * f_6; SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3 + J_42 * f_4 + J_52 * f_5 + J_62 * f_6; SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1 - J_43 * f_4 + J_53 * f_5 + J_63 * f_6; - SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6; + SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6; // normalise the gradient to estimate direction of the gyroscope error norm = 1.0 / safe_sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4); @@ -273,7 +279,9 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V // our quaternion is bad! Reset based on roll/pitch/yaw // and hope for the best ... renorm_blowup_count++; + _compass->null_offsets_disable(); quaternion_from_euler(q, roll, pitch, yaw); + _compass->null_offsets_disable(); return; } SEq_1 *= norm; @@ -299,6 +307,151 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V } +// Function to compute one quaternion iteration including magnetometer +void AP_Quaternion::update_drift(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag) +{ + // local system variables + float norm; // vector norm + float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion rate from gyroscopes elements + float f_1, f_2, f_3, f_4, f_5, f_6; // objective function elements + float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33, // objective function Jacobian elements + J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54, J_61, J_62, J_63, J_64; // + float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error + + // computed flux in the earth frame + Vector3f flux; + + // estimated direction of the gyroscope error (radians) + Vector3f w_err; + + // normalise accelerometer vector + accel.normalize(); + if (accel.is_inf()) { + // discard this data point + renorm_range_count++; + return; + } + + // normalise the magnetometer measurement + mag.normalize(); + if (mag.is_inf()) { + // discard this data point + renorm_range_count++; + return; + } + + // auxiliary variables to avoid repeated calculations + float halfSEq_1 = 0.5f * SEq_1; + float halfSEq_2 = 0.5f * SEq_2; + float halfSEq_3 = 0.5f * SEq_3; + float halfSEq_4 = 0.5f * SEq_4; + float twoSEq_1 = 2.0f * SEq_1; + float twoSEq_2 = 2.0f * SEq_2; + float twoSEq_3 = 2.0f * SEq_3; + float twoSEq_4 = 2.0f * SEq_4; + float twob_x = 2.0f * b_x; + float twob_z = 2.0f * b_z; + float twob_xSEq_1 = 2.0f * b_x * SEq_1; + float twob_xSEq_2 = 2.0f * b_x * SEq_2; + float twob_xSEq_3 = 2.0f * b_x * SEq_3; + float twob_xSEq_4 = 2.0f * b_x * SEq_4; + float twob_zSEq_1 = 2.0f * b_z * SEq_1; + float twob_zSEq_2 = 2.0f * b_z * SEq_2; + float twob_zSEq_3 = 2.0f * b_z * SEq_3; + float twob_zSEq_4 = 2.0f * b_z * SEq_4; + float SEq_1SEq_2; + float SEq_1SEq_3 = SEq_1 * SEq_3; + float SEq_1SEq_4; + float SEq_2SEq_3; + float SEq_2SEq_4 = SEq_2 * SEq_4; + float SEq_3SEq_4; + Vector3f twom = mag * 2.0; + + // compute the objective function and Jacobian + f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - accel.x; + f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - accel.y; + f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - accel.z; + f_4 = twob_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 - SEq_1SEq_3) - mag.x; + f_5 = twob_x * (SEq_2 * SEq_3 - SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) - mag.y; + f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3) - mag.z; + J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication + J_12or23 = 2.0f * SEq_4; + J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication + J_14or21 = twoSEq_2; + J_32 = 2.0f * J_14or21; // negated in matrix multiplication + J_33 = 2.0f * J_11or24; // negated in matrix multiplication + J_41 = twob_zSEq_3; // negated in matrix multiplication + J_42 = twob_zSEq_4; + J_43 = 2.0f * twob_xSEq_3 + twob_zSEq_1; // negated in matrix multiplication + J_44 = 2.0f * twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication + J_51 = twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication + J_52 = twob_xSEq_3 + twob_zSEq_1; + J_53 = twob_xSEq_2 + twob_zSEq_4; + J_54 = twob_xSEq_1 - twob_zSEq_3; // negated in matrix multiplication + J_61 = twob_xSEq_3; + J_62 = twob_xSEq_4 - 2.0f * twob_zSEq_2; + J_63 = twob_xSEq_1 - 2.0f * twob_zSEq_3; + J_64 = twob_xSEq_2; + + // compute the gradient (matrix multiplication) + SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1 - J_41 * f_4 - J_51 * f_5 + J_61 * f_6; + SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3 + J_42 * f_4 + J_52 * f_5 + J_62 * f_6; + SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1 - J_43 * f_4 + J_53 * f_5 + J_63 * f_6; + SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6; + + // normalise the gradient to estimate direction of the gyroscope error + norm = 1.0 / safe_sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4); + if (isinf(norm)) { + // discard this data point + renorm_range_count++; + return; + } + SEqHatDot_1 *= norm; + SEqHatDot_2 *= norm; + SEqHatDot_3 *= norm; + SEqHatDot_4 *= norm; + + // compute angular estimated direction of the gyroscope error + w_err.x = twoSEq_1 * SEqHatDot_2 - twoSEq_2 * SEqHatDot_1 - twoSEq_3 * SEqHatDot_4 + twoSEq_4 * SEqHatDot_3; + w_err.y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_4 - twoSEq_3 * SEqHatDot_1 - twoSEq_4 * SEqHatDot_2; + w_err.z = twoSEq_1 * SEqHatDot_4 - twoSEq_2 * SEqHatDot_3 + twoSEq_3 * SEqHatDot_2 - twoSEq_4 * SEqHatDot_1; + + // keep track of the error rates + _error_rp_sum += 0.5*(fabs(w_err.x) + fabs(w_err.y)); + _error_yaw_sum += fabs(w_err.z); + _error_rp_count++; + _error_yaw_count++; + + // compute the gyroscope bias delta + Vector3f drift_delta = w_err * (deltat * zeta); + + // don't allow the drift rate to be exceeded. This prevents a + // sudden drift change coming from a outage in the compass + float max_change = gyroMeasDrift * deltat; + drift_delta.x = constrain(drift_delta.x, -max_change, max_change); + drift_delta.y = constrain(drift_delta.y, -max_change, max_change); + drift_delta.z = constrain(drift_delta.z, -max_change, max_change); + gyro_bias += drift_delta; + + // compute flux in the earth frame + // recompute axulirary variables + SEq_1SEq_2 = SEq_1 * SEq_2; + SEq_1SEq_3 = SEq_1 * SEq_3; + SEq_1SEq_4 = SEq_1 * SEq_4; + SEq_3SEq_4 = SEq_3 * SEq_4; + SEq_2SEq_3 = SEq_2 * SEq_3; + SEq_2SEq_4 = SEq_2 * SEq_4; + flux.x = twom.x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twom.y * (SEq_2SEq_3 - SEq_1SEq_4) + twom.z * (SEq_2SEq_4 + SEq_1SEq_3); + flux.y = twom.x * (SEq_2SEq_3 + SEq_1SEq_4) + twom.y * (0.5f - SEq_2 * SEq_2 - SEq_4 * SEq_4) + twom.z * (SEq_3SEq_4 - SEq_1SEq_2); + flux.z = twom.x * (SEq_2SEq_4 - SEq_1SEq_3) + twom.y * (SEq_3SEq_4 + SEq_1SEq_2) + twom.z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3); + + // normalise the flux vector to have only components in the x and z + b_x = sqrt((flux.x * flux.x) + (flux.y * flux.y)); + b_z = flux.z; +} + + + // Function to compute one quaternion iteration void AP_Quaternion::update(void) { @@ -321,11 +474,15 @@ void AP_Quaternion::update(void) return; } - if (!_have_initial_yaw && _compass && _compass->use_for_yaw()) { + if (!_have_initial_yaw && _compass && + _compass->use_for_yaw()) { // setup the quaternion with initial compass yaw + _compass->null_offsets_disable(); quaternion_from_euler(q, 0, 0, _compass->heading); _have_initial_yaw = true; + _compass_last_update = _compass->last_update; gyro_bias.zero(); + _compass->null_offsets_enable(); } // get current IMU state @@ -356,14 +513,43 @@ void AP_Quaternion::update(void) accel.z += (gyro.y - gyro_bias.y) * veloc; } - if (_compass == NULL || !_compass->use_for_yaw()) { - update_IMU(deltat, gyro, accel); - } else { - // mag.z is reversed in quaternion code +#if 1 + _gyro_sum += gyro; + _accel_sum += accel; + _sum_count++; + + if (_compass != NULL && _compass->use_for_yaw() && + _compass->last_update != _compass_last_update && + _sum_count != 0) { + // use new compass sample for drift correction + float mag_deltat = 1.0e-6*(_compass->last_update - _compass_last_update); Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z); - update_MARG(deltat, gyro, accel, mag); + Vector3f drift_accel = _accel_sum / _sum_count; + Vector3f drift_gyro = _gyro_sum / _sum_count; + _accel_sum.zero(); + _gyro_sum.zero(); + _sum_count = 0; + update_drift(mag_deltat, drift_gyro, drift_accel, mag); + _compass_last_update = _compass->last_update; } + // step the quaternion solution + gyro -= gyro_bias; + update_IMU(deltat, gyro, accel); +#else + if (_compass != NULL && _compass->use_for_yaw()) { + // use new compass sample for drift correction + Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z); + update_MARG(deltat, gyro, accel, mag); + } else { + // step the quaternion without drift correction + gyro -= gyro_bias; + _gyro_sum += gyro; + _accel_sum += accel; + _sum_count++; + update_IMU(deltat, gyro, accel); + } +#endif #ifdef DESKTOP_BUILD if (q.is_nan()) { diff --git a/libraries/AP_Quaternion/AP_Quaternion.h b/libraries/AP_Quaternion/AP_Quaternion.h index 2819cf15be..4f069c9510 100644 --- a/libraries/AP_Quaternion/AP_Quaternion.h +++ b/libraries/AP_Quaternion/AP_Quaternion.h @@ -17,10 +17,9 @@ class AP_Quaternion { public: // Constructor - AP_Quaternion(IMU *imu, GPS *&gps, Compass *compass = NULL) : + AP_Quaternion(IMU *imu, GPS *&gps) : _imu(imu), - _gps(gps), - _compass(compass) + _gps(gps) { // reference direction of flux in earth frame b_x = 0; @@ -70,10 +69,16 @@ public: uint8_t renorm_blowup_count; float get_error_rp(void); float get_error_yaw(void); + Matrix3f get_dcm_matrix(void) { + Matrix3f ret; + quaternion_to_rotation_matrix(q, ret); + return ret; + } private: void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel); void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag); + void update_drift(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag); bool _have_initial_yaw; @@ -82,6 +87,8 @@ private: // members Compass * _compass; + // time in microseconds of last compass update + uint32_t _compass_last_update; // note: we use ref-to-pointer here so that our caller can change the GPS without our noticing // IMU under us without our noticing. @@ -91,12 +98,12 @@ private: // true if we are doing centripetal acceleration correction bool _centripetal; - // maximum gyroscope measurement error in rad/s (set to 20 degrees/second) - static const float gyroMeasError = 20.0 * (M_PI/180.0); + // maximum gyroscope measurement error in rad/s (set to 40 degrees/second) + static const float gyroMeasError = 10.0 * (M_PI/180.0); - // maximum gyroscope drift rate in radians/s/s (set to 0.02 - // degrees/s/s, which is 1.2 degrees/s/minute) - static const float gyroMeasDrift = 0.02 * (PI/180.0); + // maximum gyroscope drift rate in radians/s/s (set to 0.005 + // degrees/s/s, which is 0.3 degrees/s/minute) + static const float gyroMeasDrift = 0.005 * (PI/180.0); float beta; float zeta; @@ -115,6 +122,11 @@ private: // the current corrected gyro vector Vector3f _gyro_corrected; + // accel and gyro accumulators for drift correction + Vector3f _gyro_sum; + Vector3f _accel_sum; + uint32_t _sum_count; + // estimate of error float _error_rp_sum; uint16_t _error_rp_count;