Kalman filtering

Leslie — Tue, 08 Jan 2008 14:10:38 +0000

Engineering reality often confronts us with systems that can’t be modelled very well with fixed formulas.
Sometimes we don’t even know some of the system equation’s variables; think of signal noise, for example.

Enter the Kalman filtering algorithm. Given a number of observable and non-observable states, a Kalman filter tries to predict the non-observables ones.

Normally the state vectors have a dimension greater than 1, so you have to fiddle around with matrices. We once implemented the filter in Java; it was astonishingly simple, but the Java idiosyncrasy of making even the most simple code look complicated diminished this simplicity considerably. Here’s the core of it, i.e. the code minus the various “object-oriented” boilerplate methods:

public void filter(State state) throws MatrixException {
 
    setStateTransitionMatrix(delta, state.getAcceleration());
 
    x = lastX;
 
    y.setValue(0, 0, state.getPosition().getX());
    y.setValue(1, 0, state.getPosition().getY());
    y.setValue(2, 0, state.getPosition().getZ());
 
    /* PREDICT */
    x = A.multiply(x);
 
    if ((!E.isZeroMatrix()) && (R != null) && (!R.isZeroMatrix())) {
        E = ((A.multiply(E)).multiply(A.transpose())).add(Q);
        Matrix K = null;
 
        /* CORRECT */
        K = (E.multiply(B.transpose())).multiply(((B.multiply(E))
                .multiply(B.transpose()).add(R)).invert());
 
        if (!K.isZeroMatrix()) {
            x = x.add(K.multiply(y.subtract(B.multiply(x))));
            E = (I.subtract(K.multiply(B))).multiply(E);
        }
 
    }
    lastX = x;
    setState(state);
}

And now you may leave the torture chamber and take a look at an implementation in q. Beautiful!

Space Station Lambda » Mathematics

Kalman filtering