A Sequential Tracking Filter without Requirement of Measurement Decorrelation

Transcription

1 A Sequential racing Filter without Requient of Measuent Decorlation Gongjian Zhou, Changjun Yu, aifan Quan Deartent of Electronic Engineering Harbin Institute of echnology Harbin, China hiagalinga Kirubarajan Deartent of Electrical Couter Engineering McMaster University Hailton, Canada Abstract A sequential easuent rocessing ethod in nonlinear state estiation is of benefit to both estiation accuracy coutational efficiency. When the easuent errors a corlated, the decorlation is quid to oerate based on the covariance of the easuents in vious ethods in order for corct sequential rocessing. In this aer, a new sequential rocessing ethod without decorlation befo filtering is sented. he easuents a rocessed dictly by corsonding aroaches, while the corlation is hled in the easuent udate under the iniu ean squad error (MMSE estiation fraewor. Siulations deonstrate the effectiveness of the new ethod. Keywords-state estiation;nonlinear filtering; sequential rocessing;corlation;rmse I. INRODUCION arget otion is best odeled in Cartesian coordinates, while the easuents a usually orted in olar or sherical coordinates. In this case, tracing in Cartesian coordinates using the original easuents is a nonlinear state estiation roble. Various nonlinear filtering aroaches such as the extended Kalan filter (EKF [][] the unscented Kalan filter (UKF [6] a used to hle this roble. In a Doler radar tracing syste, the easuent consists of range, range rate (Doler one or two angles. he range angles can be converted into a linear for in Cartesian states to avoid using nonlinear filtering aroaches. his actually sults in the so called converted easuent Kalan filter (CMKF [7], which is consided to be suerior to the nonlinear aroaches (EKF UKF in ost scenarios. But when the Doler easuents a incororated, nonlinear filtering techniques a suggested to be used since the is not yet a conversion fro the Doler easuents straightly linear in Cartesian states. he roble, whether CMKF or nonlinear ethod should be used, arises when the osition easuents Doler a quid to be used to estiate target states. Sequential easuent rocessing schee, which is roven to be favorable to both estiation accuracy coutational efficiency[5][][][3], is used to rocess osition easuents Doler easuents by the CMKF nonlinear filtering techniques [3][][6][] [5], sectively. In soe radar systes, the range range rate easuent errors a statistically corlated. o erfor corct sequential easuent rocessing, decorlation based his wor was suorted by Fundaental Research Funds for the Central Universities (HI.NSRIF.97 the National Natural Science Foundation of China under grant No on Cholesy factorization is used in [3][][6][][5], to roduce a seudo easuent with errors indeendent to the osition easuents. he ain advantage of the sequential rocessing ethod is that the aroxiation of nonlinearity oerates based on the estiated state fro the osition easuents on the curnt stage, not the dicted state fro the last stage in siultaneous easuent rocessing ethod. However, the decorlation is carried out based on the easuents their covariance during all of the rocessing stages. his ay cause lose of estiation accuracy qui uch coutational cost. In this aer, a new sequential rocessing technique, which does not qui exlicit decorlation between arts of the easuent, is roosed. he osition easuents the converted Doler easuents (i.e., the roduct of the range range rate easuents a used dictly in the sequential rocessing schee. he corlation is evaluated based on the covariance after CMKF instead of the origin easuent covariance, is hled ilicitly in the iniu ean squad error (MMSE estiation fraewor. Monte Carlo siulations illustrate the effectiveness benefits of this new ethod. he st of this aer is organized as follows. Section II describes the forulation of the tracing syste. he easuent conversions the converted easuent errors a sented in Section III. In Section IV, the new tracing filter is sented, including the forulations of the CMKF, the covariance between the state errors fro CMKF the converted Doler easuents, the final state udate which hles the corlation ilicitly. Siulations a erfod in Section V followed by conclusions in Section VI. II. PROBLEM FORMULAION In Cartesian coordinates, the target otion can be odeled as X =Φ X +Γ v ( whe [,,, ] n X = x y x y, X R is the state vector consisting of osition coonents corsonding velocity coonents along x y dictions, sectively, at tie ste. If aneuvering target is consided, the state vector can be augented by other coonents such as acceleration. 3

2 n n He, Φ R is the state transition atrix, v is zero-ean Gaussian ro rocess noise with nown covariance Q, Γ is the noise gain atrix []. A D Doler radar is assued to ort easuents of targets in olar coordinates, including range, range rate aziuth. he easuent equation can be exssed as Z = [ r, θ, ] ( = hx [ ] + w = [ r, θ, ] + w whe r = x + y (3 θ = tan [ y x ] ( = [ xx + yy ] x + y (5 w [,, ] = θ (6 r, θ a easuents of the true target range, aziuth range-rate, sectively. In the above,, θ r a the corsonding easuent noises, which a all assued to be zero-ean white Gaussian noise with nown variancesσ r, σθ σ r, sectively. It is assued that the easuent noises a utually indeendent with the excetion that r a statistically corlated [] with corlation coefficient ρ, i.e. cov[, ] = ρσrσr (7 he covariance atrix of the original easuents can be written as σ r ρσrσ R = σθ (8 ρσ rσ σ III. MEASUREMEN CONVERSIONS A. Measuent conversion equations As well nown, the osition easuents including range aziuth in olar coordinates can be transfod into linear fors in Cartesian coordinates by c c Z = [ x, y] (9 In the above, c x = r cosθ = x + x ( c y = r sinθ = y + y ( whe x y a the errors of converted osition easuents along x y dictions in the Cartesian coordinates sectively. he converted Doler easuent is given as η c Z = η = r = η + η ( wheη is the converted Doler (i.e., the roduct of range range rate, with a quadratic function of Cartesian states as η = d( X = xx + yy (3 c η is the error of the converted Doler easuentη. Incororating the osition Doler easuents, the whole converted easuents fro the original sensor orts can be exssed as c c c c Z = [ x, y, η] = ( [ x, y, η ] + [ x, y, η ] B. Converted Measuent Errors In order to erfor effective filtering, aroriate error coutation should be taen to obtain accurate easuent errors atching the true statistics. Denote the bias covariance of the whole converted easuent vector (, sectively, as c η x y η μ = [ μ, μ ] = [ μ, μ, μ ] (5 η c R R R = η η (6 R R In the above, η xη yη R = R R (7 xx xy R R R = yx yy (8 R R hey can be obtained by the nested conditioning ethod, which finds the ean covariance conditioned on the unnown ideal easuent first then finds their averages (exectations conditioned on the noisy easuent [9]. he exssions of the eleents in (5 (6 a given as [][7][5] x / μ = r cosθ ( e e (6 y / μ = r sinθ ( e e xx R = ( r e cos θ cosh σ cosh σ { ( ( θ ( θ } θ ( σθ ( σ θ ( ( θ ( θ ( θ ( θ { θ ( ( σθ ( σθ } θ ( σθ ( σ θ ( ( θ ( θ ( θ ( θ σθ { } { cos cosh cosh } { sin sinh sinh } + ( r e sin sinh sinh + σ θ σ σ + σ θ σ σ yy = ( sin cosh cosh R r e σθ { } { sin cosh cosh } { cos sinh sinh } + ( r e cos sinh sinh + σ θ σ σ + σ θ σ σ xy yx = = sinθ cosθ σr R R e σθ ( ( + sinθ cos θ ( + σ e r r e (7 (8 (9

3 η μ = ρσrσ xη ηx R = R = ( σr + r ρσrσr cosθ e yη ηy R = R = ( σr + r ρσrσ sinθ e ηη R = ( r σ + σr ( + 3 ( + ρ σrσ + r ρσrσ / / IV. SEQUENIAL RACKING FILER ( η P cov( X, Z = E[ X ( η ] = η E[( I K H ΦX ( η + ( I K H Γv ( η K w ( η ] (9 η = KR In the above, the assution that the estiation error rocess noise at the last tie is indeendent to the easuent errors at the curnt tie is used thus the corsonding ters vanish. A. Position easuent filtering he converted osition easuents Z a rocessed by the de-biased CMKF [7] to roduce Cartesian state estiates. he easuent equation is exssed as Z = HX + w (9 whe H is the easuent atrix for a constant velocity (CV odel is H = ( he easuent noise w = [ x, y ] ( is zero-ean Gaussian noise with nown covariance R in (8. he tie udate easuent udate of the target state a ileented by follows: Xˆ ˆ =Φ X ( P =ΦP Φ + ΓQ Γ (3 K [ = P H HP H R ] + ( ˆ ˆ c, X ˆ = X + K[ Z μ HX ] (5 P = ( I KH P (6 B. Covariance after osition easuent filtering o ileent the sequential after the osition easuent filtering, the covariance between the state errors fro the CMKF, described in the subsection above, the converted Doler easuents should be evaluated. Fro the forulations of CMKF, (, (9-(6, one can write the state estiate of CMKF at tie as ˆ ˆ X ˆ =Φ X + K[ Z μ HΦ X ] (7 hen the corsonding estiation error is X = X Xˆ = ( Φ X +Γv ΦXˆ K { [ H( Φ X +Γ v + w ] HΦXˆ } (8 = ( I KH Φ X + ( I KH Γv Kw Multily the above by the error of the converted Doler easuents, one can get the covariance as C. Final estiation Considering the Cartesian states Xˆ at tie fro the CMKF as the rior X, the state udate by the converted Doler easuents is derived under the iniu ean squad error (MMSE estiation fraewor as ˆ η η X = X + PxzPzz ( Z - Z (3 Exing Z η around Xˆ in a aylor series with ters u to the second order as η Z ˆ ˆ = d( X + η = { d( X + D ( X X + (3 ( ˆ X ˆ X D ( X X + HO} + η whe d is the nonlinear function of the converted Doler easuents with sect to the Cartesian states. he Jacobian Hessian of d a defined, sectively, as d D = [ X d( X ] = X Xˆ (3 = X X= Xˆ d D = [ X Xd( X] = X Xˆ (33 = X X= Xˆ he higher-order-ters HO a zero, since converted Doler is quadratic in Cartesian states. he rior ean of easuent is obtained by taing exectation of (3 as Z η ( ˆ = d X + tr( DP (3 he covariance between the states to be estiated the easuent can be obtained as η η η Pxz E[( X X( Z - Z ] = P D P (35 the covariance of the easuent is η η η η ηη Pzz E[( Z - Z ( Z - Z ] = DP D + R + (36 ( η ( η tr DP DP DP DP hen, the final state estiates can be rovided by (3 the covariance associated with this cobined estiate is P ( ( = P Pxz Pzz Pxz (37 D. Filter initialization he basic idea of the two-oint diffencing initialization ethod [] is to estiate the initial osition velocity coonents of the state using the first two sets of osition easuents find the initial covariance by the 5

4 easuent covariance. he initial state is given as ˆ Z μ X = ( Z μ Z + μ the initial state is R R P = ( R R V. SIMULAIONS (38 (39 In order to evaluate the erforance of the new sequential nonlinear tracing filter, which is ferd to as, target with nearly CV constant acceleration (CA otion a consided. he transition atrix for CV odel is Φ= that for CA odel is Φ= Saling interval is = s. All of the rocess noises a generated as zero-ean uncorlated Gaussian white noise with stard deviation./s. o avoid a large nuber of figus, only the root ean squad error (RMSE of osition velocity a used to exaine erforance of the roosed filter a defined as M i i ep = [( x xˆ + ( y yˆ ] M i = ev [( x xˆ ( y yˆ ] M i i = + M i = sectively. he tracing erforance of the roosed SEKF- NEW is coad with the sequential nonlinear filtering aroach based on decorlation with Cholesy factorization ( sented in []. For the CV odel, the target oves with a seed of /s a heading of 6deg, the initial of the target is [3, 3]. he sensor is located at the origin of Cartesian coordinates to ort osition Doler easuents of targets. he easuent errors a set as σ r =.5, σ θ =.5 deg σ =. / s. he corlation coefficient ρ between the range range rate errors a consided as the influencing factors to be investigated. he tracing erforance for ρ = ρ =.9a dislayed in Fig. Fig., sectively. It is shown that the roosed ethod can rovide velocity estiation erforance close to the osition estiation erforance better than the, esecially for the case with strongly corlated easuent errors. RMSE of velocity - /s RMSE of osition tie ste (saling interval is seconds (a RMSE of velocity tie ste (saling interval is seconds (b RMSE of osition Fig. racing erforance in case of ρ = for CV odel For the CA odel, the target oves with a seed of /s a heading of 6deg, the initial of the target is [5, 5], the acceleration is [./s,./s ]. he sensor is located at the origin of Cartesian coordinates to ort osition Doler easuents of targets. he easuent errors a set as σ r =.3, σ θ =.3deg σ =. / s. he corlation coefficient ρ between the range range rate errors a consided as the influencing factors to be investigated. he tracing erforance for ρ = ρ =.9a dislayed in Fig. 3 Fig., sectively. Conclusion siilar to the CV case can be drawn for the CA odel. he roosed ethod outerfors in osition RMSE, esecially for the case of large corlation coefficient. 6

5 RMSE of velocity - /s 8 6 RMSE of osition tie ste (saling interval is seconds tie ste (saling interval is seconds (a RMSE of velocity (b RMSE of osition RMSE of osition RMSE of velocity - /s Fig.3 racing erforance in case of ρ = for CA odel tie ste (saling interval is seconds (b RMSE of osition Fig. racing erforance in case of ρ =.9 for CV odel tie ste (saling interval is seconds (a RMSE of velocity RMSE of velocity - /s RMSE of osition tie ste (saling interval is seconds (a RMSE of velocity tie ste (saling interval is seconds (b RMSE of osition Fig. racing erforance in case of ρ =.9 for CA odel 7

6 VI. CONCLUSIONS A new sequential rocessing ethod, without quient of easuent decorlation befo filtering, is sented. he case of tracing in Cartesian coordinates with osition Doler easuents observed in olar coordinates is investigated. First, the converted osition easuents a rocessed by the CMKF. he covariance between the state errors of CMKF converted Doler easuents is evaluated based on the filtering gain easuent covariance. hen, a final estiation ethod is derived, based on aylor exansion iniu ean squad error estiation, whe the nonlinearity corlation a hled siultaneously ilicitly. Siulations deonstrate the benefits fro using this new sequential rocessing ethod. REFERENCES [] Y. Bar-Shalo, Negative Corlation Otial racing with Range rate Measuents, IEEE rans. Aerosace Electronic Systes, vol. 37, no. 3,. 7-,. [] Y. Bar-Shalo, X. R. Li. Kirubarajan, Estiation with Alications to racing Navigation: heory, Algoriths, Softwa, New Yor: Wiley,. [3] Z. Duan, C. Han X. R. Li, Sequential Nonlinear racing Filter with Range-Rate Measuents in Sherical Coordinates, Proceedings of the 7th International Confence on Inforation Fusion, ,. [] Z. Duan C. Han, Radar arget racing with Range rate Measuents in Polar Coordinates, Journal of Syste Siulation(Chinese, vol. 6, no., ,. [5] Z. Duan, X. R. Li, C. Han H. Zhu, Sequential Unscented Kalan Filter for Radar arget racing with Range Rate Measuents, Proceedings of the 8th International Confence on Inforation Fusion,. 3-37, 5. [6] M. Lei C. Han, Sequential Nonlinear racing using UKF Raw Range-Rate Measuents, IEEE ransactions on Aerosace Electronic Systes, vol. 3, no.,. 39-5, 7. [7] D. Lerro Y. Bar-Shalo, racing with debiased consistent converted easuents versus EKF, IEEE rans. Aerosace Electronic Systes, vol. 9, no. 3,. 5-, 993. [8] R. Niu, P. Willett Y. Bar-Shalo, racing Considerations in Selection of Radar Wavefor for Range Range-Rate Measuents, IEEE rans. Aerosace Electronic Systes, vol. 38, no., ,. [9] X. R. Li V. P. Jilov. A Survey of Maneuvering arget racing- Part III: Measuent Models, Proceedings of SPIE Confence on Signal Data Processing of Sall argets, 73, SPIE, San Diego, CA, USA,. 3-6,. [] X. R. Li V. P. Jilov. A Survey of Maneuvering arget racing- Part I: Dynaics Models, IEEE rans. Aerosace Electronic Systes, vol. 39, no., , 3. [] S. E. Par J. G. Lee, Iroven Kalan Filter Design for he- Diensional Radar racing, IEEE rans. Aerosace Electronic Systes, vol. 37, no., ,. [] S.. Par J. G. Lee, Design of a Practical racing Algorith with Radar Measuents, IEEE rans. Aerosace Electronic Systes, vol. 3, no., , 998. [3] S.. Par J. G. Lee, Iroven Kalan Filter Design for he- Diensional Radar racing, IEEE rans. Aerosace Electronic Systes, vol. 37, no., ,. [] J. G. Wang,. Long P. K. He, Use of the Radial Velocity Measuent in arget racing, IEEE ransactions on Aerosace Electronic Systes, vol. 39, no.,. -3, 3. [5] G. Zhou, N. Zhao,. Fu,. Quan. Kirubarajan, Enhanced Sequential Nonlinear racing Filter with Denoised Pseudo Measuents, Proceedings of the th International Confence on Inforation Fusion, Chicago, Illinois, USA,. 9-5, July. 8