What is a Kalman Filter?

A Kalman Filter is a set of states, aka a vector, that approximately describe a real world system and a set of measurements that relate the set of states to a set of measurements. Well, that sounds easy enough. Here we go.

There are two basic components of the Kalman Filter equations: the model and the measurement. The model part consists of the states, or physical properties, that are used to simulate a system and the time evolution of those states. The measurement part of the Kalman Filter connects measurement inputs and the states used to describe the system.

(The Kalman Filter equations in this article are merely quoted, and not in any way, shape, or form are they derived here. Derivations of these equations do not help demonstrate why Kalman Filter is useful to locating the Wiimote. For those interested in their derivation, all of the references have something useful.)

Basic Model Equations

(1).

Equation (1) represents the assumed time propagation of an n dimensional state vector, x(t). The vector u(t) is the control vector, and the vector w(t) is the noise vector for time evolution. The noise vector, w(t), represents how much faith you have in the time evolution equations and it can represent the unknowns in the control vector. Or both. The control vector, u(t), is an input that changes often and it is not part of the state vector because it can not be estimated. Accelerometer input is a good example of an element of the control vector, it changes from frame to frame depending on the user's motion of the Wiimote, which is not predictable in the least.

(2).

The z(t) vector represents an m dimensional measurement vector, the m by n (m rows by n columns) matrix H(t) maps the state vector to the measurement vector, and v(t) is the m dimensional noise vector related with specific measurements.

Based on these equations, there are related formulas that predict the best estimate of the state vector x(t) and of the covariance matrix, P(t). These two mathematical entities are estimates of the values of real objects in the world and how precisely the estimates are known. Since the state vector can never be known we can only make a best guess at its value and that best estimate is written as.

Time Propagation Equations

The first step to successfully using equations (1) and (2) are to solve them with respect to propagating them through time. Equations (3) and (4) are the time dependent solutions of equations (1) and (2).

(3),

(4).

The matrixis the state transition matrix defined as , with the matrix F from equation (1) (Yay, matrix exponentials!). It satisfies the relations and , where I is the n by n identity matrix. This is a formal way to write the solution of the differential equation . If these equations are unfamiliar to you, this article is probably not for you. If we assume Gaussian noise models, the matrix Q(t) is related to the noise vector w(t) as , where d(t) is a delta function. Different types of noise can be used in the filter, but that is a whole different can of worms and this article does not worry about it. The concerned reader is referred to the references.

One interesting feature of equations (3) and (4), is that the time evolution of the covariance matrix, P(t), does not depend on the control vector u(t), and that the best estimate of the state vector,, does not depend on the noise vector w(t), or equivalently Q(t).

Measurement Propagation Equations

The following equations describe how to update the state vector and covariance matrix when a new measurement is made.

(5).

Equation (5) defines the gain, K(t) in terms of the H(t) matrix defined in equation (2), the covariance matrix right before the measurement is processed, P(t), and the square noise matrix R(t) defined in terms of the measurement noise vector v(t) as. The inverse in equation (5) is a matrix inverse in general and it can be a source of significant computation. In the example the measurement is a single number so the matrix inverse is just regular division.

(6).

Equation (6) describes how the best estimate of the state vector is updated using the gain, defined in equation (5), the measurement vector z(t), and the H(t) matrix from equation (2).

(7).

Finally the covariance matrix, P(t), is updated with the gain and the H(t) matrix.

One thing that might seem strange is that in order to implement these equations no random number generator is needed. Something else that might also seem a little strange, is that the spread of the distribution of the random numbers is only used in the covariance time evolution equation and not in the state time evolution equation. This is because Gaussian numbers have a mean of zero. Just something to think about.

Alright, that is enough writing down of random equations. Read up on how they work in one of the many references and you will become a better person for it, if not a little more antisocial.