Contents Previous Next Subchapters

Advancing A Linear Kalman-Bucy Filter One Step At a Time
Syntax kalman(xin, Pin, z, R, Phi, Q, H, xout, Pout)
See Also itrsmo , affinekbs , filter

Description
Executes one step of the linear Kalman-Bucy filter. The values xin, Pin, z, R, Phi, Q, and H may be either real or double-precision. These values are input and are not changed. The column vector xin specifies the estimate for the state vector at the initial time. The square matrix Pin has the same number of rows as xin and specifies the covariance of xin as an estimate for the initial state. The column vector z contains the value of the data at the new time (not the initial time). The square matrix Phi has the same number of rows as xin and specifies the transition matrix. This matrix is the deterministic part of the mapping that transforms the initial state to the new state. The square matrix Q has the same number of rows as xin and is the covariance of the random part of the mapping that transforms the initial state to the new state. The matrix H has the same number of rows as z and the same number of columns as xin has rows. This matrix is the deterministic part of the mapping from the new state value to the measurement value z. The square matrix R has the same number of rows as z and is the covariance of the random part of the mapping from the current state to the current measurement value.

If all of the input variables have the same type, the output values xout and Pout will have that type and all of the calculations will be done in the corresponding precision. The input value xout has no effect. Its output value is set to the expected value of the state vector at the new time (given the value of the measurement z). The input value of Pout has no effect. Its output value is set to the covariance of xout as an estimate of the state at the new time.

Let xinitial and xnew denote the true value of the state vector at the initial and new times, respectively. The transition model is
     xnew = Phi xinitial + w,
where w is a mean zero random variable with covariance Q. The measurement model is
     z    = H xnew + v,
where v is a mean zero random variable with covariance R. If the estimate xin and the random variables w and v are independent and normally distributed, the state estimate xout is the maximum likelihood estimate for the state. In this case, the estimate xout is normally distributed and Pout is its covariance matrix.

Example
The following example uses kalman to advance the state estimate and its covariance two time steps. The example is included as one program below to make it easier to copy and paste into the O-Matrix command line. A discussion of the example follows the program

clear
xinitial = {10., 10., 1., 1.}
alpha    = 2.
Pin      = alpha^2 * identity(4)
u        = alpha * snormal(4, 1)
xin      = xinitial + u
dt       = 1.
Phi      = {[1., 0., dt, 0.], ...
            [0., 1., 0., dt], ...
            [0., 0., 1., 0.], ...
            [0., 0., 0., 1.] ...
}
beta     = 1.
Q        = beta^2 * identity(4)
w        = beta * snormal(4, 1)
xnew     = Phi * xinitial + w
H        = {[1., 0., 0., 0.], ...
            [0., 1., 0., 0.] ...
}
gamma    = .5
R        = gamma^2 * identity(2)
v        = gamma * snormal(2, 1)
z        = H * xnew + v
xout     = novalue
Pout     = novalue
kalman(xin, Pin, z, R, Phi, Q, H, xout, Pout)
print [xnew, xout]
xinitial = xnew
xin      = xout
Pin      = Pout
beta     = .01
Q        = beta^2 * identity(4)
w        = beta * snormal(4, 1)
xnew     = Phi * xinitial + w
gamma    = .01
R        = gamma^2 * identity(2)
v        = gamma * snormal(2, 1)
z        = H * xnew + v
kalman(xin, Pin, z, R, Phi, Q, H, xout, Pout)
print [xnew, xout]

 Discussion
This example is a two-dimensional tracking application of the Kalman-Bucy filter. We initialize the random number generator and clear any existing function definitions with the command
     clear
 For simulation purposes, the true initial location is 10 units east and 10 units north, and the initial velocity is northeast with a speed of sqrt(2):
     xinitial = {10., 10., 1., 1.}
 The components of the initial state estimate are independent and each has a standard deviation alpha. We simulate noise u, with the corresponding covariance Pin, and the initial state estimate xin, by entering
     alpha    = 2.
     Pin      = alpha^2 * identity(4)
     u        = alpha * snormal(4, 1)
     xin      = xinitial + u
 The model for the new position is the old position plus the velocity times the time step dt, plus noise. The model for the new velocity is the old velocity plus noise. The corresponding transition matrix, Phi, is
     dt       = 1.
     Phi      = {[1., 0., dt, 0.], ...
                 [0., 1., 0., dt], ...
                 [0., 0., 1., 0.], ...
                 [0., 0., 0., 1.] ...
     }
 The east and north components of the noise in the transition model are independent and each has standard deviation beta. We simulate noise, w, with the corresponding covariance Q, and the true state value at the new time xnew, by entering
     beta     = 1.
     Q        = beta^2 * identity(4)
     w        = beta * snormal(4, 1)
     xnew     = Phi * xinitial + w
 The measurement has two independent components. The first is the east position and the second is the north position. The corresponding measurement matrix H, is
     H        = {[1., 0., 0., 0.], ...
                 [0., 1., 0., 0.] ...
     }
 The measurement noise is independent and each component has standard deviation gamma. We simulate noise v with the corresponding covariance R, and the measurement z, by entering
     gamma    = .5
     R        = gamma^2 * identity(2)
     v        = gamma * snormal(2, 1)
     z        = H * xnew + v
 In an actual tracking problem the noise values u, v, and w and true state values xinitial and xnew are not known. We estimate the new state vector xnew by
     xout     = novalue
     Pout     = novalue
     kalman(xin, Pin, z, R, Phi, Q, H, xout, Pout)
 We can compare the true value xnew, and its estimate xout, by entering
     print [xnew, xout]
 to which O-Matrix will respond
     {
     [ 10.456 , 10.8956 ]
     [ 11.7081 , 11.0963 ]
     [ -0.588691 , 2.18574 ]
     [ 1.4681 , 2.66793 ]
     }
 The first two components of xout are the estimates of east and north position. The second two components are estimates of east and north velocity. You will notice that the estimates of position are better than the estimates of velocity. This is because we are measuring position directly and are inferring the velocity through the transition equation.

We begin another time step by advancing the true position, the estimated position, and the covariance of the estimate.
     xinitial = xnew
     xin      = xout
     Pin      = Pout
 Next we simulate a transition with much smaller error
     beta     = .01
     Q        = beta^2 * identity(4)
     w        = beta * snormal(4, 1)
     xnew     = Phi * xinitial + w
 Next we simulate a measurement with much smaller error
     gamma    = .01
     R        = gamma^2 * identity(2)
     v        = gamma * snormal(2, 1)
     z        = H * xnew + v
 Then we compute our new estimate and compare it to the true value by entering
     kalman(xin, Pin, z, R, Phi, Q, H, xout, Pout)
     print [xnew, xout]
 to which O-Matrix will respond
     {
     [ 9.88414 , 9.89429 ]
     [ 13.1767 , 13.1708 ]
     [ -0.602134 , 0.746347 ]
     [ 1.48339 , 2.02662 ]
     }