@Test public void testPathStateConvert1() { final Path startPath = TestUtils.makeTmpPath(graph, true, 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.getDefault().createVector2D(-0d, -5d / 30d), covar); final PathStateDistribution currentBelief = new PathStateDistribution(startPath, startBelief); final Vector groundLoc = MotionStateEstimatorPredictor.getOg() .times(currentBelief.getGroundDistribution().getMean()); AssertJUnit.assertEquals("initial state x", 10d, 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", -10d, result.getMotionDistribution().getMean().getElement(0), 0d); AssertJUnit.assertEquals( "velocity", 5d / 30d, result.getMotionDistribution().getMean().getElement(1), 0d); }