Android Open Source - Cardboard Orientation E K F






From Project

Back to project page Cardboard.

License

The source code is released under:

Apache License

If you think the Android project Cardboard listed in this page is inappropriate, such as containing malicious code/tools or violating the copyright, please email info at java2s dot com, thanks.

Java Source Code

package com.google.vrtoolkit.cardboard.sensors.internal;
/*  w  ww  . jav  a 2 s . c  o  m*/
public class OrientationEKF {
  private static final float NS2S = 1.0E-9F;
  private double[] rotationMatrix = new double[16];
  private Matrix3x3d so3SensorFromWorld = new Matrix3x3d();
  private Matrix3x3d so3LastMotion = new Matrix3x3d();
  private Matrix3x3d mP = new Matrix3x3d();
  private Matrix3x3d mQ = new Matrix3x3d();
  private Matrix3x3d mR = new Matrix3x3d();
  private Matrix3x3d mRaccel = new Matrix3x3d();
  private Matrix3x3d mS = new Matrix3x3d();
  private Matrix3x3d mH = new Matrix3x3d();
  private Matrix3x3d mK = new Matrix3x3d();
  private Vector3d mNu = new Vector3d();
  private Vector3d mz = new Vector3d();
  private Vector3d mh = new Vector3d();
  private Vector3d mu = new Vector3d();
  private Vector3d mx = new Vector3d();
  private Vector3d down = new Vector3d();
  private Vector3d north = new Vector3d();
  private long sensorTimeStampGyro;
  private long sensorTimeStampAcc;
  private long sensorTimeStampMag;
  private float[] lastGyro = new float[3];
  private float filteredGyroTimestep;
  private boolean timestepFilterInit = false;
  private int numGyroTimestepSamples;
  private boolean gyroFilterValid = true;
  private Matrix3x3d getPredictedGLMatrixTempM1 = new Matrix3x3d();
  private Matrix3x3d getPredictedGLMatrixTempM2 = new Matrix3x3d();
  private Vector3d getPredictedGLMatrixTempV1 = new Vector3d();
  private Matrix3x3d setHeadingDegreesTempM1 = new Matrix3x3d();
  private Matrix3x3d processGyroTempM1 = new Matrix3x3d();
  private Matrix3x3d processGyroTempM2 = new Matrix3x3d();
  private Matrix3x3d processAccTempM1 = new Matrix3x3d();
  private Matrix3x3d processAccTempM2 = new Matrix3x3d();
  private Matrix3x3d processAccTempM3 = new Matrix3x3d();
  private Matrix3x3d processAccTempM4 = new Matrix3x3d();
  private Matrix3x3d processAccTempM5 = new Matrix3x3d();
  private Vector3d processAccTempV1 = new Vector3d();
  private Vector3d processAccTempV2 = new Vector3d();
  private Vector3d processAccVDelta = new Vector3d();
  private Vector3d processMagTempV1 = new Vector3d();
  private Vector3d processMagTempV2 = new Vector3d();
  private Vector3d processMagTempV3 = new Vector3d();
  private Vector3d processMagTempV4 = new Vector3d();
  private Vector3d processMagTempV5 = new Vector3d();
  private Matrix3x3d processMagTempM1 = new Matrix3x3d();
  private Matrix3x3d processMagTempM2 = new Matrix3x3d();
  private Matrix3x3d processMagTempM4 = new Matrix3x3d();
  private Matrix3x3d processMagTempM5 = new Matrix3x3d();
  private Matrix3x3d processMagTempM6 = new Matrix3x3d();
  private Matrix3x3d updateCovariancesAfterMotionTempM1 = new Matrix3x3d();
  private Matrix3x3d updateCovariancesAfterMotionTempM2 = new Matrix3x3d();
  private Matrix3x3d accObservationFunctionForNumericalJacobianTempM = new Matrix3x3d();
  private Matrix3x3d magObservationFunctionForNumericalJacobianTempM = new Matrix3x3d();

