/** * 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; }
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; }
rawOrientationBiases.add(rawOrientationBias); AlphaFilteredYoFrameQuaternion orientationBias = new AlphaFilteredYoFrameQuaternion("estimated" + sensorName + "QuaternionBias", "", rawOrientationBias, biasAlphaFilter, registry); orientationBias.update(); orientationBiases.add(orientationBias);
yoBaseParentJointFramePositionFiltered = AlphaFilteredYoFramePoint.createAlphaFilteredYoFramePoint(namePrefix + "BaseParentJointFrameFiltered", "", registry, alphaBaseParentJointPose, yoBaseParentJointFramePosition); yoBaseParentJointFrameOrientationFiltered = new AlphaFilteredYoFrameQuaternion(namePrefix + "BaseParentJointFrameFiltered", "", yoBaseParentJointFrameOrientation, alphaBaseParentJointPose, registry);