private void initialize(Vector3DReadOnly acceleration, Vector3DReadOnly magneticVector) { boolean success = computeOrientationError(estimatedOrientation, acceleration, magneticVector, quaternionUpdate); if (!success) return; estimatedOrientation.multiply(quaternionUpdate); yoIntegralTerm.setToZero(); hasBeenInitialized.set(true); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testMultiplication() { Random random = new Random(1776L); YoVariableRegistry registry = new YoVariableRegistry("blop"); YoFrameQuaternion yoFrameQuaternion = new YoFrameQuaternion("test", worldFrame, registry); Quaternion quat4dActual = new Quaternion(), quat4dExpected = new Quaternion(); Quaternion quat4dA, quat4dB; FrameQuaternion frameOrientation = new FrameQuaternion(worldFrame); for (int i = 0; i < 1000; i++) { quat4dA = RandomGeometry.nextQuaternion(random); quat4dB = RandomGeometry.nextQuaternion(random); quat4dExpected.multiply(quat4dA, quat4dB); yoFrameQuaternion.set(quat4dA); yoFrameQuaternion.multiply(quat4dB); quat4dActual.set(yoFrameQuaternion); assertTrue(RotationTools.quaternionEpsilonEquals(quat4dExpected, quat4dActual, EPS)); yoFrameQuaternion.set(quat4dA); frameOrientation.set(quat4dB); yoFrameQuaternion.multiply(frameOrientation); quat4dActual.set(yoFrameQuaternion); assertTrue(RotationTools.quaternionEpsilonEquals(quat4dExpected, quat4dActual, EPS)); } }
public void update(Vector3DReadOnly inputAngularVelocity, Vector3DReadOnly inputLinearAcceleration, Vector3DReadOnly inputMagneticVector) { if (!hasBeenInitialized.getValue()) { initialize(inputLinearAcceleration, inputMagneticVector); return; } boolean success = computeOrientationError((QuaternionReadOnly) estimatedOrientation, inputLinearAcceleration, inputMagneticVector, quaternionUpdate); if (success) { quaternionUpdate.getRotationVector(totalError); yoErrorTerm.set(totalError); integralTerm.scaleAdd(integralGain.getValue() * updateDT, yoErrorTerm, yoIntegralTerm); yoIntegralTerm.set(integralTerm); angularVelocityTerm.scaleAdd(proportionalGain.getValue(), totalError, inputAngularVelocity); angularVelocityTerm.add(integralTerm); } else { yoErrorTerm.setToZero(); angularVelocityTerm.set(inputAngularVelocity); } rotationUpdate.setAndScale(updateDT, angularVelocityTerm); quaternionUpdate.setRotationVector(rotationUpdate); estimatedOrientation.multiply(quaternionUpdate); if (estimatedAngularVelocity != null) estimatedAngularVelocity.add(inputAngularVelocity, integralTerm); }