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();
}
public synchronized void processAcc(Vector3d acc, long sensorTimeStamp) {
this.mz.set(acc);
updateAccelCovariance(this.mz.length());
if (this.alignedToGravity) {
accObservationFunctionForNumericalJacobian(this.so3SensorFromWorld, this.mNu);
double eps = 1.0E-07D;
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.alignedToGravity = true;
}
}
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 synchronized void processGyro(Vector3d gyro, long sensorTimeStamp) {
float kTimeThreshold = 0.04F;
float kdTdefault = 0.01F;
if (this.sensorTimeStampGyro != 0L) {
float dT = (float) (sensorTimeStamp - this.sensorTimeStampGyro) * 1.0E-09F;
if (dT > 0.04F) dT = this.gyroFilterValid ? this.filteredGyroTimestep : 0.01F;
else {
filterGyroTimestep(dT);
}
this.mu.set(gyro);
this.mu.scale(-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.set(gyro);
}
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[] getPredictedGLMatrix(double secondsAfterLastGyroEvent) {
double dT = secondsAfterLastGyroEvent;
Vector3d pmu = this.getPredictedGLMatrixTempV1;
pmu.set(this.lastGyro);
pmu.scale(-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 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]);
}
public synchronized void processMag(float[] mag, long sensorTimeStamp) {
if (!this.alignedToGravity) {
return;
}
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.alignedToNorth) {
magObservationFunctionForNumericalJacobian(this.so3SensorFromWorld, this.mNu);
double eps = 1.0E-07D;
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.mNu, this.so3LastMotion);
Matrix3x3d.mult(this.so3LastMotion, this.so3SensorFromWorld, this.processMagTempM4);
this.so3SensorFromWorld.set(this.processMagTempM4);
updateCovariancesAfterMotion();
this.alignedToNorth = true;
}
}
