extended-kalman-filter

Extended Kalman Filter implemented in Java with easy representation of model and observation functions

One-dimensional example of moving object

Let's start with a simple example of object which is moving in one dimension. Object has position x and velocity v. Object starts at unknown position and unknown velocity. Object is being observed at time points i=0,1..10 at positions y0=0, y1=1, ... y10=10. The task is to estimate the position and velocity at time i=10.

Defining state equations

State equation is defined as follows

where state vector x and system function f are defined as

In this simple example function f is linear. Nevertheless the library is written for general non-linear Extended Kalman Filter case. That is why we need to provide jacobian of f

In Java the above formulas can be provided by subclassing the ProcessModel class:

public class Linear1dProcessModel extends ProcessModel {

	@Override
	public int stateDimension() {
		return 2;
	}

	@Override
	public void initialState(double[][] x) {
		x[0][0] = 0;
		x[1][0] = 0;
	}

	@Override
	public void initialStateCovariance(double[][] cov) {
		cov[0][0] = 1000;
		cov[0][1] = 0;
		cov[1][0] = 0;
		cov[1][1] = 1000;
	}

	@Override
	public void stateFunction(double[][] x, double[][] f) {
		f[0][0] = x[1][0];
		f[1][0] = 0;
	}

	@Override
	public void stateFunctionJacobian(double[][] x, double[][] j) {
		j[0][0] = 0;
		j[0][1] = 1;
		j[1][0] = 0;
		j[1][1] = 0;
	}

	@Override
	public void processNoiseCovariance(double[][] cov) {
		cov[0][0] = 1;
		cov[0][1] = 0;
		cov[1][0] = 0;
		cov[1][1] = 1;
	}

	public double getX() {
		return getState()[0][0];
	}

	public double getV() {
		return getState()[1][0];
	}
}

As you can see, one can access state estimate variables by calling getState() from the parent class

Defining observation equations

Observation equation is defined as follows

where in our example we are observing position so h is defined as

Jacobian of function h is computed as

Above formulas are implemented in Java by subclassing the ObservationModel:

public class Linear1dObservationModel extends ObservationModel {

	private double mx;

	public void setPosition(double x) {
		this.mx = x;
	}

	@Override
	public int observationDimension() {
		return 1;
	}

	@Override
	public int stateDimension() {
		return 2;
	}

	@Override
	public void observationMeasurement(double[][] y) {
		y[0][0] = mx;
	}

	@Override
	public void observationModel(double[][] x, double[][] h) {
		h[0][0] = x[0][0];
	}

	@Override
	public void observationModelJacobian(double[][] j) {
		j[0][0] = 1;
		j[0][1] = 0;
	}

	@Override
	public void observationNoiseCovariance(double[][] cov) {
		cov[0][0] = 1;
	}
}

Running the code

Below code shows how to use defined process model, observation model and Kalman filter to compute estimates of position and velocity at particular time

public class Linear1dModelTest {

	@Test
	public void test() {
		Linear1dProcessModel model = new Linear1dProcessModel();
		Linear1dObservationModel obs = new Linear1dObservationModel();
		KalmanFilter filter = new KalmanFilter(model);

		for (int i = 0; i <= 10; ++i) {
			double time = i;
			obs.setPosition(i);
			filter.update(time, obs);
		}

		double x = model.getState()[0][0];
		double v = model.getState()[1][0];

		Assert.assertEquals(10, x, 1e-3);
		Assert.assertEquals(1, v, 1e-3);
	}
}

em3ndez / extended-kalman-filter Goto Github PK