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 [...]... þ df 1 Á x 1 " 1 ðtÞ þ x 1 ðtÞ ¼ f 1 ð" 0 ; " 1 Þ þ x x x "0 x dt dt dx 0 x dx 1 " 0 "1 x ð16:3-9Þ "1 x But from (16.3-1) [see also (16.3-4a) and (16.3-4b)] d " " XðtÞ ¼ F½Xðtފ dt ð16:3-10Þ 362 NONLINEAR OBSERVATION SCHEME AND DYNAMIC MODEL Hence (16.3-8) and (16.3-9) become [5, p 109] 1 0 1 0 df 0 df 0 d   x 0 ðtÞ C B C B dt x 0 ðtÞ B C ¼ B dx 0 dx 1 C @d A @ df 1 df 1 A " ðtÞ x 1 ðtÞ x 1 . 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,. 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