How to define state transition matrix for Kalman Filters?

Question

I am trying to understand Kalman Filter and there are some terms that I cannot understand.

I was reading about Dynamics Model transition matrix (4x4). It says that this matrix will map the equations below to the state components. The equations are simple physics equations:

xt = x(t-1) + vx(dt)
yt = y(t-1) + vy(dt)
dt = 1

The code that represents this is as follows:

dt = 0.1
DT = np.matrix([[1.,0.,dt,0],[0.,1.,0.,dt],[0.,0.,1.,0.],[0.,0.,0.,1.]])

Can someone help me understand this? What exactly is this representation?

Answer 1

Number of equations = number of state variables

Your equations describe how to calculate variables x and y at time t from their values at time t-1 (I corrected them a bit):

x(t) = x(t-1) + vx*dt
y(t) = y(t-1) + vy*dt

dt is the time between "epochs" t-1 and t (the latter are temporal indices, not actual times)

We see the calculation involves not only x and y but also vx and vy, the velocities, according to Newton's equation for a constant velocity motion (p = p0 + v*t). So your system has 4 variables, 2 are observed (by measurements of x and y), and 2 are hidden (they have no measurements but are calculated by the filter). These variables makes the "state" X of the system:

X = [x, y, vx, vy]

You provided 2 equations, 2 are still missing, the ones related to velocities. But the prediction model assumed here is a constant velocity motion, so the velocities in the state don't change, e.g. the prediction equation for vy is just:

vy = vy

Notes:

• State-variable order is not important, but the filter matrices must be designed consistently according to the order selected for the state vector.

• Don't confuse unrelated names: The state X (a vector) and variable x (part of this vector which by chance has a similar name).

Prediction and correction phases

The filter works in two phases: Prediction, then correction, the names varies. Your question is related to prediction.

• Prediction consists in predicting the state at epoch t from the state at epoch t-1, based on a model you provide to the filter in the form of equations, one for each variable in the state. Each equation is a sum of terms involving all variables (linear equations for the original Kalman filter).

• Correction (update) consists in correcting the prediction using the real measurement z provided in input, proportionally to a coefficient K, calculated and updated by the filter in each prediction/correction cycle. K (0 to 1) represents whether the filter trusts its own prediction (K=0) or the measurement (K=1), or both in proportion of 1-K and K. Trust here is quantified by the related Gaussian variances.

Building the prediction (transition) matrix F

If we look at the prediction phase, e.g. for y, we must provide this kind of equation:

y(t) = f1.x(t-1) + f2.y(t-1) + f3.vx(t-1) + f4.vy(t-1)

Now this equation with indices is the one used by mathematicians for demonstrations, but for an IT engineer, as t and t-1 are always implicit, respectively left and right side of = sign, it would be:

y = f1.x + f2.y + f3.vx + f4.vy

Based on your equation for y:

y  = y + vy*dt
   = 0.x + 1.y + 0.vx + dt.vy

A possible simplification comes from the use of matrices to perform multiple operations "at once":

[y] = [f1, f2, f3, f4] . ⎡x ⎤
                         ⎢y ⎥
                         ⎢vx⎥
                         ⎣vy⎦

You recognize the last matrix as being the state vector written vertically, and the middle matrix is the one called the "transition matrix" F. As already mentioned, the order of the coefficients in the transition matrix must match the order you selected for the state.

For those who forgot how matrices are multiplied (dot product), just look here.

Kalman filters don't have to use matrices, they can use individual equations, matrices are just much more simple after a short period of familiarization, because they allow to process multiple variables at once, with a huge advantage: The formulas based on matrix operations are still valid when the number of variables changes in the matrices.

And this can be seen here: What we did for y can be done to calculate the 3 other variables. The same transition matrix F can be used, with one row per equation, and the result will be a state vector with the 4 variables (at time t):

⎡x ⎤ = ⎡?,  ?,  ?,  ? ⎤ . ⎡x ⎤
⎢y ⎥   ⎢f1, f2, f3, f4⎥   ⎢y ⎥
⎢vx⎥   ⎢?,  ?,  ?,  ? ⎥   ⎢vx⎥
⎣vy⎦   ⎣?,  ?,  ?,  ? ⎦   ⎣vy⎦

So let's write our matrix F with the all the coefficients we know from the 4 equations:

⎡x ⎤ = ⎡1, 0, dt, 0 ⎤ . ⎡x ⎤
⎢y ⎥   ⎢0, 1, 0,  dt⎥   ⎢y ⎥
⎢vx⎥   ⎢0, 0, 1,  0 ⎥   ⎢vx⎥
⎣vy⎦   ⎣0, 0, 0,  1 ⎦   ⎣vy⎦

