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;
}

Multi-line UITableViewCell using UILabel

No need for UITextViews or custom UITableViewCells. You can use standard UITableViewCellStyles and make the detailTextLabel accept multiple lines and specify its line break mode. The code would be:

static NSString *CellIdentifier = @"MyCell"; 
UITableViewCell *cell = [tableView dequeueReusableCellWithIdentifier:CellIdentifier];
if (cell == nil) {
 cell = [[[UITableViewCell alloc] initWithStyle:UITableViewCellStyleValue2
     reuseIdentifier:CellIdentifier] autorelease];
}
cell.textLabel.text = @"Label';
cell.detailTextLabel.text = @"Multi-Line\nText";
cell.detailTextLabel.numberOfLines = 2;
cell.detailTextLabel.lineBreakMode = UILineBreakModeWordWrap;

You will also need to return a suitable height for the multi-line cell. A height of (44.0 + (numberOfLines - 1) * 19.0) should work fine.

Update: As Vaibhav mentions in the comments, you can use variants of sizeWithFont from the NSString UIKit Additions to get the required height. I guess sizeWithFont:forWidth:lineBreakMode is the one to use here. Thanks for your input!

Moving UITextFields over the keyboard without a UIScrollView

I had a UIViewController with a couple of UITextFields attached to its default UIView and everything was fine. Later on, I added a couple more text fields, so I had to make sure all text fields are displayed properly when the keyboard shows up. I thought about using a UIScrollView or maybe a UITableView which scrolls naturally, but I thought I didn't have to change my controller just for that. I found these 2 posts [1, 2] on stackoverflow pretty useful, but I still had to tweak the code a little and here's what I got:

- (void)viewWillDisappear:(BOOL)animated {
    [super viewWillDisappear:animated];
    [activeField resignFirstResponder];
}

- (BOOL)textFieldShouldBeginEditing:(UITextField *)textField {
    activeField = textField;
    [self setViewMovedUp:YES];
    return YES;
}

- (BOOL)textFieldShouldReturn:(UITextField *)textField {
    [activeField resignFirstResponder];
    [self setViewMovedUp:NO];
    activeField = nil;
    return YES;
}

- (void)setViewMovedUp:(BOOL)movedUp {
    [UIView beginAnimations:nil context:NULL];
    [UIView setAnimationDuration:0.5];
 
    CGRect viewFrame = self.view.frame;
    CGRect textFieldFrame = [activeField convertRect:[activeField bounds] toView:self.view];
 
    if (movedUp) {
        viewFrame.origin.y = -textFieldFrame.origin.y/1.8;
    } else {
        viewFrame.origin.y = 0;
    }
    self.view.frame = viewFrame;
 
    [UIView commitAnimations];
}

Let me explain a couple of things here, first: activeField is a UIControl* instance variable I added to my UIVeiwController. second: you can replace (textFieldFrame.origin.y/1.8) with any function of (textFieldFrame.origin.y) that would do the job. Maybe you can try assigning certain offsets for each range of y values, but I preferred this neat form and the result was neat too.

Tracking swine flu with Google Maps

FluTracker is a website that tracks the progress of swine flu using data from official sources, news reports and user-contributions*. The site was built using technology provided by Rhiza Labs and Google. This post on Mashable also has great information about tracking swine flu online.

Tracking epidemics was one of the first applications of geospatial information. In 1854, John Snow depicted a cholera outbreak in London using points to represent the locations of some individual cases, as mentioned on Wikipedia.

It also looks like someone is trying to make business out of this site. Check the bold section just above the map. Seems like everything can be exploited to make money.

I hope we don't see more circles, at least in our region. But I can't just stop here. I think that people, we, are very worried about pandemics and death. I mean, what if we track every sin all around the world?

(*) The main sources of information [on the web] today. Watch this great video of Eric Schmidt, Chairman and CEO of Google Inc., at the Newspaper Association of America on April 7, 2009.

Moore-Penrose Pseudoinverse in JAMA

import Jama.Matrix;
import Jama.SingularValueDecomposition;

public class Matrices {
 /**
  * The difference between 1 and the smallest exactly representable number
  * greater than one. Gives an upper bound on the relative error due to
  * rounding of floating point numbers.
  */
 public static double MACHEPS = 2E-16;

