Direction Cosine Matrix implementation

I'm trying to implement the Direction Cosine Matrix on Android. I've read some theory about DCM and I finally coded a draft

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);

    }

So every time the Gyro get updated I compute dT and I call imuUpdate

I achieved to do this code by mixing the theory http://gentlenav.googlecode.com/files/DCMDraft2.pdf and some code found here http://code.google.com/p/picquadcontroller/source/browse/trunk/imu.h?r=7 (thanks to them by the way)

But I can make this DCM work... if I try to extract the pitch for example

pitch = Math.asin(-dcm_matrix.get(0, 2));

the result it's completly wrong (it's giving me some random angles...) and if i plot the DCM well it's numbers... But I can't figure out where in my calculation I'm wrong...

Can somebody give me a hand on this please?

EDIT: I have updated my code (i had a sign error). Now I tested my DCM after what ALi suggested (corss product on 2 diferent must be 0 or almost 0 and the square sum must be 1 or almost 1 on my case)

Well.. I tried to apply my DCM using 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); 

I beg pardon for that crap coding I don't wanted to do algorithme err开发者_开发百科or so.. i put the value one by one :). so my DCM is 3x3 but following that video he says that opengl use a 4x4 (with zero on the edge and a ONE on the last diago element) thats why all thos stuff on thos code I just showed.

And the result is really bad. depending on how I move my phone I can see a little cube rotating really fast in every direction.... SO I don't know is the mistake now

Thank you