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:
https://www.youtube.com/watch?v=Ql68yM8jRvI
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[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 } }
Replies
Very nice work so far! I really like the cube you built. And great start with using DCM! I'd still like to go that route eventually, but for now I've got a complimentary filter working very well so far (my page: INS Project ), and seems very accurate. Just need a GPS to lock the yaw. It wont let you do any aerobatics, but I don't need to.
Good luck getting that straightened out! Keep us posted!