Java Code Examples for org.apache.commons.math3.linear.RealVector#setEntry()

@Test
public void testEvaluateCopiesPoint() throws IOException {
    //setup
    StatisticalReferenceDataset dataset
            = StatisticalReferenceDatasetFactory.createKirby2();
    LeastSquaresProblem lsp = builder(dataset).build();
    RealVector point = new ArrayRealVector(lsp.getParameterSize());

    //action
    Evaluation evaluation = lsp.evaluate(point);

    //verify
    Assert.assertNotSame(point, evaluation.getPoint());
    point.setEntry(0, 1);
    Assert.assertEquals(evaluation.getPoint().getEntry(0), 0, 0);
}
 

private Datum parseRecord(CSVRecord record) throws NumberFormatException {
    int vecPos = 0;

    RealVector metricVec = new ArrayRealVector(metrics.size());
    for (String metric : metrics) {
        metricVec.setEntry(vecPos, Double.parseDouble(record.get(metric)));
        vecPos += 1;
    }

    List<Integer> attrList = new ArrayList<>(attributes.size());
    for (String attr : attributes) {
        int pos = schema.get(attr);
        attrList.add(conf.getEncoder().getIntegerEncoding(pos + 1, record.get(pos)));
    }

    return new Datum(
            attrList,
            metricVec
    );
}
 

@Test
public void testEvaluateCopiesPoint() throws IOException {
    //setup
    StatisticalReferenceDataset dataset
            = StatisticalReferenceDatasetFactory.createKirby2();
    LeastSquaresProblem lsp = builder(dataset).build();
    RealVector point = new ArrayRealVector(lsp.getParameterSize());

    //action
    Evaluation evaluation = lsp.evaluate(point);

    //verify
    Assert.assertNotSame(point, evaluation.getPoint());
    point.setEntry(0, 1);
    Assert.assertEquals(evaluation.getPoint().getEntry(0), 0, 0);
}
 

private RealVector extractDenseVector(int dim, LearningInstance instance) {
    Int2DoubleMap features = ((StandardLearningInstance) instance).getFeatures();
    RealVector x = MatrixUtils.createRealVector(new double[dim]);
    for (Int2DoubleMap.Entry entry : features.int2DoubleEntrySet()) {
        x.setEntry(entry.getIntKey(), entry.getDoubleValue());
    }
    return x;
}
 

public float[] solveFToF(float[] b) {
  RealVector bVec = new ArrayRealVector(b.length);
  for (int i = 0; i < b.length; i++) {
    bVec.setEntry(i, b[i]);
  }
  RealVector resultVec = solver.solve(bVec);
  float[] result = new float[resultVec.getDimension()];
  for (int i = 0; i < result.length; i++) {
    result[i] = (float) resultVec.getEntry(i);
  }
  return result;
}
 

/**
 * Compute the normal matrix, J<sup>T</sup>J.
 *
 * @param jacobian  the m by n jacobian matrix, J. Input.
 * @param residuals the m by 1 residual vector, r. Input.
 * @return  the n by n normal matrix and  the n by 1 J<sup>Tr vector.
 */
private static Pair<RealMatrix, RealVector> computeNormalMatrix(final RealMatrix jacobian,
                                                                final RealVector residuals) {
    //since the normal matrix is symmetric, we only need to compute half of it.
    final int nR = jacobian.getRowDimension();
    final int nC = jacobian.getColumnDimension();
    //allocate space for return values
    final RealMatrix normal = MatrixUtils.createRealMatrix(nC, nC);
    final RealVector jTr = new ArrayRealVector(nC);
    //for each measurement
    for (int i = 0; i < nR; ++i) {
        //compute JTr for measurement i
        for (int j = 0; j < nC; j++) {
            jTr.setEntry(j, jTr.getEntry(j) +
                    residuals.getEntry(i) * jacobian.getEntry(i, j));
        }

        // add the the contribution to the normal matrix for measurement i
        for (int k = 0; k < nC; ++k) {
            //only compute the upper triangular part
            for (int l = k; l < nC; ++l) {
                normal.setEntry(k, l, normal.getEntry(k, l) +
                        jacobian.getEntry(i, k) * jacobian.getEntry(i, l));
            }
        }
    }
    //copy the upper triangular part to the lower triangular part.
    for (int i = 0; i < nC; i++) {
        for (int j = 0; j < i; j++) {
            normal.setEntry(i, j, normal.getEntry(j, i));
        }
    }
    return new Pair<RealMatrix, RealVector>(normal, jTr);
}
 

