IMUCalc.cpp

00001 #include "IMUCalc.h"
00002 
00003 IMUCalc::IMUCalc(void)
00004 {
00005     heading.x=1;
00006     heading.y=0;
00007     heading.z=0;
00008 
00009     top.x=0;
00010     top.y=0;
00011     top.z=1;
00012 
00013     gyroOffset.x=0;
00014     gyroOffset.y=0;
00015     gyroOffset.z=0;
00016 
00017     absGain=0.01;
00018     initialRun=true;
00019 
00020 
00021 }
00022 
00023 
00024 void IMUCalc::runCalc(float *accdat, float *gyrodat, float *magdat, float timestep)
00025 {
00026 
00027 
00028     //Variables
00029     Vector3 accdata;
00030     Vector3 gyrodata;
00031     Vector3 magdata;
00032 
00033     Vector3 heading_abs;
00034     Vector3 top_abs;
00035 
00036     //Change data to vector3
00037     for (int i = 0; i<3; i++) {
00038         accdata.af[i]=accdat[i];
00039         gyrodata.af[i]=gyrodat[i];
00040         magdata.af[i]=magdat[i];
00041     }
00042 
00043     gyrodata = gyrodata-gyroOffset; 
00044 
00045     heading = rotateVector(heading, gyrodata, -gyrodata.Length()*timestep);
00046     top = rotateVector(top, gyrodata, -gyrodata.Length()*timestep);
00047 
00048 
00049 
00050     
00051 
00052     //Rotate the magnetic data to be in plain with the earth
00053     heading_abs  = -1 * rotateMag(magdata, accdata);
00054     top_abs = -1 * accdata;
00055 
00056     heading_abs = heading_abs.Normalize();
00057     top_abs = top_abs.Normalize();
00058 
00059     //Calculate offset
00060     Vector3 currentGyroOffset, weightTop, weightHeading, tempVector;
00061 
00062 
00063 
00064     //make tempvector in X direction, do crossproduct, calculate length of result
00065     tempVector.x = 1;
00066     tempVector.y=0;
00067     tempVector.z=0;
00068     weightTop.x = top.CrossP(tempVector).Length();
00069     weightHeading.x = heading.CrossP(tempVector).Length();
00070 
00071     tempVector.x = 0;
00072     tempVector.y=1;
00073     tempVector.z=0;
00074     weightTop.y = top.CrossP(tempVector).Length();
00075     weightHeading.y = heading.CrossP(tempVector).Length();
00076 
00077     tempVector.x = 0;
00078     tempVector.y=0;
00079     tempVector.z=1;
00080     weightTop.z = top.CrossP(tempVector).Length();
00081     weightHeading.z = heading.CrossP(tempVector).Length();
00082 
00083 
00084     //Use weightfactors, then divide by their sum
00085     currentGyroOffset = weightTop * angleBetween(top_abs, top) + weightHeading * angleBetween(heading_abs, heading);
00086     currentGyroOffset = currentGyroOffset / (weightTop + weightHeading);
00087 
00088 
00089     if (currentGyroOffset.x > timestep * 0.1)
00090         currentGyroOffset.x = timestep * 0.1;
00091     if (currentGyroOffset.x < -timestep * 0.1)
00092         currentGyroOffset.x = -timestep * 0.1;
00093 
00094     if (currentGyroOffset.y > timestep * 0.1)
00095         currentGyroOffset.y = timestep * 0.1;
00096     if (currentGyroOffset.y < -timestep * 0.1)
00097         currentGyroOffset.y = -timestep * 0.1;
00098 
00099     if (currentGyroOffset.z > timestep * 0.1)
00100         currentGyroOffset.z = timestep * 0.1;
00101     if (currentGyroOffset.z < -timestep * 0.1)
00102         currentGyroOffset.z = -timestep * 0.1;
00103 
00104     gyroOffset -= 0.01 * currentGyroOffset;
00105 
00106 
00107     //Take average value of heading/heading_abs with different gains to get current estimate
00108     if (initialRun) {
00109         heading = heading_abs;
00110         top = top_abs;
00111         gyroOffset *= 0;
00112         initialRun=false;
00113     } else {
00114         heading = heading*(1-absGain) + heading_abs*absGain;
00115         top = top * (1-absGain) + top_abs * absGain;
00116     }
00117 }
00118 
00119 //Calculates the yaw
00120 float IMUCalc::getYaw( void )
00121 {
00122     //First normalize yaw vector, then calculate the heading
00123     Vector2 yawVector(heading.x, heading.y);
00124 
00125     if (yawVector.Length()>0) {
00126         yawVector = yawVector.Normalize();
00127 
00128         //check Quadrant
00129         if (yawVector.y<0) {
00130             if (yawVector.x < 0)
00131                 return -M_PI - asin(yawVector.y);
00132             else
00133                 return asin(yawVector.y);
00134         } else {
00135             if (yawVector.x < 0)
00136                 return M_PI - asin(yawVector.y);
00137             else
00138                 return asin(yawVector.y);
00139         }
00140     } else
00141         return 0;
00142 }
00143 
00144 //Calculates the pitch
00145 float IMUCalc::getPitch( void )
00146 {
00147     //First normalize pitch vector, then calculate the pitch
00148     Vector2 pitchVector(top.x, top.z);
00149 
00150     if (pitchVector.Length()>0) {
00151         pitchVector = pitchVector.Normalize();
00152 
00153         //if the top is at the bottom, invert the vector
00154         if (pitchVector.y<0)
00155             pitchVector = -pitchVector;
00156         return asin(pitchVector.x);
00157 
00158     } else
00159         return 0;
00160 }
00161 
00162 //Calculates the roll
00163 float IMUCalc::getRoll( void )
00164 {
00165     //First normalize yaw vector, then calculate the heading
00166     Vector2 rollVector(top.y, top.z);
00167 
00168     if (rollVector.Length()>0) {
00169         rollVector = rollVector.Normalize();
00170 
00171         //check Quadrant
00172         if (rollVector.y<0) {
00173             if (rollVector.x < 0)
00174                 return -M_PI - asin(rollVector.x);
00175             else
00176                 return M_PI - asin(rollVector.x);
00177         } else {
00178             return asin(rollVector.x);
00179         }
00180     } else
00181         return 0;
00182 }
00183 
00184 Vector3 IMUCalc::getGyroOffset( void )
00185 {
00186 
00187     return gyroOffset;
00188 }
00189 
00190 //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
00191 //the original rotations vector, only we rotate far enough to make it 90 degrees.
00192 Vector3 IMUCalc::rotateMag(Vector3 magdat, Vector3 ground)
00193 {
00194     //Variables
00195     Vector3 retval;
00196     Vector3 rotVector;
00197     Matrix3x3 rotMatrix;
00198     float angle;
00199 
00200     //Calculate the angle between magnetic and acceleration vector
00201     rotVector = angleBetween(magdat, ground);
00202     angle = rotVector.Length();
00203 
00204     //Calculate how far we have to rotate magnetic vector
00205     angle = 0.5 * M_PI - angle;
00206 
00207     //And do that
00208     retval = rotateVector(magdat, rotVector, -angle);
00209 
00210     return retval;
00211 }
00212 
00213 
00214 // Vector calculations not included in GTMath
00215 
00216 Vector3 IMUCalc::angleBetween(Vector3 vectorA, Vector3 vectorB)
00217 {
00218 
00219     
00220     
00221     float angle;
00222     if ((vectorA.Length()==0)||(vectorB.Length()==0))
00223         angle=0;
00224     else
00225         angle = vectorA.Angle(vectorB);
00226     // if no noticable rotation is available return zero rotation
00227     // this way we avoid Cross product artifacts
00228     if( abs(angle) < 0.0001 ) return Vector3( 0, 0, 0);
00229     // in this case there are 2 lines on the same axis
00230     if(abs(angle-M_PI) < 0.0001) {
00231         //They are in opposite directions, rotate one by 90 degrees, that picks one of the infinite amount of rotation angles you get
00232         float temp = vectorB.z;
00233         vectorB.z=vectorB.y;
00234         vectorB.y=vectorB.x;
00235         vectorB.x=temp;
00236     }
00237     Vector3 axis = (vectorA.CrossP(vectorB));
00238     axis=axis.Normalize();
00239     axis *= (angle);
00240 
00241     return axis;
00242 }
00243 
00244 
00245 Vector3 IMUCalc::rotateVector(Vector3 vector, Vector3 axis, float angle)
00246 {
00247     if (axis.Length()>0.0001) {
00248         Matrix3x3 rotMatrix = Matrix3x3::RotateAxis(axis, angle);
00249         return rotMatrix.Transform(vector);
00250     }
00251     return vector;
00252 }