  public OrientationEKF() {
    reset();
  }

  public void reset() {
    this.sensorTimeStampGyro = 0L;
    this.sensorTimeStampAcc = 0L;
    this.sensorTimeStampMag = 0L;

    this.so3SensorFromWorld.setIdentity();
    this.so3LastMotion.setIdentity();

    double initialSigmaP = 5.0D;

    this.mP.setZero();
    this.mP.setSameDiagonal(25.0D);

    double initialSigmaQ = 1.0D;
    this.mQ.setZero();
    this.mQ.setSameDiagonal(1.0D);

    double initialSigmaR = 0.25D;
    this.mR.setZero();
    this.mR.setSameDiagonal(0.0625D);

    double initialSigmaRaccel = 0.75D;
    this.mRaccel.setZero();
    this.mRaccel.setSameDiagonal(0.5625D);

    this.mS.setZero();
    this.mH.setZero();
    this.mK.setZero();
    this.mNu.setZero();
    this.mz.setZero();
    this.mh.setZero();
    this.mu.setZero();
    this.mx.setZero();

    this.down.set(0.0D, 0.0D, 9.81D);
    this.north.set(0.0D, 1.0D, 0.0D);
  }

  public boolean isReady() {
    return this.sensorTimeStampAcc != 0L;
  }

  public double getHeadingDegrees() {
    double x = this.so3SensorFromWorld.get(2, 0);
    double y = this.so3SensorFromWorld.get(2, 1);
    double mag = Math.sqrt(x * x + y * y);
    if (mag < 0.1D) {
      return 0.0D;
    }
    double heading = -90.0D - Math.atan2(y, x) / 3.141592653589793D
        * 180.0D;
    if (heading < 0.0D) {
      heading += 360.0D;
    }
    if (heading >= 360.0D) {
      heading -= 360.0D;
    }
    return heading;
  }

  public synchronized void setHeadingDegrees(double heading) {
    double currentHeading = getHeadingDegrees();
    double deltaHeading = heading - currentHeading;
    double s = Math.sin(deltaHeading / 180.0D * 3.141592653589793D);
    double c = Math.cos(deltaHeading / 180.0D * 3.141592653589793D);

    double[][] deltaHeadingRotationVals = { { c, -s, 0.0D },
        { s, c, 0.0D }, { 0.0D, 0.0D, 1.0D } };

    arrayAssign(deltaHeadingRotationVals, this.setHeadingDegreesTempM1);
    Matrix3x3d.mult(this.so3SensorFromWorld, this.setHeadingDegreesTempM1,
        this.so3SensorFromWorld);
  }

  public double[] getGLMatrix() {
    return glMatrixFromSo3(this.so3SensorFromWorld);
  }

  public double[] getPredictedGLMatrix(double secondsAfterLastGyroEvent) {
    double dT = secondsAfterLastGyroEvent;
    Vector3d pmu = this.getPredictedGLMatrixTempV1;
    pmu.set(this.lastGyro[0] * -dT, this.lastGyro[1] * -dT,
        this.lastGyro[2] * -dT);
    Matrix3x3d so3PredictedMotion = this.getPredictedGLMatrixTempM1;
    So3Util.sO3FromMu(pmu, so3PredictedMotion);

    Matrix3x3d so3PredictedState = this.getPredictedGLMatrixTempM2;
    Matrix3x3d.mult(so3PredictedMotion, this.so3SensorFromWorld,
        so3PredictedState);

    return glMatrixFromSo3(so3PredictedState);
  }

  private double[] glMatrixFromSo3(Matrix3x3d so3) {
    for (int r = 0; r < 3; r++) {
      for (int c = 0; c < 3; c++) {
        this.rotationMatrix[(4 * c + r)] = so3.get(r, c);
      }
    }
    double tmp62_61 = (this.rotationMatrix[11] = 0.0D);
    this.rotationMatrix[7] = tmp62_61;
    this.rotationMatrix[3] = tmp62_61;
    double tmp86_85 = (this.rotationMatrix[14] = 0.0D);
    this.rotationMatrix[13] = tmp86_85;
    this.rotationMatrix[12] = tmp86_85;

    this.rotationMatrix[15] = 1.0D;

    return this.rotationMatrix;
  }