/** {@inheritDoc} */
public RealVector getSigma(double covarianceSingularityThreshold) {
    final RealMatrix cov = this.getCovariances(covarianceSingularityThreshold);
    final int nC = cov.getColumnDimension();
    final RealVector sig = new ArrayRealVector(nC);
    for (int i = 0; i < nC; ++i) {
        sig.setEntry(i, FastMath.sqrt(cov.getEntry(i,i)));
    }
    return sig;
}
 

private RealVector getNewState(RealVector current, ObjectNode action) {
    RealVector curVal = current.copy();
    RealVector actionStateValue = getActionStateValue(action);
    curVal.setEntry(0, curVal.getEntry(0) + 1);
    curVal.setSubVector(1, curVal.getSubVector(1, actionStateValue.getDimension())
            .add(actionStateValue));
    return curVal;
}
 

private static RealVector createRealVector(Dataset a) {
	if (a.getRank() != 1) {
		throw new IllegalArgumentException("Dataset must be rank 1");
	}
	int size = a.getSize();
	IndexIterator it = a.getIterator(true);
	int[] pos = it.getPos();
	RealVector m = new ArrayRealVector(size);
	while (it.hasNext()) {
		m.setEntry(pos[0], a.getElementDoubleAbs(it.index));
	}
	return m;
}
 

@Test
public void testConstantAcceleration() {
    // simulates a vehicle, accelerating at a constant rate (0.1 m/s)

    // discrete time interval
    double dt = 0.1d;
    // position measurement noise (meter)
    double measurementNoise = 10d;
    // acceleration noise (meter/sec^2)
    double accelNoise = 0.2d;

    // A = [ 1 dt ]
    //     [ 0  1 ]
    RealMatrix A = new Array2DRowRealMatrix(new double[][] { { 1, dt }, { 0, 1 } });

    // B = [ dt^2/2 ]
    //     [ dt     ]
    RealMatrix B = new Array2DRowRealMatrix(
            new double[][] { { Math.pow(dt, 2d) / 2d }, { dt } });

    // H = [ 1 0 ]
    RealMatrix H = new Array2DRowRealMatrix(new double[][] { { 1d, 0d } });

    // x = [ 0 0 ]
    RealVector x = new ArrayRealVector(new double[] { 0, 0 });

    RealMatrix tmp = new Array2DRowRealMatrix(
            new double[][] { { Math.pow(dt, 4d) / 4d, Math.pow(dt, 3d) / 2d },
                             { Math.pow(dt, 3d) / 2d, Math.pow(dt, 2d) } });

    // Q = [ dt^4/4 dt^3/2 ]
    //     [ dt^3/2 dt^2   ]
    RealMatrix Q = tmp.scalarMultiply(Math.pow(accelNoise, 2));

    // P0 = [ 1 1 ]
    //      [ 1 1 ]
    RealMatrix P0 = new Array2DRowRealMatrix(new double[][] { { 1, 1 }, { 1, 1 } });

    // R = [ measurementNoise^2 ]
    RealMatrix R = new Array2DRowRealMatrix(
            new double[] { Math.pow(measurementNoise, 2) });

    // constant control input, increase velocity by 0.1 m/s per cycle
    RealVector u = new ArrayRealVector(new double[] { 0.1d });

    ProcessModel pm = new DefaultProcessModel(A, B, Q, x, P0);
    MeasurementModel mm = new DefaultMeasurementModel(H, R);
    KalmanFilter filter = new KalmanFilter(pm, mm);

    Assert.assertEquals(1, filter.getMeasurementDimension());
    Assert.assertEquals(2, filter.getStateDimension());

    assertMatrixEquals(P0.getData(), filter.getErrorCovariance());

    // check the initial state
    double[] expectedInitialState = new double[] { 0.0, 0.0 };
    assertVectorEquals(expectedInitialState, filter.getStateEstimation());

    RandomGenerator rand = new JDKRandomGenerator();

    RealVector tmpPNoise = new ArrayRealVector(
            new double[] { Math.pow(dt, 2d) / 2d, dt });

    RealVector mNoise = new ArrayRealVector(1);

    // iterate 60 steps
    for (int i = 0; i < 60; i++) {
        filter.predict(u);

        // Simulate the process
        RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());

        // x = A * x + B * u + pNoise
        x = A.operate(x).add(B.operate(u)).add(pNoise);

        // Simulate the measurement
        mNoise.setEntry(0, measurementNoise * rand.nextGaussian());

        // z = H * x + m_noise
        RealVector z = H.operate(x).add(mNoise);

        filter.correct(z);

        // state estimate shouldn't be larger than the measurement noise
        double diff = Math.abs(x.getEntry(0) - filter.getStateEstimation()[0]);
        Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
    }

    // error covariance of the velocity should be already very low (< 0.1)
    Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
                                          0.1d, 1e-6) < 0);
}
 

