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.

Diff: IMUCalc.cpp

Revision:
1:51c902d63dda
Parent:
0:4dc7e56179ff
--- a/IMUCalc.cpp	Wed Jul 11 12:26:44 2012 +0000
+++ b/IMUCalc.cpp	Wed Jan 22 21:19:18 2014 +0000
@@ -1,22 +1,252 @@
 #include "IMUCalc.h"
 
-Vector3 IMUCalc::angleBetween(Vector3 vectorA, Vector3 vectorB){
-    float angle; 
-	
-	angle = vectorA.Angle(vectorB); 
-	// if no noticable rotation is available return zero rotation
-	// this way we avoid Cross product artifacts 
-	if( abs(angle) < 0.0001 ) return Vector3( 0, 0, 0); 
-	// in this case there are 2 lines on the same axis 
-	if(abs(angle-M_PI) < 0.001) {
-	    //They are in opposite directions, rotate one by 90 degrees, that picks one of the infinite amount of rotation angles you get
-	    float temp = vectorB.z;
-  	    vectorB.z=vectorB.y;
-  	    vectorB.y=vectorB.x;
-  	    vectorB.x=temp;
-	}
- 	Vector3 axis = (vectorA.CrossP(vectorB));
-	axis=axis.Normalize();
-	axis *= (angle);
-	return axis; 
-}
\ No newline at end of file
+IMUCalc::IMUCalc(void)
+{
+    heading.x=1;
+    heading.y=0;
+    heading.z=0;
+
+    top.x=0;
+    top.y=0;
+    top.z=1;
+
+    gyroOffset.x=0;
+    gyroOffset.y=0;
+    gyroOffset.z=0;
+
+    absGain=0.01;
+    initialRun=true;
+
+
+}
+
+
+void IMUCalc::runCalc(float *accdat, float *gyrodat, float *magdat, float timestep)
+{
+
+
+    //Variables
+    Vector3 accdata;
+    Vector3 gyrodata;
+    Vector3 magdata;
+
+    Vector3 heading_abs;
+    Vector3 top_abs;
+
+    //Change data to vector3
+    for (int i = 0; i<3; i++) {
+        accdata.af[i]=accdat[i];
+        gyrodata.af[i]=gyrodat[i];
+        magdata.af[i]=magdat[i];
+    }
+
+    gyrodata = gyrodata-gyroOffset; 
+
+    heading = rotateVector(heading, gyrodata, -gyrodata.Length()*timestep);
+    top = rotateVector(top, gyrodata, -gyrodata.Length()*timestep);
+
+
+
+    
+
+    //Rotate the magnetic data to be in plain with the earth
+    heading_abs  = -1 * rotateMag(magdata, accdata);
+    top_abs = -1 * accdata;
+
+    heading_abs = heading_abs.Normalize();
+    top_abs = top_abs.Normalize();
+
+    //Calculate offset
+    Vector3 currentGyroOffset, weightTop, weightHeading, tempVector;
+
+
+
+    //make tempvector in X direction, do crossproduct, calculate length of result
+    tempVector.x = 1;
+    tempVector.y=0;
+    tempVector.z=0;
+    weightTop.x = top.CrossP(tempVector).Length();
+    weightHeading.x = heading.CrossP(tempVector).Length();
+
+    tempVector.x = 0;
+    tempVector.y=1;
+    tempVector.z=0;
+    weightTop.y = top.CrossP(tempVector).Length();
+    weightHeading.y = heading.CrossP(tempVector).Length();
+
+    tempVector.x = 0;
+    tempVector.y=0;
+    tempVector.z=1;
+    weightTop.z = top.CrossP(tempVector).Length();
+    weightHeading.z = heading.CrossP(tempVector).Length();
+
+
+    //Use weightfactors, then divide by their sum
+    currentGyroOffset = weightTop * angleBetween(top_abs, top) + weightHeading * angleBetween(heading_abs, heading);
+    currentGyroOffset = currentGyroOffset / (weightTop + weightHeading);
+
+
+    if (currentGyroOffset.x > timestep * 0.1)
+        currentGyroOffset.x = timestep * 0.1;
+    if (currentGyroOffset.x < -timestep * 0.1)
+        currentGyroOffset.x = -timestep * 0.1;
+
+    if (currentGyroOffset.y > timestep * 0.1)
+        currentGyroOffset.y = timestep * 0.1;
+    if (currentGyroOffset.y < -timestep * 0.1)
+        currentGyroOffset.y = -timestep * 0.1;
+
+    if (currentGyroOffset.z > timestep * 0.1)
+        currentGyroOffset.z = timestep * 0.1;
+    if (currentGyroOffset.z < -timestep * 0.1)
+        currentGyroOffset.z = -timestep * 0.1;
+
+    gyroOffset -= 0.01 * currentGyroOffset;
+
+
+    //Take average value of heading/heading_abs with different gains to get current estimate
+    if (initialRun) {
+        heading = heading_abs;
+        top = top_abs;
+        gyroOffset *= 0;
+        initialRun=false;
+    } else {
+        heading = heading*(1-absGain) + heading_abs*absGain;
+        top = top * (1-absGain) + top_abs * absGain;
+    }
+}
+
+//Calculates the yaw
+float IMUCalc::getYaw( void )
+{
+    //First normalize yaw vector, then calculate the heading
+    Vector2 yawVector(heading.x, heading.y);
+
+    if (yawVector.Length()>0) {
+        yawVector = yawVector.Normalize();
+
+        //check Quadrant
+        if (yawVector.y<0) {
+            if (yawVector.x < 0)
+                return -M_PI - asin(yawVector.y);
+            else
+                return asin(yawVector.y);
+        } else {
+            if (yawVector.x < 0)
+                return M_PI - asin(yawVector.y);
+            else
+                return asin(yawVector.y);
+        }
+    } else
+        return 0;
+}
+
+//Calculates the pitch
+float IMUCalc::getPitch( void )
+{
+    //First normalize pitch vector, then calculate the pitch
+    Vector2 pitchVector(top.x, top.z);
+
+    if (pitchVector.Length()>0) {
+        pitchVector = pitchVector.Normalize();
+
+        //if the top is at the bottom, invert the vector
+        if (pitchVector.y<0)
+            pitchVector = -pitchVector;
+        return asin(pitchVector.x);
+
+    } else
+        return 0;
+}
+
+//Calculates the roll
+float IMUCalc::getRoll( void )
+{
+    //First normalize yaw vector, then calculate the heading
+    Vector2 rollVector(top.y, top.z);
+
+    if (rollVector.Length()>0) {
+        rollVector = rollVector.Normalize();
+
+        //check Quadrant
+        if (rollVector.y<0) {
+            if (rollVector.x < 0)
+                return -M_PI - asin(rollVector.x);
+            else
+                return M_PI - asin(rollVector.x);
+        } else {
+            return asin(rollVector.x);
+        }
+    } else
+        return 0;
+}
+
+Vector3 IMUCalc::getGyroOffset( void )
+{
+
+    return gyroOffset;
+}
+
+//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
+//the original rotations vector, only we rotate far enough to make it 90 degrees.
+Vector3 IMUCalc::rotateMag(Vector3 magdat, Vector3 ground)
+{
+    //Variables
+    Vector3 retval;
+    Vector3 rotVector;
+    Matrix3x3 rotMatrix;
+    float angle;
+
+    //Calculate the angle between magnetic and acceleration vector
+    rotVector = angleBetween(magdat, ground);
+    angle = rotVector.Length();
+
+    //Calculate how far we have to rotate magnetic vector
+    angle = 0.5 * M_PI - angle;
+
+    //And do that
+    retval = rotateVector(magdat, rotVector, -angle);
+
+    return retval;
+}
+
+
+// Vector calculations not included in GTMath
+
+Vector3 IMUCalc::angleBetween(Vector3 vectorA, Vector3 vectorB)
+{
+
+    
+    
+    float angle;
+    if ((vectorA.Length()==0)||(vectorB.Length()==0))
+        angle=0;
+    else
+        angle = vectorA.Angle(vectorB);
+    // if no noticable rotation is available return zero rotation
+    // this way we avoid Cross product artifacts
+    if( abs(angle) < 0.0001 ) return Vector3( 0, 0, 0);
+    // in this case there are 2 lines on the same axis
+    if(abs(angle-M_PI) < 0.0001) {
+        //They are in opposite directions, rotate one by 90 degrees, that picks one of the infinite amount of rotation angles you get
+        float temp = vectorB.z;
+        vectorB.z=vectorB.y;
+        vectorB.y=vectorB.x;
+        vectorB.x=temp;
+    }
+    Vector3 axis = (vectorA.CrossP(vectorB));
+    axis=axis.Normalize();
+    axis *= (angle);
+
+    return axis;
+}
+
+
+Vector3 IMUCalc::rotateVector(Vector3 vector, Vector3 axis, float angle)
+{
+    if (axis.Length()>0.0001) {
+        Matrix3x3 rotMatrix = Matrix3x3::RotateAxis(axis, angle);
+        return rotMatrix.Transform(vector);
+    }
+    return vector;
+}