Android で方向余弦行列を実装しようとしています。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をうまくプロットすると数字になります...しかし、計算のどこが間違っているのかわかりません...
誰か私に手を貸してくれませんか？
編集: コードを更新しました (符号エラーがありました)。今、私はALiが提案した後にDCMをテストしました（私の場合、2つの異なるコルス積は0またはほぼ0でなければならず、平方和は1またはほぼ1でなければなりません）
さて..openglを使用してDCMを適用しようとしました
gl.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);
アルゴリズムエラーを起こしたくないので、そのがらくたコーディングを許してください..値を1つずつ入れます:)。私のDCMは3x3ですが、そのビデオに続いて、openglは4x4を使用していると彼は言います（端にゼロがあり、最後のディアゴ要素に1があります）。
そして結果は本当に悪いです。携帯電話の動かし方によっては、小さな立方体があらゆる方向に非常に速く回転しているのを見ることができます....だから、今は間違いかわかりません
ありがとうございました