3

我正在尝试在 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 个不同的 corss 乘积必须为 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);

请原谅我不想做算法错误的垃圾编码，所以..我一一输入值:)。所以我的 DCM 是 3x3，但是在该视频之后，他说 opengl 使用 4x4（边缘为零，最后一个 diago 元素为 ONE），这就是为什么我刚刚展示的代码中的所有内容。

结果真的很糟糕。取决于我如何移动手机，我可以看到一个小立方体向各个方向快速旋转......所以我现在不知道是错误的

谢谢

4

0 回答 0