|
// A = [ 1, dt, 0, 0 ] => x(n+1) = x(n) + vx(n)
// [ 0, 1, 0, 0 ] => vx(n+1) = vx(n)
// [ 0, 0, 1, dt ] => y(n+1) = y(n) + vy(n)
// [ 0, 0, 0, 1 ] => vy(n+1) = vy(n)
final RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {
{ 1, dt, 0, 0 },
{ 0, 1, 0, 0 },
{ 0, 0, 1, dt },
{ 0, 0, 0, 1 }
});
// 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 expects 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 }
});
// 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 }
});
// our guess of the initial state.
final RealVector initialState = MatrixUtils.createRealVector(new double[] { 0, speedX, 0, 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 },
{ 0, 1e-3, 0, 0 },
{ 0, 0, var, 0 },
{ 0, 0, 0, 1e-3 }
});
// we assume no process noise -> zero matrix
final RealMatrix Q = MatrixUtils.createRealMatrix(4, 4);
// the measurement covariance matrix
final RealMatrix R = MatrixUtils.createRealMatrix(new double[][] {
{ var, 0, 0, 0 },
{ 0, 1e-3, 0, 0 },
{ 0, 0, var, 0 },
{ 0, 0, 0, 1e-3 }
});
|