Thursday, February 23, 2012

Kalman Filtering for Dummies Part II

Using Kalman filter is all about the underlying model. Specifically discrete state space model. The continuous counterpart of Kalman filter is called the Kalman-Bucy filter, but it would be meaningless because Kalman filter is usually implemented in a digital controller. For simplicity, I would be using examples for kinematic models which are more easily understood as opposed to a dynamic model. A discrete dynamic model can be derived from first principle, applying La Place transform and discretized, to form a discrete state space. But for kinematic model, it can be easily derived from basic kinematic equation.

Using Kalman filter deals with a lot of matrices and vectors. Therefore it is better to have good basics in linear system. We shall start with a simple example.

x[n] = x[n-1] + T * v[n-1] + 0.5 * T * T * a[n-1] --- Equation 1
v[n] = v[n-1] + T * a[n-1] --- Equation 2
a[n] = a[n-1] --- Equation 3

Explanation:
- For equation 1, 2, and 3, I used the square bracket to denote sample. [n] is the value of state in the current sample, [n-1] is the value in the previous sample while [n-k] is the value is the previous k sample.
- x is the position state, v is the velocity state while a is the acceleration state. T is the sampling time, that is the period of one sample time to another sample.
- Equation 1 simply means I would like to predict the current position value from the previous sample using simple kinematic equations. So as Equation 2. As for Equation 3, I can assume that he acceleration is constant throughout the time, but of course this is not true. This is when Kalman filter comes in handy.

Arranging Equation 1, 2, and 3 in matrix form:
where q, the state vector is the vector containing three state variables, position, velocity and acceleration, and F is the state transition matrix. The whole equation is called the state equation (as in discrete state space kinematic model). Note that state equation is internal and hidden from the user. Put differently, the value is not known unless measured. If you are familiar with state space control, this model is also an inputless system.

Next, we would deal with the output equation. Output equation is the equation dealing with the the state that we are going to measure. For this example, we are going to measure all the state variables. Therefore the equation is simply an identity matrix. In other words, each state variables will have their own dedicated sensors.

Rewriting the output equation in matrix form, H is identity matrix. There is also cases where not all states variables can be measure. This case shall be deal later, but in this case, H matrix is not identity.

To sum up, discrete state equation is the equation connecting the current state from previous state through a state transition matrix while the output equation is the connecting the measured variables and the state equation.

This is a simple example of a model where Kalman filter can be used. There are many other model where Kalman filter can be used, therefore it depends on how the model was derived and used along with this filter. Also in this example, the model is a linear model where F and H matrix is constant. There are also nonlinear models where F and H is not linear but dependent on the state variables or the derivatives, or integrals, or even time. This is when Extended Kalman Filter or Unscented Kalman Filter can be a good choice; which I will not cover this due to limited knowledge.

As for the next part, I will be discussing the implementation of the Kalman filter formula and brief explanation on the formula.

Part I
Part II
Part III
Part IV
Part V
Part VI