private void processAcceleration() { // Compute acceleration in world frame, subtracting off gravity: Vector3d acceleration = new Vector3d(); rawIMUSensors.getAcceleration(acceleration, imuIndex); acceleration.sub(accelerationOffset); // use rotationMatrixBeforeOffset instead of rotationMatrix, because the accelerations are measured in IMU body! // a(M') = R(M'S') * a(S') rotationMatrixBeforeOffset.transform(acceleration); acceleration.setZ(acceleration.getZ() - localGravityZ); FrameVector frameAcceleration = new FrameVector(ReferenceFrame.getWorldFrame(), acceleration); processedSensors.setAcceleration(frameAcceleration, imuIndex); }