@Test
public void testConstantAcceleration() {
    // simulates a vehicle, accelerating at a constant rate (0.1 m/s)

    // discrete time interval
    double dt = 0.1d;
    // position measurement noise (meter)
    double measurementNoise = 10d;
    // acceleration noise (meter/sec^2)
    double accelNoise = 0.2d;

    // A = [ 1 dt ]
    //     [ 0  1 ]
    RealMatrix A = new Array2DRowRealMatrix(new double[][] { { 1, dt }, { 0, 1 } });

    // B = [ dt^2/2 ]
    //     [ dt     ]
    RealMatrix B = new Array2DRowRealMatrix(
            new double[][] { { Math.pow(dt, 2d) / 2d }, { dt } });

    // H = [ 1 0 ]
    RealMatrix H = new Array2DRowRealMatrix(new double[][] { { 1d, 0d } });

    // x = [ 0 0 ]
    RealVector x = new ArrayRealVector(new double[] { 0, 0 });

    RealMatrix tmp = new Array2DRowRealMatrix(
            new double[][] { { Math.pow(dt, 4d) / 4d, Math.pow(dt, 3d) / 2d },
                             { Math.pow(dt, 3d) / 2d, Math.pow(dt, 2d) } });

    // Q = [ dt^4/4 dt^3/2 ]
    //     [ dt^3/2 dt^2   ]
    RealMatrix Q = tmp.scalarMultiply(Math.pow(accelNoise, 2));

    // P0 = [ 1 1 ]
    //      [ 1 1 ]
    RealMatrix P0 = new Array2DRowRealMatrix(new double[][] { { 1, 1 }, { 1, 1 } });

    // R = [ measurementNoise^2 ]
    RealMatrix R = new Array2DRowRealMatrix(
            new double[] { Math.pow(measurementNoise, 2) });

    // constant control input, increase velocity by 0.1 m/s per cycle
    RealVector u = new ArrayRealVector(new double[] { 0.1d });

    ProcessModel pm = new DefaultProcessModel(A, B, Q, x, P0);
    MeasurementModel mm = new DefaultMeasurementModel(H, R);
    KalmanFilter filter = new KalmanFilter(pm, mm);

    Assert.assertEquals(1, filter.getMeasurementDimension());
    Assert.assertEquals(2, filter.getStateDimension());

    assertMatrixEquals(P0.getData(), filter.getErrorCovariance());

    // check the initial state
    double[] expectedInitialState = new double[] { 0.0, 0.0 };
    assertVectorEquals(expectedInitialState, filter.getStateEstimation());

    RandomGenerator rand = new JDKRandomGenerator();

    RealVector tmpPNoise = new ArrayRealVector(
            new double[] { Math.pow(dt, 2d) / 2d, dt });

    RealVector mNoise = new ArrayRealVector(1);

    // iterate 60 steps
    for (int i = 0; i < 60; i++) {
        filter.predict(u);

        // Simulate the process
        RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());

        // x = A * x + B * u + pNoise
        x = A.operate(x).add(B.operate(u)).add(pNoise);

        // Simulate the measurement
        mNoise.setEntry(0, measurementNoise * rand.nextGaussian());

        // z = H * x + m_noise
        RealVector z = H.operate(x).add(mNoise);

        filter.correct(z);

        // state estimate shouldn't be larger than the measurement noise
        double diff = Math.abs(x.getEntry(0) - filter.getStateEstimation()[0]);
        Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
    }

    // error covariance of the velocity should be already very low (< 0.1)
    Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
                                          0.1d, 1e-6) < 0);
}
 

