If the higher order terms in dx can be neglected, then
dx
k
% F
1
kÀ1
dx
kÀ1
w
kÀ1
; 5:7
where the ®rst-order approximation coef®cients are given by
F
1
kÀ1
@f x; k À 1
@x
xx
nom
kÀ1
5:8
@f
1
@x
1
@f
1
@x
2
@f
1
@x
3
ÁÁÁ
@f
1
@x
n
@f
2
@x
1
@f
2
@x
2
@f
2
@x
3
ÁÁÁ
@f
2
@x
n
@f
3
@x
1
@f
3
@x
2
@f
3
@x
3
ÁÁÁ
@f
3
@x
n
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
@f
n
@x
1
@f
n
@x
2
@f
n
@x
3
ÁÁÁ
@f
n
@x
n
2
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
xx
nom
kÀ1
; 5:9
an n  n constant matrix.
5.4.3 Linearization of h about a Nominal Trajectory
If h is suf®ciently differentiable, then the measurement can be represented by a
Taylor series:
hx
k
; khx
nom
k
; k
@hx; k
@x
xx
nom
k
dx
k
higher order terms, 5:10
or
dz
k
@hx; k
@x
xx
nom
k
dx
k
higher order terms. 5:11
If the higher-order terms in this expansion can be ignored, then one can represent the
perturbation in z
k
as
dz
k
H
1
k
dx
k
; 5:12
5.4 LINEARIZATION ABOUT A NOMINAL TRAJECTORY 173
where the ®rst-order variational term is
H
1
k
@hx; k
@x
xx
nom
k
5:13
@h
1
@x
1
@h
1
@x
2
@h
1
@x
3
ÁÁÁ
@h
1
@x
n
@h
2
@x
1
@h
2
@x
2
@h
2
@x
3
ÁÁÁ
@h
2
@x
n
@h
3
@x
1
@h
3
@x
2
@h
3
@x
3
ÁÁÁ
@h
3
@x
n
.
.
.
.
.
.
.
.
.
.
.
.
@h
`
@x
1
@h
`
@x
2
@h
`
@x
3
ÁÁÁ
@h
`
@x
n
2
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
6
4
3
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
7
5
xx
nom
k
; 5:14
which is an ` Â n constant matrix.
5.4.4 Summary of Perturbation Equations in the Discrete Case
From Equations 5.7 and 5.12, the linearized equations about nominal values are
dx
k
F
1
kÀ1
dx
kÀ1
w
kÀ1
; 5:15
dz
k
H
1
k
dx
k
v
k
: 5:16
If the problem is such that the actual trajectory x
k
is suf®ciently close to the nominal
trajectory x
nom
k
so that the higher order terms in the expansion can be ignored, then
this method transforms the problem to a linear problem.
5.4.5 Continuous Case
In the continuous case, the corresponding nonlinear differential equations for plant
and observation are
_
xtf xt; tGtwt; 5:17
zthxt; tvt; 5:18
with the dimensions of the vector quantities the same as in the discrete case.
174 NONLINEAR APPLICATIONS
Similar to the case of the discrete system, the linearized differential equations can
be derived as
d
_
xt
@f xt; t
@xt
xtx
nom
!
dxtGtwt5:19
F
1
dxtGtwt; 5:20
dzt
@hxt; t
@xt
xtx
nom
!
dxtvt5:21
H
1
dxtvt: 5:22
Equations 5.20 and 5.22 represent linearized continuous model equations. The
variables dxt and dzt are the perturbations about the nominal values as in discrete
case.
5.5 LINEARIZATION ABOUT THE ESTIMATED TRAJECTORY
The problem with linearization about the nominal trajectory is that the deviation of
the actual trajectory from the nominal trajectory tends to increase with time. As the
deviation increases, the signi®cance of the higher order terms in the Taylor series
expansion of the trajectory also increases.
A simple but effective remedy for the deviation problem is to replace the nominal
trajectory with the estimated trajectory, that is, to evaluate the Taylor series
expansion about the estimated trajectory. If the problem is suf®ciently observable
(as evidenced by the covariance of estimation uncertainty), then the deviations
between the estimated trajectory (along which the expansion is made) and the actual
trajectory will remain suf®ciently small that the linearization assumption is valid
[112, 113].
The principal drawback to this approach is that it tends to increase the real-time
computational burden. Whereas F, H, and
K for linearization about a nominal
trajectory may have been precomputed off-line, they must be computed in real time
as functions of the estimate for linearization about the estimated trajectory.
5.5.1 Matrix Evaluations for Discrete Systems
The only modi®cation required is to replace x
nom
kÀ1
by
^
x
kÀ1
and x
nom
k
by
^
x
k
in the
evaluations of partial derivatives. Now the matrices of partial derivatives become
F
1
^
x; k
@f x; k
@x
x
^
x
k
À
5:23
5.5 LINEARIZATION ABOUT THE ESTIMATED TRAJECTORY 175
and
H
1
^
x; k
@hx; k
@x
x
^
x
k
À
: 5:24
5.5.2 Matrix Evaluations for Continuous Systems
The matrices have the same general form as for linearization about a nominal
trajectory, except for the evaluations of the partial derivatives:
F
1
t
@f xt; t
@xt
x^xt
5:25
and
H
1
t
@hxt; t
@xt
x
^
xt
: 5:26
5.5.3 Summary of Implementation Equations
For discrete systems linearized about the estimated state,
dx
k
F
1
kÀ1
dx
kÀ1
w
kÀ1
; 5:27
dz
k
H
1
k
dx
k
v
k
: 5:28
For continuous systems linearized about the estimated state,
_
d
x
tF
1
t dxtGtwt; 5:29
dztH
1
dxtvt: 5:30
5.6 DISCRETE LINEARIZED AND EXTENDED FILTERING
These two approaches to Kalman ®lter approximations for nonlinear problems yield
decidedly different implementation equations. The linearized ®ltering approach
generally has a more ef®cient real-time implementation, but it is less robust against
nonlinear approximation errors than the extended ®ltering approach.
The real-time implementation of the linearized version can be made more
ef®cient by precomputing the measurement sensitivities, state transition matrices,
176 NONLINEAR APPLICATIONS
and Kalman gains. This off-line computation is not possible for the extended
Kalman ®lter, because these implementation parameters will be functions of the
real-time state estimates.
Nonlinear Approximation Errors. The extended Kalman ®lter generally has
better robustness because it uses linear approximation over smaller ranges of state
space. The linearized implementation assumes linearity over the range of the
trajectory perturbations plus state estimation errors, whereas the extended Kalman
®lter assumes linearity only over the range of state estimation errors. The expected
squared magnitudes of these two ranges can be analyzed by comparing the solutions
of the two equations
X
k1
F
1
k
X
k
F
1T
k
Q
k
;
P
k1
F
1
k
fP
k
À P
k
H
T
k
H
k
P
k
H
T
k
R
k
À1
H
k
P
k
gF
1T
k
Q
k
:
The ®rst of these is the equation for the covariance of trajectory perturbations, and
the second is the equation for the a priori covariance of state estimation errors. The
solution of the second equation provides an idea of the ranges over which the
extended Kalman ®lter uses linear approximation. The sum of the solutions of the
two equations provides an idea of the ranges over which the linearized ®lter assumes
linearity. The nonlinear approximation errors can be computed as functions of
perturbations (for linearized ®ltering) or estimation errors (for extended ®ltering) dx
by the formulas
e
x
f x dxÀf xÀ
@f
@x
dx;
e
z
Khx dxÀhxÀ
@h
@x
dx
;
where e
x
is the error in the temporal update of the estimated state variable due to
nonlinearity of the dynamics and e
z
is the error in the observational update of the
estimated state variable due to nonlinearity of the measurement. As a rule of thumb
for practical purposes, the magnitudes of these errors should be dominated by the
RMS estimation uncertainties. That is, jej
2
( trace P for the ranges of dx expected
in implementation.
5.6.1 Linearized Kalman Filter
The block diagram of Figure 5.1 shows the data ¯ow of the estimator linearized
about a nominal trajectory of the state dynamics. Note that the operations within the
dashed box have no inputs. These are the computations for the nominal trajectory.
Because they have no inputs from the rest of the estimator, they can be precomputed
off-line.
5.6 DISCRETE LINEARIZED AND EXTENDED FILTERING 177
The models and implementation equations for the linearized discrete Kalman
®lter that were derived in Section 5.4 are summarized in Table 5.3. Note that the last
three equations in this table are identical to those of the ``standard'' Kalman ®lter.
5.7 DISCRETE EXTENDED KALMAN FILTER
The essential idea of the extended Kalman ®lter was proposed by Stanley F.
Schmidt, and it has been called the ``Kalman±Schmidt'' ®lter [122, 123, 136].
The models and implementation equations of the extended Kalman ®lter that
were derived in Section 5.5 are summarized in Table 5.4. The last three equations in
this table are the same as those for the ``standard'' Kalman ®lter, but the other
equations are noticeably different from those of the linearized Kalman ®lter in
Table 5.3.
EXAMPLE 5.1 Consider the discrete-time system
x
k
x
2
kÀ1
w
kÀ1
;
z
k
x
3
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
;
Ex0
^
x
0
2;
x
nom
k
2;
P
0
1;
Fig. 5.1 Estimator linearized about a ``nominal'' state.
178 NONLINEAR APPLICATIONS
TABLE 5.3 Discrete Linearized Kalman Filter Equations
Nonlinear nominal trajectory model:
x
nom
k
f
kÀ1
x
nom
kÀ1
Linearized perturbed trajectory model:
dx
def
x À x
nom
dx
k
%
@f
kÀ1
@x
xx
nom
kÀ1
dx
kÀ1
w
kÀ1
w
k
$x0; Q
k
Nonlinear measurement model:
z
k
h
k
x
k
v
k
; v
k
$x0; R
k
Linearized approximation equations:
Linear perturbation prediction:
b
dx
k
À F
1
kÀ1
b
dx
kÀ1
; F
1
kÀ1
%
@f
kÀ1
@x
xx
nom
kÀ1
Conditioning the predicted perturbation on the measurement:
b
dx
k
b
dx
k
À K
k
z
k
À h
k
x
nom
k
ÀH
1
k
b
dx
k
À
H
1
k
%
@h
k
@x
xx
nom
k
Computing the a priori covariance matrix:
P
k
À F
1
kÀ1
P
kÀ1
F
1T
kÀ1
Q
kÀ1
Computing the Kalman gain:
K
k
P
k
ÀH
1T
k
H
1
k
P
k
ÀH
1T
k
R
k
À1
Computing the a posteriori covariance matrix:
P
k
fI À K
k
H
1
k
gP
k
À
5.7 DISCRETE EXTENDED KALMAN FILTER 179
TABLE 5.4 Discrete Extended Kalman Filter Equations
Nonlinear dynamic model:
x
k
f
kÀ1
x
kÀ1
w
kÀ1
; w
k
$x0; Q
k
Nonlinear measurement model:
z
k
h
k
x
k
v
k
; v
k
$x0; R
k
Nonlinear implementation equations:
Computing the predicted state estimate:
^
x
k
À f
kÀ1
^
x
kÀ1
Computing the predicted measurement:
^
z
k
h
k
^
x
k
À
Linear approximation equations:
F
1
kÀ1
%
@f
k
@x
x
^
x
kÀ1
À
Conditioning the predicted estimate on the measurement:
^
x
k
^
x
k
À K
k
z
k
À
^
z
k
; H
1
k
%
@h
k
@x
x
^
x
k
À
Computing the a priori covariance matrix:
P
k
À F
1
kÀ1
P
kÀ1
F
1T
kÀ1
Q
kÀ1
Computing the Kalman gain:
K
k
P
k
ÀH
1T
k
H
1
k
P
k
ÀH
1T
k
R
k
À1
Computing the a posteriori covariance matrix
P
k
fI À K
k
H
1
k
gP
k
À
180 NONLINEAR APPLICATIONS
for which one can use the ``nominal'' solution equations from Table 5.3,
F
1
x
nom
k
@
@x
x
2
xx
nom
4;
H
1
x
nom
k
@
@x
x
3
xx
nom
12;
to obtain the discrete linearized ®lter equations
^
x
k
b
dx
k
2;
b
dx
k
4
b
dx
kÀ1
K
k
z
k
À 8 À 48
b
dx
kÀ1
;
P
k
À 16P
kÀ1
1;
P
k
1 À 12K
k
P
k
À;
K
k
12P
k
À
144P
k
À 2
:
Given the measurements z
k
; k 1; 2; 3, the values for P
k
À; K
k
; P
k
, and
^
x,
can be computed. If z
k
are not given, then P
k
À; K
k
; and P
k
can be computed
for covariance analysis results. For large k with very small Q and R, the difference
^
x
k
À x
nom
k
will not stay small, and the results become meaningless.
This situation can be improved by using the extended Kalman ®lter as discussed
in Section 5.7:
^
x
k
^
x
2
kÀ1
K
k
fz
k
À
^
x
k
À
3
g;
P
k
À 4
^
x
kÀ1
À
2
P
kÀ1
1;
K
k
3P
k
À
^
x
k
À
2
9
^
x
k
À
4
P
k
À 2
;
P
k
f1 À 3K
k
^
x
k
À
2
gP
k
À:
These equations are now more complex but should work, provided Q and R are
small.
5.8 CONTINUOUS LINEARIZED AND EXTENDED FILTERS
The essential equations de®ning the continuous form of the extended Kalman ®lter
are summarized in Table 5.5. The linearized Kalman ®lter equations will have x
nom
in place of
^
x as the argument in the evaluations of nonlinear functions and their
derivatives.
5.8 CONTINUOUS LINEARIZED AND EXTENDED FILTERS 181
5.8.1 Higher Order Estimators
The linearized and extended Kalman ®lter equations result from truncating a Taylor
series expansion of f x; t and hx; t after the linear terms. Improved model ®delity
may be achieved at the expense of an increased computational burden by keeping the
second-order terms as well [21, 31, 75].
5.9 BIASED ERRORS IN QUADRATIC MEASUREMENTS
Quadratic dependence of a measurement on the state variables introduces an
approximation error e when the expected value of the measurement is approximated
by the formula ^z h
^
xe % h
^
x. It will be shown that the approximation is biased
(i.e., EheiT0) and how the expected error Ehei can be calculated and compensated
in the Kalman ®lter implementation.
TABLE 5.5 Continuous Extended Kalman Filter Equations
Nonlinear dynamic model:
_
xtf xt; tw t wt$x0; Qt
Nonlinear measurement model:
zthxt; tvt vt$x0; Rt
Implementation equations:
Differential equation of the state estimate:
_
^
xtf
^
xt; t
K tztÀ
^
zt
Predicted measurement:
^
zth
^
xt; t
Linear approximation equations:
F
1
t%
@f x; t
@x
x
^
xt
H
1
t%
@hx; t
@x
x
^
xt
Kalman gain equations:
_
PtF
1
tPt PtF
1T
tGtQt G
T
tÀK tRt
K
T
t
K tPtH
1T
tR
À1
t
182 NONLINEAR APPLICATIONS
Không có nhận xét nào:
Đăng nhận xét