Tài liệu Tracking and Kalman filtering made easy P16 pptx

16
NONLINEAR OBSERVATION SCHEME
AND DYNAMIC MODEL (EXTENDED
KALMAN FILTER)
16.1 INTRODUCTION
In this section we extend the results for the linear time-invariant and time-
variant cases to where the observations are nonlinearly related to the state
vector and/or the target dynamics model is a nonlinear relationship [5, pp. 105–
111, 166–171, 298–300]. The approachs involve the use of linearization
procedures. This linearization allows us to apply the linear least-squares and
minimum-variance theory results obtained so far. When these linearization
procedures are used with the Kalman filter, we obtain what is called the
extended Kalman filter [7, 122].
16.2 NONLINEAR OBSERVATION SCHEME
When the observation variables are nonlinearly related to the state vector
coordinates, (15.1-1) becomes [5, pp. 166–171]
Y
n
¼ GðX
n
ÞþN
n
ð16:2-1Þ
where GðX
n
Þ is a vector of nonlinear functions of the state variables.
Specifically,
GðX
n
Þ¼
g

1
ðX
n
Þ
g
2
ðX
n
Þ
.
.
.
g
n
ðX
n
Þ
2
6
6
6
4
3
7
7
7
5
ð16:2-2Þ
357
Tracking and Kalman Filtering Made Easy. Eli Brookner

Copyright # 1998 John Wiley & Sons, Inc.
ISBNs: 0-471-18407-1 (Hardback); 0-471-22419-7 (Electronic)
A common nonlinear observation situation for the radar is where the
measurements are obtained in polar coordinates while the target is tracked in
cartesian coordinates. Hence the state vector is given by
XðtÞ¼X ¼
x
y
z
2
4
3
5
ð16:2-3Þ
While the observation vector is
YðtÞ¼Y ¼
R
s


2
4
3
5
ð16:2-4Þ
The nonlinear equation relating R
s
, , and  to x, y, and z are given by (1.5-3),
that is g
1

ðXÞ, g
2
ðXÞ, and g
3
ðXÞ are given by, respectively, (1.5-3a) to (1.5-3c).
The inverse equations are given by (1.5-2). The least-squares and minimum-
variance estimates developed in Chapters 4 and 9 require a linear observation
scheme. It is possible to linearize a nonlinear observation scheme. Such a
linearization can be achieved when an approximate estimate of the target
trajectory has already been obtained from previous measurements.
One important class of applications where the linearization can be applied is
when the target equations of motion are exactly known with only the specific
parameters of the equations of motion not being known. Such is the case for
unpowered targets whose equations of motion are controlled by gravity and
possibly atmospheric drag. This occurs for a ballistic projectile passing through
the atmosphere, an exoatmospheric ballistic missile, a satellite in orbit, and a
planetary object. For these cases the past measurements on the target would
provide an estimate of the target state vector
"
Xðt À Þ at some past time t À 
(typically the last time measurements were made on the target). This nominal
state vector estimate of
"
Xðt À Þ would be used to estimate the parameters in the
known equations of motion for the target. In turn the equations of motion with
these estimated parameters would be used to propagate the target ahead to the
time t at which the next measurement is being made. This provides us with an
estimate for the state vector
"
XðtÞ that shall be used for linearizing the nonlinear

observation measurements. Shortly we shall give the equations of motion for
a ballistic projectile passing through the atmosphere in order to illustrate this
method more concretely.
For those applications where the exact equations of motion are not known,
such as when tracking an aircraft, the polynomial approximation of Chapters 5
to 7 can be used to estimate
"
Xðt À Þ and in turn
"
XðtÞ with, the transition matrix
È for a polynomial trajectory being used to determine
"
XðtÞ from
"
Xðt À Þ. The
prediction from t À  to t cannot be made too far into the future because the
predicted state vector would then have too large an error. In passing let us point
out that the polynomial fit can be used also to obtain the initial state estimate
358 NONLINEAR OBSERVATION SCHEME AND DYNAMIC MODEL
"
Xðt À Þ. For a satellite the insertion parameters can be used to obtain the initial
trajectory parameters, and in turn the initial state vector
"
Xðt À Þ. In the
following paragraphs we will illustrate the linearization of the nonlinear
observation equation of (16.2-1) by an example.
Assume a ballistic projectile for which a nominal state vector estimate
"
XðtÞ
has been obtained. If

