Pages: [1]   Go Down
Author Topic: DCM Algorithm with 6 DOF Atomic  (Read 1506 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Newbie
*
Karma: 0
Posts: 8
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi Guys. Im trying to use the Sparkfun Atomic 6 DOF XBee board (SEN-09184) for some head tracking and attempting to get some sensible readings from it. I've looked around and it seems that the Discrete Cosine Matrix algorithm is the way forward. Im not sure if this is correct but looking at the various youtube clips and what not it seems like the best approach. I based this on the DCM Draft from DIY Drones.

The problem is, it doesnt work. The Gyros get messed up a little too much and the accelerometer correction doesnt seem to do a lot. I have a video of it here:


Here is the code for the relevant sections

Code:
void AtomicSerial::update(double dt) {
     
#ifdef DEBUG
      cout << "Accel Raw    : " << mAccel.x << "," <<  mAccel.y << "," << mAccel.z << endl;
      cout << "Gyros Raw    : " << mGyro.x << "," <<  mGyro.y << "," << mGyro.z << endl;     
#endif
     
      if (mCalSteps == -1) {
         // calibrate sensor measurments
         
         Vec3d tA = (mAccel - mABias) * mAGain;
         Vec3d tG = (mGyro - mGBias) * mGGain;
     
#ifdef DEBUG
         cout << "Accel Biased : " << tA.x << "," <<  tA.y << "," << tA.z << endl;
         cout << "Gyros Biased : " << tG.x << "," <<  tG.y << "," << tG.z << endl;     
#endif
     
         mGyroVector = Vec3d( (mGyro - mGBias) * mGGain);
         mGyroVector.y *=-1;
         mAccelVector = Vec3d( (mAccel - mABias) * mAGain);
         mAccelVector.y += 1.0;
         doDCM(dt);
      }
   }


Code:
void AtomicSerial::doDCM(double dt) {
     
      // Based on: http://diydrones.ning.com/profiles/blogs/dcm-imu-theory-first-draft
     
      // TODO - dt should affect things because we are assuming a fixed sample rate with DCM
      // which we may or may not have
#ifdef DEBUG
      cout << "DT: " << dt << endl;
#endif
     
      if (dt < 0.02 && dt > 0.0) {
         
         // Correction - PI Controller
         if (mDoCorrection){
         Vec3d gref = mAccelVector;
            //gref.y = 0;
            gref.normalize();
            Vec3d zcoli = Vec3d(mMatrix[4],mMatrix[5],mMatrix[6]);
            Vec3d rollpitch = zcoli.cross(gref);
           
            Vec3d pcorrect = rollpitch * mKp;
            mCorrection += (rollpitch * dt * mKi);
           
            mGyroVector += pcorrect + mCorrection;
         }
         
         Matrix44d dcm;
         double ndt = 1.0/dt;
         dcm[0] = 1.0;
         dcm[1] = mGyroVector.z * dt;
         dcm[2] = -mGyroVector.y * dt;
         dcm[3] = 0.0;
         dcm[4] = -mGyroVector.z * dt;
         dcm[5] = 1.0;
         dcm[6] = mGyroVector.x * dt;
         dcm[7] = 0.0;
         dcm[8] = mGyroVector.y * dt;
         dcm[9] = -mGyroVector.x * dt;
         dcm[10] = 1.0;
         dcm[11] = 0.0;
         dcm[12] = dcm[13] = dcm[14] = 0;
         dcm[15] = 1.0;
         
         // Renormalise
         
         mMatrix = mMatrix * dcm;
         
         Vec3d xcol = Vec3d(mMatrix[0], mMatrix[1], mMatrix[2]);
         Vec3d ycol = Vec3d(mMatrix[4], mMatrix[5], mMatrix[6]);
         
         double error = xcol.dot(ycol);
         
#ifdef DEBUG
         cout << "Error Value: " << error << endl;
#endif
         error /= 2.0;
         
         Vec3d xcolo = xcol - (ycol * error);
         Vec3d ycolo = ycol - (xcol * error);
               
         Vec3d zcolo = xcolo.cross(ycolo);
     
         Vec3d xcoln = xcolo * 0.5 * ( 3 - xcolo.dot(xcolo));
         Vec3d ycoln = ycolo * 0.5 * ( 3 - ycolo.dot(ycolo));
         Vec3d zcoln = zcolo * 0.5 * ( 3 - zcolo.dot(zcolo));
         
         mMatrix[0] = xcoln.x;
         mMatrix[1] = xcoln.y;
         mMatrix[2] = xcoln.z;
         
         mMatrix[4] = ycoln.x;
         mMatrix[5] = ycoln.y;
         mMatrix[6] = ycoln.z;
         
         mMatrix[8] = zcoln.x;
         mMatrix[9] = zcoln.y;
         mMatrix[10] = zcoln.z;

         
#ifdef DEBUG
         cout << "Matrix : " << mMatrix << endl;
#endif
      }
     
   }
Logged

Global Moderator
Netherlands
Offline Offline
Shannon Member
*****
Karma: 170
Posts: 12465
In theory there is no difference between theory and practice, however in practice there are many...
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I find it is quite impressive and inspiring..

don't have an answer to your question but I see floating point math which is never exact so maybe porting to integer only math could help...
another option is to filter out small movements

Rob

Logged

Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

0
Offline Offline
Newbie
*
Karma: 0
Posts: 8
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

You know, I'd totally forgotten filtering! How silly of me! smiley-razz I've gone back to a kalman filter (from a chap on this very forum) and added filtering of the YAW values and I've seen some improvement. I'll keep looking at DCM stuff though as I think it might be more robust. Cheers for the reminder there.
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 1
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I'm really impressed by your work, especially the graphic one ( with the 3D cube ) .

Is it possible for you to share your work? It may be usefull for me.

Thank you smiley

RD
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 2
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi onidaito,

Quite good work, but yea I can see it drifting. Hope by this time, you have corrected that problem.

I also want to develop code for quad. Have been reading all over the net from pass 2 months. During that i come across different algorithm, then I planned to use complementary filter (bcoz not much math involved as compared to kalman filter). Then I saw this DCM algorithm.

Can you suggest which algorithm for filtering is good to go ? I'm bit confused here.
Logged

Pages: [1]   Go Up
Jump to: