in src/userguide/java/org/apache/commons/math4/userguide/filter/CannonballExample.java [127:263]
public static void cannonballTest(Chart chart) {
// time interval for each iteration
final double dt = 0.1;
// the number of iterations to run
final int iterations = 144;
// measurement noise (m)
final double measurementNoise = 30;
// initial velocity of the cannonball
final double initialVelocity = 100;
// shooting angle
final double angle = 45;
// the cannonball itself
final Cannonball cannonball = new Cannonball(dt, angle, initialVelocity, measurementNoise, 1000);
// 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 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 },
{ 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 }
});
final ProcessModel pm = new DefaultProcessModel(A, B, Q, initialState, initialErrorCovariance);
final MeasurementModel mm = new DefaultMeasurementModel(H, R);
final KalmanFilter filter = new KalmanFilter(pm, mm);
final List<Number> realX = new ArrayList<>();
final List<Number> realY = new ArrayList<>();
final List<Number> measuredX = new ArrayList<>();
final List<Number> measuredY = new ArrayList<>();
final List<Number> kalmanX = new ArrayList<>();
final List<Number> kalmanY = new ArrayList<>();
for (int i = 0; i < iterations; i++) {
// get real location
realX.add(cannonball.getX());
realY.add(cannonball.getY());
// get measured location
final double mx = cannonball.getMeasuredX();
final double my = cannonball.getMeasuredY();
measuredX.add(mx);
measuredY.add(my);
// iterate the cannon simulation to the next timeslice.
cannonball.step();
final double[] state = filter.getStateEstimation();
kalmanX.add(state[0]);
kalmanY.add(state[2]);
// update the kalman filter with the measurements
filter.predict(controlVector);
filter.correct(new double[] { mx, 0, my, 0 } );
}
chart.setXAxisTitle("Distance (m)");
chart.setYAxisTitle("Height (m)");
Series dataset = chart.addSeries("true", realX, realY);
dataset.setMarker(SeriesMarker.NONE);
dataset = chart.addSeries("measured", measuredX, measuredY);
dataset.setLineStyle(SeriesLineStyle.DOT_DOT);
dataset.setMarker(SeriesMarker.NONE);
dataset = chart.addSeries("kalman", kalmanX, kalmanY);
dataset.setLineColor(Color.red);
dataset.setLineStyle(SeriesLineStyle.DASH_DASH);
dataset.setMarker(SeriesMarker.NONE);
}