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