/** Path & edge are the same, directions are reverse, neg. to pos. */
@Test
public void testPathStateConvert7() {
final Path startPath =
TestUtils.makeTmpPath(
graph,
true,
new Coordinate(20, -10),
new Coordinate(10, -10),
new Coordinate(10, 0),
new Coordinate(0, 0));
final Matrix covar =
MatrixFactory.getDefault()
.copyArray(new double[][] {new double[] {126.56, 8.44}, new double[] {8.44, 0.56}});
final MultivariateGaussian startBelief =
new AdjMultivariateGaussian(VectorFactory.getDefault().createVector2D(-2.5d, 1d), covar);
final PathStateDistribution currentBelief = new PathStateDistribution(startPath, startBelief);
final Path newPath =
TestUtils.makeTmpPath(graph, false, new Coordinate(10, 0), new Coordinate(0, 0));
final PathStateDistribution result = currentBelief.convertToPath(newPath);
AssertJUnit.assertEquals("distance", 7.5d, result.getMean().getElement(0), 0d);
AssertJUnit.assertEquals("velocity", 1d, result.getMean().getElement(1), 0d);
}
@Test
public void testPathStateConvert2() {
final Path startPath =
TestUtils.makeTmpPath(graph, false, new Coordinate(0, 0), new Coordinate(10, 0));
final Matrix covar =
MatrixFactory.getDefault()
.copyArray(new double[][] {new double[] {126.56, 8.44}, new double[] {8.44, 0.56}});
final MultivariateGaussian startBelief =
new AdjMultivariateGaussian(VectorFactory.getDenseDefault().createVector2D(0d, 1d), covar);
final PathStateDistribution currentBelief = new PathStateDistribution(startPath, startBelief);
final Vector groundLoc =
MotionStateEstimatorPredictor.getOg()
.times(currentBelief.getGroundDistribution().getMean());
AssertJUnit.assertEquals("initial state x", 0d, groundLoc.getElement(0), 0d);
AssertJUnit.assertEquals("initial state y", 0d, groundLoc.getElement(1), 0d);
final Path newPath =
TestUtils.makeTmpPath(
graph,
true,
new Coordinate(20, -10),
new Coordinate(10, -10),
new Coordinate(10, 0),
new Coordinate(0, 0));
final PathStateDistribution result = currentBelief.convertToPath(newPath);
AssertJUnit.assertEquals("distance", -0d, result.getMean().getElement(0), 0d);
AssertJUnit.assertEquals("velocity", -1d, result.getMean().getElement(1), 0d);
}