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.

Java implementation of the Kalman Filter using JAMA

This is a very clear and straight forward implementation of the Discrete Kalman Filter Algorithm in the Java language using the JAMA package. I wrote this code for testing and simulation purposes.

import Jama.Matrix;

/**
 * This work is licensed under a Creative Commons Attribution 3.0 License.
 * 
 * @author Ahmed Abdelkader
 */

public class KalmanFilter {
 protected Matrix X, X0;
 protected Matrix F, B, U, Q;
 protected Matrix H, R;
 protected Matrix P, P0;
 
 public void predict() {
  X0 = F.times(X).plus(B.times(U));
  
  P0 = F.times(P).times(F.transpose()).plus(Q);
 }
 
 public void correct(Matrix Z) {
  Matrix S = H.times(P0).times(H.transpose()).plus(R); 
  
  Matrix K = P0.times(H.transpose()).times(S.inverse());

  X = X0.plus(K.times(Z.minus(H.times(X0))));
  
  Matrix I = Matrix.identity(P0.getRowDimension(), P0.getColumnDimension());
  P = (I.minus(K.times(H))).times(P0);
 }
 
 //getters and setters go here
}

To use this class you will need to create a new instance, set the system matrices (F, B, U, Q, H, R) and initialize the state and error covariance matrices (X, P). When you're done building your filter, you can start using it right away as you might expect. You will need to do the following for each step: 1) project ahead your state by calling predict. 2) update your prediction by creating a measurement matrix with the measurements you received at that step and passing it to the filter through a correct call. I am planning to post a tutorial of this in the next few days. (Update: tutorial posted here)

For more information about the Kalman Filter algorithm, I highly recommend you refer to the webpage maintained by Greg Welch and Gary Bishop. In particular, check their excellent introduction to this interesting topic.

Parsing a simple XML node on iPhone

I wanted to parse some simple XML response from a server and I really wouldn't bother to use NSXMLParser or libxml2 as discussed here on stackoverflow. I get a single node and I just wanted to get the content. I ended up with this utility method:

+ (NSString *)xmlNodeContent:(NSData *)xmlData {
 NSString *node = [[NSString alloc] initWithData:xmlData encoding:NSUTF8StringEncoding];
 NSString *content = @"";
 if([node length] > 0) {
  int start = [node rangeOfString:@">"].location + 1;
  int end = [node rangeOfString:@"<" options:NSBackwardsSearch].location;
  content = [node substringWithRange:NSMakeRange(start, end - start)];
 }
 [node release];
 return content;
}