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 /*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 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 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 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));
}