# DCM Algorithm with 6 DOF Atomic

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

``````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);
}
}
``````
``````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,mMatrix,mMatrix);
Vec3d rollpitch = zcoli.cross(gref);

Vec3d pcorrect = rollpitch * mKp;
mCorrection += (rollpitch * dt * mKi);

mGyroVector += pcorrect + mCorrection;
}

Matrix44d dcm;
double ndt = 1.0/dt;
dcm = 1.0;
dcm = mGyroVector.z * dt;
dcm = -mGyroVector.y * dt;
dcm = 0.0;
dcm = -mGyroVector.z * dt;
dcm = 1.0;
dcm = mGyroVector.x * dt;
dcm = 0.0;
dcm = mGyroVector.y * dt;
dcm = -mGyroVector.x * dt;
dcm = 1.0;
dcm = 0.0;
dcm = dcm = dcm = 0;
dcm = 1.0;

// Renormalise

mMatrix = mMatrix * dcm;

Vec3d xcol = Vec3d(mMatrix, mMatrix, mMatrix);
Vec3d ycol = Vec3d(mMatrix, mMatrix, mMatrix);

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 = xcoln.x;
mMatrix = xcoln.y;
mMatrix = xcoln.z;

mMatrix = ycoln.x;
mMatrix = ycoln.y;
mMatrix = ycoln.z;

mMatrix = zcoln.x;
mMatrix = zcoln.y;
mMatrix = zcoln.z;

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

}
``````

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

You know, I'd totally forgotten filtering! How silly of me! :P 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.

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

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.