public OrientationAngularVelocityConsistencyChecker(String namePrefix, YoFrameQuaternion orientation, YoFrameVector angularVelocityToCheck, ReferenceFrame referenceFrameUsedForComparison, double updateDT, YoVariableRegistry parentRegistry) { registry = new YoVariableRegistry(namePrefix + "OrientationVelocityCheck"); this.referenceFrameUsedForComparison = referenceFrameUsedForComparison; localVelocityFromFD = new FiniteDifferenceAngularVelocityYoFrameVector(namePrefix + "referenceFD", orientation, updateDT, registry); this.angularVelocityToCheck = angularVelocityToCheck; int windowSize = 10; localVelocityFiltered = createSimpleMovingAverageFilteredYoFrameVector(namePrefix, "_referenceFiltered", windowSize, referenceFrameUsedForComparison, registry); filteredVelocityToCheck = createSimpleMovingAverageFilteredYoFrameVector(namePrefix, "_filtered", windowSize, referenceFrameUsedForComparison, registry); DelayEstimatorBetweenTwoSignals xVelocityDelayEstimator = new DelayEstimatorBetweenTwoSignals(namePrefix + "WX", localVelocityFiltered.getYoX(), filteredVelocityToCheck.getYoX(), updateDT, registry); DelayEstimatorBetweenTwoSignals yVelocityDelayEstimator = new DelayEstimatorBetweenTwoSignals(namePrefix + "WY", localVelocityFiltered.getYoY(), filteredVelocityToCheck.getYoY(), updateDT, registry); DelayEstimatorBetweenTwoSignals zVelocityDelayEstimator = new DelayEstimatorBetweenTwoSignals(namePrefix + "WZ", localVelocityFiltered.getYoZ(), filteredVelocityToCheck.getYoZ(), updateDT, registry); delayEstimators.put(Direction.X, xVelocityDelayEstimator); delayEstimators.put(Direction.Y, yVelocityDelayEstimator); delayEstimators.put(Direction.Z, zVelocityDelayEstimator); parentRegistry.addChild(registry); }
public IMUBasedPelvisRotationalStateUpdater(FullInverseDynamicsStructure inverseDynamicsStructure, List<? extends IMUSensorReadOnly> imuProcessedOutputs, IMUBiasProvider imuBiasProvider, IMUYawDriftEstimator imuYawDriftEstimator, double dt, YoVariableRegistry parentRegistry) { this.imuBiasProvider = imuBiasProvider; this.imuYawDriftEstimator = imuYawDriftEstimator; checkNumberOfSensors(imuProcessedOutputs); imuProcessedOutput = imuProcessedOutputs.get(0); rootJoint = inverseDynamicsStructure.getRootJoint(); rootJointFrame = rootJoint.getFrameAfterJoint(); twistCalculator = inverseDynamicsStructure.getTwistCalculator(); measurementFrame = imuProcessedOutput.getMeasurementFrame(); measurementLink = imuProcessedOutput.getMeasurementLink(); yoRootJointFrameOrientation = new YoFrameOrientation("estimatedRootJoint", worldFrame, registry); yoRootJointFrameQuaternion = new YoFrameQuaternion("estimatedRootJoint", worldFrame, registry); rootJointYawOffsetFromFrozenState = new DoubleYoVariable("rootJointYawOffsetFromFrozenState", registry); yoRootJointAngularVelocity = new YoFrameVector("estimatedRootJointAngularVelocity", rootJointFrame, registry); yoRootJointAngularVelocityInWorld = new YoFrameVector("estimatedRootJointAngularVelocityWorld", worldFrame, registry); yoRootJointAngularVelocityMeasFrame = new YoFrameVector("estimatedRootJointAngularVelocityMeasFrame", measurementFrame, registry); yoRootJointAngularVelocityFromFD = new FiniteDifferenceAngularVelocityYoFrameVector("estimatedRootJointAngularVelocityFromFD", yoRootJointFrameQuaternion, dt, registry); parentRegistry.addChild(registry); angularVelocityRootJointFrameRelativeToWorld = new FrameVector(rootJointFrame); }
rateLimitedQuaternion.update(new Quaternion()); FiniteDifferenceAngularVelocityYoFrameVector angularVelocity = new FiniteDifferenceAngularVelocityYoFrameVector("rate", rateLimitedQuaternion, dt, registry);
rateLimitedOrientation.update(new Quaternion()); FiniteDifferenceAngularVelocityYoFrameVector angularVelocity = new FiniteDifferenceAngularVelocityYoFrameVector("rate", ReferenceFrame.getWorldFrame(), dt, registry);