Das Sensor Fusion-Video sieht gut aus, aber es gibt keinen Code:http://www.youtube.com/watch?v=C7JQ7Rpwn2k&feature=player_detailpage#t=1315s

Hier ist mein Code, der nur Beschleunigungsmesser und Kompass verwendet. Ich verwende auch einen Kalman-Filter für die 3 Orientierungswerte, aber das ist zu viel Code, um ihn hier anzuzeigen. Letztendlich funktioniert das in Ordnung, aber das Ergebnis ist entweder zu nervös oder zu träge, je nachdem, was ich mit den Ergebnissen mache und wie niedrig ich die Filterfaktoren einstelle.

/** Just accelerometer and magnetic sensors */
public abstract class SensorsListener2
    implements
        SensorEventListener
{
    /** The lower this is, the greater the preference which is given to previous values. (slows change) */
    private static final float accelFilteringFactor = 0.1f;
    private static final float magFilteringFactor = 0.01f;

    public abstract boolean getIsLandscape();

    @Override
    public void onSensorChanged(SensorEvent event) {
        Sensor sensor = event.sensor;
        int type = sensor.getType();

        switch (type) {
            case Sensor.TYPE_MAGNETIC_FIELD:
                mags[0] = event.values[0] * magFilteringFactor + mags[0] * (1.0f - magFilteringFactor);
                mags[1] = event.values[1] * magFilteringFactor + mags[1] * (1.0f - magFilteringFactor);
                mags[2] = event.values[2] * magFilteringFactor + mags[2] * (1.0f - magFilteringFactor);

                isReady = true;
                break;
            case Sensor.TYPE_ACCELEROMETER:
                accels[0] = event.values[0] * accelFilteringFactor + accels[0] * (1.0f - accelFilteringFactor);
                accels[1] = event.values[1] * accelFilteringFactor + accels[1] * (1.0f - accelFilteringFactor);
                accels[2] = event.values[2] * accelFilteringFactor + accels[2] * (1.0f - accelFilteringFactor);
                break;

            default:
                return;
        }




        if(mags != null && accels != null && isReady) {
            isReady = false;

            SensorManager.getRotationMatrix(rot, inclination, accels, mags);

            boolean isLandscape = getIsLandscape();
            if(isLandscape) {
                outR = rot;
            } else {
                // Remap the coordinates to work in portrait mode.
                SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR);
            }

            SensorManager.getOrientation(outR, values);

            double x180pi = 180.0 / Math.PI;
            float azimuth = (float)(values[0] * x180pi);
            float pitch = (float)(values[1] * x180pi);
            float roll = (float)(values[2] * x180pi);

            // In landscape mode swap pitch and roll and invert the pitch.
            if(isLandscape) {
                float tmp = pitch;
                pitch = -roll;
                roll = -tmp;
                azimuth = 180 - azimuth;
            } else {
                pitch = -pitch - 90;
                azimuth = 90 - azimuth;
            }

            onOrientationChanged(azimuth,pitch,roll);
        }
    }




    private float[] mags = new float[3];
    private float[] accels = new float[3];
    private boolean isReady;

    private float[] rot = new float[9];
    private float[] outR = new float[9];
    private float[] inclination = new float[9];
    private float[] values = new float[3];



    /**
    Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West
    Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis.
    Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis.
    */
    public abstract void onOrientationChanged(float azimuth, float pitch, float roll);
}

Ich habe versucht herauszufinden, wie man Gyroskopdaten hinzufügt, aber ich mache es einfach nicht richtig. Das Google Doc beihttp://developer.android.com/reference/android/hardware/SensorEvent.html zeigt einen Code zum Abrufen einer Delta-Matrix aus den Gyroskopdaten. Die Idee scheint zu sein, dass ich die Filter für den Beschleunigungsmesser und die Magnetsensoren herunterdrehen würde, damit sie wirklich stabil sind. Das würde die langfristige Ausrichtung im Auge behalten.

