Using a Discrete Wiener Process Acceleration (DWPA) model, we illustrate the usage of the Java implementation of the Kalman filter we presented in the previous post. The model we employ here is taken from Estimation with Applications to Tracking and Navigation.

We start by building the Kalman filter using this method:

public static KalmanFilter buildKF(double dt, double processNoisePSD, double measurementNoiseVariance) {Then, we simulate the accelerating target by generating random acceleration increments and updating the velocity and displacement accordingly. We also simulate the noisy measurements and feed them to the filter. We repeat this step many times to challenge the performance of the filter. Finally, we compare the state estimate provided by the filter to the true simulated state and the last measurement.

KalmanFilter KF = new KalmanFilter();

//state vector

KF.setX(new Matrix(new double[][]{{0, 0, 0}}).transpose());

//error covariance matrix

KF.setP(Matrix.identity(3, 3));

//transition matrix

KF.setF(new Matrix(new double[][]{

{1, dt, pow(dt, 2)/2},

{0, 1, dt},

{0, 0, 1}}));

//input gain matrix

KF.setB(new Matrix(new double[][]{{0, 0, 0}}).transpose());

//input vector

KF.setU(new Matrix(new double[][]{{0}}));

//process noise covariance matrix

KF.setQ(new Matrix(new double[][]{

{ pow(dt, 5) / 4, pow(dt, 4) / 2, pow(dt, 3) / 2},

{ pow(dt, 4) / 2, pow(dt, 3) / 1, pow(dt, 2) / 1},

{ pow(dt, 3) / 1, pow(dt, 2) / 1, pow(dt, 1) / 1}}

).times(processNoisePSD));

//measurement matrix

KF.setH(new Matrix(new double[][]{{1, 0, 0}}));

//measurement noise covariance matrix

KF.setR(Matrix.identity(1, 1).times(measurementNoiseVariance));

return KF;

}

import static java.lang.Math.pow;Depending on how familiar you are with target tracking and Kalman filters, you may find it interesting to consider the following:

import java.util.Random;

import Jama.Matrix;

/**

* This work is licensed under a Creative Commons Attribution 3.0 License.

*

* @author Ahmed Abdelkader

*/

public static void main(String[] args) {

//model parameters

double x = Math.random(), vx = Math.random(), ax = Math.random();

//process parameters

double dt = 1.0 / 100.0;

double processNoiseStdev = 3;

double measurementNoiseStdev = 5;

double m = 0;

//noise generators

Random jerk = new Random();

Random sensorNoise = new Random();

//init filter

KalmanFilter KF = buildKF(dt, pow(processNoiseStdev, 2)/2, pow(measurementNoiseStdev, 2));

KF.setX(new Matrix(new double[][]{{x}, {vx}, {ax}}));

//simulation

for(int i = 0; i < 1000; i++) {

//model update

ax += jerk.nextGaussian() * processNoiseStdev;

vx += dt * ax;

x += dt * vx + 0.5 * pow(dt, 2) * ax;

//measurement realization

m = x + sensorNoise.nextGaussian() * measurementNoiseStdev;

//filter update

KF.predict();

KF.correct(new Matrix(new double[][]{{m}}));

}

//results

System.out.println("True:"); new Matrix(new double[][]{{x}, {vx}, {ax}}).print(3, 1);

System.out.println("Last measurement:\n\n " + m + "\n");

System.out.println("Estimate:"); KF.getX().print(3, 1);

System.out.println("Estimate Error Cov:"); KF.getP().print(3, 3);

}

- The error covariance reaches a steady state after a certain number of steps. By studying the steady state error, we can obtain a good idea about the performance of the filter. In addition, this can be used to precalculate the steady state Kalman gain to avoid performing many calculations at each step.
- If you were to run this simulation yourself, you should experiment with differnet values of processNoiseStdev and measurementNoiseStdev and observe the steady state covariance and absolute error.
- The simulation uses a position sensor to measure the current location of the target. This may not be available for you, specially in the case of inertial navigation. We will try to look into that in a later post.