diff --git a/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java
deleted file mode 100644
index 448326c4..00000000
--- a/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java
+++ /dev/null
@@ -1,43 +0,0 @@
-package us.ihmc.yoVariables.euclid.filters;
-
-import us.ihmc.euclid.transform.RigidBodyTransform;
-
-public class AlphaFilteredRigidBodyTransform extends RigidBodyTransform
-{
- // an alpha of zero applies zero filtering, accepting the entirely new input.
- private double alpha = 0.0;
- private final RigidBodyTransform previousFiltered = new RigidBodyTransform();
-
- public AlphaFilteredRigidBodyTransform()
- {
- reset();
- }
-
- public void update(RigidBodyTransform measured)
- {
- if (containsNaN())
- {
- set(measured);
- }
- else
- {
- previousFiltered.set(this);
- interpolate(measured, previousFiltered, alpha);
- }
- }
-
- public double getAlpha()
- {
- return alpha;
- }
-
- public void setAlpha(double alpha)
- {
- this.alpha = alpha;
- }
-
- public void reset()
- {
- setToNaN();
- }
-}
diff --git a/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java
deleted file mode 100644
index 59d059c0..00000000
--- a/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java
+++ /dev/null
@@ -1,108 +0,0 @@
-package us.ihmc.yoVariables.euclid.filters;
-
-import org.apache.commons.lang3.NotImplementedException;
-
-import us.ihmc.euclid.interfaces.EuclidGeometry;
-import us.ihmc.euclid.tools.EuclidCoreTools;
-import us.ihmc.euclid.transform.interfaces.Transform;
-import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics;
-import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly;
-import us.ihmc.yoVariables.providers.DoubleProvider;
-
-public class AlphaFilteredTuple2D implements Tuple2DBasics
-{
- private final DoubleProvider alpha;
-
- private boolean resetX;
- private boolean resetY;
-
- private double x;
- private double y;
-
- public AlphaFilteredTuple2D(DoubleProvider alpha)
- {
- this.alpha = alpha;
- reset();
- }
-
- public AlphaFilteredTuple2D(Tuple2DReadOnly initialValue, DoubleProvider alpha)
- {
- this.alpha = alpha;
- reset(initialValue);
- }
-
- public void reset()
- {
- resetX = true;
- resetY = true;
- }
-
- public void reset(Tuple2DReadOnly other)
- {
- reset(other.getX(), other.getY());
- }
-
- public void reset(double x, double y)
- {
- reset();
- set(x, y);
- }
-
- @Override
- public double getX()
- {
- return x;
- }
-
- @Override
- public double getY()
- {
- return y;
- }
-
- @Override
- public void setX(double x)
- {
- if (resetX)
- {
- this.x = x;
- resetX = false;
- }
- else
- {
- this.x = EuclidCoreTools.interpolate(x, this.x, alpha.getValue());
- }
- }
-
- @Override
- public void setY(double y)
- {
- if (resetY)
- {
- this.y = y;
- resetY = false;
- }
- else
- {
- this.y = EuclidCoreTools.interpolate(y, this.y, alpha.getValue());
- }
- }
-
- @Override
- public boolean geometricallyEquals(EuclidGeometry geometry, double epsilon)
- {
- return epsilonEquals(geometry, epsilon);
- }
-
- @Override
- public void applyTransform(Transform transform, boolean checkIfTransformInXYPlane)
- {
- throw new NotImplementedException("Not supported by " + getClass().getSimpleName() + ".");
- }
-
- @Override
- public void applyInverseTransform(Transform transform, boolean checkIfTransformInXYPlane)
- {
- throw new NotImplementedException("Not supported by " + getClass().getSimpleName() + ".");
- }
-}
diff --git a/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java
deleted file mode 100644
index e2010930..00000000
--- a/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java
+++ /dev/null
@@ -1,135 +0,0 @@
-package us.ihmc.yoVariables.euclid.filters;
-
-import org.apache.commons.lang3.NotImplementedException;
-import us.ihmc.euclid.interfaces.EuclidGeometry;
-import us.ihmc.euclid.tools.EuclidCoreTools;
-import us.ihmc.euclid.transform.interfaces.Transform;
-import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;
-import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
-import us.ihmc.yoVariables.providers.DoubleProvider;
-
-public class AlphaFilteredTuple3D implements Tuple3DBasics
-{
- private final DoubleProvider alpha;
-
- private double x = Double.NaN;
- private double y = Double.NaN;
- private double z = Double.NaN;
-
- private boolean resetX = false;
- private boolean resetY = false;
- private boolean resetZ = false;
-
- public AlphaFilteredTuple3D(DoubleProvider alpha)
- {
- this.alpha = alpha;
- }
-
- public AlphaFilteredTuple3D(double x, double y, double z, DoubleProvider alpha)
- {
- this.alpha = alpha;
- reset(x, y, z);
- }
-
- public AlphaFilteredTuple3D(Tuple3DReadOnly other, DoubleProvider alpha)
- {
- this.alpha = alpha;
- reset(other);
- }
-
- public void reset()
- {
- resetX = true;
- resetY = true;
- resetZ = true;
- }
-
- public void reset(Tuple3DReadOnly other)
- {
- reset(other.getX(), other.getY(), other.getZ());
- }
-
- public void reset(double x, double y, double z)
- {
- reset();
- set(x, y, z);
- }
-
- @Override
- public void setX(double x)
- {
- if (resetX || Double.isNaN(this.x))
- {
- this.x = x;
- resetX = false;
- }
- else
- {
- this.x = EuclidCoreTools.interpolate(x, this.x, alpha.getValue());
- }
- }
-
- @Override
- public void setY(double y)
- {
- if (resetY || Double.isNaN(this.y))
- {
- this.y = y;
- resetY = false;
- }
- else
- {
- this.y = EuclidCoreTools.interpolate(y, this.y, alpha.getValue());
- }
- }
-
- @Override
- public void setZ(double z)
- {
- if (resetZ || Double.isNaN(this.z))
- {
- this.z = z;
- resetZ = false;
- }
- else
- {
- this.z = EuclidCoreTools.interpolate(z, this.z, alpha.getValue());
- }
- }
-
- @Override
- public double getX()
- {
- return x;
- }
-
- @Override
- public double getY()
- {
- return y;
- }
-
- @Override
- public double getZ()
- {
- return z;
- }
-
- @Override
- public boolean geometricallyEquals(EuclidGeometry geometry, double epsilon)
- {
- return epsilonEquals(geometry, epsilon);
- }
-
- @Override
- public void applyTransform(Transform transform)
- {
- throw new NotImplementedException("Sorry mate, " + getClass().getSimpleName() + " doesn't implement this method.");
- }
-
- @Override
- public void applyInverseTransform(Transform transform)
- {
- throw new NotImplementedException("Sorry mate, " + getClass().getSimpleName() + " doesn't implement this method.");
- }
-}
diff --git a/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java
index e06c568e..8228d169 100644
--- a/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java
+++ b/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java
@@ -4,29 +4,29 @@
import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly;
import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;
import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D;
-import us.ihmc.yoVariables.filters.MovingAverageYoDouble;
+import us.ihmc.yoVariables.filters.SimpleMovingAverageFilteredYoVariable;
import us.ihmc.yoVariables.registry.YoRegistry;
import us.ihmc.yoVariables.tools.YoGeometryNameTools;
public class MovingAverageYoFramePoint2D extends YoFramePoint2D
{
- private final MovingAverageYoDouble x, y;
+ private final SimpleMovingAverageFilteredYoVariable x, y;
- public MovingAverageYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, int beta, ReferenceFrame referenceFrame)
+ public MovingAverageYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, int windowSize, ReferenceFrame referenceFrame)
{
- this(new MovingAverageYoDouble(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta),
- new MovingAverageYoDouble(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta),
+ this(new SimpleMovingAverageFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), windowSize, registry),
+ new SimpleMovingAverageFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), windowSize, registry),
referenceFrame);
}
- public MovingAverageYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, int beta, YoFramePoint2D unfilteredPoint)
+ public MovingAverageYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, int windowSize, YoFramePoint2D unfilteredPoint)
{
- this(new MovingAverageYoDouble(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoX()),
- new MovingAverageYoDouble(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoY()),
+ this(new SimpleMovingAverageFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), windowSize, unfilteredPoint.getYoX(), registry),
+ new SimpleMovingAverageFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), windowSize, unfilteredPoint.getYoY(), registry),
unfilteredPoint.getReferenceFrame());
}
- private MovingAverageYoFramePoint2D(MovingAverageYoDouble x, MovingAverageYoDouble y, ReferenceFrame referenceFrame)
+ private MovingAverageYoFramePoint2D(SimpleMovingAverageFilteredYoVariable x, SimpleMovingAverageFilteredYoVariable y, ReferenceFrame referenceFrame)
{
super(x, y, referenceFrame);
diff --git a/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java
index cebccec1..8484cd6b 100644
--- a/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java
+++ b/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java
@@ -4,29 +4,29 @@
import us.ihmc.euclid.referenceFrame.interfaces.FrameVector2DReadOnly;
import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly;
import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D;
-import us.ihmc.yoVariables.filters.MovingAverageYoDouble;
+import us.ihmc.yoVariables.filters.SimpleMovingAverageFilteredYoVariable;
import us.ihmc.yoVariables.registry.YoRegistry;
import us.ihmc.yoVariables.tools.YoGeometryNameTools;
public class MovingAverageYoFrameVector2D extends YoFrameVector2D
{
- private final MovingAverageYoDouble x, y;
+ private final SimpleMovingAverageFilteredYoVariable x, y;
- public MovingAverageYoFrameVector2D(String namePrefix, String nameSuffix, YoRegistry registry, int beta, ReferenceFrame referenceFrame)
+ public MovingAverageYoFrameVector2D(String namePrefix, String nameSuffix, YoRegistry registry, int windowSize, ReferenceFrame referenceFrame)
{
- this(new MovingAverageYoDouble(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta),
- new MovingAverageYoDouble(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta),
+ this(new SimpleMovingAverageFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), windowSize, registry),
+ new SimpleMovingAverageFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), windowSize, registry),
referenceFrame);
}
- public MovingAverageYoFrameVector2D(String namePrefix, String nameSuffix, YoRegistry registry, int beta, YoFrameVector2D unfilteredVector)
+ public MovingAverageYoFrameVector2D(String namePrefix, String nameSuffix, YoRegistry registry, int windowSize, YoFrameVector2D unfilteredVector)
{
- this(new MovingAverageYoDouble(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoX()),
- new MovingAverageYoDouble(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoY()),
+ this(new SimpleMovingAverageFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), windowSize, unfilteredVector.getYoX(), registry),
+ new SimpleMovingAverageFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), windowSize, unfilteredVector.getYoY(), registry),
unfilteredVector.getReferenceFrame());
}
- private MovingAverageYoFrameVector2D(MovingAverageYoDouble x, MovingAverageYoDouble y, ReferenceFrame referenceFrame)
+ private MovingAverageYoFrameVector2D(SimpleMovingAverageFilteredYoVariable x, SimpleMovingAverageFilteredYoVariable y, ReferenceFrame referenceFrame)
{
super(x, y, referenceFrame);
diff --git a/src/filters/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java
deleted file mode 100644
index bad8f2db..00000000
--- a/src/filters/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java
+++ /dev/null
@@ -1,114 +0,0 @@
-package us.ihmc.yoVariables.filters;
-
-import us.ihmc.yoVariables.registry.YoRegistry;
-import us.ihmc.yoVariables.variable.YoBoolean;
-import us.ihmc.yoVariables.variable.YoDouble;
-
-/**
- * @author thutcheson
- *
- *
- *
- *
- * A BetaFilteredYoVariable is a filtered version of an input YoVar.
- * This is a moving average filter.
- * Either a YoVariable holding the unfiltered val is passed in to the
- * constructor and update() is called every tick, or update(double) is
- * called every tick. The BetaFilteredYoVariable updates it's val
- * with the current filtered version using
- *
- *
- * filtered_{n} = (raw_{0} + ... + raw_{n-1} + raw_{n}) / n
- *
- */
-public class MovingAverageYoDouble extends YoDouble
-{
- private int beta;
- private int index = 0;
- @SuppressWarnings("unused")
- private final YoDouble betaVariable;
-
- private final YoDouble position;
-
- private final double[] raw;
- private boolean bufferFull = false;
-
- private final YoBoolean hasBeenCalled;
-
- public MovingAverageYoDouble(String name, YoRegistry registry, int beta)
- {
- this(name, "", registry, beta, null);
- }
-
- public MovingAverageYoDouble(String name, String description, YoRegistry registry, int beta)
- {
- this(name, description, registry, beta, null);
- }
-
- public MovingAverageYoDouble(String name, YoRegistry registry, int beta, YoDouble positionVariable)
- {
- this(name, "", registry, beta, positionVariable);
- }
-
- public MovingAverageYoDouble(String name, String description, YoRegistry registry, int beta, YoDouble positionVariable)
- {
- super(name, description, registry);
- this.hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(name, "", registry);
-
- this.beta = beta;
- this.position = positionVariable;
- this.betaVariable = null;
-
- raw = new double[beta];
-
- reset();
- }
-
- public void reset()
- {
- hasBeenCalled.set(false);
- bufferFull = false;
- index = 0;
-
- for (int i = 0; i < beta; i++)
- {
- set(0.0);
- }
- }
-
- public void update()
- {
- if (position == null)
- {
- throw new NullPointerException("BetaFilteredYoVariable must be constructed with a non null "
- + "position variable to call update(), otherwise use update(double)");
- }
-
- update(position.getDoubleValue());
- }
-
- public void update(double currentPosition)
- {
- if (!hasBeenCalled.getBooleanValue())
- {
- hasBeenCalled.set(true);
- set(currentPosition, false);
- }
-
- raw[index++] = currentPosition;
- if (index == beta)
- {
- index = 0;
- bufferFull = true;
- }
-
- final int size = bufferFull ? beta : index;
- double value = 0.0;
- for (int i = 0; i < size; i++)
- {
- value += raw[i];
- }
-
- set(value / size);
- }
-}
diff --git a/src/filters/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java
index b6a0b8f0..5492d5e7 100644
--- a/src/filters/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java
+++ b/src/filters/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java
@@ -71,7 +71,8 @@ public void update(double value)
average += previousUpdateValues.get(i, 0);
}
- this.set(average / ((double) windowSize.getIntegerValue()));
+ final int size = bufferHasBeenFilled ? windowSize.getIntegerValue() : bufferPosition;
+ this.set(average / ((double) size));
}
public void reset()
diff --git a/src/main/java/us/ihmc/yoVariables/math/YoMatrix.java b/src/main/java/us/ihmc/yoVariables/math/YoMatrix.java
index e7327a45..a426f73d 100644
--- a/src/main/java/us/ihmc/yoVariables/math/YoMatrix.java
+++ b/src/main/java/us/ihmc/yoVariables/math/YoMatrix.java
@@ -637,6 +637,18 @@ public int getNumElements()
return getNumRows() * getNumCols();
}
+ /**
+ * Returns a yo double contained at {@code row} {@code col}
+ * @param row row in question
+ * @param col column in question
+ *
+ * @return yo double contained at that position
+ */
+ public YoDouble getYoDouble(int row, int col)
+ {
+ return variables[row][col];
+ }
+
@Override
public MatrixType getType()
{
diff --git a/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransformTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransformTest.java
deleted file mode 100644
index 91280c6d..00000000
--- a/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransformTest.java
+++ /dev/null
@@ -1,152 +0,0 @@
-package us.ihmc.yoVariables.euclid.filters;
-
-import org.junit.jupiter.api.Test;
-import us.ihmc.euclid.tools.EuclidCoreTestTools;
-import us.ihmc.euclid.transform.RigidBodyTransform;
-import us.ihmc.euclid.tuple3D.Point3D;
-import us.ihmc.euclid.yawPitchRoll.YawPitchRoll;
-
-import java.util.Random;
-
-public class AlphaFilteredRigidBodyTransformTest
-{
- private static final double EPSILON = 1.0e-15;
-
- @Test
- public void testRegression()
- {
- Random random = new Random(3453456);
-
- double alpha = random.nextDouble();
-
- AlphaFilteredRigidBodyTransform filteredRigidBodyTransform = new AlphaFilteredRigidBodyTransform();
- filteredRigidBodyTransform.setAlpha(0.9);
-
- RigidBodyTransform unfilteredRigidBodyTransform = new RigidBodyTransform();
-
- unfilteredRigidBodyTransform.getTranslation().add(new Point3D(0.1, 0.1, 0.1));
- unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, 0.1, 0.1));
-
- filteredRigidBodyTransform.update(unfilteredRigidBodyTransform);
-
- System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform));
-
- EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString("""
- 0.9900332889206207 -0.0894177463594257 0.1088051169064211 | 0.1000000000000000
- 0.0993346653975306 0.9910282997404070 -0.0894177463594257 | 0.1000000000000000
- -0.0998334166468282 0.0993346653975306 0.9900332889206207 | 0.1000000000000000
- 0.0000000000000000 0.0000000000000000 0.0000000000000000 | 1.0000000000000000
- """), filteredRigidBodyTransform, EPSILON);
- unfilteredRigidBodyTransform.getTranslation().add(new Point3D(0.1, 0.1, 0.1));
- unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, 0.1, 0.1));
-
- filteredRigidBodyTransform.update(unfilteredRigidBodyTransform);
-
- System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform));
-
- EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString("""
- 0.9879463963435010 -0.0977116814683914 0.1200597570233632 | 0.1100000000000000
- 0.1097050673132180 0.9891497487794362 -0.0977116814683914 | 0.1100000000000000
- -0.1092095058027995 0.1097050673132180 0.9879463963435011 | 0.1100000000000000
- 0.0000000000000000 0.0000000000000000 0.0000000000000000 | 1.0000000000000000
- """), filteredRigidBodyTransform, EPSILON);
- unfilteredRigidBodyTransform.getTranslation().add(new Point3D(0.1, 0.1, 0.1));
- unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, 0.1, 0.1));
-
- filteredRigidBodyTransform.update(unfilteredRigidBodyTransform);
-
- System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform));
-
- EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString("""
- 0.9834409873207434 -0.1131163390455875 0.1415927904185955 | 0.1290000000000000
- 0.1295926256343390 0.9850941301328119 -0.1131163390455875 | 0.1290000000000000
- -0.1266869205514076 0.1295926256343390 0.9834409873207435 | 0.1290000000000000
- 0.0000000000000000 0.0000000000000000 0.0000000000000000 | 1.0000000000000000
- """), filteredRigidBodyTransform, EPSILON);
- unfilteredRigidBodyTransform.getTranslation().add(new Point3D(0.1, 0.1, 0.1));
- unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, 0.1, 0.1));
-
- filteredRigidBodyTransform.update(unfilteredRigidBodyTransform);
-
- System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform));
-
- EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString("""
- 0.9757980801204758 -0.1342587634073139 0.1726055945835227 | 0.1561000000000000
- 0.1583397744951649 0.9782142404714618 -0.1342587634073140 | 0.1561000000000000
- -0.1508198350549846 0.1583397744951649 0.9757980801204760 | 0.1561000000000000
- 0.0000000000000000 0.0000000000000000 0.0000000000000000 | 1.0000000000000000
- """), filteredRigidBodyTransform, EPSILON);
- unfilteredRigidBodyTransform.getTranslation().add(new Point3D(0.1, 0.1, 0.1));
- unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, 0.1, 0.1));
-
- filteredRigidBodyTransform.update(unfilteredRigidBodyTransform);
-
- System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform));
-
- EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString("""
- 0.9640634283712346 -0.1596208716326060 0.2123743944460991 | 0.1904900000000000
- 0.1953779100890390 0.9676510990995081 -0.1596208716326060 | 0.1904900000000000
- -0.1800254935456074 0.1953779100890391 0.9640634283712346 | 0.1904900000000000
- 0.0000000000000000 0.0000000000000000 0.0000000000000000 | 1.0000000000000000
- """), filteredRigidBodyTransform, EPSILON);
- }
-
- @Test
- public void testReset()
- {
- Random random = new Random(3453456);
-
- double alpha = random.nextDouble();
-
- AlphaFilteredRigidBodyTransform filteredRigidBodyTransform = new AlphaFilteredRigidBodyTransform();
- filteredRigidBodyTransform.setAlpha(0.9);
-
- RigidBodyTransform unfilteredRigidBodyTransform = new RigidBodyTransform();
-
- unfilteredRigidBodyTransform.getTranslation().add(new Point3D(-0.1, 0.1, -0.1));
- unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, -0.1, 0.1));
- filteredRigidBodyTransform.update(unfilteredRigidBodyTransform);
- System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform));
- EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString("""
- 0.990033288920620700 -0.109251584435635540 -0.088871694747662740 | -0.100000000000000000
- 0.099334665397530610 0.989038278100834400 -0.109251584435635540 | 0.100000000000000000
- 0.099833416646828150 0.099334665397530610 0.990033288920620700 | -0.100000000000000000
- 0.000000000000000000 0.000000000000000000 0.000000000000000000 | 1.000000000000000000
- """), filteredRigidBodyTransform, EPSILON);
-
- unfilteredRigidBodyTransform.getTranslation().add(new Point3D(-0.1, 0.1, -0.1));
- unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, -0.1, 0.1));
- filteredRigidBodyTransform.update(unfilteredRigidBodyTransform);
- System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform));
- EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString("""
- 0.987946818678216400 -0.120594329665051940 -0.097046850109766770 | -0.110000000000000010
- 0.108601364045026250 0.986743508405399000 -0.120594329665051990 | 0.110000000000000010
- 0.110303341704367470 0.108601364045026240 0.987946818678216500 | -0.110000000000000010
- 0.000000000000000000 0.000000000000000000 0.000000000000000000 | 1.000000000000000000
- """), filteredRigidBodyTransform, EPSILON);
-
- filteredRigidBodyTransform.reset();
-
- unfilteredRigidBodyTransform.getTranslation().add(new Point3D(-0.1, 0.1, -0.1));
- unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, -0.1, 0.1));
- filteredRigidBodyTransform.update(unfilteredRigidBodyTransform);
- System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform));
- EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString("""
- 0.912137624808415300 -0.343802033741961170 -0.223170596189895150 | -0.300000000000000040
- 0.256378604455114070 0.903366023698333700 -0.343802033741961170 | 0.300000000000000040
- 0.319804572491560860 0.256378604455114070 0.912137624808415400 | -0.300000000000000040
- 0.000000000000000000 0.000000000000000000 0.000000000000000000 | 1.000000000000000000
- """), filteredRigidBodyTransform, EPSILON);
-
- unfilteredRigidBodyTransform.getTranslation().add(new Point3D(-0.1, 0.1, -0.1));
- unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, -0.1, 0.1));
- filteredRigidBodyTransform.update(unfilteredRigidBodyTransform);
- System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform));
- EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString("""
- 0.906330827001449200 -0.355699718189048300 -0.228127469865204740 | -0.310000000000000050
- 0.262498500897342700 0.896979513426520900 -0.355699718189048330 | 0.310000000000000050
- 0.331147956438683330 0.262498500897342700 0.906330827001449300 | -0.310000000000000050
- 0.000000000000000000 0.000000000000000000 0.000000000000000000 | 1.000000000000000000
- """), filteredRigidBodyTransform, EPSILON);
- }
-}
diff --git a/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3DTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3DTest.java
deleted file mode 100644
index 80aab55a..00000000
--- a/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3DTest.java
+++ /dev/null
@@ -1,106 +0,0 @@
-package us.ihmc.yoVariables.euclid.filters;
-
-import org.junit.jupiter.api.Test;
-import us.ihmc.commons.Epsilons;
-import us.ihmc.euclid.tuple3D.Point3D;
-import us.ihmc.euclid.tuple3D.Tuple3DBasicsTest;
-
-import java.util.Random;
-
-import static org.junit.jupiter.api.Assertions.*;
-
-public class AlphaFilteredTuple3DTest extends Tuple3DBasicsTest
-{
- private double alpha;
-
- @Test
- public void testFirstSet()
- {
- double alpha = 0.0;
- AlphaFilteredTuple3D tuple = new AlphaFilteredTuple3D(() -> alpha);
- tuple.set(0.0, 1.0, 2.0);
-
- assertEquals(0.0, tuple.getX(), getEpsilon());
- assertEquals(1.0, tuple.getY(), getEpsilon());
- assertEquals(2.0, tuple.getZ(), getEpsilon());
- }
-
- @Test
- public void testFilteredSetters()
- {
- Random random = new Random(12951L);
- for (int i = 0; i < 1000; ++i)
- {
- alpha = random.nextDouble(0.0, 1.0);
- AlphaFilteredTuple3D tuple = createRandomTuple(random);
-
- double originalX = tuple.getX();
- double originalY = tuple.getY();
- double originalZ = tuple.getZ();
-
- Point3D point = new Point3D(createRandomTuple(random));
-
- tuple.setX(point.getX());
- tuple.setY(point.getY());
- tuple.setZ(point.getZ());
-
- double expectedX = (1.0 - alpha) * point.getX() + alpha * originalX;
- double expectedY = (1.0 - alpha) * point.getY() + alpha * originalY;
- double expectedZ = (1.0 - alpha) * point.getZ() + alpha * originalZ;
-
- assertEquals(expectedX, tuple.getX(), getEpsilon());
- assertEquals(expectedY, tuple.getY(), getEpsilon());
- assertEquals(expectedZ, tuple.getZ(), getEpsilon());
- }
- }
-
- @Test
- public void testSetOther()
- {
- Random random = new Random(621541L);
- for (int i = 0; i < 1000; ++i)
- {
- alpha = random.nextDouble(0.0, 1.0);
- AlphaFilteredTuple3D tuple = createRandomTuple(random);
-
- double originalX = tuple.getX();
- double originalY = tuple.getY();
- double originalZ = tuple.getZ();
-
- Point3D point = new Point3D(createRandomTuple(random));
- tuple.set(point);
-
- double expectedX = (1.0 - alpha) * point.getX() + alpha * originalX;
- double expectedY = (1.0 - alpha) * point.getY() + alpha * originalY;
- double expectedZ = (1.0 - alpha) * point.getZ() + alpha * originalZ;
-
- assertEquals(expectedX, tuple.getX(), getEpsilon());
- assertEquals(expectedY, tuple.getY(), getEpsilon());
- assertEquals(expectedZ, tuple.getZ(), getEpsilon());
- }
- }
-
- @Override
- public AlphaFilteredTuple3D createEmptyTuple()
- {
- return new AlphaFilteredTuple3D(() -> alpha);
- }
-
- @Override
- public AlphaFilteredTuple3D createTuple(double v, double v1, double v2)
- {
- return new AlphaFilteredTuple3D(v, v1, v2, () -> alpha);
- }
-
- @Override
- public AlphaFilteredTuple3D createRandomTuple(Random random)
- {
- return new AlphaFilteredTuple3D(random.nextDouble(), random.nextDouble(), random.nextDouble(), () -> alpha);
- }
-
- @Override
- public double getEpsilon()
- {
- return Epsilons.ONE_TEN_MILLIONTH;
- }
-}
diff --git a/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java b/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java
deleted file mode 100644
index cefb7be0..00000000
--- a/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java
+++ /dev/null
@@ -1,71 +0,0 @@
-package us.ihmc.yoVariables.filters;
-
-import java.util.Random;
-
-import org.junit.jupiter.api.Test;
-
-import us.ihmc.yoVariables.registry.YoRegistry;
-import us.ihmc.yoVariables.variable.YoDouble;
-
-import static org.junit.jupiter.api.Assertions.*;
-
-public class MovingAverageYoDoubleTest
-{
- private final Random rng = new Random();
-
- @Test
- public void testBetaFilteredYoVariable()
- {
- int beta = 5000;
- double pseudoNoise = 0;
-
- YoRegistry registry = new YoRegistry("testRegistry");
- YoDouble positionVariable = new YoDouble("positionVariable", registry);
- MovingAverageYoDouble betaFilteredYoVariable = new MovingAverageYoDouble("betaFilteredYoVariable", registry, beta, positionVariable);
-
- positionVariable.set(10);
-
- for (int i = 0; i < 10000; i++)
- {
- if (i % 2 == 0)
- {
- pseudoNoise = rng.nextDouble();
- }
- positionVariable.add(Math.pow(-1, i) * pseudoNoise);
- betaFilteredYoVariable.update();
- }
-
- assertEquals(10, betaFilteredYoVariable.getDoubleValue(), 1);
- }
-
- @Test
- public void testTrueMovingAverage()
- {
- int beta = 10;
-
- YoRegistry registry = new YoRegistry("testRegistry");
- MovingAverageYoDouble betaFilteredYoVariable = new MovingAverageYoDouble("betaFilteredYoVariable", registry, beta);
-
- double epsilon = 1e-10;
-
- betaFilteredYoVariable.update(1.0);
- assertEquals(1.0, betaFilteredYoVariable.getDoubleValue(), epsilon);
-
- betaFilteredYoVariable.update(2.0);
- assertEquals(1.5, betaFilteredYoVariable.getDoubleValue(), epsilon);
-
-
- betaFilteredYoVariable.update(3.0);
- betaFilteredYoVariable.update(4.0);
- betaFilteredYoVariable.update(5.0);
- betaFilteredYoVariable.update(6.0);
- betaFilteredYoVariable.update(7.0);
- betaFilteredYoVariable.update(8.0);
- betaFilteredYoVariable.update(9.0);
- betaFilteredYoVariable.update(10.0);
-
- assertEquals(5.5, betaFilteredYoVariable.getDoubleValue(), epsilon);
- }
-
-
-}
diff --git a/src/test/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariableTest.java
index a62aa785..e8623aea 100644
--- a/src/test/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariableTest.java
+++ b/src/test/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariableTest.java
@@ -1,11 +1,11 @@
package us.ihmc.yoVariables.filters;
-import java.util.Random;
-
import org.junit.jupiter.api.Test;
-
import us.ihmc.commons.RandomNumbers;
import us.ihmc.yoVariables.registry.YoRegistry;
+import us.ihmc.yoVariables.variable.YoDouble;
+
+import java.util.Random;
import static org.junit.jupiter.api.Assertions.*;
@@ -36,4 +36,61 @@ public void testWithFixedSizeDoubleArrays()
assertEquals(expected, sma.getDoubleValue(), 1.0e-10);
}
}
+
+ @Test
+ public void testBetaFilteredYoVariable()
+ {
+ int beta = 5000;
+ double pseudoNoise = 0;
+
+ Random random = new Random(1738L);
+ YoRegistry registry = new YoRegistry("testRegistry");
+ YoDouble positionVariable = new YoDouble("positionVariable", registry);
+ SimpleMovingAverageFilteredYoVariable betaFilteredYoVariable = new SimpleMovingAverageFilteredYoVariable("betaFilteredYoVariable",
+ beta,
+ positionVariable,
+ registry);
+
+ positionVariable.set(10);
+
+ for (int i = 0; i < 10000; i++)
+ {
+ if (i % 2 == 0)
+ {
+ pseudoNoise = random.nextDouble();
+ }
+ positionVariable.add(Math.pow(-1, i) * pseudoNoise);
+ betaFilteredYoVariable.update();
+ }
+
+ assertEquals(10, betaFilteredYoVariable.getDoubleValue(), 1);
+ }
+
+ @Test
+ public void testTrueMovingAverage()
+ {
+ int beta = 10;
+
+ YoRegistry registry = new YoRegistry("testRegistry");
+ SimpleMovingAverageFilteredYoVariable betaFilteredYoVariable = new SimpleMovingAverageFilteredYoVariable("betaFilteredYoVariable", beta, registry);
+
+ double epsilon = 1e-10;
+
+ betaFilteredYoVariable.update(1.0);
+ assertEquals(1.0, betaFilteredYoVariable.getDoubleValue(), epsilon);
+
+ betaFilteredYoVariable.update(2.0);
+ assertEquals(1.5, betaFilteredYoVariable.getDoubleValue(), epsilon);
+
+ betaFilteredYoVariable.update(3.0);
+ betaFilteredYoVariable.update(4.0);
+ betaFilteredYoVariable.update(5.0);
+ betaFilteredYoVariable.update(6.0);
+ betaFilteredYoVariable.update(7.0);
+ betaFilteredYoVariable.update(8.0);
+ betaFilteredYoVariable.update(9.0);
+ betaFilteredYoVariable.update(10.0);
+
+ assertEquals(5.5, betaFilteredYoVariable.getDoubleValue(), epsilon);
+ }
}