Research Article Spherical Simplex-Radial Cubature Quadrature Kalman Filter

Size: px

Start display at page:

Download "Research Article Spherical Simplex-Radial Cubature Quadrature Kalman Filter"

Rodney Lester
5 years ago
Views:

1 Hidawi Joural of Electrical ad Computer Egieerig Volume 27, Article ID , 8 pages Research Article Spherical Simplex-Radial Cubature Quadrature Kalma Filter Zhaomig Li ad Wege Yag 2 Compay of Postgraduate Maagemet, Academy of Equipmet, Beijig 46, Chia 2 Departmet of Optical ad Electrical Equipmet, Academy of Equipmet, Beijig 46, Chia Correspodece should be addressed to Zhaomig Li; lizhaomigzbxy@63.com Received 8 February 27; Accepted July 27; Published 8 August 27 Academic Editor: Bhupedra N. Tiwari Copyright 27 Zhaomig Li ad Wege Yag. This is a ope access article distributed uder the Creative Commos Attributio Licese, which permits urestricted use, distributio, ad reproductio i ay medium, provided the origial wor is properly cited. A spherical simplex-radial cubature quadrature Kalma filter (SSRCQKF) is proposed i order to further improve the oliear filterig accuracy. The Gaussia probability weighted itegral of the oliear fuctio is decomposed ito spherical itegral ad radial itegral, which are approximated by spherical simplex cubature rule ad arbitrary order Gauss-Laguerre quadrature rule, respectively, ad the ovel spherical simplex-radial cubature quadrature rule is obtaied. Combied with the Bayesia filterig framewor, the geeral form ad the specific form of SSRCQKF are put forward, ad the umerical simulatio results idicate that the proposed algorithm ca achieve a higher filterig accuracy tha ad SSR.. Itroductio The oliear state estimatio problem widely exists i sigal processig, target tracig, itelliget sesig, ad other fields, which is a subject udergoig itese study [ 4]. I suboptimal oliear filterig uder Bayesia theory framewor, the posterior probability desity fuctio (pdf) is assumed to be Gaussia distributio, ad the core issue is to calculate the itractable itegral as oliear fuctio Gaussia pdf. Sice to achieve the aalytical solutio is difficult for the itegral, the focus of the research is seeig a highprecisio itegral rule for its umerical approximatio [5]. The most widely used oliear Kalma filterig algorithms are exteded Kalma filter (EKF) [6, 7] ad usceted Kalmafilter(UKF)[8,9],respectively.EKFusesthefirstorder Taylor formula to liearize the oliear fuctio directly, thereby has oly first-order filterig accuracy, ad eeds to calculate the Jacobia matrix, which limits its further applicatio. UKF adopts a set of sigma poits to approximate the itractable itegral ad achieves a third-order accuracy. However, the selectio of the sigma poits ad correspodig weights lacs rigorous mathematical basis, ad the stability of umerical calculatio is reduced for the high-dimesioal system. Cubature Kalma filter () proposed by Arasaratam ad Hayi [, ] decomposes the itractable itegral ito spherical itegral ad radial itegral, which are approximated by the third-order cubature rule. ot oly has a rigorous mathematical basis i selectig the cubature poits but also has a complete stability i umerical calculatio [2 4]. Moreover, Wag et al. [5] proposed the spherical simplex-radial cubature Kalma filter (SSR), i which the spherical simplex rule istead of sphericalruleisusedicalculatigthesphericalitegral [6 8]. However, radial itegral is calculated by momet matchig method i both ad SSR, which caot guarateetheoptimalsolutio.tosolvethisproblem,shova ad Swati [9] proposed cubature quadrature Kalma filter (CQKF): the algorithm adopts the same method for solvig the spherical itegral as, but, for radial itegral, arbitrary order Gaussia-Laguerre quadrature formula is used to achieve a higher radial itegral accuracy, so as to improve the filterig accuracy further. It is also poited out that is a simplified form of CQKF with the first-order Gaussia- Laguerre quadrature i radial itegral. I order to further improve the oliear Kalma filterig accuracy, this paper proposes a ovel spherical simplexradial cubature quadrature Kalma filter (SSRCQKF). The structure of this paper is as follows. The spherical simplexradial cubature quadrature rule is proposed i Sectio 2, ad the SSRCQKF algorithm is proposed i Sectio 3, the

2 2 Joural of Electrical ad Computer Egieerig umerical simulatio results of strog oliear system ad target tracig are show i Sectio 4, ad fially the coclusio is give i Sectio Spherical Simplex-Radial Cubature Quadrature Rule 2.. Spherical Simplex-Radial Cubature Rule. Cosider the itegral I(f) = R f(x)e xτx dx,letx =ry,wherey deotes the uit sphere surface that satisfies y Τ y =, r deotes the sphereradius,ad,the,i(f) ca be decomposed ito the followig spherical itegral ad radial itegral []: S (r) = f (ry)dσ(y) U () R= S (r) r e r2 dr. I geeral, the aalytical solutios of above itegrals are difficult to obtai, so the umerical approximate method is cosidered. It is poited out i [5] that the spherical itegral ca be approximated usig the spherical simplex rule that cotais 2 2 itegral poits as follows: A S (r) = [f (ra 2 () i )f ( ra i )], (2) where A = 2 π /Γ(/2) deotes the surface area of - dimesioal uit sphere with Γ() = x e x dx represetig the Gamma fuctio, a i =[a i, a i,2 a i, ] Τ,ad,2,...,deotes the ith vertex of the -dimesioal simplex, whose elemets are defied as follows: ( j2)( j), j < i a i,j =, j > i ()( i), i = j. ( i2) 2.2. Gaussia-Laguerre Quadrature Rule. For the radial itegral R = S(r)r e r2 dr, letr 2 = t ad we get R = (/2) S( t)t ( 2)/2 e t dt; furthermore, let g(t) = S( t) ad β = ( 2)/2;theR=(/2) g(t)t β e t dt is obtaied; the itegral term i R is approximated usig the followig Gaussia-Laguerre quadrature rule [2]: g (t) t β e t dt m j= (3) A j g (t j ), (4) where t j deote the quadrature poits, which ca be solved from the solutios of the followig m-order Chebyshev- Laguerre polyomial. L β m (t) = ( )m t β e t d m dt m (tβm e t )=. (5) Ad A j deote the correspodig weights, which ca be solved as follows: m!γ (β m ) A j = t j [ L β m (t j )] 2. (6) Itcabeseethattheapproximatioaccuracyofthe above rule depeds o the umber of quadrature poits Spherical Simplex-Radial Cubature Quadrature Rule. Equatio (4) is substituted ito rule R,adweobtai R= 2 g (t) t β e t dt= 2 m j= A j g(t j ). (7) The spherical simplex rule (2) ad g(t) = S( t) are plugged ito (7) to obtai R= 2 m j= A m A j j= A j S( t j )= 4 () π [f ( t j a i )f ( t j a i )] = 2 () Γ (/2) m A j j= [f ( t j a i )f ( t j a i )]. Due to R f(x)n(x; x, P x )dx = (/ π ) R f( 2P x x x)e xτx dx, the Gaussia probability weighted itegral of arbitrary oliear fuctio is obtaied as follows: f (x) N(x; x, P x )dx = R 2 () Γ (/2) m A j j= [f ( 2t j P x a i x) f ( 2t j P x a i x)]. Equatio (9) is the ovel spherical simplex-radial cubature quadrature rule that proposed i this paper, which requires the calculatio of 2()m poits adcorrespodig weights. I particular, it ca be solved that t =/2, A = Γ(/2) whe m =, ad results i the spherical simplexradial cubature rule. Thus, the spherical simplex-radial cubature rule is the degeerate form of the proposed rule, ad the proposed rule ca achieve a higher approximatio accuracy whe m SSRCQKF Algorithm 3.. The Geeral Form of SSRCQKF. Cosider the followig discrete oliear system with additive oise: x = f (x )w z = h (x ) w (,Q ) (,R ), (8) (9) ()

3 Joural of Electrical ad Computer Egieerig 3 where x R deotes the state vector, z R z deotes the measuremet vector, ad the oises w, are ucorrelated Gaussia white oise. With the system dimesio ad the order m of Chebyshev-Laguerre polyomial beig ow, t j, j =,...,m cabesolvedby(5),adthecorrespodig weights ca be calculated as follows: m!γ (β m ) 2 () Γ (/2) t [ L β m (t )] 2, m!γ (β m ) ω i = 2 () Γ (/2) t 2 [ L β m (t 2 )] 2,. m!γ (β m ) 2 () Γ (/2) t m [ L β m (t m )] 2,,...,22 i=23,...,4() i = 2(mm ),...,2() m. () The matrix a = [a a 2 a ] cosists of a i that is used to costruct the followig expasio matrix [a a] = [a a 2 a a a 2 a ].Thesubscripti i [ ] i deotes the ith colum of matrix; based o the rule (9) ad Bayesia filterig framewor, the primary calculatio process of the geeral form of SSRCQKF algorithm is listed as follows: Calculate the followig poits: x 2t P [a a] i,,...,2() x (i) = x 2t 2P [a a], i. x 2t mp [a a], i i=23,...,4() i = 2(mm ),...,2() m. (2) Calculate the oliear propagatio of the poits: X (i) = f ( x (i) ). (3) Calculate the prior state estimatio ad prior error covariace matrix: x 2()m = ω i X (i) P 2()m = ω i (X (i) Calculate the followig poits: x )(X(i) x )Τ Q. (4) x 2t P [a a] i, x (i) = x 2t 2P [a a], i. x 2t mp [a a], i,...,2() i=23,...,4() i = 2(mm ),...,2() m. (5) Calculate the oliear propagatio of the poits: Calculatethepredictedmeasuremetvalue: Z (i) = h ( x (i) ). (6) ẑ = 2()m ω i Z (i). (7)

4 4 Joural of Electrical ad Computer Egieerig Calculate the predicted measuremet covariace matrix: P z = 2()m ω i (Z (i) ẑ )(Z (i) ẑ ) Τ R. (8) Calculate the cross covariace matrix: P xz = 2()m ω i ( x (i) Calculate the Kalma filterig gai: x )(Z(i) ẑ ) Τ. (9) K = P xz P z (2) Calculate the a posteriori state estimatio: x = x K (z ẑ ). (2) Calculate the a posteriori error covariace matrix: P = P K P z K Τ. (22) Itcabeseefromthealgorithmprocessthatthefilterig accuracy depeds o the order of the Gaussia-Laguerre quadrature rule; the higher the order is, the higher that estimatio accuracy is achieved, but the more the poits ad weights are required. However, for the idetified m ad, the poits ca be calculated i advace ad stored offlie ad called directly from the memory i the process of implemetatio, that is ecessary to improve the real-time performace of the algorithm The Specific Form of SSRCQKF Whe m=2. The geeral form of SSRCQKF is preseted i Sectio 3., ad, i this sectio, the specific form of SSRCQKF algorithm whe m=2 is give. Whe m=2, (7) is simplified as follows: R= 2 [A S ( t ) A 2 S ( t 2 )]. (23) The values of t ad t 2 are eeded to be calculated. Plug m=2ito (5), we obtai L β 2 (t) =t β e t d 2 dt 2 (tβ2 e t ). (24) The item (d 2 /dt 2 )(t β2 e t ) is expaded to achieve L β 2 (t) ad its derivative L β 2 (t) as follows: L β 2 (t) =t2 2(β2)t(β)(β2) (25) L β 2 (t) =2t 2(β2). Let L β 2 (t) =, thesolutiosaret = β 2 ± β2, combied with β = ( 2)/2,adweobtai t = 2 2 t 2 = 2 2. (26) ω i The, A ad A 2 are solved from (6) as follows: A = A 2 = Γ (/2) Γ (/2) Furthermore, the weights ω i are solved as follows: 4 ()( 2 2 4),,2,...,22 = 4 ()( 2 2 4), i=23,...,44. (27) (28) The spherical simplex-radial cubature quadrature rule with m=2is obtaied by pluggig t, t 2, A,adA 2 ito rule (9) as follows: R f (x) N(x; x, P x )dx = 4 ()( 2 2 4) [f (x ( 2 2 4) P x a i ) f (x ( 2 2 4) P x a i )] 4 ()( 2 2 4) [f (x ( 2 2 4) P x a i ) f (x ( 2 2 4) P x a i )]. (29) Basedo(29),thecalculatiostepsofthespecificformof SSRCQKF whe m=2are give as follows. Step (filter iitializatio). Oe has x =E(x ) P =E[(x x )(x x )Τ ]. Cycle =,2,..., ad complete the followig steps. (3)

5 Joural of Electrical ad Computer Egieerig 5 Step 2 (time update). Oe has x (i) = x ( 2 2 4) P [a a], i x ( 2 2 4) P [a a], i 2 2,2,...,22 i=23,...,44 X (i) = f ( x (i) ) x = 4 ()( 2 2 4) 22 X (i) 4 () ( 2 2 4) 44 X (i) i=23 (3) P 22 = (X (i) 4 ()( 2 2 4) x )(X(i) x )Τ 4 ()( 2 2 4) 44 i=23 (X (i) x )(X(i) x )Τ Q. Step 3 (measuremet update). Oe has x (i) = x ( 2 2 4) P [a a], i x ( 2 2 4) P [a a], i 2 2,2,...,22 i=23,...,44 Z (i) = h ( x (i) ) ẑ = 4 ()( 2 2 4) 22 Z (i) 4 () ( 2 2 4) 44 Z (i) i=23 22 P z = (Z (i) 4 ()( 2 2 4) ẑ )(Z (i) ẑ ) Τ (32) 4 ()( 2 2 4) 44 i=23 (Z (i) ẑ )(Z (i) 22 P xz = ( x (i) 4 ()( 2 2 4) x )(Z(i) ẑ ) Τ 4 ()( 2 2 4) 44 i=23 ( x (i) ẑ ) Τ R x )(Z(i) ẑ ) Τ. Step 4 (state update). Oe has x = x K (z ẑ ) K = P xz P z P = P K P z K Τ. (33)

6 6 Joural of Electrical ad Computer Egieerig RMSE.9 RMSE Step Step SSR SSR Figure : RMSE of state. Figure 2: RMSE of state 2. Table : Average RMSE of state. Filters State State 2 State SSR Simulatio Results ad Aalysis 4.. Simulatio. The effectiveess of the proposed SSRC- QKF algorithm is verified by the followig three-dimesioal strog oliear system, which icludes trigoometric fuctio operatio, power operatio, ad expoetial operatio. [ [ x, x 2, x 3, 3 si 2 (x 2, ) = x ] [, e.5x 3, ] [ ] ] [.2x, (x 2, x 3, )] [ ] w (34) RMSE SSR Step Figure 3: RMSE of state 3. z = cos (x, )x 2, x 3, V, where Q =., R =, the theoretical iitial value of the oliear system is x = [.7 ] Τ,thefilterigiitial value is x =[ ] Τ, the iitial covariace matrix is P = I 3 3. The (whe m=2)ad(whe m=3) are compared with ad SSR, the simulatio step is, ad the total step size is. The root mea square error (RMSE) is used to describe the filterig accuracy ad ru 5 times Mote-Carlo simulatio, ad the results are show i Figures 3. I order to show the details of the curve more clearly, the data oly betwee 3 ad 8 are captured i figures. It ca be see from the figures that the RMSE of theproposedssrcqkfissigificatlysmallerthatheother two algorithms. Table 2: Variace RMSE of state. Filters State State 2 State SSR I order to compare the four filters quatitatively, the average value ad variace of RMSE of the four filters i steps are couted ad listed i Tables ad 2, respectively. From Table, we see that achieves a higher filterig accuracy tha SSR i this simulatio. Compared with, the proposed i this paper improves the