"
XðtÞ is reasonably accurate (as we shall assume it to be), it
will differ from the true state vector
"
XðtÞ by a small amount given by XðtÞ, that
is,
XðtÞ¼
"
XðtÞþXðtÞð16:2-5Þ
Using GðX
n
Þ of (16.2-1) we can calculate the observation vector Y
n
that one
expects to see at time n (which corresponds to the time t). It is given by
"
Y
n
¼ Gð
"
X
n
Þð16:2-6Þ
This nominally expected value for the measurement vector
"
Y
n
will differ from
the observed Y
n

by a small amount Y
n
given by
Y
n
¼ Y
n
À
"
Y
n
ð16:2-7Þ
Applying (16.2-1), (16.2-5), and (16.2-6) to the above equation yields
Y
n
¼ Gð
"
X
n
þ X
n
ÞÀGð
"
X
n
ÞþN
n
ð16:2-8Þ
Applying the Taylor series to the first term on the right-hand side of the above
equation yields [5, p. 169]

Y
n
¼ Mð
"
X
n
Þ X
n
þ N
n
ð16:2-9Þ
where [5, p. 169]
½Mð
"
X
n
Þ
ij
¼
dg
i
ðXÞ
dx
j




X¼
"

X
n
ð16:2-10Þ
The second-order Taylor series terms have been dropped in the above equation.
By way of example, for the rectangular-to-spherical coordinate case g
1
ðXÞ,
as indicated above, is given by the first equation of (1.5-3a) with x
1
¼ x, x
2
¼ y,
x
3
¼ z, and
½Mð
"
X
n
Þ
11
¼
"
X
"
R
s
ð16:2-11Þ
where
"

R
s
¼ð
"
x
2
þ
"
y
2
þ "z
2
Þ
1=2
ð16:2-11aÞ
NONLINEAR OBSERVATION SCHEME 359
Equation (16.2-9) is the sought after linearized observation equation, where
Y
n
replaces Y
n
and X
n
replaces X
n
. We shall shortly describe how to use the
linearized observation equation given by (16.2-9) to obtain an improved
estimate of the target trajectory. Briefly, what is done is the differential
measurement vector Y
n

is used to obtain an estimate X
Ã
ðt
n
Þ of the
differential state vector X
n
using the linear estimation theory developed up
until now. This estimate X
Ã
ðt
n
Þ is then added to the estimate
"
X
n
based on the
past data to in turn obtain the new state vector estimate X
Ã
ðt
n
Þ. This becomes
clearer if we use the notation of Section 1.2 and let X
Ã
k;k
be the estimate at some
past time k based on measurements made at time k and earlier. Using the target
dynamics model, X
Ã
k;k

is used to obtain the prediction estimate at time n
designated as X
Ã
n;k
. From Y
n
and X
Ã
n;k
an estimate for Xðt
n
Þ, designated as
X
Ã
ðt
n
Þ; is obtained. Adding the estimate X
Ã
ðt
n
Þ to X
Ã
n;k
yields the desired
updated estimate X
Ã
n;n
. To obtain the estimate X
Ã
ðt

n
Þ, which for simplicity we
write as X
Ã
, it is necessary to know the covariance matrices of X
Ã
n;k
and Y
n
.
The covariance of Y
n
is assumed known. For the linear case the covariance of
X
Ã
n;k
can be obtained from that of X
Ã
k;k
using target dynamics transition matrix È
and (4.5-10), (9.2-1c), or (17.1-1) to be given shortly. If the target dynamics are
nonlinear, then a linearization is needed. In Chapter 17 a detailed description is
given of this linearization. Discussed in Section 16.3 is how this linearization is
used to obtain the transition matrix for a target having a nonlinear dynamics
model so that an equation equivalent to (4.5-10) or (9.2-1c) can be used to
obtain the covariance of X
Ã
n;k
.
16.3 NONLINEAR DYNAMIC MODEL