 /**
  * Updates MACHEPS for the executing machine.
  */
 public static void updateMacheps() {
  MACHEPS = 1;
  do
   MACHEPS /= 2;
  while (1 + MACHEPS / 2 != 1);
 }

 /**
  * Computes the Moore–Penrose pseudoinverse using the SVD method.
  * 
  * Modified version of the original implementation by Kim van der Linde.
  */
 public static Matrix pinv(Matrix x) {
  int rows = x.getRowDimension();
  int cols = x.getColumnDimension();
  if (rows < cols) {
   Matrix result = pinv(x.transpose());
   if (result != null)
    result = result.transpose();
   return result;
  }
  SingularValueDecomposition svdX = new SingularValueDecomposition(x);
  if (svdX.rank() < 1)
   return null;
  double[] singularValues = svdX.getSingularValues();
  double tol = Math.max(rows, cols) * singularValues[0] * MACHEPS;
  double[] singularValueReciprocals = new double[singularValues.length];
  for (int i = 0; i < singularValues.length; i++)
   if (Math.abs(singularValues[i]) >= tol)
    singularValueReciprocals[i] =  1.0 / singularValues[i];
  double[][] u = svdX.getU().getArray();
  double[][] v = svdX.getV().getArray();
  int min = Math.min(cols, u[0].length);
  double[][] inverse = new double[cols][rows];
  for (int i = 0; i < cols; i++)
   for (int j = 0; j < u.length; j++)
    for (int k = 0; k < min; k++)
     inverse[i][j] += v[i][k] * singularValueReciprocals[k] * u[j][k];
  return new Matrix(inverse);
 }
}

Update 11/20/2014: As this code continues to live out there, I'm adding a basic test. Just so you know what you're getting. (As is. No warranty. Wish you the best.)

public static boolean checkEquality(Matrix A, Matrix B) {
 return A.minus(B).normInf() < 1e-9;
}
 
public static void testPinv() {
 int rows = (int) Math.floor(100 + Math.random() * 200);
 int cols = (int) Math.floor(100 + Math.random() * 200);
 double[][] mat = new double[rows][cols];
 for (int i = 0; i < rows; i++)
  for (int j = 0; j < cols; j++)
   mat[i][j] = Math.random();
 Matrix A = new Matrix(mat);
 long millis = System.currentTimeMillis();
 Matrix Aplus = pinv(A);
 millis = System.currentTimeMillis() - millis;
 if (Aplus == null) {
  System.out.println("Always check for null");
  return;
 }
 // Good to know.
 boolean c1 = checkEquality(A.times(Aplus).times(A), A);
 boolean c2 = checkEquality(Aplus.times(A).times(Aplus), Aplus);
 boolean c3 = checkEquality(A.times(Aplus), A.times(Aplus).transpose());
 boolean c4 = checkEquality(Aplus.times(A), Aplus.times(A).transpose());
 System.out.println(rows + " x " + cols + "\t" +
                    c1 + "/" + c2 + "/" + c3 + "/" + c4 + "\t" + millis);
}
 
public static void main(String[] args) {
 for (int z = 0; z < 100; z++)
  testPinv();
}

EU charges Microsoft with antitrust again

I was really pissed off when I read about the new antitrust charges Microsoft is facing from the EU. It was mentioned in the news article published under "EU: Microsoft 'shields' IE from competition" how EU looks at the case:

The evidence gathered during the investigation leads the commission to believe that the tying of Internet Explorer with Windows, which makes Internet Explorer available on 90% of the world's PCs, distorts competition on the merits between competing Web browsers insofar as it provides Internet Explorer with an artificial distribution advantage which other Web browsers are unable to match.

So, if they think that IE is available on 90% of the world's PCs, why didn't they consider FireFox, Opera, Safari or Chrome to be available on 100% of the world's PCs that has internet access? Any of these browsers can be downloaded and installed in about 10 minutes. If IE wasn't bundled with Windows, it would've been really difficult to use the PC whatsoever. A web browser is an essential part of any usable OS. Actually, IE provides instant web access to Windows user so they can do whatever they like, including navigating to/searching for competing browsers to download and install. Microsoft didn't prevent Windows users to do what they want with the software on their machines.

