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(); } }
public void update(Quat4d qMeasured) { if (hasBeenCalled.getBooleanValue()) { get(qPreviousFiltered); qNewFiltered.interpolate(qMeasured, qPreviousFiltered, alpha.getDoubleValue()); // qPreviousFiltered 'gets multiplied by alpha' set(qNewFiltered); } else { set(qMeasured); hasBeenCalled.set(true); } }
public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, double alpha, YoVariableRegistry registry) { this(namePrefix, nameSuffix, unfilteredQuaternion, createAlphaYoDouble(namePrefix, alpha, registry), registry); }
rawOrientationBiases.add(rawOrientationBias); AlphaFilteredYoFrameQuaternion orientationBias = new AlphaFilteredYoFrameQuaternion("estimated" + sensorName + "QuaternionBias", "", rawOrientationBias, biasAlphaFilter, registry); orientationBias.update(); orientationBiases.add(orientationBias);
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(); }
@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); }
/** * Add a low-pass filter stage on the orientations provided by the IMU sensors. * This is cumulative, by calling this method twice for instance, you will obtain a two pole low-pass filter. * @param alphaFilter low-pass filter parameter. * @param forVizOnly if set to true, the result will not be used as the input of the next processing stage, nor as the output of the sensor processing. * @param jointsToIgnore list of the names of the sensors to ignore. */ private Map<String, Integer> addIMUOrientationAlphaFilterWithSensorsToIgnore(DoubleYoVariable alphaFilter, boolean forVizOnly, List<String> sensorsToIgnore) { Map<String, Integer> processorIDs = new HashMap<>(); for (int i = 0; i < imuSensorDefinitions.size(); i++) { IMUDefinition imuDefinition = imuSensorDefinitions.get(i); String imuName = imuDefinition.getName(); if (sensorsToIgnore.contains(imuName)) continue; YoFrameQuaternion intermediateOrientation = intermediateOrientations.get(imuDefinition); List<ProcessingYoVariable> processors = processedOrientations.get(imuDefinition); String prefix = IMU_ORIENTATION.getProcessorNamePrefix(ALPHA_FILTER); int newProcessorID = processors.size(); processorIDs.put(imuName, newProcessorID); String suffix = IMU_ORIENTATION.getProcessorNameSuffix(imuName, newProcessorID); AlphaFilteredYoFrameQuaternion filteredOrientation = new AlphaFilteredYoFrameQuaternion(prefix, suffix, intermediateOrientation, alphaFilter, registry); processors.add(filteredOrientation); if (!forVizOnly) intermediateOrientations.put(imuDefinition, filteredOrientation); } return processorIDs; }
public void update(Quaternion qMeasured) { if (hasBeenCalled.getBooleanValue()) { qPreviousFiltered.set(this); qNewFiltered.interpolate(qMeasured, qPreviousFiltered, alpha.getValue()); // qPreviousFiltered 'gets multiplied by alpha' set(qNewFiltered); } else { set(qMeasured); hasBeenCalled.set(true); } }
@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); }
private AlphaFilteredYoFrameQuaternion createAlphaFilteredYoFrameQuaternion(DoubleProvider alpha) { YoVariableRegistry registry = new YoVariableRegistry("test"); ReferenceFrame referenceFrame = ReferenceFrame.getWorldFrame(); YoFrameQuaternion unfilteredQuaternion = new YoFrameQuaternion("qMeasured", referenceFrame, registry); AlphaFilteredYoFrameQuaternion q = new AlphaFilteredYoFrameQuaternion("qFiltered", "", unfilteredQuaternion, alpha, registry); return q; }
@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); }
yoBaseParentJointFramePositionFiltered = AlphaFilteredYoFramePoint.createAlphaFilteredYoFramePoint(namePrefix + "BaseParentJointFrameFiltered", "", registry, alphaBaseParentJointPose, yoBaseParentJointFramePosition); yoBaseParentJointFrameOrientationFiltered = new AlphaFilteredYoFrameQuaternion(namePrefix + "BaseParentJointFrameFiltered", "", yoBaseParentJointFrameOrientation, alphaBaseParentJointPose, registry);
private AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, DoubleProvider alpha, ReferenceFrame referenceFrame, YoVariableRegistry registry) { super(namePrefix, nameSuffix, referenceFrame, registry); this.unfilteredQuaternion = unfilteredQuaternion; if (alpha == null) alpha = createAlphaYoDouble(namePrefix, 0.0, registry); this.alpha = alpha; this.hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); }
@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); }
@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; } }