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 void updateCovariancesAfterMotion()
{
so3LastMotion.transpose(updateCovariancesAfterMotionTempM1);
Matrix3x3d.mult(mP, updateCovariancesAfterMotionTempM1, updateCovariancesAfterMotionTempM2);
Matrix3x3d.mult(so3LastMotion, updateCovariancesAfterMotionTempM2, mP);
so3LastMotion.setIdentity();
}
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);
}