private void setValue(RealVector var, JsonNode one, int dim) {
    for (int i=0; i<dim; i++) {
        var.setEntry(i, one.get(i + 1).asDouble());
    }
}
 

@Test
public void testConstant() {
    // simulates a simple process with a constant state and no control input
    
    double constantValue = 10d;
    double measurementNoise = 0.1d;
    double processNoise = 1e-5d;

    // A = [ 1 ]
    RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
    // no control input
    RealMatrix B = null;
    // H = [ 1 ]
    RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d });
    // x = [ 10 ]
    RealVector x = new ArrayRealVector(new double[] { constantValue });
    // Q = [ 1e-5 ]
    RealMatrix Q = new Array2DRowRealMatrix(new double[] { processNoise });
    // R = [ 0.1 ]
    RealMatrix R = new Array2DRowRealMatrix(new double[] { measurementNoise });

    ProcessModel pm
        = new DefaultProcessModel(A, B, Q,
                                  new ArrayRealVector(new double[] { constantValue }), null);
    MeasurementModel mm = new DefaultMeasurementModel(H, R);
    KalmanFilter filter = new KalmanFilter(pm, mm);

    Assert.assertEquals(1, filter.getMeasurementDimension());
    Assert.assertEquals(1, filter.getStateDimension());

    assertMatrixEquals(Q.getData(), filter.getErrorCovariance());

    // check the initial state
    double[] expectedInitialState = new double[] { constantValue };
    assertVectorEquals(expectedInitialState, filter.getStateEstimation());

    RealVector pNoise = new ArrayRealVector(1);
    RealVector mNoise = new ArrayRealVector(1);

    RandomGenerator rand = new JDKRandomGenerator();
    // iterate 60 steps
    for (int i = 0; i < 60; i++) {
        filter.predict();

        // Simulate the process
        pNoise.setEntry(0, processNoise * rand.nextGaussian());

        // x = A * x + p_noise
        x = A.operate(x).add(pNoise);

        // Simulate the measurement
        mNoise.setEntry(0, measurementNoise * rand.nextGaussian());

        // z = H * x + m_noise
        RealVector z = H.operate(x).add(mNoise);

        filter.correct(z);

        // state estimate shouldn't be larger than measurement noise
        double diff = Math.abs(constantValue - filter.getStateEstimation()[0]);
        // System.out.println(diff);
        Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
    }

    // error covariance should be already very low (< 0.02)
    Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[0][0],
                                          0.02d, 1e-6) < 0);
}
 