The linear time-invariant and time-variant differential equations given by
(8.1-10) and (15.2-1), respectively, become, for a nonlinear target dynamics
model [5, pp. 105–111],
d
dt
XðtÞ¼F½XðtÞ; tð16:3-1Þ
where, as before, XðtÞ is the state vector while F is a vector of nonlinear
functions of the elements of X, and perhaps of time t if it is also time variant. To
be more specific and by way of example let
XðtÞ¼
x
0
ðtÞ
x
1
ðtÞ

ð16:3-2Þ
and
F½XðtÞ ¼
f
0
ðx
0
; x
1
Þ
f
1
ðx

0
; x
1
Þ

ð16:3-3Þ
360 NONLINEAR OBSERVATION SCHEME AND DYNAMIC MODEL
Then (16.3-1) becomes
d
dt
x
0
ðtÞ¼f
0
½x
0
ðtÞ; x
1
ðtÞ ð16:3-4aÞ
d
dt
x
1
ðtÞ¼f
1
½x
0
ðtÞ; x
1
ðtÞ ð16:3-4bÞ

As was done for the nonlinear observation equation given by (16.2-1), we
would like to linearize (16.3-1) so that the linear estimation theory developed
up until now can be applied. As discussed before, this is possible if we have an
estimate of the target trajectory based on previous measurements and have in
turn its state vector
"
XðtÞ at time t. Differentiating (16.2-5) yields
d
dt
XðtÞ¼
d
dt
"
XðtÞþ
d
dt
XðtÞð16:3-5Þ
Using (16.3-1) and (16.3-5) yields
d
dt
"
XðtÞþ
d
dt
XðtÞ¼F½
"
XðtÞþXðtÞ ð16:3-6Þ
For simplicity we have dropped the second variable t in F, the possible variation
with time of F being implicitly understood.
Applying (16.3-6) to (16.3-4a) yields

d
dt
"
x
0
ðtÞþ
d
dt
x
0
ðtÞ¼f
0
½
"
x
0
ðtÞþx
0
ðtÞ;
"
x
1
ðtÞþx
1
ðtÞ ð16:3-7Þ
Applying the Taylor expansion to the right-hand side of (16.3-7) yields [5, p.
109]
d
dt
"

x
0
ðtÞþ
d
dt
x
0
ðtÞ¼f
0
ð
"
x
0
;
"
x
1
Þþ
df
0
dx
0




"
x
0
"

x
1
Á x
0
þ
df
0
dx
1




"
x
0
"
x
1
Á x
1
ð16:3-8Þ
where all second-order terms of the Taylor expansion have been dropped. By
the same process we obtain for (16.3-4b) [5, p. 109]
d
dt
"
x
1
ðtÞþ

d
dt
x
1
ðtÞ¼f
1
ð
"
x
0
;
"
x
1
Þþ
df
1
dx
0




"
x
0
"
x
1
Á x

0
þ
df
1
dx
1




"
x
0
"
x
1
Á x
1
ð16:3-9Þ
But from (16.3-1) [see also (16.3-4a) and (16.3-4b)]
d
dt
"
XðtÞ¼F½
"
XðtÞ ð16:3-10Þ
NONLINEAR DYNAMIC MODEL 361
Hence (16.3-8) and (16.3-9) become [5, p. 109]
d
dt

x
0
ðtÞ
d
dt
x
1
ðtÞ
0
B
B
@
1
C
C
A
¼
df
0
dx
0
df
0
dx
1
df
1
dx
0
df

1
dx
1
0
B
B
@
1
C
C
A








"x
0
ðtÞ
"x
1
ðtÞ
x
0
ðtÞ
x
1

ðtÞ

ð16:3-11Þ
The above can be rewritten as [5, p. 109]
d
dt
XðtÞ¼A½
"
X ðtÞXðtÞð16:3-12Þ
where
½A½
"
X ðtÞ
i;j
¼
df
i
ðXÞ
dx
j




X¼
"
XðtÞ
ð16:3-12aÞ
Equation (16.3-12) is the desired linearized form of the nonlinear dynamics
model given by (16.3-1). It is of the same form as (15.2-1). To achieve the

