January 8, 2008

Kalman filtering

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!

January 08th 2008 Posted to Mathematics,Programming,Rants,Software Development,Tech

Comments(0)