Cyrill Zoller / Mahony_Algorithm

..

MahonyAHRS.cpp@0:da9dac34fd93, 2015-08-13 (annotated)

Committer:
b50559
Date:
Thu Aug 13 22:12:39 2015 +0000
Revision:
0:da9dac34fd93
Child:
1:0d456c561eab
created library for mahony's ahrs algorithm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
b50559 0:da9dac34fd93 1 // Header files
b50559 0:da9dac34fd93 2
b50559 0:da9dac34fd93 3 #include "mbed.h"
b50559 0:da9dac34fd93 4 #include "MahonyAHRS.h"
b50559 0:da9dac34fd93 5 #include <math.h>
b50559 0:da9dac34fd93 6
b50559 0:da9dac34fd93 7 //---------------------------------------------------------------------------------------------------
b50559 0:da9dac34fd93 8 // Definitions
b50559 0:da9dac34fd93 9
b50559 0:da9dac34fd93 10 //#define sampleFreq 512.0f // sample frequency in Hz
b50559 0:da9dac34fd93 11 #define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
b50559 0:da9dac34fd93 12 #define twoKiDef (2.0f * 0.0f) // 2 * integral gain
b50559 0:da9dac34fd93 13 #define PI 3.14159265359f
b50559 0:da9dac34fd93 14
b50559 0:da9dac34fd93 15 //---------------------------------------------------------------------------------------------------
b50559 0:da9dac34fd93 16
b50559 0:da9dac34fd93 17 MahonyAHRS::MahonyAHRS(float Freq){
b50559 0:da9dac34fd93 18
b50559 0:da9dac34fd93 19 sampleFreq = Freq;
b50559 0:da9dac34fd93 20
b50559 0:da9dac34fd93 21 }
b50559 0:da9dac34fd93 22
b50559 0:da9dac34fd93 23 float twoKp = twoKpDef; // 2 * proportional gain (Kp)
b50559 0:da9dac34fd93 24 float twoKi = twoKiDef; // 2 * integral gain (Ki)
b50559 0:da9dac34fd93 25 float q4 = 1.0f, q5 = 0.0f, q6 = 0.0f, q7 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
b50559 0:da9dac34fd93 26 float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
b50559 0:da9dac34fd93 27
b50559 0:da9dac34fd93 28
b50559 0:da9dac34fd93 29 float inv_Sqrt(float x);
b50559 0:da9dac34fd93 30
b50559 0:da9dac34fd93 31 //====================================================================================================
b50559 0:da9dac34fd93 32 // Functions
b50559 0:da9dac34fd93 33
b50559 0:da9dac34fd93 34 //---------------------------------------------------------------------------------------------------
b50559 0:da9dac34fd93 35 // AHRS algorithm update
b50559 0:da9dac34fd93 36
b50559 0:da9dac34fd93 37 void MahonyAHRS::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
b50559 0:da9dac34fd93 38 float recipNorm;
b50559 0:da9dac34fd93 39 float q4q4, q4q5, q4q6, q4q7, q5q5, q5q6, q5q7, q6q6, q6q7, q7q7;
b50559 0:da9dac34fd93 40 float hx, hy, bx, bz;
b50559 0:da9dac34fd93 41 float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
b50559 0:da9dac34fd93 42 float halfex, halfey, halfez;
b50559 0:da9dac34fd93 43 float qa, qb, qc;
b50559 0:da9dac34fd93 44
b50559 0:da9dac34fd93 45 // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
b50559 0:da9dac34fd93 46 if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
b50559 0:da9dac34fd93 47 MahonyAHRS::updateIMU(gx, gy, gz, ax, ay, az);
b50559 0:da9dac34fd93 48 return;
b50559 0:da9dac34fd93 49 }
b50559 0:da9dac34fd93 50
b50559 0:da9dac34fd93 51 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
b50559 0:da9dac34fd93 52 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
b50559 0:da9dac34fd93 53
b50559 0:da9dac34fd93 54 // Normalise accelerometer measurement
b50559 0:da9dac34fd93 55 recipNorm = inv_Sqrt(ax * ax + ay * ay + az * az);
b50559 0:da9dac34fd93 56 ax *= recipNorm;
b50559 0:da9dac34fd93 57 ay *= recipNorm;
b50559 0:da9dac34fd93 58 az *= recipNorm;
b50559 0:da9dac34fd93 59
b50559 0:da9dac34fd93 60 // Normalise magnetometer measurement
b50559 0:da9dac34fd93 61 recipNorm = inv_Sqrt(mx * mx + my * my + mz * mz);
b50559 0:da9dac34fd93 62 mx *= recipNorm;
b50559 0:da9dac34fd93 63 my *= recipNorm;
b50559 0:da9dac34fd93 64 mz *= recipNorm;
b50559 0:da9dac34fd93 65
b50559 0:da9dac34fd93 66 // Auxiliary variables to avoid repeated arithmetic
b50559 0:da9dac34fd93 67 q4q4 = q4 * q4;
b50559 0:da9dac34fd93 68 q4q5 = q4 * q5;
b50559 0:da9dac34fd93 69 q4q6 = q4 * q6;
b50559 0:da9dac34fd93 70 q4q7 = q4 * q7;
b50559 0:da9dac34fd93 71 q5q5 = q5 * q5;
b50559 0:da9dac34fd93 72 q5q6 = q5 * q6;
b50559 0:da9dac34fd93 73 q5q7 = q5 * q7;
b50559 0:da9dac34fd93 74 q6q6 = q6 * q6;
b50559 0:da9dac34fd93 75 q6q7 = q6 * q7;
b50559 0:da9dac34fd93 76 q7q7 = q7 * q7;
b50559 0:da9dac34fd93 77
b50559 0:da9dac34fd93 78 // Reference direction of Earth's magnetic field
b50559 0:da9dac34fd93 79 hx = 2.0f * (mx * (0.5f - q6q6 - q7q7) + my * (q5q6 - q4q7) + mz * (q5q7 + q4q6));
b50559 0:da9dac34fd93 80 hy = 2.0f * (mx * (q5q6 + q4q7) + my * (0.5f - q5q5 - q7q7) + mz * (q6q7 - q4q5));
b50559 0:da9dac34fd93 81 bx = sqrt(hx * hx + hy * hy);
b50559 0:da9dac34fd93 82 bz = 2.0f * (mx * (q5q7 - q4q6) + my * (q6q7 + q4q5) + mz * (0.5f - q5q5 - q6q6));
b50559 0:da9dac34fd93 83
b50559 0:da9dac34fd93 84 // Estimated direction of gravity and magnetic field
b50559 0:da9dac34fd93 85 halfvx = q5q7 - q4q6;
b50559 0:da9dac34fd93 86 halfvy = q4q5 + q6q7;
b50559 0:da9dac34fd93 87 halfvz = q4q4 - 0.5f + q7q7;
b50559 0:da9dac34fd93 88 halfwx = bx * (0.5f - q6q6 - q7q7) + bz * (q5q7 - q4q6);
b50559 0:da9dac34fd93 89 halfwy = bx * (q5q6 - q4q7) + bz * (q4q5 + q6q7);
b50559 0:da9dac34fd93 90 halfwz = bx * (q4q6 + q5q7) + bz * (0.5f - q5q5 - q6q6);
b50559 0:da9dac34fd93 91
b50559 0:da9dac34fd93 92 // Error is sum of cross product between estimated direction and measured direction of field vectors
b50559 0:da9dac34fd93 93 halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
b50559 0:da9dac34fd93 94 halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
b50559 0:da9dac34fd93 95 halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
b50559 0:da9dac34fd93 96
b50559 0:da9dac34fd93 97 // Compute and apply integral feedback if enabled
b50559 0:da9dac34fd93 98 if(twoKi > 0.0f) {
b50559 0:da9dac34fd93 99 integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
b50559 0:da9dac34fd93 100 integralFBy += twoKi * halfey * (1.0f / sampleFreq);
b50559 0:da9dac34fd93 101 integralFBz += twoKi * halfez * (1.0f / sampleFreq);
b50559 0:da9dac34fd93 102 gx += integralFBx; // apply integral feedback
b50559 0:da9dac34fd93 103 gy += integralFBy;
b50559 0:da9dac34fd93 104 gz += integralFBz;
b50559 0:da9dac34fd93 105 }
b50559 0:da9dac34fd93 106 else {
b50559 0:da9dac34fd93 107 integralFBx = 0.0f; // prevent integral windup
b50559 0:da9dac34fd93 108 integralFBy = 0.0f;
b50559 0:da9dac34fd93 109 integralFBz = 0.0f;
b50559 0:da9dac34fd93 110 }
b50559 0:da9dac34fd93 111
b50559 0:da9dac34fd93 112 // Apply proportional feedback
b50559 0:da9dac34fd93 113 gx += twoKp * halfex;
b50559 0:da9dac34fd93 114 gy += twoKp * halfey;
b50559 0:da9dac34fd93 115 gz += twoKp * halfez;
b50559 0:da9dac34fd93 116 }
b50559 0:da9dac34fd93 117
b50559 0:da9dac34fd93 118 // Integrate rate of change of quaternion
b50559 0:da9dac34fd93 119 gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
b50559 0:da9dac34fd93 120 gy *= (0.5f * (1.0f / sampleFreq));
b50559 0:da9dac34fd93 121 gz *= (0.5f * (1.0f / sampleFreq));
b50559 0:da9dac34fd93 122 qa = q4;
b50559 0:da9dac34fd93 123 qb = q5;
b50559 0:da9dac34fd93 124 qc = q6;
b50559 0:da9dac34fd93 125 q4 += (-qb * gx - qc * gy - q7 * gz);
b50559 0:da9dac34fd93 126 q5 += (qa * gx + qc * gz - q7 * gy);
b50559 0:da9dac34fd93 127 q6 += (qa * gy - qb * gz + q7 * gx);
b50559 0:da9dac34fd93 128 q7 += (qa * gz + qb * gy - qc * gx);
b50559 0:da9dac34fd93 129
b50559 0:da9dac34fd93 130 // Normalise quaternion
b50559 0:da9dac34fd93 131 recipNorm = inv_Sqrt(q4 * q4 + q5 * q5 + q6 * q6 + q7 * q7);
b50559 0:da9dac34fd93 132 q4 *= recipNorm;
b50559 0:da9dac34fd93 133 q5 *= recipNorm;
b50559 0:da9dac34fd93 134 q6 *= recipNorm;
b50559 0:da9dac34fd93 135 q7 *= recipNorm;
b50559 0:da9dac34fd93 136 }
b50559 0:da9dac34fd93 137
b50559 0:da9dac34fd93 138 //---------------------------------------------------------------------------------------------------
b50559 0:da9dac34fd93 139 // IMU algorithm update
b50559 0:da9dac34fd93 140
b50559 0:da9dac34fd93 141 void MahonyAHRS::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
b50559 0:da9dac34fd93 142 float recipNorm;
b50559 0:da9dac34fd93 143 float halfvx, halfvy, halfvz;
b50559 0:da9dac34fd93 144 float halfex, halfey, halfez;
b50559 0:da9dac34fd93 145 float qa, qb, qc;
b50559 0:da9dac34fd93 146
b50559 0:da9dac34fd93 147 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
b50559 0:da9dac34fd93 148 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
b50559 0:da9dac34fd93 149
b50559 0:da9dac34fd93 150 // Normalise accelerometer measurement
b50559 0:da9dac34fd93 151 recipNorm = inv_Sqrt(ax * ax + ay * ay + az * az);
b50559 0:da9dac34fd93 152 ax *= recipNorm;
b50559 0:da9dac34fd93 153 ay *= recipNorm;
b50559 0:da9dac34fd93 154 az *= recipNorm;
b50559 0:da9dac34fd93 155
b50559 0:da9dac34fd93 156 // Estimated direction of gravity and vector perpendicular to magnetic flux
b50559 0:da9dac34fd93 157 halfvx = q5 * q7 - q4 * q6;
b50559 0:da9dac34fd93 158 halfvy = q4 * q5 + q6 * q7;
b50559 0:da9dac34fd93 159 halfvz = q4 * q4 - 0.5f + q7 * q7;
b50559 0:da9dac34fd93 160
b50559 0:da9dac34fd93 161 // Error is sum of cross product between estimated and measured direction of gravity
b50559 0:da9dac34fd93 162 halfex = (ay * halfvz - az * halfvy);
b50559 0:da9dac34fd93 163 halfey = (az * halfvx - ax * halfvz);
b50559 0:da9dac34fd93 164 halfez = (ax * halfvy - ay * halfvx);
b50559 0:da9dac34fd93 165
b50559 0:da9dac34fd93 166 // Compute and apply integral feedback if enabled
b50559 0:da9dac34fd93 167 if(twoKi > 0.0f) {
b50559 0:da9dac34fd93 168 integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
b50559 0:da9dac34fd93 169 integralFBy += twoKi * halfey * (1.0f / sampleFreq);
b50559 0:da9dac34fd93 170 integralFBz += twoKi * halfez * (1.0f / sampleFreq);
b50559 0:da9dac34fd93 171 gx += integralFBx; // apply integral feedback
b50559 0:da9dac34fd93 172 gy += integralFBy;
b50559 0:da9dac34fd93 173 gz += integralFBz;
b50559 0:da9dac34fd93 174 }
b50559 0:da9dac34fd93 175 else {
b50559 0:da9dac34fd93 176 integralFBx = 0.0f; // prevent integral windup
b50559 0:da9dac34fd93 177 integralFBy = 0.0f;
b50559 0:da9dac34fd93 178 integralFBz = 0.0f;
b50559 0:da9dac34fd93 179 }
b50559 0:da9dac34fd93 180
b50559 0:da9dac34fd93 181 // Apply proportional feedback
b50559 0:da9dac34fd93 182 gx += twoKp * halfex;
b50559 0:da9dac34fd93 183 gy += twoKp * halfey;
b50559 0:da9dac34fd93 184 gz += twoKp * halfez;
b50559 0:da9dac34fd93 185 }
b50559 0:da9dac34fd93 186
b50559 0:da9dac34fd93 187 // Integrate rate of change of quaternion
b50559 0:da9dac34fd93 188 gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
b50559 0:da9dac34fd93 189 gy *= (0.5f * (1.0f / sampleFreq));
b50559 0:da9dac34fd93 190 gz *= (0.5f * (1.0f / sampleFreq));
b50559 0:da9dac34fd93 191 qa = q4;
b50559 0:da9dac34fd93 192 qb = q5;
b50559 0:da9dac34fd93 193 qc = q6;
b50559 0:da9dac34fd93 194 q4 += (-qb * gx - qc * gy - q7 * gz);
b50559 0:da9dac34fd93 195 q5 += (qa * gx + qc * gz - q7 * gy);
b50559 0:da9dac34fd93 196 q6 += (qa * gy - qb * gz + q7 * gx);
b50559 0:da9dac34fd93 197 q7 += (qa * gz + qb * gy - qc * gx);
b50559 0:da9dac34fd93 198
b50559 0:da9dac34fd93 199 // Normalise quaternion
b50559 0:da9dac34fd93 200 recipNorm = inv_Sqrt(q4 * q4 + q5 * q5 + q6 * q6 + q7 * q7);
b50559 0:da9dac34fd93 201 q4 *= recipNorm;
b50559 0:da9dac34fd93 202 q5 *= recipNorm;
b50559 0:da9dac34fd93 203 q6 *= recipNorm;
b50559 0:da9dac34fd93 204 q7 *= recipNorm;
b50559 0:da9dac34fd93 205 }
b50559 0:da9dac34fd93 206
b50559 0:da9dac34fd93 207 //---------------------------------------------------------------------------------------------------
b50559 0:da9dac34fd93 208 // Fast inverse square-root
b50559 0:da9dac34fd93 209 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
b50559 0:da9dac34fd93 210
b50559 0:da9dac34fd93 211 float inv_Sqrt(float x) {
b50559 0:da9dac34fd93 212 float halfx = 0.5f * x;
b50559 0:da9dac34fd93 213 float y = x;
b50559 0:da9dac34fd93 214 long i = *(long*)&y;
b50559 0:da9dac34fd93 215 i = 0x5f3759df - (i>>1);
b50559 0:da9dac34fd93 216 y = *(float*)&i;
b50559 0:da9dac34fd93 217 y = y * (1.5f - (halfx * y * y));
b50559 0:da9dac34fd93 218 return y;
b50559 0:da9dac34fd93 219 }
b50559 0:da9dac34fd93 220
b50559 0:da9dac34fd93 221
b50559 0:da9dac34fd93 222 void MahonyAHRS::getEuler(){
b50559 0:da9dac34fd93 223
b50559 0:da9dac34fd93 224 float gx = 2*(q5*q7 - q4*q6);
b50559 0:da9dac34fd93 225 float gy = 2 * (q4*q5 + q6*q7);
b50559 0:da9dac34fd93 226 float gz = q4*q4 - q5*q5 - q6*q6 + q7*q7;
b50559 0:da9dac34fd93 227
b50559 0:da9dac34fd93 228 roll = atan(gy / sqrt(gx*gx + gz*gz));
b50559 0:da9dac34fd93 229 pitch = atan(gx / sqrt(gy*gy + gz*gz));
b50559 0:da9dac34fd93 230 yaw = atan2(2 * q5 * q6 - 2 * q4 * q7, 2 * q4*q4 + 2 * q5 * q5 - 1);
b50559 0:da9dac34fd93 231
b50559 0:da9dac34fd93 232 roll = roll*180/PI;
b50559 0:da9dac34fd93 233 pitch = pitch*180/PI;
b50559 0:da9dac34fd93 234 yaw = yaw*180/PI;
b50559 0:da9dac34fd93 235
b50559 0:da9dac34fd93 236 if (ceil(roll) - roll <= .5){
b50559 0:da9dac34fd93 237 roll = ceil(roll);
b50559 0:da9dac34fd93 238 }
b50559 0:da9dac34fd93 239 else{
b50559 0:da9dac34fd93 240 roll = floor(roll);
b50559 0:da9dac34fd93 241 }
b50559 0:da9dac34fd93 242
b50559 0:da9dac34fd93 243 if (ceil(pitch) - pitch <= .5){
b50559 0:da9dac34fd93 244 pitch = ceil(pitch);
b50559 0:da9dac34fd93 245 }
b50559 0:da9dac34fd93 246 else{
b50559 0:da9dac34fd93 247 pitch = floor(pitch);
b50559 0:da9dac34fd93 248 }
b50559 0:da9dac34fd93 249
b50559 0:da9dac34fd93 250 if (ceil(yaw) - yaw <= .5){
b50559 0:da9dac34fd93 251 yaw = ceil(yaw);
b50559 0:da9dac34fd93 252 }
b50559 0:da9dac34fd93 253 else{
b50559 0:da9dac34fd93 254 yaw = floor(yaw);
b50559 0:da9dac34fd93 255 }
b50559 0:da9dac34fd93 256 }
b50559 0:da9dac34fd93 257
b50559 0:da9dac34fd93 258 int16_t MahonyAHRS::getRoll(){
b50559 0:da9dac34fd93 259 return (int16_t)roll;
b50559 0:da9dac34fd93 260 }
b50559 0:da9dac34fd93 261
b50559 0:da9dac34fd93 262 int16_t MahonyAHRS::getPitch(){
b50559 0:da9dac34fd93 263 return (int16_t)pitch;
b50559 0:da9dac34fd93 264 }
b50559 0:da9dac34fd93 265
b50559 0:da9dac34fd93 266 int16_t MahonyAHRS::getYaw(){
b50559 0:da9dac34fd93 267 return (int16_t)yaw;
b50559 0:da9dac34fd93 268 }
b50559 0:da9dac34fd93 269
b50559 0:da9dac34fd93 270 //====================================================================================================
b50559 0:da9dac34fd93 271 // END OF CODE
b50559 0:da9dac34fd93 272 //====================================================================================================