From 89a10c584d091ec60a95294cacf9d0dcfb9740e0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Mar 2012 15:07:04 +1100 Subject: [PATCH] Quaternion: added more numerical safety in the quaternion code prevent infinities from creeping in and prevent large drift changes --- libraries/AP_Quaternion/AP_Quaternion.cpp | 94 ++++++++++++++++++----- libraries/AP_Quaternion/AP_Quaternion.h | 14 ++-- 2 files changed, 81 insertions(+), 27 deletions(-) diff --git a/libraries/AP_Quaternion/AP_Quaternion.cpp b/libraries/AP_Quaternion/AP_Quaternion.cpp index 6c9455714e..4a20d4c71a 100644 --- a/libraries/AP_Quaternion/AP_Quaternion.cpp +++ b/libraries/AP_Quaternion/AP_Quaternion.cpp @@ -31,6 +31,13 @@ AP_Quaternion::set_compass(Compass *compass) _compass = compass; } +// to keep the code as close to the original as possible, we use these +// macros for quaternion access +#define SEq_1 q.q1 +#define SEq_2 q.q2 +#define SEq_3 q.q3 +#define SEq_4 q.q4 + // Function to compute one quaternion iteration without magnetometer void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel) { @@ -55,6 +62,11 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel) // normalise accelerometer vector accel.normalize(); + if (accel.is_inf()) { + // discard this data point + renorm_range_count++; + return; + } // Compute the objective function and Jacobian f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - accel.x; @@ -75,12 +87,16 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel) // Normalise the gradient norm = 1.0/sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4); - if (!isinf(norm)) { - SEqHatDot_1 *= norm; - SEqHatDot_2 *= norm; - SEqHatDot_3 *= norm; - SEqHatDot_4 *= norm; + if (isinf(norm)) { + // we can't do an update - discard this data point and + // hope the next one is better + renorm_range_count++; + return; } + SEqHatDot_1 *= norm; + SEqHatDot_2 *= norm; + SEqHatDot_3 *= norm; + SEqHatDot_4 *= norm; // Compute the quaternion derrivative measured by gyroscopes SEqDot_omega_1 = -halfSEq_2 * gyro.x - halfSEq_3 * gyro.y - halfSEq_4 * gyro.z; @@ -96,12 +112,17 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel) // Normalise quaternion norm = 1.0/sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); - if (!isinf(norm)) { - SEq_1 *= norm; - SEq_2 *= norm; - SEq_3 *= norm; - SEq_4 *= norm; + if (isinf(norm)) { + // our quaternion is bad! Reset based on roll/pitch/yaw + // and hope for the best ... + renorm_blowup_count++; + quaternion_from_euler(q, roll, pitch, yaw); + return; } + SEq_1 *= norm; + SEq_2 *= norm; + SEq_3 *= norm; + SEq_4 *= norm; } @@ -124,9 +145,19 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V // 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; @@ -189,6 +220,11 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V // normalise the gradient to estimate direction of the gyroscope error norm = 1.0 / 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; @@ -205,8 +241,18 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V _error_rp_count++; _error_yaw_count++; - // compute and remove the gyroscope baises - gyro_bias += w_err * (deltat * zeta); + // 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; + + // correct the gyro reading for drift gyro -= gyro_bias; // compute the quaternion rate measured by gyroscopes @@ -223,12 +269,17 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V // normalise quaternion norm = 1.0/sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); - if (!isinf(norm)) { - SEq_1 *= norm; - SEq_2 *= norm; - SEq_3 *= norm; - SEq_4 *= norm; + if (isinf(norm)) { + // our quaternion is bad! Reset based on roll/pitch/yaw + // and hope for the best ... + renorm_blowup_count++; + quaternion_from_euler(q, roll, pitch, yaw); + return; } + SEq_1 *= norm; + SEq_2 *= norm; + SEq_3 *= norm; + SEq_4 *= norm; // compute flux in the earth frame // recompute axulirary variables @@ -265,6 +316,13 @@ void AP_Quaternion::update(void) return; } + if (!_have_initial_yaw && _compass && _compass->use_for_yaw()) { + // setup the quaternion with initial compass yaw + quaternion_from_euler(q, 0, 0, _compass->heading); + _have_initial_yaw = true; + gyro_bias.zero(); + } + // get current IMU state gyro = _imu->get_gyro(); accel = _imu->get_accel(); @@ -293,7 +351,7 @@ void AP_Quaternion::update(void) accel.z += (gyro.y - gyro_bias.y) * veloc; } - if (_compass == NULL) { + if (_compass == NULL || !_compass->use_for_yaw()) { update_IMU(deltat, gyro, accel); } else { // mag.z is reversed in quaternion code diff --git a/libraries/AP_Quaternion/AP_Quaternion.h b/libraries/AP_Quaternion/AP_Quaternion.h index 543c37f579..2819cf15be 100644 --- a/libraries/AP_Quaternion/AP_Quaternion.h +++ b/libraries/AP_Quaternion/AP_Quaternion.h @@ -22,12 +22,6 @@ public: _gps(gps), _compass(compass) { - // initial quaternion - SEq_1 = 1; - SEq_2 = 0; - SEq_3 = 0; - SEq_4 = 0; - // reference direction of flux in earth frame b_x = 0; b_z = -1; @@ -97,8 +91,8 @@ private: // true if we are doing centripetal acceleration correction bool _centripetal; - // maximum gyroscope measurement error in rad/s (set to 10 degrees/second) - static const float gyroMeasError = 10.0 * (M_PI/180.0); + // maximum gyroscope measurement error in rad/s (set to 20 degrees/second) + static const float gyroMeasError = 20.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) @@ -108,8 +102,10 @@ private: float zeta; // quaternion elements - float SEq_1, SEq_2, SEq_3, SEq_4; + Quaternion q; + // magnetic flux estimates. These are used for the automatic + // magnetometer calibration float b_x; float b_z;