7 Joural of Electrical ad Computer Egieerig 7 estimatio accuracy of state by.4%, improves the estimatio accuracy of state 2 by 6.26%, ad improves the estimatio accuracy of state 3 by.7%. Compared with SSRCQKF- 2, the improves the estimatio accuracy of the three states by.59%,.56%, ad 2.76%, respectively. All these idicate that the proposed filter has the optimal performace i terms of estimatio accuracy. Moreover, the RMSE variace of the two SSRCQKF are smaller tha that of the other two filters, which idicates that the estimated fluctuatio is more stable Simulatio 2. The proposed SSRCQKF is applied i target tracig i this sectio. The target is assumed to be i costat velocity (CV) motio; the state equatio of CV model i two-dimesioal case is described as follows: X = F CV X Gw, (35) where X = (x, x,y, y ) Τ represets the target states (positio ad velocity) at time idex, w deotes the process oise, F CV ad G deote the state trasformatio matrix ad the oise drive matrix, which are defied, respectively, as follows: T F CV =, T [ ] [ ] T 2 (36) 2 T G =, T 2 [ 2 ] [ T ] where T is the samplig iterval. I target tracig system, the bearigs-oly measuremet equatio is writte as follows: Z = arcta ( y y r x x r ) V, (37) where Z is the radar measuremet at time, (x r,y r ) is the locatio of radar, ad V is the measuremet oise. I the simulatio, the radar locatio is set to be (x r,y r ) = (2 m, 3 m), the simulatio time is 4 s, T =, ad the iitial state of the target is (x, x,y, y ) = ( m, 2m/s, 2 m, 2m/s). Theiitialstateadcovariace matrix are x = ( m,2m/s, 2 m,2m/s) Τ ad P = diag[.,.,.,.], respectively.thestadard deviatio of the measuremet oise is. deg. The Mote- Carlo simulatio is performed 5 times, ad the tracig accuracy is also evaluated usig the RMSE. The simulatio results, icludig the positio RMSE ad velocity RMSE of various filters, are show i Figures 4 ad 5, Positio RMSE (m) Velocity RMSE (m s ) SSR Time (s) Figure 4: The positio RMSE of various filters Time (s) SSR Figure 5: The velocity RMSE of various filters. respectively, ad the partial magificatio is give to show the details of the curve. It ca be see that the tracig accuracy of is sigificatly lower tha that of other filters, ad the proposed ad have achieved higher target tracig accuracy tha that of SSR. The average RMSE of the various filters is show i Table 3, ad compared with, the estimated positio accuracy of SSR is improved by 2.85%, idicatig that the spherical simplexradial cubature rule has higher accuracy tha spherical-radial cubature rule i this simulatio. Sice the secod-order Gauss-Laguerre rule is used i the, the accuracy is improved by.86% compared to SSR. Due to the more quadrature poits used i, its accuracy is higher tha that of by.5%. Through the aalysis of

