Then Equation 4.11 can be reduced to the form
F
k1
Ex
k1
z
T
i
K
1
k
E
^
x
k
z
T
i
K
k
H
k
F
k1
Ex
k1
z
T
i
K
k
Ev
k
z
T
i
0;
F
k1
Ex
k1
z
T
i
K
1
k
E
^
x
k
z
T
i
K
k
H
k
F
k1
Ex
k1
z
T
i
0;
Ex
k
K
k
H
k
x
k
K
1
k
x
k
K
1
k
^
x
k
x
k
z
T
i
0;
I K
1
k
K
k
H
k
Ex
k
z
T
i
0: 4:12
Equation 4.12 can be satis®ed for any given x
k
if
K
1
k
I K
k
H
k
: 4:13
Clearly, this choice of K
1
k
causes Equation 4.7 to satisfy a portion of the condition
given by Equation 4.8, which was derived in Section 3.8. The choice of
K
k
is such
that Equation 4.9 is satis®ed.
Let the errors
~
x
k
^
x
k
x
k
; 4:14
~
x
k
^
x
k
x
k
; 4:15
~z
k
^z
k
z
k
H
k
^
x
k
z
k
: 4:16
Vectors
~
x
k
and
~
x
k
are the estimation errors after and before updates,
respectively.
3
The parameter
^
x
k
depends linearly on x
k
, which depends linearly on z
k
. Therefore,
from Equation 4.9
Ex
k
^
x
k
z
T
k
0 4:17
and also (by subtracting Equation 4.9 from Equation 4.17)
Ex
k
^
x
k
~z
T
k
0: 4:18
Substitute for x
k
;
^
x
k
and ~z
k
from Equations 4.1, 4.7, and 4.16, respectively. Then
EF
k1
x
k1
w
k1
K
1
k
K
k
z
k
H
k
^
x
k
z
k
T
0:
However, by the system structure
Ew
k
z
T
k
Ew
k
^
x
T
k
0;
EF
k1
x
k1
K
1
k
^
x
k
K
k
z
k
H
k
^
x
k
z
k
T
0:
3
The symbol
is of®cially called a tilde but often called a ``squiggle.''
118 LINEAR OPTIMAL FILTERS AND PREDICTORS
Substituting for K
1
k
, z
k
; and
~
x
k
and using the fact that E
~
x
k
v
T
k
0, this last
result can be modi®ed as follows:
0 EF
k1
x
k1
^
x
k
K
k
H
k
^
x
k
K
k
H
k
x
k
K
k
v
k
H
k
^
x
k
H
k
x
k
v
k
T
Ex
k
^
x
k
K
k
H
k
x
k
^
x
k
K
k
v
k
H
k
~
x
k
v
k
T
E
~
x
k
K
k
H
k
~
x
k
K
k
v
k
H
k
~
x
k
v
k
T
:
By de®nition, the a priori covariance (the error covariance matrix before the
update) is
P
k
E
~
x
k
~
x
T
k
:
It satis®es the equation
I
K
k
H
k
P
k
H
T
k
K
k
R
k
0;
and therefore the gain can be expressed as
K
k
P
k
H
T
k
H
k
P
k
H
T
k
R
k
1
; 4:19
which is the solution we seek for the gain as a function of the a priori covariance.
One can derive a similar formula for the a posteriori covariance (the error
covariance matrix after update), which is de®ned as
P
k
E
~
x
k
~
x
T
k
: 4:20
By substituting Equation 4.13 into Equation 4.7, one obtains the equations
^
x
k
I K
k
H
k
^
x
k
K
k
z
k
;
^
x
k
^
x
k
K
k
z
k
H
k
^
x
k
: 4:21
Subtract x
k
from both sides of the latter equation to obtain the equations
^
x
k
x
k
^
x
k
K
k
H
k
x
k
K
k
v
k
K
k
H
k
^
x
k
x
k
;
~
x
k
~
x
k
K
k
H
k
~
x
k
K
k
v
k
;
~
x
k
I K
k
H
k
~
x
k
K
k
v
k
: 4:22
By substituting Equation 4.22 into Equation 4.20 and noting that E
~
x
k
v
T
k
0, one
obtains
P
k
EI K
k
H
k
~
x
k
~
x
T
k
I K
k
H
k
T
K
k
v
k
v
T
k
K
T
k
I
K
k
H
k
P
k
I K
k
H
k
T
K
k
R
k
K
T
k
: 4:23
4.2 KALMAN FILTER 119
This last equation is the so-called ``Joseph form'' of the covariance update equation
derived by P. D. Joseph [15]. By substituting for
K
k
from Equation 4.19, it can be put
into the following forms:
P
k
P
k
K
k
H
k
P
k
P
k
H
T
k
K
T
k
K
k
H
k
P
k
H
T
k
K
T
k
K
k
R
k
K
T
k
I K
k
H
k
P
k
P
k
H
T
k
K
T
k
K
k
H
k
P
k
H
T
k
R
k
|{z}
P
k
H
T
k
K
T
k
I K
k
H
k
P
k
; 4:24
the last of which is the one most often used in computation. This implements the
effect that conditioning on the measurement has on the covariance matrix of
estimation uncertainty.
Error covariance extrapolation models the effects of time on the covariance
matrix of estimation uncertainty, which is re¯ected in the a priori values of the
covariance and state estimates,
P
k
E
~
x
k
~
x
T
k
;
^
x
k
F
k1
^
x
k1
; 4:25
respectively. Subtract x
k
from both sides of the last equation to obtain the equations
^
x
k
x
k
F
k1
^
x
k1
x
k
;
~
x
k
F
k1
^
x
k1
x
k1
w
k1
F
k1
~
x
k1
w
k1
for the propagation of the estimation error,
~
x. Postmultiply it by
~
x
T
k
(on both sides
of the equation) and take the expected values. Use the fact that E
~
x
k1
w
T
k1
0to
obtain the results
P
k
def
E
~
x
k
~
x
T
k
F
k1
E
~
x
k1
~
x
T
k1
F
T
k1
Ew
k1
w
T
k1
F
k1
P
k1
F
T
k1
Q
k1
; 4:26
which gives the a priori value of the covariance matrix of estimation uncertainty as a
function of the previous a posteriori value.
120 LINEAR OPTIMAL FILTERS AND PREDICTORS
4.2.1 Summary of Equations for the Discrete-Time Kalman Estimator
The equations derived in the previous section are summarized in Table 4.3. In this
formulation of the ®lter equations, G has been combined with the plant covariance
by multiplying G
k1
and G
T
k1
, for example,
Q
k1
G
k1
Ew
k1
w
T
K1
G
T
k1
G
k1
Q
k1
G
T
k1
:
The relation of the ®lter to the system is illustrated in the block diagram of Figure
4.1. The basic steps of the computational procedure for the discrete-time Kalman
estimator are as follows:
1. Compute P
k
using P
k1
, F
k1
, and Q
k1
.
2. Compute
K
k
using P
k
(computed in step 1), H
k
, and R
k
.
3. Compute P
k
using K
k
(computed in step 2) and P
k
(from step 1).
4. Compute successive values of
^
x
k
recursively using the computed values of
K
k
(from step 3), the given initial estimate
^
x
0
, and the input data z
k
.
TABLE 4.3 Discrete-Time Kalman Filter Equations
System dynamic model:
x
k
F
k1
x
k1
w
k1
w
k
0; Q
k
Measurement model:
z
k
H
k
x
k
v
k
v
k
0; R
k
Initial conditions:
Ex
0
^
x
0
E
~
x
0
~
x
T
0
P
0
Independence assumption:
Ew
k
v
T
j
0 for all k and j
State estimate extrapolation (Equation 4.25):
^
x
k
F
k1
^
x
k1
Error covariance extrapolation (Equation 4.26):
P
k
F
k1
P
k1
F
T
k1
Q
k1
State estimate observational update (Equation 4.21):
^
x
k
^
x
k
K
k
z
k
H
k
^
x
k
Error covariance update (Equation 4.24):
P
k
I K
k
H
k
P
k
Kalman gain matrix (Equation 4.19):
K
k
P
k
H
T
k
H
k
P
k
H
T
k
R
k
1
4.2 KALMAN FILTER 121
Step 4 of the Kalman ®lter implementation [computation of
^
x
k
] can be
implemented only for state vector propagation where simulator or real data sets
are available. An example of this is given in Section 4.12.
In the design trade-offs, the covariance matrix update (steps 1 and 3) should be
checked for symmetry and positive de®niteness. Failure to attain either condition is a
sign that something is wrongÐeither a program ``bug'' or an ill-conditioned
problem. In order to overcome ill-conditioning, another equivalent expression for
P
k
is called the ``Joseph form,''
4
as shown in Equation 4.23:
P
k
I K
k
H
k
P
k
I K
k
H
k
T
K
k
R
k
K
T
k
:
Note that the right-hand side of this equation is the summation of two symmetric
matrices. The ®rst of these is positive de®nite and the second is nonnegative de®nite,
thereby making P
k
a positive de®nite matrix.
There are many other forms
5
for K
k
and P
k
that might not be as useful for
robust computation. It can be shown that state vector update, Kalman gain, and error
covariance equations represent an asymptotically stable system, and therefore, the
estimate of state
^
x
k
becomes independent of the initial estimate
^
x
0
, P
0
as k is
increased.
Figure 4.2 shows a typical time sequence of values assumed by the ith component
of the estimated state vector (plotted with solid circles) and its corresponding
variance of estimation uncertainty (plotted with open circles). The arrows show the
successive values assumed by the variables, with the annotation (in parentheses) on
the arrows indicating which input variables de®ne the indicated transitions. Note that
each variable assumes two distinct values at each discrete time: its a priori value
w
v
M
Fig. 4.1 Block diagram of system, measurement model, and discrete-time Kalman ®lter.
4
after Bucy and Joseph [15].
5
Some of the alternative forms for computing K
k
and P
k
can be found in Jazwinski [23], Kailath [24],
and Sorenson [46].
122 LINEAR OPTIMAL FILTERS AND PREDICTORS
corresponding to the value before the information in the measurement is used, and
the a posteriori value corresponding to the value after the information is used.
EXAMPLE 4.1 Let the system dynamics and observations be given by the
following equations:
x
k
x
k1
w
k1
; z
k
x
k
v
k
;
Ev
k
Ew
k
0;
Ev
k
1
v
k
2
2Dk
2
k
1
; Ew
k
1
w
k
2
Dk
2
k
1
;
z
1
2; z
2
3;
Ex0
^
x
0
1;
Ex0
^
x
0
x0
^
x
o
T
P
0
10:
The objective is to ®nd
^
x
3
and the steady-state covariance matrix P
. One can use
the equations in Table 4.3 with
F 1 H; Q 1; R 2;
Fig. 4.2 Representative sequence of values of ®lter variables in discrete time.
4.2 KALMAN FILTER 123
for which
P
k
P
k1
1 ;
K
k
P
k
P
k
2
P
k1
1
P
k1
3
;
P
k
1
P
k1
1
P
k1
3
!
P
k1
1
ÀÁ
;
P
k
2P
k1
1
P
k1
3
;
^
x
k
^
x
k1
K
k
z
k
^
x
k1
:
Let
P
k
P
k1
P steady-state covariance;
P
2P 1
P 3
;
P
2
P 2 0;
P 1; positive-definite solution:
For k 1
^
x
1
^
x
0
P
0
1
P
0
3
2
^
x
0
1
11
13
2 1
24
13
Following is a table for the various values of the Kalman ®lter:
kP
k
P
k
K
k
^
x
k
111
22
13
11
13
24
13
2
45
23
70
61
131
253
49
20
4.2.2 Treating Vector Measurements with Uncorrelated Errors as
Scalars
In many (if not most) applications with vector-valued measurement z, the corre-
sponding matrix R of measurement noise covariance is a diagonal matrix, meaning
that the individual components of v
k
are uncorrelated. For those applications, it is
124 LINEAR OPTIMAL FILTERS AND PREDICTORS
advantageous to consider the components of z as independent scalar measurements,
rather than as a vector measurement. The principal advantages are as follows:
1. Reduced Computation Time. The number of arithmetic computations
required for processing an `-vector z as ` successive scalar measurements is
signi®cantly less than the corresponding number of operations for vector
measurement processing. (It is shown in Chapter 6 that the number of
computations for the vector implementation grows as `
3
, whereas that of
the scalar implementation grows only as `.)
2. Improved Numerical Accuracy. Avoiding matrix inversion in the implemen-
tation of the covariance equations (by making the expression HPH
T
R a
scalar) improves the robustness of the covariance computations against
roundoff errors.
The ®lter implementation in these cases requires ` iterations of the observational
update equations using the rows of H as measurement ``matrices'' (with row
dimension equal to 1) and the diagonal elements of R as the corresponding
(scalar) measurement noise covariance. The updating can be implemented iteratively
as the following equations:
K
i
k
1
H
i
k
P
i1
k
H
iT
R
i
k
P
i1
k
H
iT
k
;
P
i
k
P
i1
k
K
i
k
P
i1
k
H
i
k
;
^
x
i
^
x
i1
k
K
i
k
z
k
i
H
i
k
^
x
i1
k
;
for i 1; 2; 3; ;`, using the initial values
P
0
k
P
k
;
^
x
0
k
^
x
k
;
intermediate variables
R
i
k
ith diagonal element of the ` ` diagonal matrix R
k
;
H
i
k
ith row of the ` n matrix H
k
;
and ®nal values
P
`
k
P
k
;
^
x
`
k
^
x
k
:
4.2.3 Using the Covariance Equations for Design Analysis
It is important to remember that the Kalman gain and error covariance equations are
independent of the actual observations. The covariance equations alone are all that is
required for characterizing the performance of a proposed sensor system before it is
4.2 KALMAN FILTER 125
actually built. At the beginning of the design phase of a measurement and estimation
system, when neither real nor simulated data are available, just the covariance
calculations can be used to obtain preliminary indications of estimator performance.
Covariance calculations consist of solving the estimator equations with steps 1±3 of
the previous subsection, repeatedly. These covariance calculations will involve the
plant noise covariance matrix Q, measurement noise covariance matrix R, state
transition matrix F, measurement sensitivity matrix H, and initial covariance matrix
P
0
Ðall of which must be known for the designs under consideration.
4.3 KALMAN±BUCY FILTER
Analogous to the discrete-time case, the continuous-time random process x(t)and
the observation z(t) are given by
_
xtFtxtGtwt; 4:27
ztHtxtvt; 4:28
EwtEvt0;
Ewt
1
w
T
t
2
Qtdt
2
t
1
; 4:29
Evt
1
v
T
t
2
Rtdt
2
t
1
; 4:30
Ewtv
T
Z0; 4:31
where F(t), G(t), H(t), Q(t), and R(t) are n n; n n, l n, n n, and l l
matrices, respectively. The term dt
2
t
1
is the Dirac delta. The covariance matrices
Q and R are positive de®nite.
It is desired to ®nd the estimate of n state vector x(t) represented by
^
xt which is a
linear function of the measurements zt,0 t T, which minimizes the scalar
equation
Ext
^
xt
T
Mxt
^
xt; 4:32
where M is a symmetric positive-de®nite matrix.
The initial estimate and covariance matrix are
^
x
0
and P
0
.
This section provides a formal derivation of the continuous-time Kalman
estimator. A rigorous derivation can be achieved by using the orthogonality principle
as in the discrete-time case. In view of the main objective (to obtain ef®cient and
practical estimators), less emphasis is placed on continuous-time estimators.
Let Dt be the time interval t
k
t
k1
. As shown in Chapters 2 and 3, the
following relationships are obtained:
Ft
k
; t
k1
F
k
I Ft
k1
Dt 0Dt
2
;
126 LINEAR OPTIMAL FILTERS AND PREDICTORS
where 0Dt
2
consists of terms with powers of Dt greater than or equal to two. For
measurement noise
R
k
Rt
k
Dt
;
and for process noise
Q
k
Gt
k
Qt
k
G
T
t
k
Dt:
Equations 4.24 and 4.26 can be combined. By substituting the above relations, one
can get the result
P
k
I FtDtI K
k1
H
k1
P
k1
I FtDt
T
GtQtG
T
tDt; 4:33
P
k
P
k1
Dt
FtP
k1
P
k1
F
T
t
GtQtG
T
t
K
k1
H
k1
P
k1
Dt
Ft
K
k1
H
k1
P
k1
F
T
t Dt
higher order terms: 4:34
The Kalman gain of Equation 4.19 becomes, in the limit,
lim
Dt0
K
k1
Dt
!
lim
Dt0
P
k1
H
T
k1
H
k1
P
k1
H
T
k1
Dt Rt
1
ÈÉ
PH
T
R
1
Kt: 4:35
Substituting Equation 4.35 in 4.34 and taking the limit as Dt 0, one obtains the
desired result
_
PtFtPtPtF
T
tGtQtG
T
t
PtH
T
tR
1
tHtPt4:36
with Pt
0
as the initial condition. This is called the matrix Riccati differential
equation. Methods for solving it will be discussed in Section 4.8. The differential
equation can be rewritten by using the identity
PtH
T
tR
1
tRtR
1
tHtPtKtRtK
T
t
to transform Equation 4.36 to the form
_
PtFtPtPtF
T
tGtQtG
T
tKtRtK
T
t: 4:37
4.3 KALMAN ± BUCY FILTER 127
Không có nhận xét nào:
Đăng nhận xét