Tài liệu Lọc Kalman - lý thuyết và thực hành bằng cách sử dụng MATLAB (P5) pdf
Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (393.88 KB, 33 trang )
5
Nonlinear Applications
The principal uses of linear ®ltering theory are for solving nonlinear problems.
Harold W. Sorenson, in a private conversation
5.1 CHAPTER FOCUS
5.1.1 Nonlinear Estimation Problems
Linear estimators for discrete and continuous systems were derived in Chapter 4.
The combination of functional linearity, quadratic performance criteria, and Gaus-
sian statistics is essential to this development. The resulting optimal estimators are
simple in form and powerful in effect.
Many dynamic systems and sensors are not absolutely linear, but they are not far
from it. Following the considerable success enjoyed by linear estimation methods on
linear problems, extensions of these methods were applied to such nonlinear
problems. In this chapter, we investigate the model extensions and approximation
methods used for applying the methodology of Kalman ®ltering to these ``slightly
nonlinear'' problems. More formal derivations of these nonlinear ®lters and
predictors can be found in references [1, 21, 23, 30, 36, 75, 112].
5.1.2 Main Points to Be Covered
Many estimation problems that are of practical interest are nonlinear but
``smooth.'' That is, the functional dependences of the measurement or state
dynamics on the system state are nonlinear, but approximately linear for small
perturbations in the values of the state variables.
Methods of linear estimation theory can be applied to such nonlinear
problems by linear approximation of the effects of small perturbations in
the state of the nonlinear system from a ``nominal'' value.
169
Kalman Filtering: Theory and Practice Using MATLAB, Second Edition,
Mohinder S. Grewal, Angus P. Andrews
Copyright # 2001 John Wiley & Sons, Inc.
ISBNs: 0-471-39254-5 (Hardback); 0-471-26638-8 (Electronic)
For some problems, the nominal values of the state variables are fairly well
known beforehand. These include guidance and control applications for which
operational performance depends on staying close to an optimal trajectory. For
these applications, the estimation problem can often be effectively linearized
about the nominal trajectory and the Kalman gains can be precomputed to
relieve the real-time computational burden.
The nominal trajectory can also be de®ned ``on the ¯y'' as the current best
estimate of the actual trajectory. This approach is called extended Kalman
®ltering. It has the advantage that the perturbations include only the state
estimation errors, which are generally smaller than the perturbations from
any prede®ned nominal trajectory and therefore better conditioned for
linear approximation. The major disadvantage of extended Kalman ®ltering
is the added real-time computational cost of linearization about an
unpredictable trajectory, for which the Kalman gains cannot be computed
beforehand.
Extensions of the linear model to include quadratic terms yield optimal ®lters
of greater applicability but increased computational complexity.
5.2 PROBLEM STATEMENT
Suppose that a continuous or discrete stochastic system can be represented by
nonlinear plant and measurement models as shown in Table 5.1, with dimensions of
the vector and matrix quantities as shown in Table 5.2 and where the symbols
Dk À ` stand for the Kronecker delta function and the symbols dt À s stand for
the Dirac delta function (actually, a generalized function).
The function f is a continuously differentiable function of the state vector x, and
the function h is a continuously differentiable function of the state vector.
Whereas af®ne (i.e., linear and additive) transformations of Gaussian RVs have
Gaussian distributions, the same is not always true in the nonlinear case. Conse-
quently, it is not necessary that w and v be Gaussian. They may be included as
arguments of the nonlinear functions f and h, respectively. However, the initial value
TABLE 5.1 Nonlinear Plant and Measurement Models
Model Continuous Time Discrete Time
Plant
_
x fx; twt x
k
fx
kÀ1
; k À 1w
kÀ1
Measurement zthxt; tvt z
k
hx
k
; kv
k
Plant noise Ehwti 0 Ehw
k
i0
Ehwtw
T
si dt À sQt Ehw
k
w
T
i
iDk À iQ
k
Measurement noise Ehvti 0 Ehv
k
i0
Ehvtv
T
si dt À sRt Ehv
k
v
T
i
iDk À iR
k
170 NONLINEAR APPLICATIONS
x
0
may be assumed to be a Gaussian random variate with known mean and known
n  n covariance matrix P
0
.
The objective is to estimate x
k
or xt to satisfy a speci®ed performance criterion
as given in Chapter 4.
5.3 LINEARIZATION METHODS
Applying linearization techniques to get simple approximate solutions to nonlinear
estimation problems requires that f and h be twice-continuously differentiable [112,
133].
5.4 LINEARIZATION ABOUT A NOMINAL TRAJECTORY
5.4.1 Nominal Trajectory
A trajectory is a particular solution of a stochastic system, that is, with a particular
instantiation of the random variates involved. The trajectory is a vector-valued
sequence fx
k
jk 0; 1; 2; 3; g for discrete-time systems and a vector-valued
function xt; 0 t, for continuous-time systems.
The term ``nominal'' in this case refers to that trajectory obtained when the
random variates assume their expected values. For example, the sequence fx
nom
k
g
obtained as a solution of the equation
x
nom
k
f x
nom
kÀ1
; k À 15:1
with zero process noise and with the mean x
nom
0
as the initial condition would be a
nominal trajectory for a discrete-time system.
5.4.2 Perturbations about a Nominal Trajectory
The word ``perturbation'' has been used by astronomers to describe a minor change
in the trajectory of a planet (or any free-falling body) due to secondary forces, such
as those produced by other gravitational bodies. Astronomers learned long ago that
the actual trajectory can be accurately modeled as the sum of the solution of the two-
body problem (which is available in closed form) and a linear dynamic model for the
TABLE 5.2 Dimensions of Vectors and Matrices in Nonlinear Model
Symbol Dimensions Symbol Dimensions
x; f ; wn 1 z; h; v `  1
Qn nR`  `
D; d Scalars
5.4 LINEARIZATION ABOUT A NOMINAL TRAJECTORY 171
perturbations due to the secondary forces. This technique also works well for many
other nonlinear problems, including the problem at hand. In this case, the perturba-
tions are due to the presence of random process noise and errors in the assumed
initial conditions.
If the function f in the previous example is continuous, then the state vector
x
k
at any instant on the trajectory will vary smoothly with small perturbations of
the state vector x
kÀ1
at the previous instant. These perturbations are the result of
``off-nominal'' (i.e., off-mean) values of the random variates involved. These
random variates include the initial value of the state vector (x
0
), the process
noise (w
k
), and (in the case of the estimated trajectory) the measurement noise
(v
k
).
If f is continuously differentiable in®nitely often, then the in¯uence of the
perturbations on the trajectory can be represented by a Taylor series expansion about
the nominal trajectory. The likely magnitudes of the perturbations are determined by
the variances of the variates involved. If these perturbations are suf®ciently small
relative to the higher order coef®cients of the expansion, then one can obtain a good
approximation by ignoring terms beyond some order. (However, one must usually
evaluate the magnitudes of the higher order coef®cients before making such an
assumption.)
Let the symbol d denote perturbations from the nominal,
dx
k
x
k
À x
nom
k
;
dz
k
z
k
À hx
nom
k
; k;
so that the Taylor series expansion of f x; k À 1 with respect to x at x x
nom
kÀ1
is
x
k
f x
kÀ1
; k À 15:2
f x
nom
kÀ1
; k À 1
@f x; k À 1
@x
xx
nom
kÀ1
dx
kÀ1
higher order terms 5:3
x
nom
k
@f x; k À 1
@x
xx
nom
kÀ1
dx
kÀ1
higher order terms; 5:4
or
dx
k
x
k
À x
nom
k
5:5
@f x; k À 1
@x
xx
nom
kÀ1
dx
kÀ1
higher order terms. 5:6
172 NONLINEAR APPLICATIONS
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
Quadratic Measurement Model. For the sake of simplicity, we consider the
case of a scalar measurement. (The resulting correction can be applied to each
component of a measurement vector, however.) Suppose that its dependence on the
state vector can be represented in the form
z hx5:31
H
1
x x
T
H
2
x v; 5:32
where H
1
represents the linear (®rst-order) dependence of the measurement
component on the state vector and H
2
represents the quadratic (second-order)
dependence. The matrix H
1
will then be a 1 Â n-dimensioned array and H
2
will
be an n  n-dimensioned array, where n is the dimension of the state vector.
Quadratic Error Model. If one de®nes the estimation error as
~
x
^
x À x, then the
expected measurement
^z Ehhxi 5:33
EhH
1
x x
T
H
2
xi5:34
EhH
1
^
x À
~
x
^
x À
~
x
T
H
2
^
x À
~
xi 5:35
H
1
^
x
^
x
T
H
2
^
x Eh
~
x
T
H
2
~
xi5:36
h
^
xEhtrace
~
x
T
H
2
~
xi 5:37
h
^
xEhtrace H
2
~
x
~
x
T
i 5:38
h
^
xtrace H
2
PÀ 5:39
h
^
xe; 5:40
e trace H
2
PÀ; 5:41
PÀ Eh
~
x
~
x
T
i; 5:42
where PÀ is the covariance matrix of a priori estimation uncertainty. The quadratic
error correction should be added in the extended Kalman ®lter implementation.
EXAMPLE 5.2 Quadratic measurement functions commonly occur in the cali-
bration of linear sensors for which the scale factor s (the ratio between variations of
its output z and variations of its input y) and bias b (the value of the output when the
input is zero) are also part of the system state vector, along with the input y itself:
x sby; 5:43
z hxv 5:44
sy b v: 5:45
The measurement is proportional to the product of the two states x
1
s and x
3
y.
The quadratic form of the second-order measurement model in this example is:
5.9 BIASED ERRORS IN QUADRATIC MEASUREMENTS 183
H
2
1
2
@
2
@x
2
hx
x0
5:46
1
2
@
2
h
@s
2
@
2
h
@s@b
@
2
h
@s@y
@
2
h
@s@b
@
2
h
@b
2
@
2
h
@b@y
@
2
h
@s@y
@
2
h
@b@y
@
2
h
@y
2
2
6
6
6
6
6
6
6
6
4
3
7
7
7
7
7
7
7
7
5
sby0
5:47
00
1
2
000
1
2
00
2
6
6
4
3
7
7
5
; 5:48
and the correct form for the expected measurement is
^z h
^
xtrace
00
1
2
000
1
2
00
2
6
6
4
3
7
7
5
PÀ
8
>
>
<
>
>
:
9
>
>
=
>
>
;
5:49
^
s
^
y
^
b p
13
À; 5:50
where p
13
À is the a priori covariance between the scale factor uncertainty and the
input uncertainty.
5.10 APPLICATION OF NONLINEAR FILTERS
EXAMPLE 5.3: Damping Parameter Estimation This example uses Example
4.3 from Chapter 4. Assume that z (damping coef®cient) is unknown and is a
constant. Therefore, the damping coef®cient can be modeled as a state vector and its
value is estimated via linearized and extended Kalman estimators.
The conversion from a parameter estimation problem to a state estimation
problem shown in Example 4.3 results in a nonlinear problem.
Let
x
3
tz 5:51
and
_
x
3
t0 5:52
184 NONLINEAR APPLICATIONS
Then the plant equation of Example 4.3 becomes
_
x
1
t
_
x
2
t
_
x
3
t
2
6
4
3
7
5
x
2
Ào
2
x
1
À 2x
2
x
3
o
0
2
6
4
3
7
5
0
1
0
2
6
4
3
7
5
wt
0
12
0
2
6
4
3
7
5
: 5:53
The observation equation is
ztx
1
tvt5:54
One hundred data points were generated with plant noise and measurement noise set
equal to zero, z 0:1, w 5 rad=s, and initial conditions
x
1
0
x
2
0
x
3
0
2
6
6
4
3
7
7
5
0ft
0ft=s
0
2
6
6
4
3
7
7
5
;
P0
200
020
002
2
6
6
4
3
7
7
5
;
Q 4:47 ft/s
2
;
R 0:01ft
2
:
The discrete nonlinear plant and linear observation equations for this model are
x
k
1
x
kÀ1
1
Tx
kÀ1
2
; 5:55
x
k
2
À25Tx
kÀ1
1
1 À 10Tx
kÀ1
3
x
kÀ1
2
12T Tw
kÀ1
; 5:56
x
k
3
x
kÀ1
3
; 5:57
z
k
x
k
1
v
k
: 5:58
The relevant equations from Table 5.3 (discrete linearized Kalman ®lter equations)
and Table 5.4 (discrete extended Kalman ®lter equations) have been programmed in
MATLAB as exam53.m on the accompanying diskette. T is sampling interval.
Figure 5.2 shows the estimated position, velocity, and damping factor states (note
non-convergence due to vanishing gain) using the noise-free data generated from
simulating the second-order equation (the same data as in Example 4.1). Figure 5.3
shows the corresponding RMS uncertainties from the extended Kalman ®lter. (See
Appendix A for descriptions of exam53.m and modi®ed versions.)
For the noise-free data, the linearized and extended Kalman ®lter (EKF) results
are very close. But for noisy data, convergence for the discrete linearized results is
5.10 APPLICATION OF NONLINEAR FILTERS 185
not as fast, compared to convergence for the extended ®lter. Results are a little better
with the EKF [122, 196, 200, 202, 211].
EXAMPLE 5.4 Inertial reference systems maintain a computational reference
frame, which is a set of orthogonal reference axes de®ned with respect to the inertial
sensors (gyroscopes and accelerometers). The attitude error of an inertial reference
system is a set of rotations about these axes, representing the rotations between
where the system thinks these axes are and where they really are.
Fig. 5.2 State variables estimated by extended Kalman ®lter.
Fig. 5.3 RMS uncertainties in estimates.
186 NONLINEAR APPLICATIONS
Gyroscope Filter. This error can be represented by a model of the form
_
C C o e; 5:59
where
C 3 Â 1 vector containing the attitude alignment errors between the sensor
axes frame and the computational reference frame
vector cross-product operator
o 3 Â 1 vector of platform inertial angular rate from the trajectory generator
e 3 Â 1 vector of composite gyroscope drift rates (algebraic sum of all error
sources)
This attitude error model can be augmented by a 48-state model of the gyroscope
parameters (see the beginning paragraphs of Chapter 3) as random walks and
random ramps. The ®rst-order vector differential equation in a state-space form for
the augmented 51 Â 1 state vector is
_
x
g
tFtx
g
tW
1
t; 5:60
where the 51-component state vector x
g
t is composed of the nonredundant
components of the following arrays:
Symbol C b
g
h
1
g
U
1
g
K
1
g
T
g
b
gt
U
1
gt
Dimension 3 Â 13Â 13Â 33Â 33Â 63Â 13Â 13Â 6
Subvector 3 Â 13Â 19Â 19Â 115Â 13Â 13Â 16Â 1
x À Indices 1 À 34À 67À 15 16 À 24 25 À 39 40 À 42 43 À 45 46 À 51
5:61
The symbol at the top is the array name, with its dimensions shown below it, and the
bottom dimension refers to the dimension of the corresponding subvector of its non-
redundant terms in the system state vector, shown at the bottom. The matrices
h
g
; U
g
; K
g
; and U
gt
are de®ned as follows:
h
g
is a 3 Â 3 matrix containing unknown scale factor S
gi
and linear axes
alignment errors D
ij
as components i; j 1; 2; 3:
S
g1
D
12
D
13
D
21
S
g2
D
23
D
31
D
32
S
g3
2
6
4
3
7
5
5:62
5.10 APPLICATION OF NONLINEAR FILTERS 187
U
g
is a 3 Â 3 matrix of unknown gyroscope mass unbalance parameters d
k; j
:
d
I1
d
01
d
S1
d
S2
d
I2
d
02
d
03
d
S3
d
I3
2
6
4
3
7
5
5:63
K
g
is a 3 Â 6 matrix of unknown gyroscope compliance (g-squared) errors k
kji
:
k
II1
k
001
k
SS1
k
I01
k
IS1
k
S01
k
SS2
k
II2
k
002
k
IS2
k
S02
k
I02
k
003
k
SS3
k
II3
k
S03
k
I03
k
IS3
2
6
4
3
7
5
5:64
U
gt
is a 3 Â 6 matrix of unknown gyroscope mass unbalance trend parameters.
h
g
; U
g
; k
g
; U
gt
have been redimensioned rowwise to form column vectors
h
1
g
; U
1
g
; K
1
g
; and U
1
gt
:
b
g
is a 3 Â 1 vector of unknown gyroscope ®xed-drift rate parameters.
T
g
is a 3 Â 1 vector of unknown nonlinear gyroscope torquer scale factor errors,
with elements dS
gi
.
b
gt
is a 3 Â 1 vector of unknown gyroscope ®xed-drift trend parameters.
In expanded form,
_
C
_
b
g
_
h
1
g
_
U
1
g
_
K
1
g
_
T
g
_
b
gt
_
U
1
gt
2
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
5
F
11
F
12
F
13
F
14
F
15
F
16
F
17
F
18
000000F
27
0
00000000
0000000F
48
00000000
00000000
00000000
00000000
2
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
5
C
b
g
h
1
g
U
1
g
K
1
g
T
g
b
gt
U
1
gt
2
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
5
w
C
t
w
bg
t
w
hg
t
w
ug
t
w
kg
t
w
tg
t
w
bgt
t
w
ugt
t
2
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
5
;
5:65
where
w
T
1
tw
T
C
t w
T
bg
t w
T
hg
t w
T
ug
t w
T
kg
t w
T
tg
t w
T
bgt
t w
T
ugt
t
5:66
is a noise vector of unmodeled effects and
F
11
0 o
3
Ào
2
Ào
3
0 o
1
o
2
Ào
1
0
2
4
3
5
; F
12
100
010
001
2
4
3
5
; 5:67
188 NONLINEAR APPLICATIONS
F
13
o
1
o
2
o
3
000
000
000
o
1
o
2
o
3
000
000
000
o
1
o
2
o
3
2
4
3
5
; 5:68
F
14
b
1
b
2
b
3
000
000
000
b
1
b
2
b
3
000
000
000
b
1
b
2
b
3
2
4
3
5
; 5:69
F
15
b
11
b
33
b
12
b
13
b
23
00000
00000
00000
b
11
b
22
b
12
b
13
b
23
00000
00000
00000
b
22
b
33
b
12
b
13
b
23
2
4
3
5
;
5:70
b
ij
def
b
i
b
j
; 5:71
F
16
jo
1
j 00
0 jo
2
j 0
00jo
3
j
2
4
3
5
; 5:72
F
17
100
010
001
2
4
3
5
t; 5:73
F
18
b
1
b
3
0000
00b
1
b
2
00
0000b
2
b
3
2
4
3
5
t; 5:74
F
27
100
010
001
2
4
3
5
; 5:75
5.10 APPLICATION OF NONLINEAR FILTERS 189
F
48
100000
000000
010000
001000
000100
000000
000000
000010
000001
2
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
5
; 5:76
b is a 3 Â 1 vector of vertical direction cosines (normalized gravity)
b b
1
; b
2
; b
3
T
; 5:77
b
1
is a 6 Â 1 with products of b
i
as components
b
2
1
; b
2
2
; b
2
3
; b
1
b
2
; b
1
b
3
; b
2
b
3
; 5:78
and o
i
and b
i
are time dependent. Thus a different system description matrix is
computed in each ®lter cycle.
The corresponding difference equation of continuous Equation 5.60,
_
x
g
tFtx
g
tw
1
t; 5:79
is
x
g
j
F
g
j;jÀ1
x
g
jÀ1
w
g
jÀ1
; 5:80
where the gyroscope state transition matrix F
g
j;jÀ1
is approximated by the ®rst two
terms of the power series expansion of the exponential function,
F
g
j;jÀ1
I Ft F
2
t
2
2
higher order terms 5:81
and w
g
j
is normally distributed white noise with zero mean and covariance Q and
accounts for gyroscope modeling and truncation errors,
w
g
j
$x0; Q: 5:82
The scalar t is the ®lter cycle time.
The gyro observation equation is
2Â151Â1
z
g
j
H
g
x
g
j
v
g
j
;
5:83
190 NONLINEAR APPLICATIONS
where
H
g
a
T
g
T
F
11
F
12
F
13
F
14
F
15
F
16
F
17
F
18
2Â51 2Â33Â51
a is north direction cosine vector 3 Â 1, g is west direction cosine vector 3 Â 1,
and F
1m
m 1; 2; 3; 4; 5; 6; 7; 8 are the appropriate submatrices of the gyroscope
state transition matrix F
g
j;jÀ1
; and v
g
j
$x0; R, which includes noise and errors
from sensors.
Accelerometer ®lter. The difference equation for the accelerometers is
X
A
j
F
A
j;jÀ1
X
A
jÀ1
w
A
jÀ1
; 5:84
where
F
A
j;jÀ1
I: 5:85
The 12 Â 1 accelerometer state vector S
A
is composed of
X
A
T
b
A
S
1
d
12
S
2
d
13
d
23
S
3
FX 1
T
À FI1
T
5:86
where b
A
; FX 1, and FI 1 are 3 Â 1 vectors as de®ned:
b
A
is a 3 Â 1 vector of unknown accelerometer biases, normalized to the
magnitude of gravity;
FI1isa3Â 1 unknown acceleration-squared nonlinearity for acceleration along
the accelerometer input axis; and
FX 1isa3Â 1 unknown acceleration-squared nonlinearity for acceleration
normal to the accelerometer input axis.
Here, S
1
; d
12
; S
2
; d
13
; d
23
; S
3
are elements of matrix h
A
.
Twelve unknown parameters are modeled as random walk and w
A
j
$x0; Q and
white noise includes accelerometer modeling and truncation errors:
h
A
S
1
d
12
d
13
0 S
2
d
23
00S
3
2
4
3
5
; 5:87
where
S
i
unknown accelerometer scale factor errors i 1; 2; 3
d
ij
unknown accelerometer axes nonorthogonalities (misalignments) 1
i
Á 1
j
5.10 APPLICATION OF NONLINEAR FILTERS 191
Here, b
m
is a 3-vector b
1
; b
2
; b
3
T
of midpoint components of acceleration in
platform coordinates,
b
2
m
b
2
1
00
0 b
2
2
0
00b
2
3
2
4
3
5
: 5:88
The accelerometer observation equation is
Z
A
j
H
A
X
A
j
V
A
j
; 5:89
where
H
A
b
1
; b
3
; b
2
1
; b
1
; b
2
; b
1
; b
3
; b
2
2
; b
2
; b
3
; b
2
3
; 1 À b
2
1
b
1
; 1 À b
2
2
b
2
; 1 À b
2
3
b
3
5:90
and V
A
j
is $x0; R and white, including sensor noise.
The dimension of the observation vector
Z Z
g
; Z
A
T
5:91
is 3 Â 1: The EKF equations of Section 5.4.2 are applied to Equations 5.80 and 5.83
to obtain gyroscope estimates. The EKF equations are applied to Equations 5.84 and
5.89 to obtain accelerometer estimates.
Typical plots of gyroscope ®xed-drift, accelerometer ®xed-drift, and scale factor
estimates are shown in Figures 5.4, 5.5, and 5.6, respectively. Innovation sequence
plots for accelerometer and gyroscope are shown in Figures 5.7 and 5.8, respectively.
The results are completely described in reference [170].
Fig. 5.4 Gyro ®xed-drift rate estimates.
192 NONLINEAR APPLICATIONS
EXAMPLE 5.5 This is an application of a discrete-time, extended Kalman ®lter
to estimate the parameters in a macroscopic freeway traf®c model [166].
A dynamic equation is given by
u
n1
j
u
n
j
Dt Àu
n
j
u
n
j
À u
n
jÀ1
Dx
À
1
T
u
n
j
À a À br
n
j
vr
n
j1
À r
n
j
r
n
j
Dx
#")
w
n
j
Fig. 5.5 Accelerometer ®xed drift estimates.
Fig. 5.6 Accelerometer scale factor estimates.
5.10 APPLICATION OF NONLINEAR FILTERS 193
