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 nonli
Trang 1BAYES 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Þ ¼ Xn 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
Copyright # 1998 John Wiley & Sons, Inc ISBNs: 0-471-18407-1 (Hardback); 0-471-22419-7 (Electronic)
Trang 2using [5, p 431]
S n;k¼ ðtn; tk; XÞSk;kðtn; tk; XÞT ð17:1-1Þ assuming we know Sk;k Assume that at time t (or n) we get the measurement
Yn Thus at time n we have Yn and the estimate X n;k, based on the past data, with their corresponding covariance matrices R1 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 Yn and X n;kare replaced by their differentials in (9.2-1) They are designated as, respectively, Ynand 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ðYn MX n;kÞ by Yn GðXnÞ without using the linearization of the observations However, as we see shortly, the calculation of
Hnrequires the use of MðXnÞ; see (17.1-5a) and (17.1-5b) Also MðXÞ is needed
to calculate S n1;nfor use in the next update calculation, as done using (17.1-5b) Physically we need to calculate MðXnÞ to find the minimum variance estimate
X n;n as the weighted sum of Yn 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ðXnÞ 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 Ynin terms of X obtained as described above Using in turn (16.2-7), we calculate Yn to be given by
Yn ¼ Yn Yn ð17:1-2Þ where
Yn ¼ 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 If we knew Xn, the differential X n;k could be given by
X n;k¼ X n;k Xn ð17:1-3Þ But we do not know Xn, 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
Trang 3of X based on the past data, which is X n;k As a result
X n;k¼ X n;k X n;k
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 Ynþ1 and
X
nþ1;n in (9.2-1) to (9.2-1d) we use respectively, Yn 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þ Hn½Yn MðX n;kÞX n;k ð17:1-5Þ where X n;k¼ 0 in the above and
Hn¼ Sn;n½MðX n;kÞ TR11 ð17:1-5aÞ
S n;n¼ fðSn;kÞ1þ ½MðX n;kÞ TR11 MðX n;kÞg1 ð17:1-5bÞ But
X n;n¼ X n;n X n;k ð17:1-6Þ all the diferential vectors being referred to Xn;k: The updated estimate Xn;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 Xn 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;nobtained 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 Yn and X n;k, which we shall callðYnÞ2 and ðX n;kÞ2, given by
ðYnÞ2 ¼ Yn ðYnÞ2 ð17:1-8Þ where
ðYnÞ2¼ G½ðX n;nÞ1 ð17:1-8aÞ and
ðX Þ ¼ X ðX Þ ð17:1-8bÞ
Trang 4Note thatðXn;kÞ2 is now not equal to zero This is becauseðX n;nÞ1is no longer
X n;k The covariance of ðXn;kÞ22 still is S n;k and that of ðYnÞ2 still is R1 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Þ1is 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 Ynþ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, Ynþ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),
ðtnþm; tn; XÞ would be obtained, from which in turn Snþm;nwould 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 tnL, tnLþ1; ; tn1, and tn, where these times are not necessarily equally spaced Let these Lþ 1 measurement be given by
YnL; YnLþ1; ; Yn1; Yn ð17:2-1Þ where Yni 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
yni was not a vector but just the measurement of one target parameter, e.g.,
370 BAYES ALGORITHM WITH ITERATIVE DIFFERENTIAL CORRECTION
Trang 5range.] Let us put the Lþ 1 vector measurements in the form
YðnÞ¼
Yn
Yn1
YnL
0 B B B B B
1 C C C C C
ð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;kbe 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;kforward to the times of the Lþ 1 new measurements to obtain X n;k; X n1;k; ; X nL;k from which in turn we obtain Yn; Yn1; ; YnL through the use of (17.1-2a) and then in turn obtain the differential measurement vector
YðnÞ¼
Yn
-Yn1 -
-YnL
0 B B B B B
@
1 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 n1;k; ; X nL;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ðXnÞ Xn -MðXn1Þ Xn1 -
-MðXnLÞ XnL
2 6 6 6 6 6 4
3 7 7 7 7 7 5 þ
Nn
Nn1
NnL
2 6 6 6 6 6 4
3 7 7 7 7 7 5
ð17:2-4Þ
Trang 6where M is defined by (16.2-10) and where, for simplicity of notation, Xniis used in place of X ni;k, which is X k;k brought forward to time n i It is the
Xn, Xn1, , XnL 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 Xni at time tni; i¼ 0; ; L,
to some reference time tc;n ¼ tcn It is generally best to choose the time
tcn to be at or near the center observation of the Lþ 1 observations The transition matrix from the time tcn to the time tni of any measurement can
be obtained by integrating (16.3-14) Using these transition matrices, (17.2-4) becomes
YðnÞ ¼
MðXnÞðtn; tcn; XcnÞXcn -MðXn1Þðtn1; tcn; XcnÞXcn
-
-MðXnLÞðtnL; tcn; XcnÞXcn
2 6 6 6 6 6 6
3 7 7 7 7 7 7 þ
Nn
Nn1
NnL
2 6 6 6 6 6 6
3 7 7 7 7 7 7
ð17:2-5Þ
where Xcn, also designated as X cn;k, is the value of X k;kbrought forward to time
tcn
Equation (17.2-5) can now be written as
YðnÞ¼ Tc;n Xcnþ NðnÞ ð17:2-6Þ where
Tc;n ¼
Mnðtn; tcn; XcnÞ
-Mn1ðtn1; tcn; XcnÞ -
-MnLðtnL; tcn; XcnÞ
2 6 6 6 6 6 4
3 7 7 7 7 7 5
ð17:2-6aÞ
and
NðnÞ¼
Nn
Nn1
NnL
2 6 6 6 6 6
3 7 7 7 7 7
ð17:2-6bÞ
372 BAYES ALGORITHM WITH ITERATIVE DIFFERENTIAL CORRECTION
Trang 7Mni¼ MðXniÞ ð17:2-6cÞ Referencing X relative to Xcn ¼ X cn;k yields
X cn;k ¼ X cn;k X cn;k
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 Tcn and R1 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þ HcnðYðnÞ TcnX cn;kÞ ð17:2-8Þ where
Hcn¼ Scn;cnTcnTR1ðnÞ ð17:2-8aÞ
S cn;cn¼ ½ðScn;kÞ1þ TT
cnR1ðnÞTcn 1 ð17:2-8bÞ and from (17.1-1)
S
cn;k¼ ðtcn; tk; XcnÞ Sk;kðtcn; tk; XcnÞT ð17:2-8cÞ Having X cn;cn; one can obtain the desired update
Having this new first estimate X cn;cn for X at time tcn, 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¼ Xcnþ ðTT
cnR1ðnÞTcnÞ1TcnTR1ðnÞ½YðnÞ GðXcnÞ ð17:2-10Þ
Trang 8The 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