( 33)

 

For state estimation, we need only the mean and covariance matrix of xt|t− 1. The predictor box in Figure 6 computes these values; the covariance matrix is obtained from Lemma 2 under the assumption that wt is uncorrelated with xt− 1|t− 1, which is justified here.

Fusing complete observations
of the state. If the entire state can
be measured at each time step, the
imprecise measurement at time t is
modeled as follows:
( 34)

 

where vt is a zero-mean noise term with covariance matrix Rt. The noise terms in different time steps are assumed to be uncorrelated with each other (such as, E[vivj] is zero if i≠j) as well as with x0|0 and all wk. A subtle point here is that xt in this equation is the actual state of the system at time t (that is, a particular realization of the random variable xt), so variability in zt comes only from vt and its covariance matrix Rt.

Therefore, we have two imprecise estimates for the state at each time step t = 1, 2, …, the a priori estimate from the predictor and the one from the measurement (zt). If zt is uncorrelated to xt|t− 1, we can use Equations 20–22 to fuse the estimates as shown in Figure 6c.

The assumptions that (i) xt− 1|t− 1 is
uncorrelated with wt, which is used in
prediction, and (ii) xt|t− 1 is uncorrelated
with zt, which is used in fusion, are eas-
ily proved to be correct by induction on
t, using Lemma 2(ii). Figure 6b gives the
intuition: xt|t− 1 for example is an affine
function of the random variables x0|0, w1,
v1, w2, v2, …, wt, and is therefore uncor-
related with vt (by assumption about vt
and Lemma 2(ii) ) and hence with zt.
Figure 7 shows the computation picto-
rially using confidence ellipses to illus-
trate uncertainty. The dotted arrows at
the bottom of the figure show the evolu-
tion of the state, and the solid arrows show
the computation of the a priori estimates
and their fusion with measurements.

Fusing partial observations of the state. In some problems, only a portion of the state can be measured directly. The observable portion of the state is specified formally using a full row-rank matrix Ht called the observation matrix: if the state is x, what is observable is Htx. For example, if the state vector has two components and only the first component is observable, Ht can be [ 1 0]. In general, the Ht matrix can specify a linear relationship between the state and the observation, and it can be time-dependent. The imprecise measurement model introduced in Equation 34 becomes:

 

( 35)

 

The hidden portion of the state can be specified using a matrix Ct, which is an orthogonal complement of Ht. For example, if Ht = [ 1 0], one choice for Ct is [0 1].

Figure 6d shows the computation for this case. The fusion phase can be understood intuitively in terms of the following steps.

 

i. The observable part of the a pri-
ori estimate of the state
is fused with the measurement
(zt), using Equations 20–22.
The quantity is
called the innovation. The result is
the a posteriori estimate of the
observable state .
ii. The BLUE of Theorem 4 is used to
obtain the a posteriori estimate
of the hidden state by adding
to the a priori estimate of the hid-
den state a value obtained
from the product of the covariance
between Htxt|t– 1 and Ctxt|t– 1 and the
differencebetween and .
iii. The a posteriori estimates of the
observable and hidden portions
of the state are composed to pro-
duce the a posteriori estimate of
the entire state .

 

The actual implementation produces the final result directly without going through these steps as shown in Figure 6d, but these incremental steps are useful for understanding how all this works, and their implementation is shown in more detail in Figure 8.

Figure 6d puts all this together. In the literature, this dataflow is referred to as Kalman filtering. Unlike in Equations 18 and 21, the

Figure 9. Estimates of the object’s state over time.

0 5 10 15 20
Time (s)

0 500 1000 1500 2000 2500

Dis t a nc e ( m)

Model-only
Ground Truth
Estimated
(a) Evolution of state: Distance

0 5 10 15 20
Time (s)

0 50 100 150 200 250 300

Ve lo ci t y (m /s )

0 25 50 75 100 125 150

V a ri a nc e

Model-only

Ground Truth
Estimated
Measured
Variance
(b) Evolution of state: Velocity

0 5 10 15 20

Time (s)

0

500 1000 1500 2000 2500

Dis t a nc e ( m)

Model-only
Ground Truth
Estimated
(a) Evolution of state: Distance

0 5 10 15 20

Time (s)

0

50

100

150

200

250

300

Ve lo ci t y (m /s )

0

25

50

75

100

125

150

V a ri a nc e

Model-only
Ground Truth
Estimated
Measured
Variance
(b) Evolution of state: Velocity