linearization, the matrix AðtÞ is replaced by A½
"
XðtÞ while the state vector X is
replaced by the differential state vector X.
We are now in a position to apply the linear estimation theory developed up
until now to the differential state vector X to obtain its estimate X
Ã
. Having
this we can then form the new estimate X
Ã
by adding X
Ã
to
"
X. We shall give
the details of how this is done in the next section. Before doing this a few
additional points will be made and an example of the linearization of the
nonlinear dynamics model given.
Because (16.3-12) is linear and time variant, it follows from (15.2-2) that the
transition equation for X is [5, p. 111]
Xðt
n
þ Þ¼Èðt
n
þ ; t
n
;
"
XÞXðt
n

Þð16:3-13Þ
where È depends on
"
X as well as on time. This transition matrix and its inverse
satisfy the differential equations [5, p. 111]
d
d
Èðt
n
þ ; t
n
;
"
XÞ¼A½
"
X ðt
n
þ ÞÈðt
n
þ ; t
n
;
"
XÞð16:3-14Þ
d
d
ðt
n
þ ; t
n

;
"
XÞ¼À ðt
n
þ ; t
n
;
"
XÞA½
"
X ðt
n
þ Þ ð16:3-15Þ
corresponding to the respective linear time-variant forms given by (15.3-1) and
(15.3-9). The desired transition matrix Èðt
n
þ ; t
n
Þ or actually Èðt
n
; t
k
Þ can be
obtained by numerical integration of (16.3-14) using the dynamics model
matrix A½
"
X ðtÞ and the initial condition Èðt
n
; t
n

;
"
XÞ¼I. This in turn lets us
362 NONLINEAR OBSERVATION SCHEME AND DYNAMIC MODEL
determine the covariance matrix of X
Ã
n;k
from that of X
Ã
k;k
as mentioned at the
end of Section 16.2. The predicted estimate X
Ã
n;k
is determined from X
Ã
k;k
itself
by numerically integrating the original nonlinear target dynamics equations
given by (16.3-1); see also (16.3-4). Having X
Ã
n;k
and Y
n
and their covariances,
the minimum variance theory can be applied to obtain the combined estimate
for X
Ã
n;n
as done in Section 4.5 and Chapter 9; see, for example (9.4-1). This is

detailed in the next chapter. Before proceeding to that chapter an example
linearization of the nonlinear differential dynamic equation shall be given. This
example shall be used in the next chapter.
Assume we wish to track a ballistic projectile through the atmosphere. We
will now develop its nonlinear differential equations of motion corresponding to
(16.3-1). For simplicity, the assumption is made that the radar is located in the
plane of the projectiles trajectory. As a result, it is necessary to consider only
two coordinates, the horizontal coordinate x
1
and the vertical coordinate x
2
.For
further simplicity a flat earth is assumed. We define the target state vector as
X ¼
x
1
x
2
_
x
1
_
x
2
2
6
6
4
3
7

7
5
ð16:3-16Þ
The derivative of the state vector becomes
dx
dt
¼
_
x
1
_
x
2

x
1

x
2
2
6
6
4
3
7
7
5
ð16:3-17Þ
The acceleration components in the vector on the right-hand side of the
above equation depend on the atmospheric drag force and the pull of gravity.

Once we have replaced these acceleration components in (16.3-17) by their
relationship in terms of the atmospheric drag and gravity, we have obtained the
sought after form of the nonlinear dynamics model cooresponding to (16.3-1).
The atmospheric drag equation for the projectile is approximated by [5, p. 105]
f
d
¼
1
2
v
2
 ð16:3-18Þ
where  is the atmospheric density, v is the projectile speed, and  is an
atmospheric drag constant. Specifically,
 ¼ C
D
A ð16:3-19Þ
where C
D
is an atmospheric drag coefficient dependent on the body shape and A
is the projection of the cross-sectional area of the target on a plane
perpendicular to the direction of motion. The parameter  is related to the
NONLINEAR DYNAMIC MODEL 363
ballistic coefficient  of (2.4-6) in Section 2.4, by the relationship
 ¼
m

