互补滤波器陀螺加速度计

Question

I'm using complementary filter for gyro and accelerometer just for azimuth... I've got it from this site : http://www.thousand-thoughts.com/2012/03/android-sensor-fusion-tutorial/1/

the core of filter is :

         /*
         * Fix for 179° <--> -179° transition problem:
         * Check whether one of the two orientation angles (gyro or accMag) is negative while the other one is positive.
         * If so, add 360° (2 * math.PI) to the negative value, perform the sensor fusion, and remove the 360° from the result
         * if it is greater than 180°. This stabilizes the output in positive-to-negative-transition cases.
         */

        // azimuth
        if (gyroOrientation[0] < -0.5 * Math.PI && accMagOrientation[0] > 0.0) {
            fusedOrientation[0] = (float) (FILTER_COEFFICIENT * (gyroOrientation[0] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[0]);
            fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0;
            Log.d("test","gyro Is Negative");
        }
        else if (accMagOrientation[0] < -0.5 * Math.PI && gyroOrientation[0] > 0.0) {
            fusedOrientation[0] = (float) (FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * (accMagOrientation[0] + 2.0 * Math.PI));
            fusedOrientation[0] -= (fusedOrientation[0] > Math.PI)? 2.0 * Math.PI : 0;
            Log.d("test","accel Is Negative");

        }
        else {
            fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * accMagOrientation[0];
        }
        gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
        System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);

I want to compare this with Real gyro data that has drift ... for this, I've used gyroOreintationReal... and add some codes to save gyroOreintation, like this:

 if(initState) {
        float[] initMatrix = new float[9];
        initMatrix = getRotationMatrixFromOrientation(accMagOrientation);
        float[] test = new float[3];
        SensorManager.getOrientation(initMatrix, test);
        gyroMatrix = matrixMultiplication(gyroMatrix, initMatrix);
        gyroMatrixReal = matrixMultiplication(gyroMatrixReal, initMatrix);

        initState = false;
    }

    // copy the new gyro values into the gyro array
    // convert the raw gyro data into a rotation vector
    float[] deltaVector = new float[4];
    float[] deltaVectorReal = new float[4];

    if(timestamp != 0) {
        final float dT = (event.timestamp - timestamp) * NS2S;


    System.arraycopy(event.values, 0, gyro, 0, 3);
    System.arraycopy(event.values, 0, gyroReal, 0, 3);


    getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);
    getRotationVectorFromGyro(gyroReal, deltaVectorReal, dT / 2.0f);

    }

    // measurement done, save current time for next interval
    timestamp = event.timestamp;

    // convert rotation vector into rotation matrix
    float[] deltaMatrix = new float[9];
    float[] deltaMatrixReal = new float[9];

    SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);
    SensorManager.getRotationMatrixFromVector(deltaMatrixReal, deltaVectorReal);


    // apply the new rotation interval on the gyroscope based rotation matrix
    gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);
    gyroMatrixReal = matrixMultiplication(gyroMatrixReal, deltaMatrixReal);


    // get the gyroscope based orientation from the rotation matrix
    SensorManager.getOrientation(gyroMatrix, gyroOrientation);
    SensorManager.getOrientation(gyroMatrixReal, gyroOrientationReal);     

and I saved the result and ploted them with matlab ... but the plot shows that gyro's orientation is negative ... but fusedOreientation is less than +150 and accel orientation is a little more than +150 ...

how can I fix the problem?? I add some codes to core of complementary filter :

//RealGyro
       if (gyroOrientationReal[0] < -0.5 * Math.PI && accMagOrientation[0] > 0.0) {
            gyroOrientationReal[0] =  (float) (gyroOrientation[0] + 2.0 * Math.PI);
            gyroOrientationReal[0] -= (gyroOrientationReal[0] > Math.PI) ? 2.0 * Math.PI : 0;
       }        

it's Ok sometimes But I don't know what can I do if I have negative accel data and positive gyro data?

Answer 1

when I tried to rotate the phone 360 degree .. I got this plot from matlab : http://www.uppic.com/uploads/14205374821.jpg

the green lone is for Gyro, red for fused Orientation and the blue one is for accel.

• why Gyro's time has offset?? the reason is drift ?? the plot is correct?
• how can I use 0-360 period instead of 0_180 and -180_0?

Answer 2

The link to the image you posted does not work, so I couldn't check the plot from matlab. Gyro drift does not add any time offset to the data. Maybe the offset is caused by some internal delay (unprobable) or, more likely by a delay in the algorithm. I can't tell exactly because I cannot see, how big the time offset is.

If you ounly want the gyro data, the piece you added to the complementary filter is unnecessary. All gyro data should be already in the gyroOrientationReal vector. No further processing needed. The real gyro data should not be influenced by accMagOrientation (which you do, by changing gyroOrientationReal if accMagOrientation[0] > 0.0 .

If you want to go from a [0, 360] period to a [0, 180, -180, 0] period you just need to subtract 180 from your result, and you're done.

I hope I could help you with this.