private void magObservationFunctionForNumericalJacobian(Matrix3x3d so3SensorFromWorldPred, Vector3d result)
{
Matrix3x3d.mult(so3SensorFromWorldPred, north, mh);
So3Util.sO3FromTwoVec(mh, mz, magObservationFunctionForNumericalJacobianTempM);
So3Util.muFromSO3(magObservationFunctionForNumericalJacobianTempM, result);
}
public static void sO3FromMu(Vector3d w, Matrix3x3d result)
{
double thetaSq = Vector3d.dot(w, w);
double theta = Math.Sqrt(thetaSq);
double kA, kB;
if (thetaSq < 1.0E-08D)
{
kA = 1.0D - 0.16666667163372D * thetaSq;
kB = 0.5D;
}
else
{
if (thetaSq < 1.0E-06D)
{
kB = 0.5D - 0.0416666679084301D * thetaSq;
kA = 1.0D - thetaSq * 0.16666667163372D * (1.0D - 0.16666667163372D * thetaSq);
}
else
{
double invTheta = 1.0D / theta;
kA = Math.Sin(theta) * invTheta;
kB = (1.0D - Math.Cos(theta)) * (invTheta * invTheta);
}
}
rodriguesSo3Exp(w, kA, kB, result);
}
private static void rodriguesSo3Exp(Vector3d w, double kA, double kB, Matrix3x3d result)
{
double wx2 = w.x * w.x;
double wy2 = w.y * w.y;
double wz2 = w.z * w.z;
result.set(0, 0, 1.0D - kB * (wy2 + wz2));
result.set(1, 1, 1.0D - kB * (wx2 + wz2));
result.set(2, 2, 1.0D - kB * (wx2 + wy2));
double a, b;
a = kA * w.z;
b = kB * (w.x * w.y);
result.set(0, 1, b - a);
result.set(1, 0, b + a);
a = kA * w.y;
b = kB * (w.x * w.z);
result.set(0, 2, b + a);
result.set(2, 0, b - a);
a = kA * w.x;
b = kB * (w.y * w.z);
result.set(1, 2, b - a);
result.set(2, 1, b + a);
}
public /*synchronized*/ void processGyro(float[] gyro, long sensorTimeStamp)
{
float kTimeThreshold = 0.04F;
float kdTdefault = 0.01F;
if (sensorTimeStampGyro != 0L)
{
float dT = (float)(sensorTimeStamp - sensorTimeStampGyro) * 1.0E-09F;
if (dT > kTimeThreshold)
{
dT = gyroFilterValid ? filteredGyroTimestep : kdTdefault;
}
else
{
filterGyroTimestep(dT);
}
mu.set(gyro[0] * -dT, gyro[1] * -dT, gyro[2] * -dT);
So3Util.sO3FromMu(mu, so3LastMotion);
processGyroTempM1.set(so3SensorFromWorld);
Matrix3x3d.mult(so3LastMotion, so3SensorFromWorld, processGyroTempM1);
so3SensorFromWorld.set(processGyroTempM1);
updateCovariancesAfterMotion();
processGyroTempM2.set(mQ);
processGyroTempM2.scale(dT * dT);
mP.plusEquals(processGyroTempM2);
}
sensorTimeStampGyro = sensorTimeStamp;
lastGyro[0] = gyro[0];
lastGyro[1] = gyro[1];
lastGyro[2] = gyro[2];
}
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]);
}
private void updateCovariancesAfterMotion()
{
so3LastMotion.transpose(updateCovariancesAfterMotionTempM1);
Matrix3x3d.mult(mP, updateCovariancesAfterMotionTempM1, updateCovariancesAfterMotionTempM2);
Matrix3x3d.mult(so3LastMotion, updateCovariancesAfterMotionTempM2, mP);
so3LastMotion.setIdentity();
}
public /*synchronized*/ void processAcc(float[] acc, long sensorTimeStamp)
{
mz.set(acc[0], acc[1], acc[2]);
if (sensorTimeStampAcc != 0L)
{
accObservationFunctionForNumericalJacobian(so3SensorFromWorld, mNu);
double eps = 1.0E-07D;
for (int dof = 0; dof < 3; dof++)
{
Vector3d delta = processAccVDelta;
delta.setZero();
delta.setComponent(dof, eps);
So3Util.sO3FromMu(delta, processAccTempM1);
Matrix3x3d.mult(processAccTempM1, so3SensorFromWorld, processAccTempM2);
accObservationFunctionForNumericalJacobian(processAccTempM2, processAccTempV1);
Vector3d withDelta = processAccTempV1;
Vector3d.sub(mNu, withDelta, processAccTempV2);
processAccTempV2.scale(1.0D / eps);
mH.setColumn(dof, processAccTempV2);
}
mH.transpose(processAccTempM3);
Matrix3x3d.mult(mP, processAccTempM3, processAccTempM4);
Matrix3x3d.mult(mH, processAccTempM4, processAccTempM5);
Matrix3x3d.add(processAccTempM5, mRaccel, mS);
mS.invert(processAccTempM3);
mH.transpose(processAccTempM4);
Matrix3x3d.mult(processAccTempM4, processAccTempM3, processAccTempM5);
Matrix3x3d.mult(mP, processAccTempM5, mK);
Matrix3x3d.mult(mK, mNu, mx);
Matrix3x3d.mult(mK, mH, processAccTempM3);
processAccTempM4.setIdentity();
processAccTempM4.minusEquals(processAccTempM3);
Matrix3x3d.mult(processAccTempM4, mP, processAccTempM3);
mP.set(processAccTempM3);
So3Util.sO3FromMu(mx, so3LastMotion);
Matrix3x3d.mult(so3LastMotion, so3SensorFromWorld, so3SensorFromWorld);
updateCovariancesAfterMotion();
}
else
{
So3Util.sO3FromTwoVec(down, mz, so3SensorFromWorld);
}
sensorTimeStampAcc = sensorTimeStamp;
}
private static void rotationPiAboutAxis(Vector3d v, Matrix3x3d result)
{
rotationPiAboutAxisTemp.set(v);
rotationPiAboutAxisTemp.scale(3.141592653589793D / rotationPiAboutAxisTemp.length());
double invTheta = 0.3183098861837907D;
double kA = 0.0D;
double kB = 0.2026423672846756D;
rodriguesSo3Exp(rotationPiAboutAxisTemp, kA, kB, result);
}
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 = new double[][] { new[] { c, -s, 0.0D }, new[] { s, c, 0.0D }, new[] { 0.0D, 0.0D, 1.0D } };
arrayAssign(deltaHeadingRotationVals, setHeadingDegreesTempM1);
Matrix3x3d.mult(so3SensorFromWorld, setHeadingDegreesTempM1, so3SensorFromWorld);
}
public static void muFromSO3(Matrix3x3d so3, Vector3d result)
{
double cosAngle = (so3.get(0, 0) + so3.get(1, 1) + so3.get(2, 2) - 1.0D) * 0.5D;
result.set((so3.get(2, 1) - so3.get(1, 2)) / 2.0D, (so3.get(0, 2) - so3.get(2, 0)) / 2.0D, (so3.get(1, 0) - so3.get(0, 1)) / 2.0D);
double sinAngleAbs = result.length();
if (cosAngle > M_SQRT1_2)
{
if (sinAngleAbs > 0.0D)
{
result.scale(Math.Asin(sinAngleAbs) / sinAngleAbs);
}
}
else if (cosAngle > -M_SQRT1_2)
{
double angle = Math.Acos(cosAngle);
result.scale(angle / sinAngleAbs);
}
else
{
double angle = 3.141592653589793D - Math.Asin(sinAngleAbs);
double d0 = so3.get(0, 0) - cosAngle;
double d1 = so3.get(1, 1) - cosAngle;
double d2 = so3.get(2, 2) - cosAngle;
Vector3d r2 = muFromSO3R2;
if ((d0 * d0 > d1 * d1) && (d0 * d0 > d2 * d2))
{
r2.set(d0, (so3.get(1, 0) + so3.get(0, 1)) / 2.0D, (so3.get(0, 2) + so3.get(2, 0)) / 2.0D);
}
else if (d1 * d1 > d2 * d2)
{
r2.set((so3.get(1, 0) + so3.get(0, 1)) / 2.0D, d1, (so3.get(2, 1) + so3.get(1, 2)) / 2.0D);
}
else
{
r2.set((so3.get(0, 2) + so3.get(2, 0)) / 2.0D, (so3.get(2, 1) + so3.get(1, 2)) / 2.0D, d2);
}
if (Vector3d.dot(r2, result) < 0.0D)
{
r2.scale(-1.0D);
}
r2.normalize();
r2.scale(angle);
result.set(r2);
}
}
private double[] glMatrixFromSo3(Matrix3x3d so3)
{
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 3; c++)
{
rotationMatrix[(4 * c + r)] = so3.get(r, c);
}
}
double tmp62_61 = (rotationMatrix[11] = 0.0D); rotationMatrix[7] = tmp62_61; rotationMatrix[3] = tmp62_61;
double tmp86_85 = (rotationMatrix[14] = 0.0D); rotationMatrix[13] = tmp86_85; rotationMatrix[12] = tmp86_85;
rotationMatrix[15] = 1.0D;
return(rotationMatrix);
}
public double[] getPredictedGLMatrix(double secondsAfterLastGyroEvent)
{
double dT = secondsAfterLastGyroEvent;
Vector3d pmu = getPredictedGLMatrixTempV1;
pmu.set(lastGyro[0] * -dT, lastGyro[1] * -dT, lastGyro[2] * -dT);
Matrix3x3d so3PredictedMotion = getPredictedGLMatrixTempM1;
So3Util.sO3FromMu(pmu, so3PredictedMotion);
Matrix3x3d so3PredictedState = getPredictedGLMatrixTempM2;
Matrix3x3d.mult(so3PredictedMotion, so3SensorFromWorld, so3PredictedState);
return(glMatrixFromSo3(so3PredictedState));
}
public static void sO3FromTwoVec(Vector3d a, Vector3d b, Matrix3x3d result)
{
Vector3d.cross(a, b, sO3FromTwoVecN);
if (sO3FromTwoVecN.length() == 0.0D)
{
double dot = Vector3d.dot(a, b);
if (dot >= 0.0D)
{
result.setIdentity();
}
else
{
Vector3d.ortho(a, sO3FromTwoVecRotationAxis);
rotationPiAboutAxis(sO3FromTwoVecRotationAxis, result);
}
return;
}
sO3FromTwoVecA.set(a);
sO3FromTwoVecB.set(b);
sO3FromTwoVecN.normalize();
sO3FromTwoVecA.normalize();
sO3FromTwoVecB.normalize();
Matrix3x3d r1 = sO3FromTwoVec33R1;
r1.setColumn(0, sO3FromTwoVecA);
r1.setColumn(1, sO3FromTwoVecN);
Vector3d.cross(sO3FromTwoVecN, sO3FromTwoVecA, temp31);
r1.setColumn(2, temp31);
Matrix3x3d r2 = sO3FromTwoVec33R2;
r2.setColumn(0, sO3FromTwoVecB);
r2.setColumn(1, sO3FromTwoVecN);
Vector3d.cross(sO3FromTwoVecN, sO3FromTwoVecB, temp31);
r2.setColumn(2, temp31);
r1.transpose();
Matrix3x3d.mult(r2, r1, result);
}
public static void generatorField(int i, Matrix3x3d pos, Matrix3x3d result)
{
result.set(i, 0, 0.0D);
result.set((i + 1) % 3, 0, -pos.get((i + 2) % 3, 0));
result.set((i + 2) % 3, 0, pos.get((i + 1) % 3, 0));
}
public void processMag(float[] mag, long sensorTimeStamp)
{
mz.set(mag[0], mag[1], mag[2]);
mz.normalize();
Vector3d downInSensorFrame = new Vector3d();
so3SensorFromWorld.getColumn(2, downInSensorFrame);
Vector3d.cross(mz, downInSensorFrame, processMagTempV1);
Vector3d perpToDownAndMag = processMagTempV1;
perpToDownAndMag.normalize();
Vector3d.cross(downInSensorFrame, perpToDownAndMag, processMagTempV2);
Vector3d magHorizontal = processMagTempV2;
magHorizontal.normalize();
mz.set(magHorizontal);
if (sensorTimeStampMag != 0L)
{
magObservationFunctionForNumericalJacobian(so3SensorFromWorld, mNu);
double eps = 1.0E-07D;
for (int dof = 0; dof < 3; dof++)
{
Vector3d delta = processMagTempV3;
delta.setZero();
delta.setComponent(dof, eps);
So3Util.sO3FromMu(delta, processMagTempM1);
Matrix3x3d.mult(processMagTempM1, so3SensorFromWorld, processMagTempM2);
magObservationFunctionForNumericalJacobian(processMagTempM2, processMagTempV4);
Vector3d withDelta = processMagTempV4;
Vector3d.sub(mNu, withDelta, processMagTempV5);
processMagTempV5.scale(1.0D / eps);
mH.setColumn(dof, processMagTempV5);
}
mH.transpose(processMagTempM4);
Matrix3x3d.mult(mP, processMagTempM4, processMagTempM5);
Matrix3x3d.mult(mH, processMagTempM5, processMagTempM6);
Matrix3x3d.add(processMagTempM6, mR, mS);
mS.invert(processMagTempM4);
mH.transpose(processMagTempM5);
Matrix3x3d.mult(processMagTempM5, processMagTempM4, processMagTempM6);
Matrix3x3d.mult(mP, processMagTempM6, mK);
Matrix3x3d.mult(mK, mNu, mx);
Matrix3x3d.mult(mK, mH, processMagTempM4);
processMagTempM5.setIdentity();
processMagTempM5.minusEquals(processMagTempM4);
Matrix3x3d.mult(processMagTempM5, mP, processMagTempM4);
mP.set(processMagTempM4);
So3Util.sO3FromMu(mx, so3LastMotion);
Matrix3x3d.mult(so3LastMotion, so3SensorFromWorld, processMagTempM4);
so3SensorFromWorld.set(processMagTempM4);
updateCovariancesAfterMotion();
}
else
{
magObservationFunctionForNumericalJacobian(so3SensorFromWorld, mNu);
So3Util.sO3FromMu(mx, so3LastMotion);
Matrix3x3d.mult(so3LastMotion, so3SensorFromWorld, processMagTempM4);
so3SensorFromWorld.set(processMagTempM4);
updateCovariancesAfterMotion();
}
sensorTimeStampMag = sensorTimeStamp;
}