  public synchronized void processGyro(float[] gyro, long sensorTimeStamp) {
    float kTimeThreshold = 0.04F;
    float kdTdefault = 0.01F;
    if (this.sensorTimeStampGyro != 0L) {
      float dT = (float) (sensorTimeStamp - this.sensorTimeStampGyro) * 1.0E-9F;
      if (dT > 0.04F) {
        dT = this.gyroFilterValid ? this.filteredGyroTimestep : 0.01F;
      } else {
        filterGyroTimestep(dT);
      }
      this.mu.set(gyro[0] * -dT, gyro[1] * -dT, gyro[2] * -dT);
      So3Util.sO3FromMu(this.mu, this.so3LastMotion);

      this.processGyroTempM1.set(this.so3SensorFromWorld);
      Matrix3x3d.mult(this.so3LastMotion, this.so3SensorFromWorld,
          this.processGyroTempM1);
      this.so3SensorFromWorld.set(this.processGyroTempM1);

      updateCovariancesAfterMotion();

      this.processGyroTempM2.set(this.mQ);
      this.processGyroTempM2.scale(dT * dT);
      this.mP.plusEquals(this.processGyroTempM2);
    }
    this.sensorTimeStampGyro = sensorTimeStamp;
    this.lastGyro[0] = gyro[0];
    this.lastGyro[1] = gyro[1];
    this.lastGyro[2] = gyro[2];
  }

  public synchronized void processAcc(float[] acc, long sensorTimeStamp) {
    this.mz.set(acc[0], acc[1], acc[2]);
    if (this.sensorTimeStampAcc != 0L) {
      accObservationFunctionForNumericalJacobian(this.so3SensorFromWorld,
          this.mNu);

      double eps = 1.0E-7D;
      for (int dof = 0; dof < 3; dof++) {
        Vector3d delta = this.processAccVDelta;
        delta.setZero();
        delta.setComponent(dof, eps);

        So3Util.sO3FromMu(delta, this.processAccTempM1);
        Matrix3x3d.mult(this.processAccTempM1, this.so3SensorFromWorld,
            this.processAccTempM2);

        accObservationFunctionForNumericalJacobian(
            this.processAccTempM2, this.processAccTempV1);

        Vector3d withDelta = this.processAccTempV1;

        Vector3d.sub(this.mNu, withDelta, this.processAccTempV2);
        this.processAccTempV2.scale(1.0D / eps);
        this.mH.setColumn(dof, this.processAccTempV2);
      }
      this.mH.transpose(this.processAccTempM3);
      Matrix3x3d.mult(this.mP, this.processAccTempM3,
          this.processAccTempM4);
      Matrix3x3d.mult(this.mH, this.processAccTempM4,
          this.processAccTempM5);
      Matrix3x3d.add(this.processAccTempM5, this.mRaccel, this.mS);

      this.mS.invert(this.processAccTempM3);
      this.mH.transpose(this.processAccTempM4);
      Matrix3x3d.mult(this.processAccTempM4, this.processAccTempM3,
          this.processAccTempM5);
      Matrix3x3d.mult(this.mP, this.processAccTempM5, this.mK);

      Matrix3x3d.mult(this.mK, this.mNu, this.mx);

      Matrix3x3d.mult(this.mK, this.mH, this.processAccTempM3);
      this.processAccTempM4.setIdentity();
      this.processAccTempM4.minusEquals(this.processAccTempM3);
      Matrix3x3d.mult(this.processAccTempM4, this.mP,
          this.processAccTempM3);
      this.mP.set(this.processAccTempM3);

      So3Util.sO3FromMu(this.mx, this.so3LastMotion);

      Matrix3x3d.mult(this.so3LastMotion, this.so3SensorFromWorld,
          this.so3SensorFromWorld);

      updateCovariancesAfterMotion();
    } else {
      So3Util.sO3FromTwoVec(this.down, this.mz, this.so3SensorFromWorld);
    }
    this.sensorTimeStampAcc = sensorTimeStamp;
  }

