Contents Previous Next Subchapters

Nonlinear Kalman-Bucy Filtering And Smoothing
Syntax itrsmo(function meas, function tran, ...
                level, M, N, xini, Pinv)
itrsmo(function meas, function tran, ...
                level, M, N, xini, Pinv, Cout)
See Also affinekbs , kalman , filter

Description
Returns the extended Kalman-Bucy filter or smoother estimate for the value of the state vector at each time point. The k-th column of the return value is the estimate for the state vector at the k-th time point. This routine allows for general nonlinear measurement and transition functions. The input arguments xini and Pinv and the outputs from the functions meas and tran must be real or double-precision. If they all have the same type, Cout and the return value of itrsmo will have that type and all of the calculations will be done in the corresponding precision.

The iterated smoother combines the decomposition technique of Kalman filtering with a Gauss-Newton method to solve a large nonlinear least squares problem. The integer scalar M specifies the number of Gauss-Newton iterations that the iterated smoother performs. If M is 0, the result is the same as for the extended Kalman filter; if M is 1, the result is the same as for the affine Kalman smoother. If the integer scalar level is 1, the residual sum of squares is printed at each iteration. If level is 0, no tracing is done. The integer scalar N specifies the number of time points at which there are measurement values. The column vector xini specifies an estimate for the initial value of the true state vector. The square matrix Pinv has the same number of rows as xini and specifies the inverse of the covariance for the estimate xini. The model for the initial estimate is
     xini = x1 + e1
where x1 is the true, but unknown, value of the state vector at time point number one, e1 is a mean zero random vector with the same dimension as xini, and the covariance of e1 is equal to the inverse of Pinv.

If Cout is present, its input value does not matter and its output value contains the covariance that corresponds to each of the state estimates. The row dimension of Cout is the square of the row dimension of xini and its column dimension is equal to N. The k-th column of Cout contains the variance of the state estimate for the k-th time point. This could be converted to a covariance matrix C as follows
     C  = Cout.col(k)
     dim C(N, N) 

 meas(k, sk, hk, dhk, zk, Rinvk)
The measurement model and the data values are specified by the function meas. The measurement model for the k-th time point is
     zk = h(xk) + vk
where h(x) is a known function, xk is the true, but unknown, value of the state vector at time point k, vk is a mean zero random vector with the same dimension as zk, and the covariance of vk is equal to the inverse of Rinvk. The input value of k is an integer scalar between 1 and N that specifies the current time point. The input value of sk has the same dimension as xini and specifies a value for the state vector at time index k. The other arguments to meas are outputs and their input values have no effect. The value hk is set to the value of the measurement function at sk; that is, h(sk). This value must have the same dimensions as zk. The value dhk is set to the derivative of the measurement function at sk; that is, h'(sk). (The prime symbol here denotes the derivative of h, not the transposition of a matrix.) The (i,j)-th element of dhk is the partial derivative of the i-th element of h(x) with respect to the j-th element of x. The matrix dhk must have the same number of rows as zk and its column dimension must equal the row dimension of xini. The value zk is set to the data value for this time point. The matrix Rinvk is set to the inverse of the data covariance for this time point. It must be a symmetric positive definite matrix with the same number of rows as zk. (The matrix Rinvk is positive definite if
         T
     u  Rinvk u > 0
whenever u is a nonzero column vector with the same number of rows as Rinvk.)

tran(k, sk, gk, dgk, Qinvk)
The transition model is specified by the function tran. This model for the k-th time interval is
     xkp = g(xk) + wk
where g(x) is a known function, xk is the true, but unknown value of the state vector at time point k, xkp is the true, but unknown value of the state vector at time point k+1, wk is a mean zero random vector with the same dimension as xini, and the covariance of wk is equal to the inverse of Qinvk. The input value of k is an integer scalar between 1 and N-1 that specifies the current time interval. (Interval k is the time between the measurement at time point k and at time point k+1.) The input value of sk has the same dimension as xini and specifies a value for the state vector at time point k. The other arguments to tran are outputs and their input values have no effect. The value gk is set to the value of the transition function at sk; that is, g(sk). This value must have the same dimensions as xini. The value dgk is set to the derivative of the transition function at sk; that is, g'(sk). The (i,j)-th element of dgk is the partial derivative of the i-th element of g(x) with respect to the j-th element of x. The matrix dgk must be a square matrix with the same number of rows as xini. The matrix Qinvk is set to the inverse of the transition covariance for the time interval; it must be a symmetric positive definite matrix with the same number of rows as xini.

Example
The following example runs an iterated smoother, tracing the residual sum of squares and then compares the estimates with the true solution that was used for simulating the data. 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
x1    = {10., 10., 1., 1.}
alpha = 2.
Pinv  = identity(4) / alpha^2
e1    = alpha * snormal(4, 1)
xini  = x1 + e1
dt    = 1.
Phi   = {[1., 0., dt, 0.], ...
         [0., 1., 0., dt], ...
         [0., 0., 1., 0.], ...
         [0., 0., 0., 1.] ...
}
beta  = 1.
w1    = beta * snormal(4, 1)
x2    = Phi * x1 + w1
function tran(k, sk, gk, dgk, Qinvk) begin
     global Phi, beta
     gk    = Phi * sk
     dgk   = Phi
     Qinvk = identity(4) / beta^2
     return