public LinearConstraint getLinearConstraint(AtomPairVoxel voxel, double tolerance) {

		BoundaryPoint p = findBoundaryNewton(voxel, tolerance);

		if (p == null) {
			// dofs don't affect this atom pair, not useful for pruning, so don't make a constraint at all
			return null;
		}

		// did we not find a zero-point in the violation function?
		if (!p.atBoundary()) {

			// if we didn't find a zero-point, then assume all zero points lie outside the voxel
			// since all voxel points correspond to violations, this constraint is unsatisfiable
			if (p.violation > 0.0) {
				throw new NoFeasibleSolutionException();
			}

			// voxel always satisfiable for this atom pair, no constraint needed
			return null;
		}

		// use the boundary point to make a linear constraint on the dofs
		int n = p.dofValues.length;

		// make the linear constraint u.x >= w, where:
		//    u = -g
		//    w = -g.x*
		//    x* is the boundary point where the atom pair overlap is approx 0
		//    g is the gradient at x*
		// ie, the tangent hyperplane (d-1 linear subspace) to the isosurface at this point in the violation function
		RealVector u = new ArrayRealVector(n);
		double w = 0.0;
		for (int d=0; d<n; d++) {
			double g = -p.gradient[d];
			u.setEntry(d, g);
			w += p.dofValues[d]*g;
		}

		return new LinearConstraint(u, Relationship.GEQ, w);
	}
 

/**
 * Computes the approximate sum of paths through the graph where the weight
 * of each path is the product of edge weights along the path;
 * 
 * If consumer c is not null, it will be given the intermediate estimates as
 * they are available
 */
public static double approxSumPaths(WeightedIntDiGraph g, RealVector startWeights, RealVector endWeights,
        Iterator<DiEdge> seq, DoubleConsumer c) {
    // we keep track of the total weight of discovered paths ending along
    // each edge and the total weight
    // of all paths ending at each node (including the empty path); on each
    // time step, we
    // at each step, we pick an edge (s, t), update the sum at s, and extend
    // each of those (including
    // the empty path starting there) with the edge (s, t)

    DefaultDict<DiEdge, Double> prefixWeightsEndingAt = new DefaultDict<DiEdge, Double>(Void -> 0.0);

    // we'll maintain node sums and overall sum with subtraction rather than
    // re-adding (it's an approximation anyway!)
    RealVector currentSums = startWeights.copy();
    double currentTotal = currentSums.dotProduct(endWeights);
    if (c != null) {
        c.accept(currentTotal);
    }
    for (DiEdge e : ScheduleUtils.iterable(seq)) {
        int s = e.get1();
        int t = e.get2();
        // compute the new sums
        double oldTargetSum = currentSums.getEntry(t);
        double oldEdgeSum = prefixWeightsEndingAt.get(e);
        // new edge sum is the source sum times the edge weight
        double newEdgeSum = currentSums.getEntry(s) * g.getWeight(e);
        // new target sum is the old target sum plus the difference between
        // the new and old edge sums
        double newTargetSum = oldTargetSum + (newEdgeSum - oldEdgeSum);
        // the new total is the old total plus the difference in new and
        // target
        double newTotal = currentTotal + (newTargetSum - oldTargetSum) * endWeights.getEntry(t);
        // store the new sums
        prefixWeightsEndingAt.put(e, newEdgeSum);
        currentSums.setEntry(t, newTargetSum);
        currentTotal = newTotal;
        // and report the new total to the consumer
        if (c != null) {
            c.accept(currentTotal);
        }
    }
    return currentTotal;
}
 

