Go Down

### Topic: DCM Algorithm with 6 DOF Atomic (Read 2562 times)previous topic - next topic

#### onidaito

##### Jun 15, 2011, 11:46 am
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: [Select]
`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: [Select]
`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      }         }`

#### robtillaart

#1
##### Jun 15, 2011, 11:53 am
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

Rob Tillaart

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

#### onidaito

#2
##### Jun 16, 2011, 02:22 pm
You know, I'd totally forgotten filtering! How silly of me! 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.

#### rdoume

#3
##### Aug 16, 2011, 02:39 pm
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

RD

#### gaurav_sharma132

#4
##### May 04, 2012, 11:24 am
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.

Go Up