@Override public void update() { if (unfilteredQuaternion == null) { throw new NullPointerException("AlphaFilteredYoFrameQuaternion must be constructed with a non null " + "quaternion variable to call update(), otherwise use update(Quat4d)"); } qMeasured.set(unfilteredQuaternion); update(qMeasured); }
@Override public void update() { if (unfilteredQuaternion == null) { throw new NullPointerException("AlphaFilteredYoFrameQuaternion must be constructed with a non null " + "quaternion variable to call update(), otherwise use update(Quat4d)"); } unfilteredQuaternion.get(qMeasured); update(qMeasured); }
private void doRandomUpdates(AlphaFilteredYoFrameQuaternion q, Random random, int nUpdates) { for (int i = 0; i < nUpdates; i++) { // set measurement randomly and updated filtered version Quaternion qMeasured = RandomGeometry.nextQuaternion(random); q.getUnfilteredQuaternion().set(qMeasured); q.update(); } }
orientationBias.update(); orientationBiases.add(orientationBias);
yoOrientationBias.update(); yoOrientationBias.get(biasAxisAngle); orientationBiasMagnitudes.get(imuIndex).set(Math.abs(biasAxisAngle.getAngle()));
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testInitialValue() { MutableDouble alpha = new MutableDouble(); AlphaFilteredYoFrameQuaternion q = createAlphaFilteredYoFrameQuaternion(() -> alpha.doubleValue()); // set measurement randomly Random random = new Random(12351235L); Quaternion qMeasured = RandomGeometry.nextQuaternion(random); q.getUnfilteredQuaternion().set(qMeasured); // call update once q.update(); Quaternion qFiltered = new Quaternion(q); // verify measurement equals filtered EuclidCoreTestTools.assertQuaternionEquals(qMeasured, qFiltered, 1e-12); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testAlpha1() { MutableDouble alpha = new MutableDouble(); AlphaFilteredYoFrameQuaternion q = createAlphaFilteredYoFrameQuaternion(() -> alpha.doubleValue()); alpha.setValue(1.0); Random random = new Random(73464L); // update once Quaternion qInitial = RandomGeometry.nextQuaternion(random); q.getUnfilteredQuaternion().set(qInitial); q.update(); // update 100 more times int nUpdates = 100; doRandomUpdates(q, random, nUpdates); Quaternion qFiltered = new Quaternion(q); EuclidCoreTestTools.assertQuaternionGeometricallyEquals(qInitial, qFiltered, 1e-12); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testAlpha0() { MutableDouble alpha = new MutableDouble(); AlphaFilteredYoFrameQuaternion q = createAlphaFilteredYoFrameQuaternion(() -> alpha.doubleValue()); alpha.setValue(0.0); Random random = new Random(12525123L); // update 100 times int nUpdates = 100; doRandomUpdates(q, random, nUpdates); // update one more time Quaternion qFinal = RandomGeometry.nextQuaternion(random); q.getUnfilteredQuaternion().set(qFinal); q.update(); Quaternion qFiltered = new Quaternion(q); EuclidCoreTestTools.assertQuaternionGeometricallyEquals(qFinal, qFiltered, 1e-12); }
private void updateLocalBaseFrame() { originalBasePose.setToZero(originalBaseParentJointFrame); originalBasePose.changeFrame(worldFrame); originalBasePose.getPoseIncludingFrame(baseParentJointFramePosition, baseParentJointFrameOrientation); yoBaseParentJointFrameOrientation.set(baseParentJointFrameOrientation); yoBaseParentJointFramePosition.set(baseParentJointFramePosition); yoBaseParentJointFrameOrientationFiltered.update(); yoBaseParentJointFramePositionFiltered.update(); yoBaseParentJointFrameOrientationFiltered.getFrameOrientationIncludingFrame(baseParentJointFrameOrientation); yoBaseParentJointFramePositionFiltered.getFrameTupleIncludingFrame(baseParentJointFramePosition); localBaseParentJointFrame.setPoseAndUpdate(baseParentJointFramePosition, baseParentJointFrameOrientation); updateFrames(); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testStepChange() { MutableDouble alpha = new MutableDouble(); AlphaFilteredYoFrameQuaternion q = createAlphaFilteredYoFrameQuaternion(() -> alpha.doubleValue()); alpha.setValue(0.5); Random random = new Random(12525123L); // update once Quaternion qInitial = RandomGeometry.nextQuaternion(random); q.getUnfilteredQuaternion().set(qInitial); q.update(); // update a whole bunch of times using the same quaternion Quaternion qFinal = RandomGeometry.nextQuaternion(random); q.getUnfilteredQuaternion().set(qFinal); double angleDifference = getAngleDifference(qInitial, qFinal); double epsilon = 1e-3; int nUpdates = 100; Quaternion qFiltered = new Quaternion(); for (int i = 0; i < nUpdates; i++) { q.update(); qFiltered.set(q); double newAngleDifference = getAngleDifference(qFiltered, qFinal); // System.out.println(i + ": " + newAngleDifference); boolean sameQuaternion = newAngleDifference == 0.0; assertTrue(sameQuaternion || newAngleDifference < (1.0 + epsilon) * alpha.doubleValue() * angleDifference); angleDifference = newAngleDifference; } }