  public void processMag(float[] mag, long sensorTimeStamp) {
    this.mz.set(mag[0], mag[1], mag[2]);
    this.mz.normalize();

    Vector3d downInSensorFrame = new Vector3d();
    this.so3SensorFromWorld.getColumn(2, downInSensorFrame);

    Vector3d.cross(this.mz, downInSensorFrame, this.processMagTempV1);
    Vector3d perpToDownAndMag = this.processMagTempV1;
    perpToDownAndMag.normalize();

    Vector3d.cross(downInSensorFrame, perpToDownAndMag,
        this.processMagTempV2);
    Vector3d magHorizontal = this.processMagTempV2;

    magHorizontal.normalize();
    this.mz.set(magHorizontal);
    if (this.sensorTimeStampMag != 0L) {
      magObservationFunctionForNumericalJacobian(this.so3SensorFromWorld,
          this.mNu);

      double eps = 1.0E-7D;
      for (int dof = 0; dof < 3; dof++) {
        Vector3d delta = this.processMagTempV3;
        delta.setZero();
        delta.setComponent(dof, eps);

        So3Util.sO3FromMu(delta, this.processMagTempM1);
        Matrix3x3d.mult(this.processMagTempM1, this.so3SensorFromWorld,
            this.processMagTempM2);

        magObservationFunctionForNumericalJacobian(
            this.processMagTempM2, this.processMagTempV4);

        Vector3d withDelta = this.processMagTempV4;

        Vector3d.sub(this.mNu, withDelta, this.processMagTempV5);
        this.processMagTempV5.scale(1.0D / eps);

        this.mH.setColumn(dof, this.processMagTempV5);
      }
      this.mH.transpose(this.processMagTempM4);
      Matrix3x3d.mult(this.mP, this.processMagTempM4,
          this.processMagTempM5);
      Matrix3x3d.mult(this.mH, this.processMagTempM5,
          this.processMagTempM6);
      Matrix3x3d.add(this.processMagTempM6, this.mR, this.mS);

      this.mS.invert(this.processMagTempM4);
      this.mH.transpose(this.processMagTempM5);
      Matrix3x3d.mult(this.processMagTempM5, this.processMagTempM4,
          this.processMagTempM6);
      Matrix3x3d.mult(this.mP, this.processMagTempM6, this.mK);

      Matrix3x3d.mult(this.mK, this.mNu, this.mx);

      Matrix3x3d.mult(this.mK, this.mH, this.processMagTempM4);
      this.processMagTempM5.setIdentity();
      this.processMagTempM5.minusEquals(this.processMagTempM4);
      Matrix3x3d.mult(this.processMagTempM5, this.mP,
          this.processMagTempM4);
      this.mP.set(this.processMagTempM4);

      So3Util.sO3FromMu(this.mx, this.so3LastMotion);

      Matrix3x3d.mult(this.so3LastMotion, this.so3SensorFromWorld,
          this.processMagTempM4);
      this.so3SensorFromWorld.set(this.processMagTempM4);

      updateCovariancesAfterMotion();
    } else {
      magObservationFunctionForNumericalJacobian(this.so3SensorFromWorld,
          this.mNu);
      So3Util.sO3FromMu(this.mx, this.so3LastMotion);

      Matrix3x3d.mult(this.so3LastMotion, this.so3SensorFromWorld,
          this.processMagTempM4);
      this.so3SensorFromWorld.set(this.processMagTempM4);

      updateCovariancesAfterMotion();
    }
    this.sensorTimeStampMag = sensorTimeStamp;
  }

