我正在尝试在Android上实现Direction Cosine Matrix。我已经阅读了一些关于DCM的理论，最后我编写了草稿
public void imuUpdate(float dT, float gyro[], float accel[]) {
int i;
Tuple3d Kacc = new Tuple3d();
// Acc can estimate global K vector(zenith) measured in body's
// coordinate systems (the reverse of gravitation vector)
Kacc.x = -accel[0];
Kacc.y = -accel[1];
Kacc.z = -accel[2];
// vector3d_normalize(Kacc);
Kacc.normalize();
// calculate correction vector to bring dcmGyro's K vector closer to Acc
// vector (K vector according to accelerometer)
Tuple3d wA = new Tuple3d();
Tuple3d dcmGyroX = new Tuple3d();
dcmGyroX.x = dcm_matrix.get(0, 2);
dcmGyroX.y = dcm_matrix.get(1, 2);
dcmGyroX.z = dcm_matrix.get(2, 2);
wA.cross(dcmGyroX, Kacc);
// calculate correction vector to bring dcmGyro's I vector closer to Mag
// vector (I vector according to magnetometer)
Tuple3d Imag = new Tuple3d();
Tuple3d wM = new Tuple3d();
// in the absense of magnetometer let's assume North vector (I) is
// always in XZ plane of the device (y coordinate is 0)
Imag.x = Math.sqrt(1 - Math.pow(dcm_matrix.get(0, 2), 2));
Imag.y = 0;
Imag.z = dcm_matrix.get(0, 2);
dcmGyroX.x = dcm_matrix.get(0, 0);
dcmGyroX.y = dcm_matrix.get(1, 0);
dcmGyroX.z = dcm_matrix.get(2, 0);
wM.cross(dcmGyroX, Imag);
// ---------------
// dcmGyro
// ---------------
Tuple3d w = new Tuple3d();
// Acc can estimate global K vector(zenith) measured in body's
// coordinate systems (the reverse of gravitation vector)
w.x = -gyro[0];
w.y = -gyro[1];
w.z = -gyro[2];
float wA2[] = new float[3];
wA2[0] = (float) wA.x;
wA2[1] = (float) wA.y;
wA2[2] = (float) wA.z;
w.x *= dT;
w.x = (w.x + ACC_WEIGHT * wA.x + MAG_WEIGHT * wM.x)
/ (1.0 + ACC_WEIGHT + MAG_WEIGHT);
w.y *= dT;
w.y = (w.y + ACC_WEIGHT * wA.y + MAG_WEIGHT * wM.y)
/ (1.0 + ACC_WEIGHT + MAG_WEIGHT);
w.z *= dT;
w.z = (w.z + ACC_WEIGHT * wA.z + MAG_WEIGHT * wM.z)
/ (1.0 + ACC_WEIGHT + MAG_WEIGHT);
dcm_rotate(w);
}
// rotate DCM matrix by a small rotation given by angular rotation vector w
// see
public void dcm_rotate(Tuple3d w) {
// float W[3][3];
// creates equivalent skew symetric matrix plus identity matrix
// vector3d_skew_plus_identity((float*)w,(float*)W);
// float dcmTmp[3][3];
// matrix_multiply(3,3,3,(float*)W,(float*)dcm,(float*)dcmTmp);
Tuple3d dcmXX = new Tuple3d();
Tuple3d dcmXY = new Tuple3d();
Tuple3d dcmXZ = new Tuple3d();
dcmXX.x = dcm_matrix.get(0, 0);
dcmXX.y = dcm_matrix.get(1, 0);
dcmXX.z = dcm_matrix.get(2, 0);
dcmXY.x = dcm_matrix.get(0, 1);
dcmXY.y = dcm_matrix.get(1, 1);
dcmXY.z = dcm_matrix.get(2, 1);
dcmXZ.x = dcm_matrix.get(0, 2);
dcmXZ.y = dcm_matrix.get(1, 2);
dcmXZ.z = dcm_matrix.get(2, 2);
Tuple3d dR = new Tuple3d();
dR.cross(w, dcmXX);
dcmXX.add(dR);
// System.out.println("WA: " + w.x + " " + w.y + " " + w.z);
dR.cross(w, dcmXY);
dcmXY.add(dR);
dR.cross(w, dcmXZ);
dcmXZ.add(dR);
// Orthonormalize
Tuple3d dcmXXtmp = dcmXX;
Tuple3d dcmXYtmp = dcmXY;
Tuple3d dcmXZtmp = dcmXZ;
// err = X . Y , X = X - err/2 * Y , Y = Y - err/2 * X (DCMDraft2
// Eqn.19)
// float err = vector3d_dot((float*)(dcm[0]),(float*)(dcm[1]));
float err = (float) dcmXX.Dot(dcmXY);
dcmXXtmp.scaleAdd(-err / 2, dcmXY);
dcmXYtmp.scaleAdd(-err / 2, dcmXX);
dcmXZtmp.cross(dcmXXtmp, dcmXYtmp);
dcmXXtmp.normalize();
dcmXYtmp.normalize();
dcmXZtmp.normalize();
dcm_matrix.set(0, 0, (float) dcmXXtmp.x);
dcm_matrix.set(1, 0, (float) dcmXXtmp.y);
dcm_matrix.set(2, 0, (float) dcmXXtmp.z);
dcm_matrix.set(0, 1, (float) dcmXYtmp.x);
dcm_matrix.set(1, 1, (float) dcmXYtmp.y);
dcm_matrix.set(2, 1, (float) dcmXYtmp.z);
dcm_matrix.set(0, 2, (float) dcmXZtmp.x);
dcm_matrix.set(1, 2, (float) dcmXZtmp.y);
dcm_matrix.set(2, 2, (float) dcmXZtmp.z);
}
因此，每次陀螺仪更新时，我都会计算dT，我会调用imuUpdate
我通过混合理论http://gentlenav.googlecode.com/files/DCMDraft2.pdf和这里找到的一些代码http://code.google.com/p/picquadcontroller/source/browse/trunk/imu.h?r=7来实现这个代码（顺便说一句，感谢他们）
但我可以让这个DCM工作......如果我尝试提取音高例如
pitch = Math.asin(-dcm_matrix.get(0, 2));
结果它是完全错误的（它给了我一些随机的角度......）如果我很好地绘制了DCM的数字......但是我无法弄清楚在我的计算中我错了... < / p>
有人可以帮我解决这个问题吗？
修改 我更新了我的代码（我有一个标志错误）。现在我在ALi建议之后测试我的DCM（2个不同的corss产品必须是0或几乎为0，并且在我的情况下平方和必须是1或几乎为1）
嗯..我尝试使用opengl
应用我的DCMgl.glTranslatef(0, 0, -10.0f);
// setup code
float vertexData[] = new float[16];
for (int i=0;i<16;i++)
{
vertexData[i] = 0;
}
vertexData[15] = 1;
vertexData[0] = this.main.dcm_matrix.mMat[0];
vertexData[1] = this.main.dcm_matrix.mMat[1];
vertexData[2] = this.main.dcm_matrix.mMat[2];
vertexData[3] = this.main.dcm_matrix.mMat[3];
vertexData[5] = this.main.dcm_matrix.mMat[4];
vertexData[6] = this.main.dcm_matrix.mMat[5];
vertexData[8] = this.main.dcm_matrix.mMat[6];
vertexData[9] = this.main.dcm_matrix.mMat[7];
vertexData[10] = this.main.dcm_matrix.mMat[8];
gl.glMultMatrixf(vertexData,0);
我请原谅那个垃圾编码，我不想做算法错误所以..我把价值逐一放:)。 所以我的DCM是3x3，但是在那个视频之后他说opengl使用4x4（边缘为零，最后一个diago元素为1）这就是为什么所有的东西都在我刚刚显示的代码上。
结果非常糟糕。根据我移动手机的方式，我可以看到一个小方块在各个方向旋转得非常快......所以我现在不知道是错误
谢谢