/** * Multiplies this quaternion by the inverse of quaternion q1 and places * the value into this quaternion. The value of the argument quaternion * is preserved (this = this * q^-1). * @param q1 the other quaternion */ public final void mulInverse(Quat4d q1) { Quat4d tempQuat = new Quat4d(q1); tempQuat.inverse(); this.mul(tempQuat); }
/** * Multiplies quaternion q1 by the inverse of quaternion q2 and places * the value into this quaternion. The value of both argument quaternions * is preservered (this = q1 * q2^-1). * @param q1 the first quaternion * @param q2 the second quaternion */ public final void mulInverse(Quat4d q1, Quat4d q2) { Quat4d tempQuat = new Quat4d(q2); tempQuat.inverse(); this.mul(q1, tempQuat); }
public void startComputation() { imuMount.getOrientation(rotationMatrix); yoFrameQuaternionPerfect.set(rotationMatrix); corrupt(rotationMatrix); orientationOutputPort.setData(rotationMatrix); yoFrameQuaternionNoisy.set(rotationMatrix); yoFrameQuaternionPerfect.get(tempQuaternionOne); yoFrameQuaternionNoisy.get(tempQuaternionTwo); tempQuaternionTwo.inverse(); tempQuaternionOne.mul(tempQuaternionTwo); tempAxisAngle.set(tempQuaternionOne); double noiseAngle = tempAxisAngle.getAngle(); if (noiseAngle > Math.PI) noiseAngle = noiseAngle - 2.0 * Math.PI; if (noiseAngle < -Math.PI) noiseAngle = noiseAngle + 2.0 * Math.PI; rotationAngleNoise.set(Math.abs(noiseAngle)); }
public void startComputation() { frameUsedForPerfectOrientation.getTransformToDesiredFrame(worldFrame).getRotation(rotationMatrix); yoFrameQuaternionPerfect.set(rotationMatrix); corrupt(rotationMatrix); orientationOutputPort.setData(rotationMatrix); yoFrameQuaternionNoisy.set(rotationMatrix); yoFrameQuaternionPerfect.get(tempQuaternionOne); yoFrameQuaternionNoisy.get(tempQuaternionTwo); tempQuaternionTwo.inverse(); tempQuaternionOne.mul(tempQuaternionTwo); tempAxisAngle.set(tempQuaternionOne); double noiseAngle = tempAxisAngle.getAngle(); if (noiseAngle > Math.PI) noiseAngle = noiseAngle - 2.0 * Math.PI; if (noiseAngle < -Math.PI) noiseAngle = noiseAngle + 2.0 * Math.PI; rotationAngleNoise.set(Math.abs(noiseAngle)); }
orientationResidual.inverse(); orientationResidual.mul(actualOrientationMeasurement);
quatFD1.inverse(); quatFDDelta.mul(quatFD3, quatFD1); quatFDDelta.normalize();