@Test public void testPathStateConvert3() { 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.getDefault().createVector2D(2.5d, 1d), covar); final PathStateDistribution currentBelief = new PathStateDistribution(startPath, startBelief); final Vector groundLoc = MotionStateEstimatorPredictor.getOg() .times(currentBelief.getGroundDistribution().getMean()); AssertJUnit.assertEquals("initial state x", 2.5d, groundLoc.getElement(0), 0d); AssertJUnit.assertEquals("initial state y", 0d, groundLoc.getElement(1), 0d); final Path newPath = TestUtils.makeTmpPath( graph, false, new Coordinate(0, 0), new Coordinate(10, 0), new Coordinate(10, -10), new Coordinate(20, -10)); final PathStateDistribution result = currentBelief.convertToPath(newPath); AssertJUnit.assertEquals("distance", 2.5d, result.getMean().getElement(0), 0d); AssertJUnit.assertEquals("velocity", 1d, result.getMean().getElement(1), 0d); }