Thursday, December 31, 2009

Simulation and Kalman filter for a 3rd order kinematic model

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) {
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;
}
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.
import static java.lang.Math.pow;
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);
}
Depending on how familiar you are with target tracking and Kalman filters, you may find it interesting to consider the following:
  • 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.

12 comments:

Ismael said...

Hi, I'm trying to run your implementation of the Kalman Filter but I can't seem to make it work. Apparently I have no idea how to put the setters in the KalmanFilter class.

I was wondering if you could send me the source files of your implementation (amezcua.ismael #at# gmail.com) so I can study the code and try to make an implementation of my own.

Well, I hope this is possible. Thanks in advance.

David said...

I have made an example application based on this code, which tracks your computer mouse in a window frame.

It is available at:
http://web.ist.utl.pt/davidmiguel/KalmanExample.html

rp181 said...

I am working on a project for a IMU using 3 acceleration sensors and 3 gyro sensors. I understand that a Kalman Filter can be used to fuse the data, but I don't understand how. I downloaded your example for the mouse (really cool btw), but I don't see how i can adapt it for an IMU? Do you have any idea? Can i contact you (details, etc).

Ahmed Abdelkader said...

Check Eric Foxlin's publications on IMUs and tracking at InterSense. Good luck.

Dan said...

Hello,
can you give me a hint for choosing the elements of Q, not only by geussing. I've read the book where you obtained the model and the choice of process noise, but it seems not to fit best to my problem.
I want to track a vehicle moving (accelerating) in one dimension. I figuered out that choosing the off-diagonal elements to zero is a good choice. Can you give me any advise or paper for choosing the matrix Q with respect to the system behaviour (max. acceleration, speed).
Thank you and kind regards

Ahmed Abdelkader said...

@Dan: if the book didn't work for you then I guess you didn't choose a correct model or maybe the model is more complicated than the way you put it. However, if the only problem is choosing Q, you may find this useful http://en.wikipedia.org/wiki/Kalman_filter#Estimation_of_the_noise_covariances_Qk_and_Rk. Good luck and keep us updated with your progress.

Dan said...

Thanks for your answer,

i used the model from the book again and found a mistake in scaling the process noise variance. I'll use the ALS-Toolbox as well to verify my results. But still i have the problem to determine wich (fine) choice of process noise is the best with real data and without any reference measured. How can i use the elements of the error covariance matrix to rate the performance of the filter? Is there another solution?

Thank you very much.

Beginner said...

Hi, I want to predict the next GPS position using a Kalman filter. Is there any example for this in C++?
Thanks.

Ahmed Abdelkader said...

@Dan: regarding performance evaluation, check this http://en.wikipedia.org/wiki/Kalman_filter#Optimality_and_performance_of_the_Kalman_filter.

Zewusp said...

Hello, I am looking for Implementation of Kalman filter example called "Tracking a falling object" in Java.

Is the solution available ??
Please do let me know

CF said...

How would this code change if the time between measurements wasn't constant?

Not entirely comfortable with the math behind the filtering, but this code is a great practical Kalman filter in Java.

Thanks so much for it!

Azizi van Abdullah said...

A very nice tutorial. Do you have a tutorial that describe the same problem using EKF?