@Test
public void testConstant() {
    // simulates a simple process with a constant state and no control input
    
    double constantValue = 10d;
    double measurementNoise = 0.1d;
    double processNoise = 1e-5d;

    // A = [ 1 ]
    RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
    // no control input
    RealMatrix B = null;
    // H = [ 1 ]
    RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d });
    // x = [ 10 ]
    RealVector x = new ArrayRealVector(new double[] { constantValue });
    // Q = [ 1e-5 ]
    RealMatrix Q = new Array2DRowRealMatrix(new double[] { processNoise });
    // R = [ 0.1 ]
    RealMatrix R = new Array2DRowRealMatrix(new double[] { measurementNoise });

    ProcessModel pm
        = new DefaultProcessModel(A, B, Q,
                                  new ArrayRealVector(new double[] { constantValue }), null);
    MeasurementModel mm = new DefaultMeasurementModel(H, R);
    KalmanFilter filter = new KalmanFilter(pm, mm);

    Assert.assertEquals(1, filter.getMeasurementDimension());
    Assert.assertEquals(1, filter.getStateDimension());

    assertMatrixEquals(Q.getData(), filter.getErrorCovariance());

    // check the initial state
    double[] expectedInitialState = new double[] { constantValue };
    assertVectorEquals(expectedInitialState, filter.getStateEstimation());

    RealVector pNoise = new ArrayRealVector(1);
    RealVector mNoise = new ArrayRealVector(1);

    RandomGenerator rand = new JDKRandomGenerator();
    // iterate 60 steps
    for (int i = 0; i < 60; i++) {
        filter.predict();

        // Simulate the process
        pNoise.setEntry(0, processNoise * rand.nextGaussian());

        // x = A * x + p_noise
        x = A.operate(x).add(pNoise);

        // Simulate the measurement
        mNoise.setEntry(0, measurementNoise * rand.nextGaussian());

        // z = H * x + m_noise
        RealVector z = H.operate(x).add(mNoise);

        filter.correct(z);

        // state estimate shouldn't be larger than measurement noise
        double diff = Math.abs(constantValue - filter.getStateEstimation()[0]);
        // System.out.println(diff);
        Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
    }

    // error covariance should be already very low (< 0.02)
    Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[0][0],
                                          0.02d, 1e-6) < 0);
}
 

@Test
public void testConstant() {
    // simulates a simple process with a constant state and no control input
    
    double constantValue = 10d;
    double measurementNoise = 0.1d;
    double processNoise = 1e-5d;

    // A = [ 1 ]
    RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
    // no control input
    RealMatrix B = null;
    // H = [ 1 ]
    RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d });
    // x = [ 10 ]
    RealVector x = new ArrayRealVector(new double[] { constantValue });
    // Q = [ 1e-5 ]
    RealMatrix Q = new Array2DRowRealMatrix(new double[] { processNoise });
    // R = [ 0.1 ]
    RealMatrix R = new Array2DRowRealMatrix(new double[] { measurementNoise });

    ProcessModel pm
        = new DefaultProcessModel(A, B, Q,
                                  new ArrayRealVector(new double[] { constantValue }), null);
    MeasurementModel mm = new DefaultMeasurementModel(H, R);
    KalmanFilter filter = new KalmanFilter(pm, mm);

    Assert.assertEquals(1, filter.getMeasurementDimension());
    Assert.assertEquals(1, filter.getStateDimension());

    assertMatrixEquals(Q.getData(), filter.getErrorCovariance());

    // check the initial state
    double[] expectedInitialState = new double[] { constantValue };
    assertVectorEquals(expectedInitialState, filter.getStateEstimation());

    RealVector pNoise = new ArrayRealVector(1);
    RealVector mNoise = new ArrayRealVector(1);

    RandomGenerator rand = new JDKRandomGenerator();
    // iterate 60 steps
    for (int i = 0; i < 60; i++) {
        filter.predict();

        // Simulate the process
        pNoise.setEntry(0, processNoise * rand.nextGaussian());

        // x = A * x + p_noise
        x = A.operate(x).add(pNoise);

        // Simulate the measurement
        mNoise.setEntry(0, measurementNoise * rand.nextGaussian());

        // z = H * x + m_noise
        RealVector z = H.operate(x).add(mNoise);

        filter.correct(z);

        // state estimate shouldn't be larger than measurement noise
        double diff = Math.abs(constantValue - filter.getStateEstimation()[0]);
        // System.out.println(diff);
        Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
    }

    // error covariance should be already very low (< 0.02)
    Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[0][0],
                                          0.02d, 1e-6) < 0);
}
 

