Я пытаюсь реализовать Направление Косинус Матрица на 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, это числа ... Но я не могу понять, где в моемрасчет я ошибаюсь ...
Может кто-нибудь помочь мне, пожалуйста?
РЕДАКТИРОВАТЬ: Я обновил свой код (у меня была ошибка знака),Теперь я протестировал свой DCM после того, что предложил ALi (в моем случае произведение corss на 2 отличается от 0 или почти 0, а квадратная сумма должна быть 1 или почти 1)
Хорошо .. Я пытался применить мой DCMиспользуя opengl
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);
Прошу прощения за это дерьмовое кодирование, я не хотел делать ошибку алгоритма, так что ... я поставил значение по одному :).так что мой DCM - 3x3, но после этого видео он говорит, что opengl использует 4x4 (с нулем на краю и ONE на последнем элементе diago), поэтому все эти вещи в коде, который я только что показал.
Ирезультат действительно плохой.в зависимости от того, как я двигаю свой телефон, я вижу маленький кубик, вращающийся очень быстро во всех направлениях .... ТАК, я не знаю, сейчас ошибка
Спасибо