Erik - / IMUCalc

Never actually tested in practise

This library has only been 'hand tested', it never was actually included in a quadcopter. It is now published so it might help someone, but please verify it works for you before you crash your setup (that is of course for every library you use). It is a while ago I made this, so everything that follows might be slightly different than I remember :D.

Inputs are SI units (probably), so gyro data should be in rad/s. Magnetometer and accelerometer only uses normalized vectors. You will require the following library which isn't included in this one: http://mbed.org/users/BlazeX/code/GTMath/. I am fairly certain things like normalizing a vector twice happens currently, so it can be more efficient.

Basic functionality

The library doesn't use quaternions, since they are hard, but instead two 3D vectors. Those last 2 floats aren't going to fill your memory. One vector is the in the length of the aircraft/device/etc ('heading'), the other one points up ('top'). Together they define the angle of your craft.

The currently measured vectors by the accelerometer and magnetometer are defined. The top simply calculated from the accelerometer data. For the heading the magnetometer data is used, which is moved to be at 90 degrees from the top (this is required since unless you live at the equator that won't be the case). This directly makes sure they have the 90 degree angle between them they are supposed to have.

At the same time the gyroscope offset (later more) is removed from the gyroscope data, and that is used to rotate the old heading and top vectors to new values according to your gyroscope data.

We calculate the difference between the gyroscope measurements and the accelero/magneto measurements. We call this the offset of the gyroscope. Now with a certain weight factor we combine the two measurement types into a final result, which is also used for the next gyroscope measurement. This already cancels part of the gyroscope drift.

The second part is that we average out the gyroscope offset measurements, and the result of that is used to compensate new gyroscope measurements.

IMUCalc.cpp@1:51c902d63dda, 2014-01-22 (annotated)

Committer:
Sissors
Date:
Wed Jan 22 21:19:18 2014 +0000
Revision:
1:51c902d63dda
Parent:
0:4dc7e56179ff 
v0.1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Sissors 0:4dc7e56179ff 1 #include "IMUCalc.h"
Sissors 0:4dc7e56179ff 2
Sissors 1:51c902d63dda 3 IMUCalc::IMUCalc(void)
Sissors 1:51c902d63dda 4 {
Sissors 1:51c902d63dda 5 heading.x=1;
Sissors 1:51c902d63dda 6 heading.y=0;
Sissors 1:51c902d63dda 7 heading.z=0;
Sissors 1:51c902d63dda 8
Sissors 1:51c902d63dda 9 top.x=0;
Sissors 1:51c902d63dda 10 top.y=0;
Sissors 1:51c902d63dda 11 top.z=1;
Sissors 1:51c902d63dda 12
Sissors 1:51c902d63dda 13 gyroOffset.x=0;
Sissors 1:51c902d63dda 14 gyroOffset.y=0;
Sissors 1:51c902d63dda 15 gyroOffset.z=0;
Sissors 1:51c902d63dda 16
Sissors 1:51c902d63dda 17 absGain=0.01;
Sissors 1:51c902d63dda 18 initialRun=true;
Sissors 1:51c902d63dda 19
Sissors 1:51c902d63dda 20
Sissors 1:51c902d63dda 21 }
Sissors 1:51c902d63dda 22
Sissors 1:51c902d63dda 23
Sissors 1:51c902d63dda 24 void IMUCalc::runCalc(float *accdat, float *gyrodat, float *magdat, float timestep)
Sissors 1:51c902d63dda 25 {
Sissors 1:51c902d63dda 26
Sissors 1:51c902d63dda 27
Sissors 1:51c902d63dda 28 //Variables
Sissors 1:51c902d63dda 29 Vector3 accdata;
Sissors 1:51c902d63dda 30 Vector3 gyrodata;
Sissors 1:51c902d63dda 31 Vector3 magdata;
Sissors 1:51c902d63dda 32
Sissors 1:51c902d63dda 33 Vector3 heading_abs;
Sissors 1:51c902d63dda 34 Vector3 top_abs;
Sissors 1:51c902d63dda 35
Sissors 1:51c902d63dda 36 //Change data to vector3
Sissors 1:51c902d63dda 37 for (int i = 0; i<3; i++) {
Sissors 1:51c902d63dda 38 accdata.af[i]=accdat[i];
Sissors 1:51c902d63dda 39 gyrodata.af[i]=gyrodat[i];
Sissors 1:51c902d63dda 40 magdata.af[i]=magdat[i];
Sissors 1:51c902d63dda 41 }
Sissors 1:51c902d63dda 42
Sissors 1:51c902d63dda 43 gyrodata = gyrodata-gyroOffset;
Sissors 1:51c902d63dda 44
Sissors 1:51c902d63dda 45 heading = rotateVector(heading, gyrodata, -gyrodata.Length()*timestep);
Sissors 1:51c902d63dda 46 top = rotateVector(top, gyrodata, -gyrodata.Length()*timestep);
Sissors 1:51c902d63dda 47
Sissors 1:51c902d63dda 48
Sissors 1:51c902d63dda 49
Sissors 1:51c902d63dda 50
Sissors 1:51c902d63dda 51
Sissors 1:51c902d63dda 52 //Rotate the magnetic data to be in plain with the earth
Sissors 1:51c902d63dda 53 heading_abs = -1 * rotateMag(magdata, accdata);
Sissors 1:51c902d63dda 54 top_abs = -1 * accdata;
Sissors 1:51c902d63dda 55
Sissors 1:51c902d63dda 56 heading_abs = heading_abs.Normalize();
Sissors 1:51c902d63dda 57 top_abs = top_abs.Normalize();
Sissors 1:51c902d63dda 58
Sissors 1:51c902d63dda 59 //Calculate offset
Sissors 1:51c902d63dda 60 Vector3 currentGyroOffset, weightTop, weightHeading, tempVector;
Sissors 1:51c902d63dda 61
Sissors 1:51c902d63dda 62
Sissors 1:51c902d63dda 63
Sissors 1:51c902d63dda 64 //make tempvector in X direction, do crossproduct, calculate length of result
Sissors 1:51c902d63dda 65 tempVector.x = 1;
Sissors 1:51c902d63dda 66 tempVector.y=0;
Sissors 1:51c902d63dda 67 tempVector.z=0;
Sissors 1:51c902d63dda 68 weightTop.x = top.CrossP(tempVector).Length();
Sissors 1:51c902d63dda 69 weightHeading.x = heading.CrossP(tempVector).Length();
Sissors 1:51c902d63dda 70
Sissors 1:51c902d63dda 71 tempVector.x = 0;
Sissors 1:51c902d63dda 72 tempVector.y=1;
Sissors 1:51c902d63dda 73 tempVector.z=0;
Sissors 1:51c902d63dda 74 weightTop.y = top.CrossP(tempVector).Length();
Sissors 1:51c902d63dda 75 weightHeading.y = heading.CrossP(tempVector).Length();
Sissors 1:51c902d63dda 76
Sissors 1:51c902d63dda 77 tempVector.x = 0;
Sissors 1:51c902d63dda 78 tempVector.y=0;
Sissors 1:51c902d63dda 79 tempVector.z=1;
Sissors 1:51c902d63dda 80 weightTop.z = top.CrossP(tempVector).Length();
Sissors 1:51c902d63dda 81 weightHeading.z = heading.CrossP(tempVector).Length();
Sissors 1:51c902d63dda 82
Sissors 1:51c902d63dda 83
Sissors 1:51c902d63dda 84 //Use weightfactors, then divide by their sum
Sissors 1:51c902d63dda 85 currentGyroOffset = weightTop * angleBetween(top_abs, top) + weightHeading * angleBetween(heading_abs, heading);
Sissors 1:51c902d63dda 86 currentGyroOffset = currentGyroOffset / (weightTop + weightHeading);
Sissors 1:51c902d63dda 87
Sissors 1:51c902d63dda 88
Sissors 1:51c902d63dda 89 if (currentGyroOffset.x > timestep * 0.1)
Sissors 1:51c902d63dda 90 currentGyroOffset.x = timestep * 0.1;
Sissors 1:51c902d63dda 91 if (currentGyroOffset.x < -timestep * 0.1)
Sissors 1:51c902d63dda 92 currentGyroOffset.x = -timestep * 0.1;
Sissors 1:51c902d63dda 93
Sissors 1:51c902d63dda 94 if (currentGyroOffset.y > timestep * 0.1)
Sissors 1:51c902d63dda 95 currentGyroOffset.y = timestep * 0.1;
Sissors 1:51c902d63dda 96 if (currentGyroOffset.y < -timestep * 0.1)
Sissors 1:51c902d63dda 97 currentGyroOffset.y = -timestep * 0.1;
Sissors 1:51c902d63dda 98
Sissors 1:51c902d63dda 99 if (currentGyroOffset.z > timestep * 0.1)
Sissors 1:51c902d63dda 100 currentGyroOffset.z = timestep * 0.1;
Sissors 1:51c902d63dda 101 if (currentGyroOffset.z < -timestep * 0.1)
Sissors 1:51c902d63dda 102 currentGyroOffset.z = -timestep * 0.1;
Sissors 1:51c902d63dda 103
Sissors 1:51c902d63dda 104 gyroOffset -= 0.01 * currentGyroOffset;
Sissors 1:51c902d63dda 105
Sissors 1:51c902d63dda 106
Sissors 1:51c902d63dda 107 //Take average value of heading/heading_abs with different gains to get current estimate
Sissors 1:51c902d63dda 108 if (initialRun) {
Sissors 1:51c902d63dda 109 heading = heading_abs;
Sissors 1:51c902d63dda 110 top = top_abs;
Sissors 1:51c902d63dda 111 gyroOffset *= 0;
Sissors 1:51c902d63dda 112 initialRun=false;
Sissors 1:51c902d63dda 113 } else {
Sissors 1:51c902d63dda 114 heading = heading*(1-absGain) + heading_abs*absGain;
Sissors 1:51c902d63dda 115 top = top * (1-absGain) + top_abs * absGain;
Sissors 1:51c902d63dda 116 }
Sissors 1:51c902d63dda 117 }
Sissors 1:51c902d63dda 118
Sissors 1:51c902d63dda 119 //Calculates the yaw
Sissors 1:51c902d63dda 120 float IMUCalc::getYaw( void )
Sissors 1:51c902d63dda 121 {
Sissors 1:51c902d63dda 122 //First normalize yaw vector, then calculate the heading
Sissors 1:51c902d63dda 123 Vector2 yawVector(heading.x, heading.y);
Sissors 1:51c902d63dda 124
Sissors 1:51c902d63dda 125 if (yawVector.Length()>0) {
Sissors 1:51c902d63dda 126 yawVector = yawVector.Normalize();
Sissors 1:51c902d63dda 127
Sissors 1:51c902d63dda 128 //check Quadrant
Sissors 1:51c902d63dda 129 if (yawVector.y<0) {
Sissors 1:51c902d63dda 130 if (yawVector.x < 0)
Sissors 1:51c902d63dda 131 return -M_PI - asin(yawVector.y);
Sissors 1:51c902d63dda 132 else
Sissors 1:51c902d63dda 133 return asin(yawVector.y);
Sissors 1:51c902d63dda 134 } else {
Sissors 1:51c902d63dda 135 if (yawVector.x < 0)
Sissors 1:51c902d63dda 136 return M_PI - asin(yawVector.y);
Sissors 1:51c902d63dda 137 else
Sissors 1:51c902d63dda 138 return asin(yawVector.y);
Sissors 1:51c902d63dda 139 }
Sissors 1:51c902d63dda 140 } else
Sissors 1:51c902d63dda 141 return 0;
Sissors 1:51c902d63dda 142 }
Sissors 1:51c902d63dda 143
Sissors 1:51c902d63dda 144 //Calculates the pitch
Sissors 1:51c902d63dda 145 float IMUCalc::getPitch( void )
Sissors 1:51c902d63dda 146 {
Sissors 1:51c902d63dda 147 //First normalize pitch vector, then calculate the pitch
Sissors 1:51c902d63dda 148 Vector2 pitchVector(top.x, top.z);
Sissors 1:51c902d63dda 149
Sissors 1:51c902d63dda 150 if (pitchVector.Length()>0) {
Sissors 1:51c902d63dda 151 pitchVector = pitchVector.Normalize();
Sissors 1:51c902d63dda 152
Sissors 1:51c902d63dda 153 //if the top is at the bottom, invert the vector
Sissors 1:51c902d63dda 154 if (pitchVector.y<0)
Sissors 1:51c902d63dda 155 pitchVector = -pitchVector;
Sissors 1:51c902d63dda 156 return asin(pitchVector.x);
Sissors 1:51c902d63dda 157
Sissors 1:51c902d63dda 158 } else
Sissors 1:51c902d63dda 159 return 0;
Sissors 1:51c902d63dda 160 }
Sissors 1:51c902d63dda 161
Sissors 1:51c902d63dda 162 //Calculates the roll
Sissors 1:51c902d63dda 163 float IMUCalc::getRoll( void )
Sissors 1:51c902d63dda 164 {
Sissors 1:51c902d63dda 165 //First normalize yaw vector, then calculate the heading
Sissors 1:51c902d63dda 166 Vector2 rollVector(top.y, top.z);
Sissors 1:51c902d63dda 167
Sissors 1:51c902d63dda 168 if (rollVector.Length()>0) {
Sissors 1:51c902d63dda 169 rollVector = rollVector.Normalize();
Sissors 1:51c902d63dda 170
Sissors 1:51c902d63dda 171 //check Quadrant
Sissors 1:51c902d63dda 172 if (rollVector.y<0) {
Sissors 1:51c902d63dda 173 if (rollVector.x < 0)
Sissors 1:51c902d63dda 174 return -M_PI - asin(rollVector.x);
Sissors 1:51c902d63dda 175 else
Sissors 1:51c902d63dda 176 return M_PI - asin(rollVector.x);
Sissors 1:51c902d63dda 177 } else {
Sissors 1:51c902d63dda 178 return asin(rollVector.x);
Sissors 1:51c902d63dda 179 }
Sissors 1:51c902d63dda 180 } else
Sissors 1:51c902d63dda 181 return 0;
Sissors 1:51c902d63dda 182 }
Sissors 1:51c902d63dda 183
Sissors 1:51c902d63dda 184 Vector3 IMUCalc::getGyroOffset( void )
Sissors 1:51c902d63dda 185 {
Sissors 1:51c902d63dda 186
Sissors 1:51c902d63dda 187 return gyroOffset;
Sissors 1:51c902d63dda 188 }
Sissors 1:51c902d63dda 189
Sissors 1:51c902d63dda 190 //The angle between the magnetic vector and the ground vector should be 90 degrees (0.5 pi). We calculate the angle, and rotate the magnetic vector while not changing the angle of
Sissors 1:51c902d63dda 191 //the original rotations vector, only we rotate far enough to make it 90 degrees.
Sissors 1:51c902d63dda 192 Vector3 IMUCalc::rotateMag(Vector3 magdat, Vector3 ground)
Sissors 1:51c902d63dda 193 {
Sissors 1:51c902d63dda 194 //Variables
Sissors 1:51c902d63dda 195 Vector3 retval;
Sissors 1:51c902d63dda 196 Vector3 rotVector;
Sissors 1:51c902d63dda 197 Matrix3x3 rotMatrix;
Sissors 1:51c902d63dda 198 float angle;
Sissors 1:51c902d63dda 199
Sissors 1:51c902d63dda 200 //Calculate the angle between magnetic and acceleration vector
Sissors 1:51c902d63dda 201 rotVector = angleBetween(magdat, ground);
Sissors 1:51c902d63dda 202 angle = rotVector.Length();
Sissors 1:51c902d63dda 203
Sissors 1:51c902d63dda 204 //Calculate how far we have to rotate magnetic vector
Sissors 1:51c902d63dda 205 angle = 0.5 * M_PI - angle;
Sissors 1:51c902d63dda 206
Sissors 1:51c902d63dda 207 //And do that
Sissors 1:51c902d63dda 208 retval = rotateVector(magdat, rotVector, -angle);
Sissors 1:51c902d63dda 209
Sissors 1:51c902d63dda 210 return retval;
Sissors 1:51c902d63dda 211 }
Sissors 1:51c902d63dda 212
Sissors 1:51c902d63dda 213
Sissors 1:51c902d63dda 214 // Vector calculations not included in GTMath
Sissors 1:51c902d63dda 215
Sissors 1:51c902d63dda 216 Vector3 IMUCalc::angleBetween(Vector3 vectorA, Vector3 vectorB)
Sissors 1:51c902d63dda 217 {
Sissors 1:51c902d63dda 218
Sissors 1:51c902d63dda 219
Sissors 1:51c902d63dda 220
Sissors 1:51c902d63dda 221 float angle;
Sissors 1:51c902d63dda 222 if ((vectorA.Length()==0)||(vectorB.Length()==0))
Sissors 1:51c902d63dda 223 angle=0;
Sissors 1:51c902d63dda 224 else
Sissors 1:51c902d63dda 225 angle = vectorA.Angle(vectorB);
Sissors 1:51c902d63dda 226 // if no noticable rotation is available return zero rotation
Sissors 1:51c902d63dda 227 // this way we avoid Cross product artifacts
Sissors 1:51c902d63dda 228 if( abs(angle) < 0.0001 ) return Vector3( 0, 0, 0);
Sissors 1:51c902d63dda 229 // in this case there are 2 lines on the same axis
Sissors 1:51c902d63dda 230 if(abs(angle-M_PI) < 0.0001) {
Sissors 1:51c902d63dda 231 //They are in opposite directions, rotate one by 90 degrees, that picks one of the infinite amount of rotation angles you get
Sissors 1:51c902d63dda 232 float temp = vectorB.z;
Sissors 1:51c902d63dda 233 vectorB.z=vectorB.y;
Sissors 1:51c902d63dda 234 vectorB.y=vectorB.x;
Sissors 1:51c902d63dda 235 vectorB.x=temp;
Sissors 1:51c902d63dda 236 }
Sissors 1:51c902d63dda 237 Vector3 axis = (vectorA.CrossP(vectorB));
Sissors 1:51c902d63dda 238 axis=axis.Normalize();
Sissors 1:51c902d63dda 239 axis *= (angle);
Sissors 1:51c902d63dda 240
Sissors 1:51c902d63dda 241 return axis;
Sissors 1:51c902d63dda 242 }
Sissors 1:51c902d63dda 243
Sissors 1:51c902d63dda 244
Sissors 1:51c902d63dda 245 Vector3 IMUCalc::rotateVector(Vector3 vector, Vector3 axis, float angle)
Sissors 1:51c902d63dda 246 {
Sissors 1:51c902d63dda 247 if (axis.Length()>0.0001) {
Sissors 1:51c902d63dda 248 Matrix3x3 rotMatrix = Matrix3x3::RotateAxis(axis, angle);
Sissors 1:51c902d63dda 249 return rotMatrix.Transform(vector);
Sissors 1:51c902d63dda 250 }
Sissors 1:51c902d63dda 251 return vector;
Sissors 1:51c902d63dda 252 }