private static RealVector toVector(double x) {
    RealVector v = new ArrayRealVector(1);
    v.setEntry(0, x);
    return v;
}
 

@Test
public void testConstantAcceleration() {
    // simulates a vehicle, accelerating at a constant rate (0.1 m/s)

    // discrete time interval
    double dt = 0.1d;
    // position measurement noise (meter)
    double measurementNoise = 10d;
    // acceleration noise (meter/sec^2)
    double accelNoise = 0.2d;

    // A = [ 1 dt ]
    //     [ 0  1 ]
    RealMatrix A = new Array2DRowRealMatrix(new double[][] { { 1, dt }, { 0, 1 } });

    // B = [ dt^2/2 ]
    //     [ dt     ]
    RealMatrix B = new Array2DRowRealMatrix(
            new double[][] { { Math.pow(dt, 2d) / 2d }, { dt } });

    // H = [ 1 0 ]
    RealMatrix H = new Array2DRowRealMatrix(new double[][] { { 1d, 0d } });

    // x = [ 0 0 ]
    RealVector x = new ArrayRealVector(new double[] { 0, 0 });

    RealMatrix tmp = new Array2DRowRealMatrix(
            new double[][] { { Math.pow(dt, 4d) / 4d, Math.pow(dt, 3d) / 2d },
                             { Math.pow(dt, 3d) / 2d, Math.pow(dt, 2d) } });

    // Q = [ dt^4/4 dt^3/2 ]
    //     [ dt^3/2 dt^2   ]
    RealMatrix Q = tmp.scalarMultiply(Math.pow(accelNoise, 2));

    // P0 = [ 1 1 ]
    //      [ 1 1 ]
    RealMatrix P0 = new Array2DRowRealMatrix(new double[][] { { 1, 1 }, { 1, 1 } });

    // R = [ measurementNoise^2 ]
    RealMatrix R = new Array2DRowRealMatrix(
            new double[] { Math.pow(measurementNoise, 2) });

    // constant control input, increase velocity by 0.1 m/s per cycle
    RealVector u = new ArrayRealVector(new double[] { 0.1d });

    ProcessModel pm = new DefaultProcessModel(A, B, Q, x, P0);
    MeasurementModel mm = new DefaultMeasurementModel(H, R);
    KalmanFilter filter = new KalmanFilter(pm, mm);

    Assert.assertEquals(1, filter.getMeasurementDimension());
    Assert.assertEquals(2, filter.getStateDimension());

    assertMatrixEquals(P0.getData(), filter.getErrorCovariance());

    // check the initial state
    double[] expectedInitialState = new double[] { 0.0, 0.0 };
    assertVectorEquals(expectedInitialState, filter.getStateEstimation());

    RandomGenerator rand = new JDKRandomGenerator();

    RealVector tmpPNoise = new ArrayRealVector(
            new double[] { Math.pow(dt, 2d) / 2d, dt });

    RealVector mNoise = new ArrayRealVector(1);

    // iterate 60 steps
    for (int i = 0; i < 60; i++) {
        filter.predict(u);

        // Simulate the process
        RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());

        // x = A * x + B * u + pNoise
        x = A.operate(x).add(B.operate(u)).add(pNoise);

        // Simulate the measurement
        mNoise.setEntry(0, measurementNoise * rand.nextGaussian());

        // z = H * x + m_noise
        RealVector z = H.operate(x).add(mNoise);

        filter.correct(z);

        // state estimate shouldn't be larger than the measurement noise
        double diff = Math.abs(x.getEntry(0) - filter.getStateEstimation()[0]);
        Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
    }

    // error covariance of the velocity should be already very low (< 0.1)
    Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
                                          0.1d, 1e-6) < 0);
}
 

