// The control vector, which adds acceleration to the kinematic equations.
// 0 => x(n+1) = x(n+1)
// 0 => vx(n+1) = vx(n+1)
// -9.81*dt^2 => y(n+1) = y(n+1) - 1/2 * 9.81 * dt^2
// -9.81*dt => vy(n+1) = vy(n+1) - 9.81 * dt
final RealVector controlVector =
MatrixUtils.createRealVector(new double[] { 0, 0, 0.5 * -9.81 * dt * dt, -9.81 * dt } );
// The control matrix B only update y and vy, see control vector
final RealMatrix B = MatrixUtils.createRealMatrix(new double[][] {
{ 0, 0, 0, 0 },
{ 0, 0, 0, 0 },
{ 0, 0, 1, 0 },
{ 0, 0, 0, 1 }
});
// After state transition and control, here are the equations:
//
// x(n+1) = x(n) + vx(n)
// vx(n+1) = vx(n)
// y(n+1) = y(n) + vy(n) - 0.5 * 9.81 * dt^2
// vy(n+1) = vy(n) + -9.81 * dt
//
// Which, if you recall, are the equations of motion for a parabola.
// We only observe the x/y position of the cannonball
final RealMatrix H = MatrixUtils.createRealMatrix(new double[][] {
{ 1, 0, 0, 0 },
{ 0, 0, 0, 0 },
{ 0, 0, 1, 0 },
{ 0, 0, 0, 0 }
});
// This is our guess of the initial state. I intentionally set the Y value
// wrong to illustrate how fast the Kalman filter will pick up on that.
final double speedX = cannonball.getXVelocity();
final double speedY = cannonball.getYVelocity();
final RealVector initialState = MatrixUtils.createRealVector(new double[] { 0, speedX, 100, speedY } );
// The initial error covariance matrix, the variance = noise^2
final double var = measurementNoise * measurementNoise;
final RealMatrix initialErrorCovariance = MatrixUtils.createRealMatrix(new double[][] {
{ var, 0, 0, 0 },