/** 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);
}
}