@Test
public void testConstantAcceleration() {
    // simulates a vehicle, accelerating at a constant rate (0.1 m/s)

    // discrete time interval
    double dt = 0.1d;
    // position measurement noise (meter)
    double measurementNoise = 10d;
    // acceleration noise (meter/sec^2)
    double accelNoise = 0.2d;

    // A = [ 1 dt ]
    //     [ 0  1 ]
    RealMatrix A = new Array2DRowRealMatrix(new double[][] { { 1, dt }, { 0, 1 } });

    // B = [ dt^2/2 ]
    //     [ dt     ]
    RealMatrix B = new Array2DRowRealMatrix(
            new double[][] { { Math.pow(dt, 2d) / 2d }, { dt } });

    // H = [ 1 0 ]
    RealMatrix H = new Array2DRowRealMatrix(new double[][] { { 1d, 0d } });

    // x = [ 0 0 ]
    RealVector x = new ArrayRealVector(new double[] { 0, 0 });

    RealMatrix tmp = new Array2DRowRealMatrix(
            new double[][] { { Math.pow(dt, 4d) / 4d, Math.pow(dt, 3d) / 2d },
                             { Math.pow(dt, 3d) / 2d, Math.pow(dt, 2d) } });

    // Q = [ dt^4/4 dt^3/2 ]
    //     [ dt^3/2 dt^2   ]
    RealMatrix Q = tmp.scalarMultiply(Math.pow(accelNoise, 2));

    // P0 = [ 1 1 ]
    //      [ 1 1 ]
    RealMatrix P0 = new Array2DRowRealMatrix(new double[][] { { 1, 1 }, { 1, 1 } });

    // R = [ measurementNoise^2 ]
    RealMatrix R = new Array2DRowRealMatrix(
            new double[] { Math.pow(measurementNoise, 2) });

    // constant control input, increase velocity by 0.1 m/s per cycle
    RealVector u = new ArrayRealVector(new double[] { 0.1d });

    ProcessModel pm = new DefaultProcessModel(A, B, Q, x, P0);
    MeasurementModel mm = new DefaultMeasurementModel(H, R);
    KalmanFilter filter = new KalmanFilter(pm, mm);

    Assert.assertEquals(1, filter.getMeasurementDimension());
    Assert.assertEquals(2, filter.getStateDimension());

    assertMatrixEquals(P0.getData(), filter.getErrorCovariance());

    // check the initial state
    double[] expectedInitialState = new double[] { 0.0, 0.0 };
    assertVectorEquals(expectedInitialState, filter.getStateEstimation());

    RandomGenerator rand = new JDKRandomGenerator();

    RealVector tmpPNoise = new ArrayRealVector(
            new double[] { Math.pow(dt, 2d) / 2d, dt });

    RealVector mNoise = new ArrayRealVector(1);

    // iterate 60 steps
    for (int i = 0; i < 60; i++) {
        filter.predict(u);

        // Simulate the process
        RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());

        // x = A * x + B * u + pNoise
        x = A.operate(x).add(B.operate(u)).add(pNoise);

        // Simulate the measurement
        mNoise.setEntry(0, measurementNoise * rand.nextGaussian());

        // z = H * x + m_noise
        RealVector z = H.operate(x).add(mNoise);

        filter.correct(z);

        // state estimate shouldn't be larger than the measurement noise
        double diff = Math.abs(x.getEntry(0) - filter.getStateEstimation()[0]);
        Assert.assertTrue(Precision.compareTo(diff, measurementNoise, 1e-6) < 0);
    }

    // error covariance of the velocity should be already very low (< 0.1)
    Assert.assertTrue(Precision.compareTo(filter.getErrorCovariance()[1][1],
                                          0.1d, 1e-6) < 0);
}