8 8 Joural of Electrical ad Computer Egieerig Table 3: Average RMSE of various filters. Filters Positio RMSE/m Velocity RMSE/(m/s) SSR the simulatio results, the effectiveess of the proposed filter is verified. 5. Coclusio I order to further improve the filterig accuracy of the oliear system, this paper proposes a SSRCQKF algorithm by combiig the spherical simplex-radial cubature rule with arbitrary order Gaussia-Laguerre quadrature rule. The proposed algorithm has a higher filterig accuracy tha adssr.theresultsofthetwoumericalsimulatios, icludig the three-dimesioal strogly oliear system ad target tracig, verify the validity of the proposed algorithm. Coflicts of Iterest The authors declare that there are o coflicts of iterest regardig the publicatio of this paper. Refereces [] J. Q. Li, R. H. Zhao, J. L. CHEN, ad Y. Zhu, Target tracig algorithm based o adaptive strog tracig particle filter, IET Sciece, Measuremet ad Techology,vol.,o.7,pp.74 7, 26. [2] D. Satoro ad M. Vadursi, Performace aalysis of a DEKF for available badwidth measuremet, Joural of Electrical ad Computer Egieerig, vol.26,articleid75388,8pages, 26. [3] S.Y.Che, Kalmafilterforrobotvisio:asurvey, IEEE Trasactios o Idustrial Electroics, vol.59,o.,pp , 22. [4] A.N.Bishop,P.N.Pathiraa,adA.V.Savi, Radartarget tracig via robust liear filterig, IEEE Sigal Processig Letters,vol.4,o.2,pp.28 3,27. [9] K.Xiog,H.Y.Zhag,adC.W.Che, Performaceevaluatio of UKF-based oliear filterig, Automatica. A Joural of IFAC, the Iteratioal Federatio of Automatic Cotrol,vol. 42,o.2,pp.26 27,26. [] I. Arasaratam ad S. Hayi, Cubature alma filters, Istitute of Electrical ad Electroics Egieers. Trasactios o Automatic Cotrol,vol.54,o.6,pp ,29. [] I. Arasaratam ad S. Hayi, Cubature Kalma smoothers, Automatica. A Joural of IFAC, the Iteratioal Federatio of Automatic Cotrol,vol.47,o.,pp ,2. [2] L. Zhag, H. Yag, H. Lu, S. Zhag, H. Cai, ad S. Qia, Cubature Kalma filterig for relative spacecraft attitude ad positio estimatio, Acta Astroautica,vol.5,o.,pp , 24. [3]D.Poturu,K.P.B.Chadra,I.Arasaratam,D.-W.Gu,K. A. Mary, ad S. B. Ch, Derivative-free square-root cubature Kalma filter for o-liear brushless DC motors, IET Electric Power Applicatios, vol., o. 5, pp , 26. [4] Y. Zhao, Performace evaluatio of Cubature Kalma filter i a GPS/IMU tightly-coupled avigatio system, Sigal Processig, vol. 9, pp , 26. [5] S.Wag,J.Feg,adC.K.Tse, Sphericalsimplex-radialcubature Kalma filter, IEEE Sigal Processig Letters, vol. 2, o., pp.43 46,24. [6] Q. Zhu, M. Yua, Z. Wag, Y. Che, ad W. Che, A robot spherical simplex-radial cubature FastSlam algorithm, Robot, vol.37,o.6,pp.79 76,25. [7] Z. Zhag, Q. X. Jiag, ad J. F. Pa, Sigle observer passive locatio based o spherical simplex-radial cubature particle filter, Joural of Detectio Cotrol,vol.36,o.4,pp.88 9,24. [8]H.Wu,S.Che,B.Yag,adX.Luo, Rage-parameterised orthogoal simplex cubature Kalma filter for bearigs-oly measuremets, IET Sciece, Measuremet ad Techology, vol., o. 4, pp , 26. [9] B. Shova ad Swati, Cubature quadrature Kalma filter, IET Sigal Processig,vol.7,o.7,pp ,23. [2] B. Shova ad Swati, Square-root cubature-quadrature Kalma filter, Asia Joural of Cotrol, vol.6,o.2,pp , 24. [5] K. P. Chadra, D.-W. Gu, ad I. Postlethwaite, Cubature H iformatio filter ad its extesios, Europea Joural of Cotrol,vol.29,pp.7 32,26. [6] K. Reif, S. Guther, E. Yaz, ad R. Ubehaue, Stochastic stability of the discrete-time exteded Kalma filter, Istitute of Electrical ad Electroics Egieers. Trasactios o Automatic Cotrol, vol. 44, o. 4, pp , 999. [7] M. L. Psiai, Bacward-smoothig exteded Kalma filter, Joural of Guidace, Cotrol, ad Dyamics, vol.28,o.5,pp , 25. [8] S. Julier, J. Uhlma, ad H. F. Durrat-Whyte, A ew method for the oliear trasformatio of meas ad covariaces i filters ad estimators, Istitute of Electrical ad Electroics Egieers. Trasactios o Automatic Cotrol,vol.45,o.3,pp , 2.

9 Volume 2 Iteratioal Joural of Rotatig Machiery Joural of Volume 2 The Scietific World Joural Joural of Sesors Iteratioal Joural of Distributed Sesor Networs Joural of Cotrol Sciece ad Egieerig Advaces i Civil Egieerig Submit your mauscripts at Joural of Robotics Joural of Electrical ad Computer Egieerig Advaces i OptoElectroics Volume 24 VLSI Desig Iteratioal Joural of Navigatio ad Observatio Modellig & Simulatio i Egieerig Iteratioal Joural of Iteratioal Joural of Ateas ad Chemical Egieerig Propagatio Active ad Passive Electroic Compoets Shoc ad Vibratio Advaces i Acoustics ad Vibratio

Research Article A Unified Weight Formula for Calculating the Sample Variance from Weighted Successive Differences

Research Article A Unified Weight Formula for Calculating the Sample Variance from Weighted Successive Differences Discrete Dyamics i Nature ad Society Article ID 210761 4 pages http://dxdoiorg/101155/2014/210761 Research Article A Uified Weight Formula for Calculatig the Sample Variace from Weighted Successive Differeces