FusionAhrs.c

00001 /**
00002  * @file FusionAhrs.c
00003  * @author Seb Madgwick
00004  * @brief The AHRS sensor fusion algorithm to combines gyroscope, accelerometer,
00005  * and magnetometer measurements into a single measurement of orientation
00006  * relative to the Earth (NWU convention).
00007  *
00008  * The algorithm behaviour is governed by a gain.  A low gain will decrease the
00009  * influence of the accelerometer and magnetometer so that the algorithm will
00010  * better reject disturbances causes by translational motion and temporary
00011  * magnetic distortions.  However, a low gain will also increase the risk of
00012  * drift due to gyroscope calibration errors.  A typical gain value suitable for
00013  * most applications is 0.5.
00014  *
00015  * The algorithm allows the application to define a minimum and maximum valid
00016  * magnetic field magnitude.  The algorithm will ignore magnetic measurements
00017  * that fall outside of this range.  This allows the algorithm to reject
00018  * magnetic measurements that do not represent the direction of magnetic North.
00019  * The typical magnitude of the Earth's magnetic field is between 20 uT and
00020  * 70 uT.
00021  *
00022  * The algorithm can be used without a magnetometer.  Measurements of
00023  * orientation obtained using only gyroscope and accelerometer measurements
00024  * can be expected to drift in the yaw component of orientation only.  The
00025  * application can reset the drift in yaw by setting the yaw to a specified
00026  * angle at any time.
00027  *
00028  * The algorithm provides the measurement of orientation as a quaternion.  The
00029  * library includes functions for converting this quaternion to a rotation
00030  * matrix and Euler angles.
00031  *
00032  * The algorithm also provides a measurement of linear acceleration and Earth
00033  * acceleration.  Linear acceleration is equal to the accelerometer  measurement
00034  * with the 1 g of gravity removed.  Earth acceleration is a measurement of
00035  * linear acceleration in the Earth coordinate frame.
00036  */
00037 
00038 //------------------------------------------------------------------------------
00039 // Includes
00040 
00041 #include "FusionAhrs.h"
00042 #include <float.h> // FLT_MAX
00043 #include <math.h> // atan2f, cosf, sinf
00044 
00045 //------------------------------------------------------------------------------
00046 // Definitions
00047 
00048 /**
00049  * @brief Initial gain used during the initialisation period.  The gain used by
00050  * each algorithm iteration will ramp down from this initial gain to the
00051  * specified algorithm gain over the initialisation period.
00052  */
00053 #define INITIAL_GAIN (10.0f)
00054 
00055 /**
00056  * @brief Initialisation period (in seconds).
00057  */
00058 #define INITIALISATION_PERIOD (3.0f)
00059 
00060 //------------------------------------------------------------------------------
00061 // Functions
00062 
00063 /**
00064  * @brief Initialises the AHRS algorithm structure.
00065  * @param fusionAhrs AHRS algorithm structure.
00066  * @param gain AHRS algorithm gain.
00067  */
00068 void FusionAhrsInitialise(FusionAhrs * const fusionAhrs, const float gain) {
00069     fusionAhrs->gain = gain;
00070     fusionAhrs->minimumMagneticFieldSquared = 0.0f;
00071     fusionAhrs->maximumMagneticFieldSquared = FLT_MAX;
00072     fusionAhrs->quaternion = FUSION_QUATERNION_IDENTITY;
00073     fusionAhrs->linearAcceleration = FUSION_VECTOR3_ZERO;
00074     fusionAhrs->rampedGain = INITIAL_GAIN;
00075     fusionAhrs->zeroYawPending = false;
00076 }
00077 
00078 /**
00079  * @brief Sets the AHRS algorithm gain.  The gain must be equal or greater than
00080  * zero.
00081  * @param gain AHRS algorithm gain.
00082  */
00083 void FusionAhrsSetGain(FusionAhrs * const fusionAhrs, const float gain) {
00084     fusionAhrs->gain = gain;
00085 }
00086 
00087 /**
00088  * @brief Sets the minimum and maximum valid magnetic field magnitudes in uT.
00089  * @param fusionAhrs AHRS algorithm structure.
00090  * @param minimumMagneticField Minimum valid magnetic field magnitude.
00091  * @param maximumMagneticField Maximum valid magnetic field magnitude.
00092  */
00093 void FusionAhrsSetMagneticField(FusionAhrs * const fusionAhrs, const float minimumMagneticField, const float maximumMagneticField) {
00094     fusionAhrs->minimumMagneticFieldSquared = minimumMagneticField * minimumMagneticField;
00095     fusionAhrs->maximumMagneticFieldSquared = maximumMagneticField * maximumMagneticField;
00096 }
00097 
00098 /**
00099  * @brief Updates the AHRS algorithm.  This function should be called for each
00100  * new gyroscope measurement.
00101  * @param fusionAhrs AHRS algorithm structure.
00102  * @param gyroscope Gyroscope measurement in degrees per second.
00103  * @param accelerometer Accelerometer measurement in g.
00104  * @param magnetometer Magnetometer measurement in uT.
00105  * @param samplePeriod Sample period in seconds.  This is the difference in time
00106  * between the current and previous gyroscope measurements.
00107  */
00108 void FusionAhrsUpdate(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const FusionVector3 magnetometer, const float samplePeriod) {
00109 #define Q fusionAhrs->quaternion.element // define shorthand label for more readable code
00110 
00111     // Calculate feedback error
00112     FusionVector3 halfFeedbackError = FUSION_VECTOR3_ZERO; // scaled by 0.5 to avoid repeated multiplications by 2
00113     do {
00114         // Abandon feedback calculation if accelerometer measurement invalid
00115         if ((accelerometer.axis.x == 0.0f) && (accelerometer.axis.y == 0.0f) && (accelerometer.axis.z == 0.0f)) {
00116             break;
00117         }
00118 
00119         // Calculate direction of gravity assumed by quaternion
00120         const FusionVector3 halfGravity = {
00121             .axis.x = Q.x * Q.z - Q.w * Q.y,
00122             .axis.y = Q.w * Q.x + Q.y * Q.z,
00123             .axis.z = Q.w * Q.w - 0.5f + Q.z * Q.z,
00124         }; // equal to 3rd column of rotation matrix representation scaled by 0.5
00125 
00126         // Calculate accelerometer feedback error
00127         halfFeedbackError = FusionVectorCrossProduct(FusionVectorFastNormalise(accelerometer), halfGravity);
00128 
00129         // Abandon magnetometer feedback calculation if magnetometer measurement invalid
00130         const float magnetometerMagnitudeSquared = FusionVectorMagnitudeSquared(magnetometer);
00131         if ((magnetometerMagnitudeSquared < fusionAhrs->minimumMagneticFieldSquared) || (magnetometerMagnitudeSquared > fusionAhrs->maximumMagneticFieldSquared)) {
00132             break;
00133         }
00134 
00135         // Compute direction of 'magnetic west' assumed by quaternion
00136         const FusionVector3 halfWest = {
00137             .axis.x = Q.x * Q.y + Q.w * Q.z,
00138             .axis.y = Q.w * Q.w - 0.5f + Q.y * Q.y,
00139             .axis.z = Q.y * Q.z - Q.w * Q.x
00140         }; // equal to 2nd column of rotation matrix representation scaled by 0.5
00141 
00142         // Calculate magnetometer feedback error
00143         halfFeedbackError = FusionVectorAdd(halfFeedbackError, FusionVectorCrossProduct(FusionVectorFastNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)), halfWest));
00144 
00145     } while (false);
00146 
00147     // Ramp down gain until initialisation complete
00148     if (fusionAhrs->gain == 0) {
00149         fusionAhrs->rampedGain = 0; // skip initialisation if gain is zero
00150     }
00151     float feedbackGain = fusionAhrs->gain;
00152     if (fusionAhrs->rampedGain > fusionAhrs->gain) {
00153         fusionAhrs->rampedGain -= (INITIAL_GAIN - fusionAhrs->gain) * samplePeriod / INITIALISATION_PERIOD;
00154         feedbackGain = fusionAhrs->rampedGain;
00155     }
00156 
00157     // Convert gyroscope to radians per second scaled by 0.5
00158     FusionVector3 halfGyroscope = FusionVectorMultiplyScalar(gyroscope, 0.5f * FusionDegreesToRadians(1.0f));
00159 
00160     // Apply feedback to gyroscope
00161     halfGyroscope = FusionVectorAdd(halfGyroscope, FusionVectorMultiplyScalar(halfFeedbackError, feedbackGain));
00162 
00163     // Integrate rate of change of quaternion
00164     fusionAhrs->quaternion = FusionQuaternionAdd(fusionAhrs->quaternion, FusionQuaternionMultiplyVector(fusionAhrs->quaternion, FusionVectorMultiplyScalar(halfGyroscope, samplePeriod)));
00165 
00166     // Normalise quaternion
00167     fusionAhrs->quaternion = FusionQuaternionFastNormalise(fusionAhrs->quaternion);
00168 
00169     // Calculate linear acceleration
00170     const FusionVector3 gravity = {
00171         .axis.x = 2.0f * (Q.x * Q.z - Q.w * Q.y),
00172         .axis.y = 2.0f * (Q.w * Q.x + Q.y * Q.z),
00173         .axis.z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z),
00174     }; // equal to 3rd column of rotation matrix representation
00175     fusionAhrs->linearAcceleration = FusionVectorSubtract(accelerometer, gravity);
00176 
00177 #undef Q // undefine shorthand label
00178 }
00179 
00180 /**
00181  * @brief Updates the AHRS algorithm.  This function should be called for each
00182  * new gyroscope measurement.
00183  * @param fusionAhrs AHRS algorithm structure.
00184  * @param gyroscope Gyroscope measurement in degrees per second.
00185  * @param accelerometer Accelerometer measurement in g.
00186  * @param samplePeriod Sample period in seconds.  This is the difference in time
00187  * between the current and previous gyroscope measurements.
00188  */
00189 void FusionAhrsUpdateWithoutMagnetometer(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const float samplePeriod) {
00190 
00191     // Update AHRS algorithm
00192     FusionAhrsUpdate(fusionAhrs, gyroscope, accelerometer, FUSION_VECTOR3_ZERO, samplePeriod);
00193 
00194     // Zero yaw once initialisation complete
00195     if (FusionAhrsIsInitialising(fusionAhrs) == true) {
00196         fusionAhrs->zeroYawPending = true;
00197     } else {
00198         if (fusionAhrs->zeroYawPending == true) {
00199             FusionAhrsSetYaw(fusionAhrs, 0.0f);
00200             fusionAhrs->zeroYawPending = false;
00201         }
00202     }
00203 }
00204 
00205 /**
00206  * @brief Gets the quaternion describing the sensor relative to the Earth.
00207  * @param fusionAhrs AHRS algorithm structure.
00208  * @return Quaternion describing the sensor relative to the Earth.
00209  */
00210 FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs * const fusionAhrs) {
00211     return FusionQuaternionConjugate(fusionAhrs->quaternion);
00212 }
00213 
00214 /**
00215  * @brief Gets the linear acceleration measurement equal to the accelerometer
00216  * measurement with the 1 g of gravity removed.
00217  * @param fusionAhrs AHRS algorithm structure.
00218  * @return Linear acceleration measurement.
00219  */
00220 FusionVector3 FusionAhrsGetLinearAcceleration(const FusionAhrs * const fusionAhrs) {
00221     return fusionAhrs->linearAcceleration;
00222 }
00223 
00224 /**
00225  * @brief Gets the Earth acceleration measurement equal to linear acceleration
00226  * in the Earth coordinate frame.
00227  * @param fusionAhrs AHRS algorithm structure.
00228  * @return Earth acceleration measurement.
00229  */
00230 FusionVector3 FusionAhrsGetEarthAcceleration(const FusionAhrs * const fusionAhrs) {
00231 #define Q fusionAhrs->quaternion.element // define shorthand labels for more readable code
00232 #define A fusionAhrs->linearAcceleration.axis
00233     const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
00234     const float qwqx = Q.w * Q.x;
00235     const float qwqy = Q.w * Q.y;
00236     const float qwqz = Q.w * Q.z;
00237     const float qxqy = Q.x * Q.y;
00238     const float qxqz = Q.x * Q.z;
00239     const float qyqz = Q.y * Q.z;
00240     const FusionVector3 earthAcceleration = {
00241         .axis.x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z),
00242         .axis.y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z),
00243         .axis.z = 2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z),
00244     }; // transpose of a rotation matrix representation of the quaternion multiplied with the linear acceleration
00245     return earthAcceleration;
00246 #undef Q // undefine shorthand label
00247 #undef A
00248 }
00249 
00250 /**
00251  * @brief Reinitialise the AHRS algorithm.
00252  * @param fusionAhrs AHRS algorithm structure.
00253  */
00254 void FusionAhrsReinitialise(FusionAhrs * const fusionAhrs) {
00255     fusionAhrs->quaternion = FUSION_QUATERNION_IDENTITY;
00256     fusionAhrs->linearAcceleration = FUSION_VECTOR3_ZERO;
00257     fusionAhrs->rampedGain = INITIAL_GAIN;
00258 }
00259 
00260 /**
00261  * @brief Returns true while the AHRS algorithm is initialising.
00262  * @param fusionAhrs AHRS algorithm structure.
00263  * @return True while the AHRS algorithm is initialising.
00264  */
00265 bool FusionAhrsIsInitialising(const FusionAhrs * const fusionAhrs) {
00266     return fusionAhrs->rampedGain > fusionAhrs->gain;
00267 }
00268 
00269 /**
00270  * @brief Sets the yaw component of the orientation measurement provided by the
00271  * AHRS algorithm.  This function can be used to reset drift in yaw when the
00272  * AHRS algorithm is being used without a magnetometer.
00273  * @param fusionAhrs AHRS algorithm structure.
00274  * @param yaw Yaw angle in degrees.
00275  */
00276 void FusionAhrsSetYaw(FusionAhrs * const fusionAhrs, const float yaw) {
00277 #define Q fusionAhrs->quaternion.element // define shorthand label for more readable code
00278     fusionAhrs->quaternion = FusionQuaternionNormalise(fusionAhrs->quaternion); // quaternion must be normalised accurately (approximation not sufficient)
00279     const float inverseYaw = atan2f(Q.x * Q.y + Q.w * Q.z, Q.w * Q.w - 0.5f + Q.x * Q.x); // Euler angle of conjugate
00280     const float halfInverseYawMinusOffset = 0.5f * (inverseYaw - FusionDegreesToRadians(yaw));
00281     const FusionQuaternion inverseYawQuaternion = {
00282         .element.w = cosf(halfInverseYawMinusOffset),
00283         .element.x = 0.0f,
00284         .element.y = 0.0f,
00285         .element.z = -1.0f * sinf(halfInverseYawMinusOffset),
00286     };
00287     fusionAhrs->quaternion = FusionQuaternionMultiply(inverseYawQuaternion, fusionAhrs->quaternion);
00288 #undef Q // undefine shorthand label
00289 }
00290 
00291 //------------------------------------------------------------------------------
00292 // End of file