Microsoft should make a secure and standards-compliant browser, but separating IE from windows is another thing. We all know how bad IE can be, for instance, it used to crash anytime I closed a tab loaded with Facebook. Also, as a web developer, I can tell you that IE is not fun to work with as it doesn't follow the standards, which makes it harder to produce something that can be viewed by any user given that most users indeed use IE.

Opera claimed back in December that Microsoft abuses its dominant position on the desktop by tying IE browser to Windows and asked the EU's Competition Commission to force Microsoft into separating IE from Windows. Why don't we hear similar claims for Safari that is bundled with OSX or FireFox that comes with almost all open source OS distributions like Ubuntu, OpenSUSE and Fedora?

Maybe PC users are not interested enough to change IE, or maybe IE is good enough for their common needs. However, this is the current state of the browser market. It doesn't only happen in the software world, the first move guarantees an advantage in any market as well as in chess. In addition, untying IE from Windows is not going to change this any soon. Windows is already sold and used on so many machines worldwide.

I think competing browsers should focus on the quality of their products instead of blaming Microsoft for their first-hand advantage, after Browser War I. They should also think of innovative ways to advertise their products and expand their user base. Each product has its pros and cons, say, IE comes with Windows and Safari with OSX, FireFox has lots of add-ons and comes with Ubuntu and finally, Chrome is made by Google. Opera should work on marketing and add more features instead of just being jealous of IE, because the new Google browser is already doing better in terms of market share according to the W3C browser statistics. On the other hand, IE should follow the standards and work on its security and stability issues.

Indoor RF Propagation Simulation Software

I was looking for a free program to help me simulate Wi-Fi signal propagation for indoor environments. I wanted a program that could take a floor map and do some ray tracing or apply some known propagation model. I was disappointed when I couldn't find any open source software in that area. All the free programs I found were very naive and targeted radio hobbyists. On the other hand, I found a number of high quality commercial programs, fortunately, one of which had a free student edition. I compiled this list with the information I got:

1) RF-vu from iBwave with the RF-Propagation add-on.
2) PropMan from AWE Communications.
3) InterpretAir from Fluke Networks.
4) SignalPro from EDX with the Microcell/Indoor add-on (free evaluation version)
5) Radioplan from Actix. (student edition free to download)

I'd also like to mention these free tools, none of them support indoor propagation and I didn't try them myself, anyway they seemed good to me so I decided to include them here for completion.

1) Radio Mobile: the screen shots available at the website are interesting and it seems to have an active user base. They've created a Yahoo Group that currently has 5382 members. The group description points to that tutorial that looked good too.

2) SPLAT!: an open source RF Signal Propagation, Loss, And Terrain analysis tool for the spectrum between 20 MHz and 20 GHz. It may be redistributed and/or modified under the terms of GPLv2. It looks well made and documented. The website mentions interesting projects that used this tool.

Netbeans modified web.xml when it shouldn't

A couple of days ago, I was working on a J2EE project using EJB 3.0 and plain JSP/Servlets. I used Netbeans 6.0.1 with GlassFish 9.1 on my XP Professional PC. For some reason, the project build kept on failing, without any changes in the configuration files which I barely touched. After digging through the stack traces and log files I got, I found this message "The ResourceConfig instance does not contain any root resource classes". I googled for that and found that others got the same problem. I followed this thread on java.net forums, and was really angry to find that Netbeans added stuff to my web.xml, which of course couldn't be resolved by the deployment script. This is the extra parts the guys mentioned on the forum and I found in my web.xml:

<servlet>
 <servlet-name>ServletAdaptor</servlet-name>
 <servlet-class>com.sun.ws.rest.impl.container.servlet.ServletAdaptor</servlet-class>
 <load-on-startup>1</load-on-startup>
</servlet>

and:

<servlet-mapping>
 <servlet-name>ServletAdaptor</servlet-name>
 <url-pattern>/resources/*</url-pattern>
</servlet-mapping>

It seemed like it had to do with the RESTful webservices plug-ins which I didn't even reference in that project. I removed the extra parts from my web.xml and also removed the RESTful plug-ins to make sure it doesn't happen again and everything went fine.