To be clear, how to read that: x at t (first variable of the state vector on the left) is equal to the sum of terms which coefficients are 1, 0, dt, 0 (first row of F matrix) and variables are respectively x, y, vx, vy of the state vector at time t-1 (vector on the right)

Important caveat

The model used for prediction is a constant velocity motion. Normally there is no need to have a filter, as the position is exactly p = p0 + v * constant, represented by a straight line when plotted. We use a filter because we known the system departs a bit from that model. But if the model is not accurate, then why do we use it?

The filter is able to process a slowly-varying velocity situation with a constant velocity model, but you need to introduce an uncertainty floor, both in the prediction and in the correction phases.

This uncertainty is actually a variance/covariance matrix. Variances and covariances tell the filter to adjust its correction phase using a coefficient K in order to be not too much confident in its own prediction (matrix Q) nor in the measurements (matrix R). Providing these matrices implies you have some mean to estimate the variances upfront, or we'll need to tweak the matrices with some tests, it can be tedious.

Example

You'll notice I don't use matrices in the code, but simple Numpy arrays. The reason is this is better and matrices don't provide additional advantages, but inconveniences (see deprecation). Dot product in Numpy is simplified by the use of syntax sugar array operator @.

# Prediction function
def predict(x, P):
    x = F @ x
    P = F @ P @ F.T + Q
    return x, P

# Correction/update function
def update(x, P, z):
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    y = z - H @ x

    x = x + K @ y
    P = P - K @ H @ P
    return x, P

dt = 1
Q_std = 0.04
R_std = 0.35

# Transition
# NOTE: order of state variables is x, dx, y, dy   
F = np.array([[1, dt, 0, 0],
              [0, 1, 0, 0],
              [0, 0, 1, dt],
              [0, 0, 0, 1]])

# Process noise
q = np.array([[0.0004, 0.0008], 
              [0.0008, 0.0016]])
Q = la.block_diag(q, q)

# Measurement matrix H
# NOTE: order of state variables is x, dx, y, dy   
H = np.array([[1, 0, 0, 0],
              [0, 0, 1, 0]])

# Measurement noise matrix R
R = np.eye(2) * R_std**2

# Unknown initial positions and velocities.
# We assume no x/y or p/v correlation.
x = np.array([[0, 0, 0, 0]]).T
P = np.eye(4) * 500

for z in zs:
    x, P = update(*predict(x, P), z)
    # do something with estimated x and P

Answer 2

The state transition matrix describes how your states propagate with time given an initial state. For a Linear Time-Invariant (LTI)system, this is a constant matrix.

For example, assuming I have a 2-dimensional discrete-time LTI model given below:

x(k+1) = x(k) ---- (1)

y(k+1) = y(k) + 2x(k) ----- (2)

This can be written in matrix form by looking at the coefficients of the states in each equation as shown below:

[x(k+1), y(k+1)] = [[1.0, 0.0],[2.0, 1.0]]* [x(k),y(k)]

The matrix [[1.0, 0.0],[2.0, 1.0]] is known as the state transition matrix. Take note, this is similar to how you write linear systems of equations in matrix form to solve them simultaneously using the Cramer's rule or matrix inversion.

As you can see, only x(k) appears in (1) with a coefficient of 1 hence the first row of the transition matrix is [1.0, 0.0]. Similarly, the second row is [2.0, 1.0].

Taking a look at the structure of your matrix

DT = np.matrix([[1.,0.,dt,0],[0.,1.,0.,dt],[0.,0.,1.,0.],[0.,0.,0.,1.]])

I can tell you have 4 variables [x(t-1), y(t-1), vx, vy]. You have shown only two state equations (x(t) and y(t)) and the first 2 rows of your matrix correspond well with the coefficient of the variables in the equations.

From your matrix, I can infer that the last two equations are

vx(t) = vx(t-1) and vy(t) = vy(t-1).

I'd suggest you read more on state space models (LTI should be sufficient). https://en.wikipedia.org/wiki/State-space_representation

Note: For continuous-time models, getting the state transition matrix will require finding the matrix exponential.

Answer 3

So the transition matrix is describing the spontaneous transition from one time point i to the next i+1. Say, you have a little robot that drives through your house. Then sometimes it will slide a little bit on the floor because it will not always have good traction. The transition matrix tries to model it.

The transition model is then used in several parts in the Kalman filter. First, to describe the variance and the position of your robot at time point i. And it is part of formulating the prediction error (Kalman gain) of your sensor model to minimize the variance of your next measure.

Basically, it is a big part of the Kalman filter, but also a trivial one. It just tries to model a spontaneous transition over time (a.k.a sliding, slipping, being pushed by wind...)

Please ask more if this didn't help.