ð16:3-20Þ
since  is given by (2.4-9). Physically,  represents the effective target drag
area. The atmospheric density as a function of altitude is fairly well

approximated by the exponential law given by [5, p. 105]
 ¼ 
0
e
Àkx
2
ð16:3-21Þ
where 
0
and k are known constants.
To replace the acceleration components in (16.3-17) by their atmospheric
drag and gravity relationships, we proceed as follows. First, the drag force is
resolved into its x
1
and x
2
components by writing the velocity as a velocity
vector given by
V ¼ v
^
V ð16:3-22Þ
where
^
V is the unit velocity vector along the ballistic target velocity
direction. The atmospheric drag force can then be written as a vector F
d
given
by
F
d

¼À
1
2
v
2
^
V ð16:3-23Þ
Let
^
i and
^
k be the unit vectors along the x
1
and x
2
coordinates. Then
V ¼
_
x
1
^
i þ
_
x
2
^
k ð16:3-24Þ
and
^
V ¼

_
x
1
^
i þ
_
x
2
^
k
v
ð16:3-25Þ
Thus
F
d
¼À
1
2
vð
_
x
1
^
i þ
_
x
2
^
kÞð16:3-26Þ
and hence

m

x
1
¼À
1
2
v
_
x
1
ð16:3-27Þ
and
m

x
2
¼À
1
2
v
_
x
2
À mg ð16:3-28Þ
364 NONLINEAR OBSERVATION SCHEME AND DYNAMIC MODEL
Substituting the above two equations in (16.3-1) and using (16.3-16) and
(16.3-21) yield [5, p. 107]
_
x

1
_
x
2

x
1

x
2
2
6
6
6
6
6
4
3
7
7
7
7
7
5
¼
_
x
1
_
x

2
À

0

2m
e
Àkx
2
ð
_
x
2
1
þ
_
x
2
2
Þ
1=2
_
x
1
À
0

2m
e
Àkx

2
ð
_
x
2
1
þ
_
x
2
2
Þ
1=2
_
x
2
À g
2
6
6
6
6
6
4
3
7
7
7
7
7

5
ð16:3-29Þ
Applying (16.3-12a) yields [5, p. 110]
A ½XðtÞ ¼
0 j 0 j 1 j 0

0 j 0 j 0 j 1

0 j ck
"
v
"
_
x
1
exp Àk
"
x
2
ðÞjÀc
"v
2
þ
"
_x
2
1
"
v


exp Àk
"
x
2
ðÞjÀ
c
"
_x
1
"
_x
2
v

exp Àk
"
x
2
ðÞ

0 j ck
"
v
"
_
x
2
exp Àk
"
x

2
ðÞjÀ
c
"
_
x
1
"
_
x
2
"
v

exp Àk
"
x
2
ðÞjÀc
"
v
2
þ
"
_
x
2
2
"
v


exp Àk
"
x
2
ðÞ
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
ð16:3-30Þ
where
c ¼

0

2m
ð16:3-30aÞ
"
v ¼ð
"
_
x
2
1
þ

"
_
x
2
2
Þ
1=2
ð16:3-30bÞ
"
x
1
¼
"
x
1
ðtÞ etc: ð16:3-30cÞ
In the above , the target effective drag area, was assumed to be known.
More generally, it is unknown. In this case it must also be estimated based on
the projectile trajectory measurements. The dependence of  and
_
 on the
trajectory velocity and other state vector parameters provides a nonlinear
differential equation of the form given by [5, p. 299]
d
dt
ðtÞ
_
ðtÞ

¼ F½x

1
; x
2
;

x
1
;

x
2
;ðtÞ;

ðtÞ ð16:3-31Þ
The above equation is of the same form as the nonlinear differential target
dynamics equation given by (16.3-1). The two-element state vector given on the
NONLINEAR DYNAMIC MODEL 365
left side of the above equation must be now estimated. This is done by
adding this two-element state vector to the four-estimate state vector given
by (16.3-16) to give a six-state vector instead of a four-state vector. [In
Section 2.4 we gave an example where the target drag area  had to be
estimated; see (2.4-6).]
366 NONLINEAR OBSERVATION SCHEME AND DYNAMIC MODEL