public boolean getHasBufferWindowFilled() { return x.getHasBufferWindowFilled() && y.getHasBufferWindowFilled() && z.getHasBufferWindowFilled(); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testWithFixedSizeDoubleArrays() throws Exception { for (int i = 0; i < 100; i++) { YoVariableRegistry registry = new YoVariableRegistry("Blop"); Random random = new Random(6541654L); int windowSize = RandomNumbers.nextInt(random, 1, 1000); SimpleMovingAverageFilteredYoVariable sma = new SimpleMovingAverageFilteredYoVariable("tested", windowSize, registry); double amplitude = 100.0; double[] randomArray = RandomNumbers.nextDoubleArray(random, windowSize, amplitude); double expected = 0.0; for (double val : randomArray) expected += val / windowSize; for (int j = 0; j < randomArray.length; j++) { assertFalse(sma.getHasBufferWindowFilled()); sma.update(randomArray[j]); } assertTrue(sma.getHasBufferWindowFilled()); assertEquals(expected, sma.getDoubleValue(), 1.0e-10); } } }
@Override public void update() { referenceVelocity.update(); referenceVelocityFiltered.update(); filteredRawVelocityToCheck.update(); filteredProcessedVelocityToCheck.update(); if (!referenceVelocityFiltered.getHasBufferWindowFilled()) return; delayEstimatorProcessedPosition.update(); delayEstimatorRawVelocity.update(); delayEstimatorProcessedVelocity.update(); }
public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, ReferenceFrame referenceFrame, YoVariableRegistry registry) { String xName = createXName(namePrefix, nameSuffix); String yName = createYName(namePrefix, nameSuffix); String zName = createZName(namePrefix, nameSuffix); SimpleMovingAverageFilteredYoVariable x = new SimpleMovingAverageFilteredYoVariable(xName, windowSize, registry); SimpleMovingAverageFilteredYoVariable y = new SimpleMovingAverageFilteredYoVariable(yName, windowSize, registry); SimpleMovingAverageFilteredYoVariable z = new SimpleMovingAverageFilteredYoVariable(zName, windowSize, registry); return new SimpleMovingAverageFilteredYoFrameVector(x, y, z, referenceFrame); }
public void update(double value) { if (previousUpdateValues.getNumRows() != windowSize.getIntegerValue()) { reset(); } previousUpdateValues.set(bufferPosition, 0, value); bufferPosition++; if (bufferPosition >= windowSize.getIntegerValue()) { bufferPosition = 0; bufferHasBeenFilled = true; } double average = 0; for (int i = 0; i < windowSize.getIntegerValue(); i++) { average += previousUpdateValues.get(i, 0); } this.set(average / windowSize.getIntegerValue()); }
public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, ReferenceFrame referenceFrame, YoVariableRegistry registry) { String xName = createXName(namePrefix, nameSuffix); String yName = createYName(namePrefix, nameSuffix); String zName = createZName(namePrefix, nameSuffix); SimpleMovingAverageFilteredYoVariable x = new SimpleMovingAverageFilteredYoVariable(xName, windowSize, registry); SimpleMovingAverageFilteredYoVariable y = new SimpleMovingAverageFilteredYoVariable(yName, windowSize, registry); SimpleMovingAverageFilteredYoVariable z = new SimpleMovingAverageFilteredYoVariable(zName, windowSize, registry); return new SimpleMovingAverageFilteredYoFrameVector(x, y, z, referenceFrame); }
public void update(double value) { if (previousUpdateValues.getNumRows() != windowSize.getIntegerValue()) { reset(); } previousUpdateValues.set(bufferPosition, 0, value); bufferPosition++; if (bufferPosition >= windowSize.getIntegerValue()) { bufferPosition = 0; bufferHasBeenFilled = true; } double average = 0; for (int i = 0; i < windowSize.getIntegerValue(); i++) { average += previousUpdateValues.get(i, 0); } this.set(average / windowSize.getIntegerValue()); }
public PositionVelocity1DConsistencyChecker(String namePrefix, DoubleYoVariable rawPosition, DoubleYoVariable rawVelocityToCheck, DoubleYoVariable processedPositionToCheck, DoubleYoVariable processedVelocityToCheck, double dt, YoVariableRegistry parentRegistry) { this.dt = dt; registry = new YoVariableRegistry(namePrefix + "PositionVelocity1DCheck"); referenceVelocity = new FilteredVelocityYoVariable(namePrefix + "_referenceVelocity", "", 0.0, rawPosition, dt, registry); int windowSize = 10; referenceVelocityFiltered = new SimpleMovingAverageFilteredYoVariable(namePrefix + "_referenceVelocityFiltered", windowSize, referenceVelocity, registry); filteredRawVelocityToCheck = new SimpleMovingAverageFilteredYoVariable(namePrefix + "_filteredRawVelocity", windowSize, rawVelocityToCheck, registry); filteredProcessedVelocityToCheck = new SimpleMovingAverageFilteredYoVariable(namePrefix + "_filteredProcessedVelocity", windowSize, processedVelocityToCheck, registry); delayEstimatorProcessedPosition = new DelayEstimatorBetweenTwoSignals(namePrefix + "ProcessedPosition", rawPosition, processedPositionToCheck, dt, registry); delayEstimatorRawVelocity = new DelayEstimatorBetweenTwoSignals(namePrefix + "RawVelocity", referenceVelocityFiltered, filteredRawVelocityToCheck, dt, registry); delayEstimatorProcessedVelocity = new DelayEstimatorBetweenTwoSignals(namePrefix + "ProcessedVelocity", referenceVelocityFiltered, filteredProcessedVelocityToCheck, dt, registry); parentRegistry.addChild(registry); }
public boolean getHasBufferWindowFilled() { return x.getHasBufferWindowFilled() && y.getHasBufferWindowFilled() && z.getHasBufferWindowFilled(); } }
public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, YoFrameVector3D unfilteredVector, YoVariableRegistry registry) { String xName = createXName(namePrefix, nameSuffix); String yName = createYName(namePrefix, nameSuffix); String zName = createZName(namePrefix, nameSuffix); SimpleMovingAverageFilteredYoVariable x = new SimpleMovingAverageFilteredYoVariable(xName, windowSize, unfilteredVector.getYoX(), registry); SimpleMovingAverageFilteredYoVariable y = new SimpleMovingAverageFilteredYoVariable(yName, windowSize, unfilteredVector.getYoY(), registry); SimpleMovingAverageFilteredYoVariable z = new SimpleMovingAverageFilteredYoVariable(zName, windowSize, unfilteredVector.getYoZ(), registry); return new SimpleMovingAverageFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); }
public void setWindowSize(int windowSize) { this.windowSize.set(windowSize); reset(); }
public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, YoFrameVector unfilteredVector, YoVariableRegistry registry) { String xName = createXName(namePrefix, nameSuffix); String yName = createYName(namePrefix, nameSuffix); String zName = createZName(namePrefix, nameSuffix); SimpleMovingAverageFilteredYoVariable x = new SimpleMovingAverageFilteredYoVariable(xName, windowSize, unfilteredVector.getYoX(), registry); SimpleMovingAverageFilteredYoVariable y = new SimpleMovingAverageFilteredYoVariable(yName, windowSize, unfilteredVector.getYoY(), registry); SimpleMovingAverageFilteredYoVariable z = new SimpleMovingAverageFilteredYoVariable(zName, windowSize, unfilteredVector.getYoZ(), registry); return new SimpleMovingAverageFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); }
public void setWindowSize(int windowSize) { this.windowSize.set(windowSize); reset(); }