DCM.cpp

00001 #include"DCM.h"
00002 #include "HelperMath.h"
00003 
00004 float DCM::constrain(float x, float a, float b) 
00005 {
00006     float result = x;
00007     
00008     if (x < a) result = a;
00009     if (x > b) result = b;
00010  
00011     return result;  
00012 }
00013 
00014 DCM::DCM(){
00015 
00016   G_Dt=0.005;
00017   
00018   for(int i=0;i<3;i++){
00019   Accel_Vector[i]=0;
00020   Gyro_Vector[i]= 0;
00021   Omega_Vector[i]=0;
00022   Omega_P[i]=0;
00023   Omega_I[i]=0;
00024   Omega[i]= 0;
00025   errorRollPitch[i]=0;
00026   }
00027   
00028   DCM_Matrix[0][0]=1;
00029   DCM_Matrix[0][1]=0;
00030   DCM_Matrix[0][2]=0;
00031   DCM_Matrix[1][0]=0;
00032   DCM_Matrix[1][1]=1;
00033   DCM_Matrix[1][2]=0;
00034   DCM_Matrix[2][0]=0;
00035   DCM_Matrix[2][1]=0;
00036   DCM_Matrix[2][2]=1;
00037   
00038   Temporary_Matrix[0][0]=0;
00039   Temporary_Matrix[0][1]=0;
00040   Temporary_Matrix[0][2]=0;
00041   Temporary_Matrix[1][0]=0;
00042   Temporary_Matrix[1][1]=0;
00043   Temporary_Matrix[1][2]=0;
00044   Temporary_Matrix[2][0]=0;
00045   Temporary_Matrix[2][1]=0;
00046   Temporary_Matrix[2][2]=0;
00047   
00048   Update_Matrix[0][0]=0;
00049   Update_Matrix[0][1]=1;
00050   Update_Matrix[0][2]=2;
00051   Update_Matrix[1][0]=3;
00052   Update_Matrix[1][1]=4;
00053   Update_Matrix[1][2]=5;
00054   Update_Matrix[2][0]=6;
00055   Update_Matrix[2][1]=7;
00056   Update_Matrix[2][2]=8;
00057 }
00058 /**************************************************/
00059 void DCM::Normalize(void)
00060 {
00061   float error=0;
00062   float temporary[3][3];
00063   float renorm=0;
00064   bool problem=false;
00065   
00066   error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
00067 
00068   Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
00069   Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
00070   
00071   Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
00072   Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
00073   
00074   Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
00075   
00076   renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]); 
00077   if (renorm < 1.5625f && renorm > 0.64f) {
00078     renorm= .5 * (3-renorm);                                                 //eq.21
00079   } else if (renorm < 100.0f && renorm > 0.01f) {
00080     renorm= 1. / sqrt(renorm);
00081   }
00082       Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
00083   
00084   renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]); 
00085   if (renorm < 1.5625f && renorm > 0.64f) {
00086     renorm= .5 * (3-renorm);                                                 //eq.21
00087   } else if (renorm < 100.0f && renorm > 0.01f) {
00088     renorm= 1. / sqrt(renorm);  
00089   } else {
00090     problem = true;
00091   }
00092   Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
00093   
00094   renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]); 
00095   if (renorm < 1.5625f && renorm > 0.64f) {
00096     renorm= .5 * (3-renorm);                                                 //eq.21
00097   } else if (renorm < 100.0f && renorm > 0.01f) {
00098     renorm= 1. / sqrt(renorm);   
00099 
00100   } else {
00101     problem = true;  
00102   }
00103   Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
00104   
00105   if (problem) {                // Our solution is blowing up and we will force back to initial condition.  Hope we are not upside down!
00106       DCM_Matrix[0][0]= 1.0f;
00107       DCM_Matrix[0][1]= 0.0f;
00108       DCM_Matrix[0][2]= 0.0f;
00109       DCM_Matrix[1][0]= 0.0f;
00110       DCM_Matrix[1][1]= 1.0f;
00111       DCM_Matrix[1][2]= 0.0f;
00112       DCM_Matrix[2][0]= 0.0f;
00113       DCM_Matrix[2][1]= 0.0f;
00114       DCM_Matrix[2][2]= 1.0f;
00115       problem = false;  
00116   }
00117 }
00118 
00119 /**************************************************/
00120 void DCM::Drift_correction(void)
00121 {
00122   //Compensation the Roll, Pitch and Yaw drift. 
00123   static float Scaled_Omega_I[3];
00124   float Accel_magnitude;
00125   float Accel_weight;
00126   
00127   //*****Roll and Pitch***************
00128 
00129   // Calculate the magnitude of the accelerometer vector
00130   Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
00131   Accel_magnitude /=GRAVITY; // Scale to gravity.
00132   // Dynamic weighting of accelerometer info (reliability filter)
00133   // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
00134   Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //  
00135   
00136   
00137   Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
00138   Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
00139   
00140   Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
00141   Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);     
00142   
00143 }
00144 
00145 void DCM::Matrix_update(float GyroX,float GyroY,float GyroZ, float AccX, float AccY, float AccZ)
00146 {
00147   Gyro_Vector[0]=GyroX; //gyro x roll
00148   Gyro_Vector[1]=GyroY; //gyro y pitch
00149   Gyro_Vector[2]=GyroZ; //gyro Z yaw
00150   
00151   Accel_Vector[0]=AccX*GRAVITY; // acc x
00152   Accel_Vector[1]=AccY*GRAVITY; // acc y
00153   Accel_Vector[2]=AccZ*GRAVITY; // acc z
00154   
00155   Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
00156   Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
00157        
00158   Update_Matrix[0][0]=0;
00159   Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
00160   Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
00161   Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
00162   Update_Matrix[1][1]=0;
00163   Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
00164   Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
00165   Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
00166   Update_Matrix[2][2]=0;
00167 
00168   Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
00169 
00170   for(int x=0; x<3; x++) //Matrix Addition (update)
00171   {
00172     for(int y=0; y<3; y++)
00173     {
00174       DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
00175     } 
00176   }
00177 }
00178 
00179 void DCM::Euler_angles(void)
00180 {
00181     pitch = -asin(DCM_Matrix[2][0]);
00182     roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
00183     yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
00184 }
00185 
00186 void DCM::Update_DCM(float dt,float a, float b, float c, float d, float e, float f){
00187          
00188          G_Dt = dt;
00189          
00190          Matrix_update(a,b,c,d,e,f); 
00191 
00192          Normalize();
00193 
00194          Drift_correction();
00195    
00196          Euler_angles();
00197 }