Dann würde ich eine Historie der neuesten N-Delta-Matrizen aus dem Gyroskop aufbewahren. Jedes Mal, wenn ich eine neue bekam, ließ ich die älteste los und multiplizierte sie alle zusammen, um eine endgültige Matrix zu erhalten, die ich mit der stabilen Matrix multiplizierte, die vom Beschleunigungsmesser und den Magnetsensoren zurückgegeben wurde.

Das scheint nicht zu funktionieren. Oder zumindest funktioniert meine Implementierung nicht. Das Ergebnis ist weitaus nervöser als nur der Beschleunigungsmesser. Wenn Sie den Gyroskopverlauf vergrößern, wird der Jitter tatsächlich größer, sodass ich denke, dass ich mit dem Gyroskop nicht die richtigen Werte berechne.

public abstract class SensorsListener3
    implements
        SensorEventListener
{
    /** The lower this is, the greater the preference which is given to previous values. (slows change) */
    private static final float kFilteringFactor = 0.001f;
    private static final float magKFilteringFactor = 0.001f;


    public abstract boolean getIsLandscape();

    @Override
    public void onSensorChanged(SensorEvent event) {
        Sensor sensor = event.sensor;
        int type = sensor.getType();

        switch (type) {
            case Sensor.TYPE_MAGNETIC_FIELD:
                mags[0] = event.values[0] * magKFilteringFactor + mags[0] * (1.0f - magKFilteringFactor);
                mags[1] = event.values[1] * magKFilteringFactor + mags[1] * (1.0f - magKFilteringFactor);
                mags[2] = event.values[2] * magKFilteringFactor + mags[2] * (1.0f - magKFilteringFactor);

                isReady = true;
                break;
            case Sensor.TYPE_ACCELEROMETER:
                accels[0] = event.values[0] * kFilteringFactor + accels[0] * (1.0f - kFilteringFactor);
                accels[1] = event.values[1] * kFilteringFactor + accels[1] * (1.0f - kFilteringFactor);
                accels[2] = event.values[2] * kFilteringFactor + accels[2] * (1.0f - kFilteringFactor);
                break;

            case Sensor.TYPE_GYROSCOPE:
                gyroscopeSensorChanged(event);
                break;

            default:
                return;
        }




        if(mags != null && accels != null && isReady) {
            isReady = false;

            SensorManager.getRotationMatrix(rot, inclination, accels, mags);

            boolean isLandscape = getIsLandscape();
            if(isLandscape) {
                outR = rot;
            } else {
                // Remap the coordinates to work in portrait mode.
                SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR);
            }

            if(gyroUpdateTime!=0) {
                matrixHistory.mult(matrixTmp,matrixResult);
                outR = matrixResult;
            }

            SensorManager.getOrientation(outR, values);

            double x180pi = 180.0 / Math.PI;
            float azimuth = (float)(values[0] * x180pi);
            float pitch = (float)(values[1] * x180pi);
            float roll = (float)(values[2] * x180pi);

            // In landscape mode swap pitch and roll and invert the pitch.
            if(isLandscape) {
                float tmp = pitch;
                pitch = -roll;
                roll = -tmp;
                azimuth = 180 - azimuth;
            } else {
                pitch = -pitch - 90;
                azimuth = 90 - azimuth;
            }

            onOrientationChanged(azimuth,pitch,roll);
        }
    }



    private void gyroscopeSensorChanged(SensorEvent event) {
        // This timestep's delta rotation to be multiplied by the current rotation
        // after computing it from the gyro sample data.
        if(gyroUpdateTime != 0) {
            final float dT = (event.timestamp - gyroUpdateTime) * NS2S;
            // Axis of the rotation sample, not normalized yet.
            float axisX = event.values[0];
            float axisY = event.values[1];
            float axisZ = event.values[2];

            // Calculate the angular speed of the sample
            float omegaMagnitude = (float)Math.sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ);

            // Normalize the rotation vector if it's big enough to get the axis
            if(omegaMagnitude > EPSILON) {
                axisX /= omegaMagnitude;
                axisY /= omegaMagnitude;
                axisZ /= omegaMagnitude;
            }

            // Integrate around this axis with the angular speed by the timestep
            // in order to get a delta rotation from this sample over the timestep
            // We will convert this axis-angle representation of the delta rotation
            // into a quaternion before turning it into the rotation matrix.
            float thetaOverTwo = omegaMagnitude * dT / 2.0f;
            float sinThetaOverTwo = (float)Math.sin(thetaOverTwo);
            float cosThetaOverTwo = (float)Math.cos(thetaOverTwo);
            deltaRotationVector[0] = sinThetaOverTwo * axisX;
            deltaRotationVector[1] = sinThetaOverTwo * axisY;
            deltaRotationVector[2] = sinThetaOverTwo * axisZ;
            deltaRotationVector[3] = cosThetaOverTwo;
        }
        gyroUpdateTime = event.timestamp;
        SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector);
        // User code should concatenate the delta rotation we computed with the current rotation
        // in order to get the updated rotation.
        // rotationCurrent = rotationCurrent * deltaRotationMatrix;
        matrixHistory.add(deltaRotationMatrix);
    }



    private float[] mags = new float[3];
    private float[] accels = new float[3];
    private boolean isReady;

    private float[] rot = new float[9];
    private float[] outR = new float[9];
    private float[] inclination = new float[9];
    private float[] values = new float[3];

    // gyroscope stuff
    private long gyroUpdateTime = 0;
    private static final float NS2S = 1.0f / 1000000000.0f;
    private float[] deltaRotationMatrix = new float[9];
    private final float[] deltaRotationVector = new float[4];
