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