17
BAYES ALGORITHM WITH ITERATIVE
DIFFERENTIAL CORRECTION
FOR NONLINEAR SYSTEMS
17.1 DETERMINATION OF UPDATED ESTIMATES
We are now in a position to obtain the updated estimate for the nonlinear
observation and target dynamic model cases [5, pp. 424–443]. We shall use the
example of the ballistic projectile traveling through the atmosphere for
definiteness in our discussion. Assume that the past measurements have
permitted us to obtain the state vector estimate
"
Xðt À Þ at the time t À , the
last time observations were made on the target. As done at the end of Section
16.2, it is convenient to designate this past time t À with the index k and write
"
Xðt À Þ as X
Ã
k;k
. Assume that the measurements are being made in polar
coordinates while the projectile is being tracked in rectangular coordinates, that
is, the state-vector is given in rectangular coordinates, as done in Section 16.2.
[Although previously the projectile trajectory plane was assumed to contain the
radar, we will no longer make this assumption. We will implicitly assume that
the radar is located outside the plane of the trajectory. It is left to the reader to
extend (16.3-29) and (16.3-30) to this case. This is rather straightforward, and
we shall refer to these equations in the following discussions as if this
generalization has been made. This extension is given elsewhere [5, pp. 106–
110].]
By numerically integrating the differential equation given by (16.3-29)
starting with
"
Xðt À Þ at time t À we can determine
"
XðtÞ. As before we
now find it convenient to also refer to
"
XðtÞ¼
"
X
n
as X
Ã
n;k
, it being the estimate
of the predicted state vector at time t (or n) based on the measurement at
time <t (or k < n). We can compute the transition matrix È
n;k
by numerical
integration of the differential equation given by (16.3-14) with A given by
(16.3-30). In turn È
n;k
can be used to determine the covariance matrix of
"
XðtÞ
367
Tracking andKalmanFilteringMade Easy. Eli Brookner
Copyright # 1998 John Wiley & Sons, Inc.
ISBNs: 0-471-18407-1 (Hardback); 0-471-22419-7 (Electronic)
using [5, p. 431]
S
Ã
n;k
¼ Èðt
n
; t
k
;
"
XÞS
Ã
k;k
Èðt
n
; t
k
;
"
XÞ
T
ð17:1-1Þ
assuming we know S
k;k
. Assume that at time t (or n) we get the measurement
Y
n
. Thus at time n we have Y
n
and the estimate X
Ã
n;k
, based on the past data,
with their corresponding covariance matrices R
1
and S
Ã
n;k
, respectively. What is
desired is to combine these to obtain the updated estimate X
Ã
n;n
. We would like
to use the Bayes filter to do this. We can do this by using the linearized
nonlinear observation equations. This is done by replacing the nonlinear
observation equation (16.2-1) by its linearized version of (16.2-9). The linear-
ized observation matrix MðX
Ã
n;n
Þ [which replaces the nonlinear G of (16.2-1)] is
then used for M in (9.2-1) of the Bayes filter. Also Y
n
and X
Ã
n;k
are replaced by
their differentials in (9.2-1). They are designated as, respectively, Y
n
and X
Ã
n;k
.
These differentials are determined shortly.
One might think at first that the Bayes filter can be applied without the use
of the linearization of the nonlinear observation equations. This is because in
(9.2-1) for X
Ã
n;n
, we can replace ðY
n
À MX
Ã
n;k
Þ by Y
n
À Gð
"
X
n
Þ without using the
linearization of the observations. However, as we see shortly, the calculation of
H
n
requires the use of Mð
"
X
n
Þ; see (17.1-5a) and (17.1-5b). Also Mð
"
XÞ is needed
to calculate S
Ã
n1;n
for use in the next update calculation, as done using (17.1-5b).
Physically we need to calculate Mð
"
X
n
Þ to find the minimum variance estimate
X
Ã
n;n
as the weighted sum of Y
n
and X
Ã
n;k
because to do this we need both of the
variates to be combined in the same coordinate system and we need their
variances in this same coordinate system. Calculating Mð
"
X
n
Þ allows us to do the
latter by using (16.2-9). The Bayes filter of (17.1-5) implicitly chooses the
coordinates of X
Ã
n;s
for the common coordinate system.
The differentials are now determined. Using (16.2-6) with G given by
(1.5-3), we can calculate
"
Y
n
in terms of
"
X obtained as described above. Using in
turn (16.2-7), we calculate Y
n
to be given by
Y
n
¼ Y
n
À
"
Y
n
ð17:1-2Þ
where
"
Y
n
¼ GðX
Ã
n;k
Þð17:1-2aÞ
from (16.2-6).
For convenience the differential X
Ã
n;k
is referenced relative to X
Ã
n;k
.Ifwe
knew X
n
, the differential X
Ã
n;k
could be given by
X
Ã
n;k
¼ X
Ã
n;k
À X
n
ð17:1-3Þ
But we do not know X
n
, it being what we are trying to estimate. As a result, we
do the next best thing. The differential is referenced relative to our best estimate
368 BAYES ALGORITHM WITH ITERATIVE DIFFERENTIAL CORRECTION
of X based on the past data, which is X
Ã
n;k
. As a result
X
Ã
n;k
¼ X
Ã
n;k
À X
Ã
n;k
¼ 0
ð17:1-4Þ
This may seem strange, but it is due to our choice of reference for X
Ã
n;k
. This
will become clearer as we proceed.
We are now in a position to apply the Bayes filter. In place of Y
nþ1
and
X
Ã
nþ1;n
in (9.2-1) to (9.2-1d) we use respectively, Y
n
and X
Ã
n;k
[= 0, because of
(17.1-4)]. Moreover, the Bayes filter of (9.2-1) to (9.2-1d) now becomes, for
finding our updated differential estimate X
Ã
n;n
[5, pp. 431–432],
X
Ã
n;n
¼ X
Ã
n;k
þ H
n
½ Y
n
À MðX
Ã
n;k
ÞX
Ã
n;k
ð17:1-5Þ
where X
Ã
n;k
¼ 0 in the above and
H
n
¼ S
Ã
n;n
½MðX
Ã
n;k
Þ
T
R
À1
1
ð17:1-5aÞ
S
Ã
n;n
¼fðS
Ã
n;k
Þ
À1
þ½MðX
Ã
n;k
Þ
T
R
À1
1
MðX
Ã
n;k
Þg
À1
ð17:1-5bÞ
But
X
Ã
n;n
¼ X
Ã
n;n
À X
Ã
n;k
ð17:1-6Þ
all the diferential vectors being referred to X
Ã
n;k
: The updated estimate X
Ã
n;n
is
thus obtained from (17.1-6) to be
X
Ã
n;n
¼ X
Ã
n;k
þ X
Ã
n;n
ð17:1-7Þ
This is our desired updated estimate.
In obtaining the above update X
Ã
n;n
, we have used a linearization of the
nonlinear observation equations. The new updated estimate will have a bias, but
it should be less than the bias in the original estimate
"
X
n
. In fact, having the
updated estimate X
Ã
n;n
, it is now possible to iterate the whole process we just
went through a second time to obtain a still better estimate for X
Ã
n;n
. Let us
designate the above updated estimate X
Ã
n;n
obtained on the first cycle as ðX
Ã
n;n
Þ
1
and correspondingly designate X
Ã
n;n
as ðX
Ã
n;n
Þ
1
.Nowð X
Ã
n;n
Þ
1
can be used in
place of X
Ã
n;k
to obtain a new Y
n
and X
Ã
n;k
, which we shall call ðY
n
Þ
2
and
ðX
Ã
n;k
Þ
2
, given by
ðY
n
Þ
2
¼ Y
n
Àð
"
Y
n
Þ
2
ð17:1-8Þ
where
ð
"
Y
n
Þ
2
¼ G½ðX
Ã
n;n
Þ
1
ð17:1-8aÞ
and
ðX
Ã
n;k
Þ
2
¼ X
Ã
n;k
ÀðX
Ã
n;n
Þ
1
ð17:1-8bÞ
DETERMINATION OF UPDATED ESTIMATES 369
Note that ðX
n;k
Þ
2
is now not equal to zero. This is because ðX
Ã
n;n
Þ
1
is no longer
X
Ã
n;k
. The covariance of ðX
n;k
Þ
2
2
still is S
Ã
n;k
and that of ðY
n
Þ
2
still is R
1
.
Applying the Bayes filter, again using the new differential measurement and
prediction estimate vector, yields ðX
Ã
n;n
Þ
2
and in turn ðX
Ã
n;n
Þ
2
from
ðX
Ã
n;n
Þ
2
¼ðX
Ã
n;n
Þ
1
þðX
Ã
n;n
Þ
2
ð17:1-9Þ
This procedure could be repeated with still better and better estimates
obtained for X
Ã
n;n
. The procedure would be terminated when [5, p. 433]
½ðX
Ã
n;n
Þ
rþ1
À Xð
Ã
n;n
Þ
r
T
½ðX
Ã
n;n
Þ
rþ1
À Xð
Ã
n;n
Þ
r
<" ð17:1-10Þ
Generally, the first cycle estimate ðX
Ã
n;n
Þ
1
is sufficiently accurate. The above use
of the Bayes algorithm with no iteration is basically the filter developed by
Swerling [123] before Kalman; see the Appendix.
Once the final update X
Ã
n;n
has been obtained, the whole process would be
repeated when a new observation Y
nþm
is obtained at a later time n þ m. The
subscript n þ m is used here instead of n þ 1 to emphasize that the time instant
between measurements need not necessarily be equal. Then X
Ã
nþm;n
would be
obtained by integrating forward the nonlinear equation of motion given by
(16.3-1). This X
Ã
nþm;n
would be used to obtain X
Ã
nþm;n
, Y
nþm
, and MðX
Ã
nþm;n
Þ
using (17.1-2), (17.1-2a), (17.1-4), and (16.2-10). Integrating (16.3-14),
Èðt
nþm
; t
n
;
"
XÞ would be obtained, from which in turn S
Ã
nþm;n
would be obtained
using (17.1-1). Using the Bayes filter, specifically (17.1-5) to (17.1-5b),
X
Ã
nþm;nþm
would then be obtained and, in turn, the desired next update state
vector X
Ã
nþm;nþm
.
17.2 EXTENSION TO MULTIPLE MEASUREMENT CASE
We will now extend the results of Section 17.2 to the case where a number of
measurements, let us say L þ 1 measurements, are simultaneously used to
update the target trajectory estimate as done in Section 9.5 for the Bayes
and Kalman filters when the observation scheme and target dynamics model
are linear. For concreteness we will still use the example consisting of a
projectile passing through the atmosphere. Assume measurements are made
at the L þ 1 time instances t
nÀL
, t
nÀLþ1
; ; t
nÀ1
, and t
n
, where these times
are not necessarily equally spaced. Let these L þ 1 measurement be given
by
Y
nÀL
; Y
nÀLþ1
; ; Y
nÀ1
; Y
n
ð17:2-1Þ
where Y
nÀi
is a measurement vector of the projectile position in polar
coordinates; see (16.2-4). [This is in contrast to (5.2-1) where the measurement
y
nÀi
was not a vector but just the measurement of one target parameter, e.g.,
370 BAYES ALGORITHM WITH ITERATIVE DIFFERENTIAL CORRECTION
range.] Let us put the L þ 1 vector measurements in the form
Y
ðnÞ
¼
Y
n
Y
nÀ1
.
.
.
Y
nÀL
0
B
B
B
B
B
B
B
B
@
1
C
C
C
C
C
C
C
C
A
ð17:2-2Þ
For the case of the projectile target being considered the observation scheme
and the target dynamics model are nonlinear. To use the measurement vector
(17.2-2) for updating the Bayes filter or Kalman filter, as done at the end of
Chapter 9 through the use of T, requires the linearization of the L þ 1
observations. Using the development given above for linearizing the nonlinear
observation and dynamic model, this becomes a simple procedure [5] and will
now be detailed.
As before, let X
Ã
k;k
be an estimate of the state vector of the projectile based on
measurements prior to the L þ 1 new measurements. Using the dynamic
equations given by (16.3-1) we can bring X
Ã
k;k
forward to the times of the L þ 1
new measurements to obtain X
Ã
n;k
; X
Ã
nÀ1;k
; ; X
Ã
nÀL;k
from which in turn we
obtain
"
Y
n
;
"
Y
nÀ1
; ;
"
Y
nÀL
through the use of (17.1-2a) and then in turn obtain
the differential measurement vector
Y
ðnÞ
¼
Y
n
Y
nÀ1
.
.
.
Y
nÀL
0
B
B
B
B
B
B
B
B
@
1
C
C
C
C
C
C
C
C
A
ð17:2-3Þ
by the use of (17.1-2).
Having the above Y
ðnÞ
we wish to obtain an update to the predicted X’s
given by X
Ã
n;k
; X
Ã
nÀ1;k
; ; X
Ã
nÀL;k
. However, rather than update all L þ 1 X ’s, it is
better to update a representative X along the trajectory. This representative X
would be updated using all the L þ 1 measurements contained in Y
ðnÞ
or
equivalently Y
ðnÞ
. First, using (16.2-9), we obtain
Y
ðnÞ
¼
Mð
"
X
n
Þ X
n
Mð
"
X
nÀ1
Þ X
nÀ1
.
.
.
Mð
"
X
nÀL
Þ X
nÀL
2
6
6
6
6
6
6
6
6
4
3
7
7
7
7
7
7
7
7
5
þ
N
n
N
nÀ1
.
.
.
N
nÀL
2
6
6
6
6
6
6
6
6
4
3
7
7
7
7
7
7
7
7
5
ð17:2-4Þ
EXTENSION TO MULTIPLE MEASUREMENT CASE 371
where M is defined by (16.2-10) and where, for simplicity of notation,
"
X
nÀi
is
used in place of X
Ã
nÀi;k
, which is X
Ã
k;k
brought forward to time n À i. It is the
X
n
, X
nÀ1
, , X
nÀL
that we want to update based on the differential
measurement matrix Y
ðnÞ
. Instead, as mentioned above, we will now
reference all the differential state vectors X
nÀi
at time t
nÀi
; i ¼ 0; ; L,
to some reference time t
c;n
¼ t
cn
. It is generally best to choose the time
t
cn
to be at or near the center observation of the L þ 1 observations. The
transition matrix from the time t
cn
to the time t
nÀi
of any measurement can
be obtained by integrating (16.3-14). Using these transition matrices, (17.2-4)
becomes
Y
ðnÞ
¼
Mð
"
X
n
ÞÈðt
n
; t
cn
;
"
X
cn
ÞX
cn
Mð
"
X
nÀ1
ÞÈðt
nÀ1
; t
cn
;
"
X
cn
ÞX
cn
.
.
.
Mð
"
X
nÀL
ÞÈðt
nÀL
; t
cn
;
"
X
cn
ÞX
cn
2
6
6
6
6
6
6
6
6
4
3
7
7
7
7
7
7
7
7
5
þ
N
n
N
nÀ1
.
.
.
N
nÀL
2
6
6
6
6
6
6
6
6
4
3
7
7
7
7
7
7
7
7
5
ð17:2-5Þ
where
"
X
cn
, also designated as X
Ã
cn;k
, is the value of X
Ã
k;k
brought forward to time
t
cn
.
Equation (17.2-5) can now be written as
Y
ðnÞ
¼ T
c;n
X
cn
þ N
ðnÞ
ð17:2-6Þ
where
T
c;n
¼
M
n
Èðt
n
; t
cn
;
"
X
cn
Þ
M
nÀ1
Èðt
nÀ1
; t
cn
;
"
X
cn
Þ
.
.
.
M
nÀL
Èðt
nÀL
; t
cn
;
"
X
cn
Þ
2
6
6
6
6
6
6
6
6
4
3
7
7
7
7
7
7
7
7
5
ð17:2-6aÞ
and
N
ðnÞ
¼
N
n
N
nÀ1
.
.
.
N
nÀL
2
6
6
6
6
6
6
6
6
4
3
7
7
7
7
7
7
7
7
5
ð17:2-6bÞ
372 BAYES ALGORITHM WITH ITERATIVE DIFFERENTIAL CORRECTION
and
M
nÀi
¼ Mð
"
X
nÀi
Þð17:2-6cÞ
Referencing X relative to
"
X
cn
¼ X
Ã
cn;k
yields
X
Ã
cn;k
¼ X
Ã
cn;k
À X
Ã
cn;k
¼ 0
ð17:2-7Þ
The above parallels (17.1-4)
We can now apply the Bayes Filter of (17.1-5) to (17.1-5b) or Kalman filter
of (9.3-1) to (9.3-1d) to update the projectile trajectory based on the L þ 1
measurements. This is done for the Bayes filter of (17.1-5) to (17.1-5b) by
replacing M by T
cn
and R
1
by R
ðnÞ
, which is the covariance matrix of Y
ðnÞ
;
specifically, (17.1-5) to (17.1-5b) become
X
Ã
cn;cn
¼ X
Ã
cn;k
þ H
cn
ðY
ðnÞ
À T
cn
X
Ã
cn;k
Þð17:2-8Þ
where
H
cn
¼ S
Ã
cn;cn
T
T
cn
R
À1
ðnÞ
ð17:2-8aÞ
S
Ã
cn;cn
¼½ðS
Ã
cn;k
Þ
À1
þ T
T
cn
R
À1
ðnÞ
T
cn
À1
ð17:2-8bÞ
and from (17.1-1)
S
Ã
cn;k
¼ Èðt
cn
; t
k
;
"
X
cn
Þ S
Ã
k;k
Èðt
cn
; t
k
;
"
X
cn
Þ
T
ð17:2-8cÞ
Having X
Ã
cn;cn
; one can obtain the desired update
X
Ã
cn;cn
¼
"
X
cn
þ X
Ã
cn;cn
ð17:2-9Þ
Having this new first estimate X
Ã
cn;cn
for X at time t
cn
, which we now
designate as ðX
Ã
cn;cn
Þ
1
, we could obtain an improved estimate designated as
ðX
Ã
cn;cn
Þ
2
. This is done by iterating the whole process described above with X
now referenced relative to ðX
Ã
cn;cn
Þ
1
, as done in (17.1-8b) when Y
ðnÞ
consisted of
one measurement.
If, in applying the above recursive Bayes filter, it was assumed that the
variance of the estimate based on the past data was infinite or at least extremely
large, then the recursive relation would actually degenerate into a nonrecursive
minimum variance estimate based on the most recent L þ 1 measurements as
given by
X
Ã
cn;cn
¼
"
X
cn
þðT
T
cn
R
À1
ðnÞ
T
cn
Þ
À1
T
T
cn
R
À1
ðnÞ
½Y
ðnÞ
À Gð
"
X
cn
Þ ð17:2-10Þ
EXTENSION TO MULTIPLE MEASUREMENT CASE 373
The above equation follows from (4.1-30) with W given by (4.5-4) for the
minimum variance estimate.
17.3 HISTORICAL BACKGROUND
The iterative differential correction procedure described in this chapter was first
introduced by Gauss in 1795 [5]. There is an interesting story [5, 122, 124]
relating to Gauss’s development of his least-squares estimate and the iterative
differential correction. At that time the astronomers of the world had been
looking for a missing planet for about 30 years. There was Mercury, Venus,
Earth, Mars, and then the missing planet or planetoid. It has been theorized that
because the planet had fragmented into planetoids (asteroids) it was so difficult
to locate. It was finally on January 1, 1801, that an Italian astronomer, Giuseppe
Piazzi, spotted for the first time one of these planetoids. There was great
rejoicing among the world’s astronomers. However, the astronomers soon
became concerned because the planetoid was out of view after 41 days, and
they feared it would possibly not be found for another 30 years. At this time
Gauss, who was then 23 years old, gathered the data that Piazzi had obtained on
the planetoid Ceres. Over a period of a few months he applied his weighted
least-squares estimate and the iterative differential correction techniques to
determine the orbit of Ceres. In December of 1801, he sent his results to Piazzi
who was then able to sight it again on the last day of 1801.
374 BAYES ALGORITHM WITH ITERATIVE DIFFERENTIAL CORRECTION
. used to determine the covariance matrix of
"
XðtÞ
367
Tracking and Kalman Filtering Made Easy. Eli Brookner
Copyright # 1998 John Wiley & Sons,. trajectory estimate as done in Section 9.5 for the Bayes
and Kalman filters when the observation scheme and target dynamics model
are linear. For concreteness