//TODO: I have no idea how small this value should be.
    private static final float EPSILON = 0.000001f;
    private float[] matrixMult = new float[9];
    private MatrixHistory matrixHistory = new MatrixHistory(100);
    private float[] matrixTmp = new float[9];
    private float[] matrixResult = new float[9];


    /**
    Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West 
    Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis. 
    Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis.
    */
    public abstract void onOrientationChanged(float azimuth, float pitch, float roll);
}


public class MatrixHistory
{
    public MatrixHistory(int size) {
        vals = new float[size][];
    }

    public void add(float[] val) {
        synchronized(vals) {
            vals[ix] = val;
            ix = (ix + 1) % vals.length;
            if(ix==0)
                full = true;
        }
    }

    public void mult(float[] tmp, float[] output) {
        synchronized(vals) {
            if(full) {
                for(int i=0; i<vals.length; ++i) {
                    if(i==0) {
                        System.arraycopy(vals[i],0,output,0,vals[i].length);
                    } else {
                        MathUtils.multiplyMatrix3x3(output,vals[i],tmp);
                        System.arraycopy(tmp,0,output,0,tmp.length);
                    }
                }
            } else {
                if(ix==0)
                    return;
                for(int i=0; i<ix; ++i) {
                    if(i==0) {
                        System.arraycopy(vals[i],0,output,0,vals[i].length);
                    } else {
                        MathUtils.multiplyMatrix3x3(output,vals[i],tmp);
                        System.arraycopy(tmp,0,output,0,tmp.length);
                    }
                }
            }
        }
    }


    private int ix = 0;
    private boolean full = false;
    private float[][] vals;
}

Der zweite Codeblock enthält meine Änderungen gegenüber dem ersten Codeblock, mit denen das Gyroskop zur Mischung hinzugefügt wird.

Insbesondere wird der Filterfaktor für die Beschleunigung kleiner gemacht (wodurch der Wert stabiler wird). Die MatrixHistory-Klasse verfolgt die letzten 100 deltaRotationMatrix-Werte des Gyroskops, die mit der gyroscopeSensorChanged-Methode berechnet wurden.

Ich habe auf dieser Site viele Fragen zu diesem Thema gesehen. Sie haben mir geholfen, an diesen Punkt zu gelangen, aber ich kann nicht herausfinden, was ich als nächstes tun soll. Ich wünschte wirklich, der Sensor Fusion-Typ hätte gerade irgendwo einen Code gepostet. Er hatte offensichtlich alles zusammen.