end
function h(x) begin
     h1 = sqrt( x(1)^2 + x(2)^2 )
     h2 = sqrt( (x(1) - 20.)^2 + x(2)^2 )
     return {h1, h2}
end
function dh(x) begin
     hx   = h(x)
     dh11 = x(1) / hx(1)
     dh12 = x(2) / hx(1)
     dh21 = (x(1) - 20.) / hx(2)
     dh22 = x(2) / hx(2)
     return {[dh11, dh12, 0., 0.], ...
             [dh21, dh22, 0., 0.]}
end
gamma = .5
v1    = gamma * snormal(2, 1)
v2    = gamma * snormal(2, 1)
z     = [ h(x1) + v1, h(x2) + v2 ]
function meas(k, sk, hk, dhk, zk, Rinvk) begin
     global z, gamma
     hk    = h(sk)
     dhk   = dh(sk)
     zk    = z.col(k)
     Rinvk = identity(2) / gamma^2
     return
end
level = 1
M     = 3
N     = 2
xhat  = itrsmo(function meas, function tran, level, M, N, xini, Pinv)
print [xhat.col(1), x1]
print [xhat.col(2), x2]
Discussion
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 speed equal to sqrt(2)
     x1    = {10., 10., 1., 1.}
 The components of the initial state estimate are independent and each has a standard deviation alpha. We simulate noise, e1, with the corresponding inverse covariance, Pinv, and the initial state estimate, xini, by entering
     alpha = 2.
     Pinv  = identity(4) / alpha^2
     e1    = alpha * snormal(4, 1)
     xini  = x1 + e1
 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, w1, and the true state value at the new time, x2, by entering
     beta  = 1.
     w1    = beta * snormal(4, 1)
     x2    = Phi * x1 + w1
 We define the transition model by entering
     function tran(k, sk, gk, dgk, Qinvk) begin
          global Phi, beta
          gk    = Phi * sk
          dgk   = Phi
          Qinvk = identity(4) / beta^2
          return
     end
 The transition function, g(x) = Phi x, is linear, and its derivative is thus simple to calculate. The measurement function is more complicated because it is nonlinear.

The measurement has two independent components. The first component is the distance from the state vector's position to the point (0, 0). The second is the distance from the state vector to the point (20, 0). The corresponding measurement function, h(x), is defined by
     function h(x) begin
          h1 = sqrt( x(1)^2 + x(2)^2 )
          h2 = sqrt( (x(1) - 20.)^2 + x(2)^2 )
          return {h1, h2}
     end
 We also use the derivative of the measurement function; that is,
     function dh(x) begin
          hx   = h(x)
          dh11 = x(1) / hx(1)
          dh12 = x(2) / hx(1)
          dh21 = (x(1) - 20.) / hx(2)
          dh22 = x(2) / hx(2)
          return {[dh11, dh12, 0., 0.], ...
                  [dh21, dh22, 0., 0.]}
     end
 Note that the (i,j)-th element of the return value is the partial derivative of the i-th element of h(x) with respect to the j-th element of x. The function dh(x) calculates these derivatives one at a time.

The measurement noise is independent and each component has standard deviation gamma. We simulate noise (v1 and v2) and the measurements (z.col(1) and z2.col(2)) by entering
     gamma = .5
     v1    = gamma * snormal(2, 1)
     v2    = gamma * snormal(2, 1)
     z     = [ h(x1) + v1, h(x2) + v2 ]
 We define the measurement model by entering
     function meas(k, sk, hk, dhk, zk, Rinvk) begin
          global z, gamma
          hk    = h(sk)
          dhk   = dh(sk)
          zk    = z.col(k)
          Rinvk = identity(2) / gamma^2
          return
     end
 In an actual tracking problem the noise values e1, v1, e2, v2, and true state values x1 and x2 are not known. We estimate the state vectors x1 and x2 by
     level = 1
     M     = 3
     N     = 2
     xhat  = itrsmo(function meas, function tran, level, M, N, xini, Pinv)
 to which O-Matrix responds
     itrsmo: residual sum of squares = 45.1148 
     itrsmo: residual sum of squares = 3.27125 
     itrsmo: residual sum of squares = 2.94872 
 These are the residual sum of squares corresponding to the state estimate at the beginning of each iteration. We compare the true and estimated state vector at time point 1 by entering
     print [xhat.col(1), x1]
 to which O-Matrix responds
     {
     [ 10.5083 , 10 ]
     [ 9.75987 , 10 ]
     [ 1.14223 , 1 ]
     [ 2.31955 , 1 ]
     }
 We then compare the true and estimated state vector at time point 2 by entering
     print [xhat.col(2), x2]
 to which O-Matrix responds
     {
     [ 11.244 , 10.456 ]
     [ 12.2182 , 11.7081 ]
     [ 1.14223 , -0.588691 ]
     [ 2.31955 , 1.4681 ]
     }
 The second two components of xhat.col(1) and xhat.col(2) are the velocity estimates at time point 1 and time point 2. These estimates are the same because we are not measuring velocity directly. We are inferring velocity by differencing positions and we only have two time points.