/** Checks to see if the forward propagation computes the expected values in the state */
@Test
public void checkPropagationValues() {
KalmanFilter filter = (KalmanFilter) createFilter();
MultivariateGaussianDM x = createState(9.0, 0, 1, 2);
filter.predict(x, null, -1);
// see if the mean is correct
assertEquals(2.0, x.getMean().get(0, 0), 1e-6);
assertEquals(3.0, x.getMean().get(1, 0), 1e-6);
assertEquals(2.0, x.getMean().get(2, 0), 1e-6);
}
@Override
protected MultivariateGaussianDM createPerfectMeas(
KalmanFilterInterface f, DenseMatrix64F state) {
KalmanFilter filter = (KalmanFilter) f;
DenseMatrix64F H = filter.getProjector().getProjectionMatrix();
DenseMatrix64F X = state;
DenseMatrix64F z = new DenseMatrix64F(2, 1);
CommonOps.mult(H, X, z);
return createState(2.0, z.get(0, 0), z.get(1, 0));
}
/** Checks to see if the control input is used and not used when appropriate. */
@Test
public void checkControlUse() {
DenseMatrix64F F = CommonOps.identity(3);
DenseMatrix64F Q = CommonOps.identity(3);
DenseMatrix64F G = new DenseMatrix64F(3, 2);
DenseMatrix64F u = new DenseMatrix64F(2, 1);
CommonOps.fill(G, 1.0);
CommonOps.fill(u, 2.0);
FixedKalmanPredictor prop = new FixedKalmanPredictor(F, G, Q);
DenseMatrix64F H = new DenseMatrix64F(new double[][] {{1, 1, 1}, {0, 1, 2}});
KalmanProjector projector = new FixedKalmanProjector(H);
// see how it works with the control
KalmanFilter kf = new KalmanFilter(prop, projector);
kf.setControlInput(u);
MultivariateGaussianDM x = new MultivariateGaussianDM(3);
kf.predict(x, null, -1);
for (int i = 0; i < 3; i++) {
assertTrue(x.getMean().get(i, 0) > 0);
}
// now do it without the control
prop.setControlTransition(null);
x = new MultivariateGaussianDM(3);
kf.predict(x, null, -1);
for (int i = 0; i < 3; i++) {
assertTrue(x.getMean().get(i, 0) == 0);
}
}
/** @see KalmanFilter#setState(org.apache.commons.math.linear.RealMatrix) */
@Override
public void setState(RealMatrix x) {
super.setState(x);
}