  private void filterGyroTimestep(float timeStep) {
    float kFilterCoeff = 0.95F;
    float kMinSamples = 10.0F;
    if (!this.timestepFilterInit) {
      this.filteredGyroTimestep = timeStep;
      this.numGyroTimestepSamples = 1;
      this.timestepFilterInit = true;
    } else {
      this.filteredGyroTimestep = (0.95F * this.filteredGyroTimestep + 0.050000012F * timeStep);
      if (++this.numGyroTimestepSamples > 10.0F) {
        this.gyroFilterValid = true;
      }
    }
  }

  private void updateCovariancesAfterMotion() {
    this.so3LastMotion.transpose(this.updateCovariancesAfterMotionTempM1);
    Matrix3x3d.mult(this.mP, this.updateCovariancesAfterMotionTempM1,
        this.updateCovariancesAfterMotionTempM2);

    Matrix3x3d.mult(this.so3LastMotion,
        this.updateCovariancesAfterMotionTempM2, this.mP);
    this.so3LastMotion.setIdentity();
  }

  private void accObservationFunctionForNumericalJacobian(
      Matrix3x3d so3SensorFromWorldPred, Vector3d result) {
    Matrix3x3d.mult(so3SensorFromWorldPred, this.down, this.mh);
    So3Util.sO3FromTwoVec(this.mh, this.mz,
        this.accObservationFunctionForNumericalJacobianTempM);

    So3Util.muFromSO3(this.accObservationFunctionForNumericalJacobianTempM,
        result);
  }

  private void magObservationFunctionForNumericalJacobian(
      Matrix3x3d so3SensorFromWorldPred, Vector3d result) {
    Matrix3x3d.mult(so3SensorFromWorldPred, this.north, this.mh);
    So3Util.sO3FromTwoVec(this.mh, this.mz,
        this.magObservationFunctionForNumericalJacobianTempM);

    So3Util.muFromSO3(this.magObservationFunctionForNumericalJacobianTempM,
        result);
  }

  public static void arrayAssign(double[][] data, Matrix3x3d m) {
    assert (3 == data.length);
    assert (3 == data[0].length);
    assert (3 == data[1].length);
    assert (3 == data[2].length);
    m.set(data[0][0], data[0][1], data[0][2], data[1][0], data[1][1],
        data[1][2], data[2][0], data[2][1], data[2][2]);
  }
}




Java Source Code List

com.google.vrtoolkit.cardboard.BuildConfig.java
com.google.vrtoolkit.cardboard.CardboardActivity.java
com.google.vrtoolkit.cardboard.CardboardDeviceParams.java
com.google.vrtoolkit.cardboard.CardboardView.java
com.google.vrtoolkit.cardboard.DistortionRenderer.java
com.google.vrtoolkit.cardboard.Distortion.java
com.google.vrtoolkit.cardboard.EyeParams.java
com.google.vrtoolkit.cardboard.EyeTransform.java
com.google.vrtoolkit.cardboard.FieldOfView.java
com.google.vrtoolkit.cardboard.HeadMountedDisplay.java
com.google.vrtoolkit.cardboard.HeadTransform.java
com.google.vrtoolkit.cardboard.ScreenParams.java
com.google.vrtoolkit.cardboard.Viewport.java
com.google.vrtoolkit.cardboard.samples.treasurehunt.CardboardOverlayView.java
com.google.vrtoolkit.cardboard.samples.treasurehunt.MainActivity.java
com.google.vrtoolkit.cardboard.samples.treasurehunt.WorldLayoutData.java
com.google.vrtoolkit.cardboard.sensors.HeadTracker.java
com.google.vrtoolkit.cardboard.sensors.MagnetSensor.java
com.google.vrtoolkit.cardboard.sensors.NfcSensor.java
com.google.vrtoolkit.cardboard.sensors.internal.Matrix3x3d.java
com.google.vrtoolkit.cardboard.sensors.internal.OrientationEKF.java
com.google.vrtoolkit.cardboard.sensors.internal.So3Util.java
com.google.vrtoolkit.cardboard.sensors.internal.Vector3d.java