public void add(QuaternionBasics orientation, Vector3DReadOnly rotationVector) { tempRotation.setRotationVector(rotationVector); orientation.preMultiply(tempRotation); }
private QuaternionReadOnly exp(double alpha, Vector3DReadOnly rotation) { tempLogExpVector3D.setAndScale(alpha, rotation); tempLogExpQuaternion.setRotationVector(tempLogExpVector3D); return tempLogExpQuaternion; }
/** * Integrates the given {@code angularVelocity} to estimate the {@code finalOrientation}. The * given velocity is expected to be expressed in the local frame described by the given * {@code initialOrientation}. * <p> * Pseudo-code: * * <pre> * finalOrientation = initialOrientation + dt * angularVelocity * </pre> * </p> * * @param angularVelocity the angular velocity to integrate. Not modified. * @param initialOrientation the initial orientation to append the integrated angular velocity * to. Not modified. * @param finalOrientation the estimated orientation after integration. Modified. */ public void integrate(Vector3DReadOnly angularVelocity, Orientation3DReadOnly initialOrientation, Orientation3DBasics finalOrientation) { rotationVector.setAndScale(dt, angularVelocity); integrated.setRotationVector(rotationVector); finalOrientation.set(initialOrientation); finalOrientation.append(integrated); }
integrated.setRotationVector(rotationVector); finalOrientation.set(initialOrientation); finalOrientation.append(integrated);
angularVelocityTwo.scale(integrationDT); quaternionFutureOne.setRotationVector(angularVelocityOne); quaternionFutureTwo.setRotationVector(angularVelocityTwo);
difference.setRotationVector(limitedRotationVector); multiply(difference);
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); }
difference.setRotationVector(limitedRotationVector); quaternionFiltered.multiply(difference); set(quaternionFiltered);
actualQuaternion.setRotationVector(rotationVector); EuclidCoreTestTools.assertQuaternionGeometricallyEquals(expectedQuaternion, actualQuaternion, EPS);
assertEquals(initialFrame, frameQuaternion.getReferenceFrame()); frameQuaternion.setRotationVectorIncludingFrame(newFrame, rotationVector); quaternion.setRotationVector(rotationVector); assertEquals(newFrame, frameQuaternion.getReferenceFrame()); EuclidCoreTestTools.assertTuple4DEquals(quaternion, frameQuaternion, EPSILON); assertEquals(initialFrame, frameQuaternion.getReferenceFrame()); frameQuaternion.setRotationVectorIncludingFrame(rotationVector); quaternion.setRotationVector(rotationVector); assertEquals(newFrame, frameQuaternion.getReferenceFrame()); EuclidCoreTestTools.assertTuple4DEquals(quaternion, frameQuaternion, EPSILON);