## 17 Filters

### 17.1 Overview

The filter package currently provides only an implementation of a Kalman filter.

### 17.2 Kalman Filter

KalmanFilter provides a discrete-time filter to estimate a stochastic linear process.

A Kalman filter is initialized with a ProcessModel and a MeasurementModel, which contain the corresponding transformation and noise covariance matrices. The parameter names used in the respective models correspond to the following names commonly used in the mathematical literature:

• A - state transition matrix
• B - control input matrix
• H - measurement matrix
• Q - process noise covariance matrix
• R - measurement noise covariance matrix
• P - error covariance matrix

Initialization
The following code will create a Kalman filter using the provided DefaultMeasurementModel and DefaultProcessModel classes. To support dynamically changing process and measurement noises, simply implement your own models.
// A = [ 1 ]
RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
// no control input
RealMatrix B = null;
// H = [ 1 ]
RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d });
// Q = [ 0 ]
RealMatrix Q = new Array2DRowRealMatrix(new double[] { 0 });
// R = [ 0 ]
RealMatrix R = new Array2DRowRealMatrix(new double[] { 0 });

ProcessModel pm
= new DefaultProcessModel(A, B, Q, new ArrayRealVector(new double[] { 0 }), null);
MeasurementModel mm = new DefaultMeasurementModel(H, R);
KalmanFilter filter = new KalmanFilter(pm, mm);

Iteration
The following code illustrates how to perform the predict/correct cycle:
for (;;) {
// predict the state estimate one time-step ahead
// optionally provide some control input
filter.predict();

// obtain measurement vector z
RealVector z = getMeasurement();

// correct the state estimate with the latest measurement
filter.correct(z);

double[] stateEstimate = filter.getStateEstimation();
// do something with it
}

Constant Voltage Example
The following example creates a Kalman filter for a static process: a system with a constant voltage as internal state. We observe this process with an artificially imposed measurement noise of 0.1V and assume an internal process noise of 1e-5V.
double constantVoltage = 10d;
double measurementNoise = 0.1d;
double processNoise = 1e-5d;

// A = [ 1 ]
RealMatrix A = new Array2DRowRealMatrix(new double[] { 1d });
// B = null
RealMatrix B = null;
// H = [ 1 ]
RealMatrix H = new Array2DRowRealMatrix(new double[] { 1d });
// x = [ 10 ]
RealVector x = new ArrayRealVector(new double[] { constantVoltage });
// Q = [ 1e-5 ]
RealMatrix Q = new Array2DRowRealMatrix(new double[] { processNoise });
// P = [ 1 ]
RealMatrix P0 = new Array2DRowRealMatrix(new double[] { 1d });
// R = [ 0.1 ]
RealMatrix R = new Array2DRowRealMatrix(new double[] { measurementNoise });

ProcessModel pm = new DefaultProcessModel(A, B, Q, x, P0);
MeasurementModel mm = new DefaultMeasurementModel(H, R);
KalmanFilter filter = new KalmanFilter(pm, mm);

// process and measurement noise vectors
RealVector pNoise = new ArrayRealVector(1);
RealVector mNoise = new ArrayRealVector(1);

RandomGenerator rand = new JDKRandomGenerator();
// iterate 60 steps
for (int i = 0; i < 60; i++) {
filter.predict();

// simulate the process
pNoise.setEntry(0, processNoise * rand.nextGaussian());

// x = A * x + p_noise

// simulate the measurement
mNoise.setEntry(0, measurementNoise * rand.nextGaussian());

// z = H * x + m_noise

filter.correct(z);

double voltage = filter.getStateEstimation()[0];
}

Increasing Speed Vehicle Example
The following example creates a Kalman filter for a simple linear process: a vehicle driving along a street with a velocity increasing at a constant rate. The process state is modeled as (position, velocity) and we only observe the position. A measurement noise of 10m is imposed on the simulated measurement.
// discrete time interval
double dt = 0.1d;
// position measurement noise (meter)
double measurementNoise = 10d;
// acceleration noise (meter/sec^2)
double accelNoise = 0.2d;

// A = [ 1 dt ]
//     [ 0  1 ]
RealMatrix A = new Array2DRowRealMatrix(new double[][] { { 1, dt }, { 0, 1 } });
// B = [ dt^2/2 ]
//     [ dt     ]
RealMatrix B = new Array2DRowRealMatrix(new double[][] { { Math.pow(dt, 2d) / 2d }, { dt } });
// H = [ 1 0 ]
RealMatrix H = new Array2DRowRealMatrix(new double[][] { { 1d, 0d } });
// x = [ 0 0 ]
RealVector x = new ArrayRealVector(new double[] { 0, 0 });

RealMatrix tmp = new Array2DRowRealMatrix(new double[][] {
{ Math.pow(dt, 4d) / 4d, Math.pow(dt, 3d) / 2d },
{ Math.pow(dt, 3d) / 2d, Math.pow(dt, 2d) } });
// Q = [ dt^4/4 dt^3/2 ]
//     [ dt^3/2 dt^2   ]
RealMatrix Q = tmp.scalarMultiply(Math.pow(accelNoise, 2));
// P0 = [ 1 1 ]
//      [ 1 1 ]
RealMatrix P0 = new Array2DRowRealMatrix(new double[][] { { 1, 1 }, { 1, 1 } });
// R = [ measurementNoise^2 ]
RealMatrix R = new Array2DRowRealMatrix(new double[] { Math.pow(measurementNoise, 2) });

// constant control input, increase velocity by 0.1 m/s per cycle
RealVector u = new ArrayRealVector(new double[] { 0.1d });

ProcessModel pm = new DefaultProcessModel(A, B, Q, x, P0);
MeasurementModel mm = new DefaultMeasurementModel(H, R);
KalmanFilter filter = new KalmanFilter(pm, mm);

RandomGenerator rand = new JDKRandomGenerator();

RealVector tmpPNoise = new ArrayRealVector(new double[] { Math.pow(dt, 2d) / 2d, dt });
RealVector mNoise = new ArrayRealVector(1);

// iterate 60 steps
for (int i = 0; i < 60; i++) {
filter.predict(u);

// simulate the process
RealVector pNoise = tmpPNoise.mapMultiply(accelNoise * rand.nextGaussian());

// x = A * x + B * u + pNoise