private void updateUsingDifference(double difference) { double previousFilteredDerivative = getDoubleValue(); double currentRawDerivative = difference / dt; double alpha = alphaVariable == null ? alphaDouble : alphaVariable.getValue(); set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); }
private void updateUsingDifference(double difference) { double previousFilteredDerivative = getDoubleValue(); double currentRawDerivative = difference / dt; double alpha = alphaVariable == null ? alphaDouble : alphaVariable.getDoubleValue(); set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testUpdateForTranslationalVelocity() { YoVariableRegistry registry = new YoVariableRegistry("testRegistry"); double alpha = 0.3; YoDouble positionVariable = new YoDouble("positionVariable", registry); FilteredVelocityYoVariable filteredVelocityYoVariable = new FilteredVelocityYoVariable("filteredVelocityYoVariable", "test description", alpha, positionVariable, DT, registry); filteredVelocityYoVariable.set(0); positionVariable.set(0); for (int i = 0; i < 10000; i++) { positionVariable.add(10); filteredVelocityYoVariable.update(); } assertEquals(100, filteredVelocityYoVariable.getDoubleValue(), 1e-7); }
assertEquals(5, filteredVelocityYoVariable.getDoubleValue(), 1e-5);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testWithoutBacklash2() { Random rand = new Random(1798L); YoVariableRegistry registry = new YoVariableRegistry("blop"); YoDouble alphaVariable = new YoDouble("alpha", registry); alphaVariable.set(RandomNumbers.nextDouble(rand, 0.0, 1.0)); double dt = RandomNumbers.nextDouble(rand, 1e-8, 1.0); YoDouble slopTime = new YoDouble("slop", registry); YoDouble rawPosition = new YoDouble("rawPosition", registry); FilteredVelocityYoVariable filtVelocity = new FilteredVelocityYoVariable("filtVelocity", "", alphaVariable, rawPosition, dt, registry); BacklashCompensatingVelocityYoVariable filteredOnly = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, rawPosition, dt, slopTime, registry); filtVelocity.update(); filteredOnly.update(); for (int i = 0; i < 1000; i++) { alphaVariable.set(RandomNumbers.nextDouble(rand, 0.1, 1.0)); rawPosition.set(RandomNumbers.nextDouble(rand, -100.0, 100.0)); filtVelocity.update(); filteredOnly.update(); assertEquals(filtVelocity.getDoubleValue(), filteredOnly.getDoubleValue(), EPSILON); } }
rawAcceleration.update(rawRate.getDoubleValue()); processed.update(raw.getDoubleValue());
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testVelocityPositiveWithoutCrossingZero2() { Random rand = new Random(1798L); YoVariableRegistry registry = new YoVariableRegistry("blop"); YoDouble alphaVariable = new YoDouble("alpha", registry); alphaVariable.set(RandomNumbers.nextDouble(rand, 0.0, 1.0)); double dt = RandomNumbers.nextDouble(rand, 1e-8, 1.0); YoDouble slopTime = new YoDouble("slop", registry); YoDouble rawPosition = new YoDouble("rawPosition", registry); FilteredVelocityYoVariable filtVelocity = new FilteredVelocityYoVariable("filtVelocity", "", alphaVariable, rawPosition, dt, registry); BacklashCompensatingVelocityYoVariable backlashAndFiltered = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, rawPosition, dt, slopTime, registry); filtVelocity.update(); backlashAndFiltered.update(); for (int i = 0; i < 1000; i++) { slopTime.set(RandomNumbers.nextDouble(rand, 0.0, 100.0)); alphaVariable.set(RandomNumbers.nextDouble(rand, 0.1, 1.0)); rawPosition.add(RandomNumbers.nextDouble(rand, 0.0, 101.0)); filtVelocity.update(); backlashAndFiltered.update(); assertEquals(filtVelocity.getDoubleValue(), backlashAndFiltered.getDoubleValue(), EPSILON); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testWithoutBacklash1() { Random rand = new Random(1798L); YoVariableRegistry registry = new YoVariableRegistry("blop"); YoDouble alphaVariable = new YoDouble("alpha", registry); alphaVariable.set(RandomNumbers.nextDouble(rand, 0.1, 1.0)); double dt = RandomNumbers.nextDouble(rand, 1e-8, 1.0); YoDouble slopTime = new YoDouble("slop", registry); YoDouble rawPosition = new YoDouble("rawPosition", registry); FilteredVelocityYoVariable filtVelocity = new FilteredVelocityYoVariable("filtVelocity", "", alphaVariable, rawPosition, dt, registry); BacklashCompensatingVelocityYoVariable filteredOnly = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, dt, slopTime, registry); filtVelocity.update(); filteredOnly.update(rawPosition.getDoubleValue()); for (int i = 0; i < 1000; i++) { alphaVariable.set(RandomNumbers.nextDouble(rand, 0.1, 1.0)); rawPosition.set(RandomNumbers.nextDouble(rand, -100.0, 100.0)); filtVelocity.update(); filteredOnly.update(rawPosition.getDoubleValue()); assertEquals(filtVelocity.getDoubleValue(), filteredOnly.getDoubleValue(), EPSILON); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testVelocityNegativeWithoutCrossingZero2() { Random rand = new Random(1798L); YoVariableRegistry registry = new YoVariableRegistry("blop"); YoDouble alphaVariable = new YoDouble("alpha", registry); alphaVariable.set(RandomNumbers.nextDouble(rand, 0.0, 1.0)); double dt = RandomNumbers.nextDouble(rand, 1e-8, 1.0); YoDouble slopTime = new YoDouble("slop", registry); YoDouble rawPosition = new YoDouble("rawPosition", registry); FilteredVelocityYoVariable filtVelocity = new FilteredVelocityYoVariable("filtVelocity", "", alphaVariable, rawPosition, dt, registry); BacklashCompensatingVelocityYoVariable backlashAndFiltered = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, rawPosition, dt, slopTime, registry); filtVelocity.update(); backlashAndFiltered.update(); for (int i = 0; i < 1000; i++) { slopTime.set(RandomNumbers.nextDouble(rand, 0.0, 100.0)); alphaVariable.set(RandomNumbers.nextDouble(rand, 0.0, 1.0)); rawPosition.sub(RandomNumbers.nextDouble(rand, 0.0, 101.0)); filtVelocity.update(); backlashAndFiltered.update(); assertEquals(filtVelocity.getDoubleValue(), backlashAndFiltered.getDoubleValue(), EPSILON); } }
rawAcceleration.update(rawRate.getDoubleValue()); processed.update(raw.getDoubleValue()); if (VISUALIZE)
rawAcceleration.update(rawRate.getDoubleValue()); processed.update(raw.getDoubleValue());
rawAcceleration.update(rawRate.getDoubleValue()); processed.update(raw.getDoubleValue());
rawAcceleration.update(rawRate.getDoubleValue()); processed.update(raw.getDoubleValue()); if (VISUALIZE)
rawAcceleration.update(rawRate.getDoubleValue());
double velocityFromFiniteDifferences = finiteDifferenceVelocity.getDoubleValue();
double velocityFromFiniteDifferences = finiteDifferenceVelocity.getDoubleValue();
rawAcceleration.update(rawRate.getDoubleValue()); processed.update(raw.getDoubleValue());
yoIsFootDropPastThreshold.set(yoFootDropOrLift.getDoubleValue() < yoFootDropThreshold.getDoubleValue()); yoIsLoRStable.set(Math.abs(yoLoRAngularVelocityFiltered.getDoubleValue()) < yoStableLoRAngularVelocityThreshold.getDoubleValue());