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; }