Maneuvering Target Tracking Based on Adaptive Square Root Cubature Kalman Filter Algorithm

Chapter95Maneuvering Target Tracking Basedon Adaptive Square Root CubatureKalman Filter AlgorithmSisi Wang and Lijun WangAbstract Concerning low accuracy even divergence of maneuvering target tracking due to inaccurate tracking model and statistical property,an adaptive Square Root Cubature Kalman Filter(SCKF)is proposed based on the standard SCKF and modified Sage-Husa estimator.The proposed algorithm can estimate the statistical parameters of unknown system noises online,and restrain the tracking error caused by unknown system noises effectively;hence it is applied to maneuvering target tracking.The simulation is preformed latterly and experi-mental results show that comparing with the standard SCKF algorithm,the adaptive SCKF can achieve better accuracy and stability for maneuvering target tracking while the system noises is unknown and time variation. Keywords Adaptive SCKFÁManeuvering target tracking95.1IntroductionManeuvering Target Tracking is a hotspot and difficulty in radar information processingfields at all times.In maneuvering tracking target applications,target dynamics are usually modeled in Cartesian coordinates.State vector encompass velocity and position components.The corresponding measurement information is expressed in polar coordinates,including distance components,bearing S.Wang(&)ÁL.WangSchool of Navigation,Guangdong Ocean University,40#,East Jiefang Road,Zhanjiang,Chinae-mail:mars32lin@L.Wange-mail:123wanglijun@W.Lu et al.(eds.),Proceedings of the2012International Conference on Information901 Technology and Software Engineering,Lecture Notes in Electrical Engineering210,DOI:10.1007/978-3-642-34528-9_95,ÓSpringer-Verlag Berlin Heidelberg2013902S.Wang and L.Wang components even elevation components and so on.Hence,Maneuvering Target Tracking is considered as a typical multidimensional nonlinear estimate problem.The square root cubature Kalmanfilter proposed in recent year is a effectively method for the multidimensional nonlinear estimate problem[1].The simulation result proved that under the hypothesis of equal computational complexity,the SCKF algorithm has the higher accuracy than the common nonlinear estimate method such as Particle Filter and Unscented Kalman Filter and so on[2].But while thefilter is based on the inaccurate model and noise statistical property,it will lead to larger estimate error even divergence.And more often than not,the process noise is difficult represented its statistics property due to external inter-ference,acceleration physical characteristic and manipulate and so on.Therefore, process noise is usually unknown and time variation.To solve this problem,an adaptive SCKF(ASCKF)is proposed,which is a combination of modified Sage-Husa estimator and SCKF.The modified Sage-Husa suboptimal unbiased estimator which is embedded into the algorithm can recursively estimates and corrects the unknown noise on line,and thus the ASCKF can handle the recursive statefiltering in the presence of unknown process noise covariance matrices.95.2Problem StatementConsidering general maneuvering target tracking problem in Cartesian coordi-nates,the discrete time state system and measurement equation can be described as following state space model.x k¼f x kÀ1ðÞþv kÀ1ð95:1ÞðÞþx kð95:2Þz k¼h x kWhere x k2R n x is the state of the dynamic system at discrete time k;f: R n xÂR n x!R n x and h:R n xÂR n x!R n x are some known functions;z k2R n x is the measurement;v kÀ1f g and x kf g are independent process and measurement Gaussian noise sequences with means q and r and covariances Q kÀ1and R k, respectively.95.3Square Root Cubature Kalman FilterThe symmetry and positive definiteness of error covariance matrix are often lost in update cycle of the CKF,and then Ienkaran Arasaratnam proposed the SCKF algorithm base on the standard CKF.The rest of this section is devoted to describe the SCKF in detail.Firstly,according the 3rd spherical-radial rule,a total of m ¼2n cubature points is chosen,where n is the state-vector dimension,and cubature points set w i ;n i f g are used to piecewise linear approach the probability distribution function of state vector.Wherew i ¼1m ;n i ¼ffiffiffiffim 2r l ½ i;i ¼1;2;...;m ;m ¼2n ;ð95:3Þwhere l ½ 2R n is a generator,and l ½ i denote the ith element of l ½ .For example,l ½ 2R 2represents the following set of points:10;01 ;À10 ;0À1 &'ð95:4ÞAssume the posterior density probability p x k À1j z 1:k À1ðÞ$N x k À1;x k À1=k À1;ÀP k À1=k À1Þat time k À1is known,the Cholesky factor S k À1=k À1of error covariance is available as S k À1=k À1¼chol P k À1=k À1ÈÉ.The SCKF algorithm can be repre-sented as following three steps:Step 1:Initializationx 0j 0;S 0j 0¼chol P 0j 0ÀÁStep 2:Time updateX i ;k À1j k À1¼S k À1j k À1n i þ^x k À1j k À1X Ãi ;k j k À1¼f X i ;k À1j k À1;u k À1ÀÁ^x k j k À1¼1m X m i ¼1X Ãi ;k j k À1þq S k j k À1¼Tria v Ãk j k À1;S Q ;k À1h i ð95:5ÞWhereQ k À1¼S Q ;k À1S T Q ;k À1v Ãk j k À1¼1ffiffiffiffimp X Ã1;k j k À1À^x k j k À1X Ã2;k j k À1ÂÀ^x k j k À1ÁÁÁX Ãm ;k j k À1À^x k j k À1Ãð95:6ÞStep 3:Measurement updateX i ;k j k À1¼S k j k À1n i þ^x k j k À1Z i ;k j k À1¼h X i ;k j k À1;u k ÀÁ95Maneuvering Target Tracking Based on Adaptive Square Root 903^z k j k À1¼1m X m i ¼1Z i ;k j k À1S zz ;k j k À1¼Tria f k j k À1S R ;k ÂÃÀÁP xz ;k =k À1¼v k =k À1v T k =k À1W k ¼P xz ;k j k À1=S T zz ;k j k À1 =S zz ;k j k À1^x k j k ¼^x k j k À1þW k z k À^z k j k À1ÀÁð95:7ÞWhere R k ¼S R ;k S T R ;kf k j k À1¼1ffiffiffiffimp Z 1;k j k À1À^z k j k À1Z 2;k j k À1ÂÀ^z k j k À1ÁÁÁZ 3;k j k À1À^z k j k À1Ãv k j k À1¼1ffiffiffiffimp X 1;k j k À1À^x k j k À1X 2;k j k À1ÂÀ^x k j k À1ÁÁÁX m ;k j k À1À^x k j k À1Ãð95:8Þ95.4Adaptive SCKFMany adaptive filtering methods are proposed to avoid divergence due to inac-curate and time variation of the noise statistics property [3,4].Among them,the Sage-Husa noise estimator is used most extensively [5].However,it should be noted that adaptive filtering algorithms which the Sage-Husa estimator is applied can not estimate process and measurement noise simultaneously.And while the process noise is time variation,the latest data need be centered on,so the sub-optimal modified Sage-Husa estimator is introduced.Accordingly,aim at Maneuvering Target Tracking,an ASCKF which combines the advantages of modified Sage-Husa noise estimator and SCKF can recursively estimate the unknown process noise,and then filter in the nonlinear system.The detailed SCKF algorithm is described as below formulas.Step 1:Initializationx 0j 0;S 0j 0¼chol P 0j 0ÀÁ;^q 0¼q 0;^Q 0¼Q 0ð95:9ÞStep 2:Time updateCompute from (95.5–95.6)on the basis of given S k À1j k À1,^x k À1j k À1,^q k À1and ^Qk À1.Step 3:Measurement updateCompute from (95.7–95.8),the updated state estimate ^x k j k is achieved.904S.Wang and L.Wang^q k ¼1Àd k ðÞ^q k À1þd k À1^x k j k ÀU k ^x k À1j k À1ÂÃ^Q k ¼1Àd k ðÞ^Q k À1þd k W k ~z k ~z T k W T k ÂþP k j k ÀU k P k À1j k À1U T kÃð95:10ÞWhere~z k ¼z k Àh x k =k À1ÀÁ;d k ¼1Àb 1Àb ;0:95\b \0:99ð95:11Þb is a forgetting factor,~z k is predict residual and U k is transition matrix.95.5Numerical Simulations95.5.1Simulation ScenarioConsider Maneuvering Target Tracking scenario where a target executes maneu-vering turn in a horizontal plane at a constant,but unknown turn rate X ,and the process noise is unknown.So the kinematics of the turning motion can be modeled by the following nonlinear process equation [6].Where the state of target isx ¼x _xy _y ½ ;x and y denote position,and _x and _y denote velocities in the x and y directions,respectively;T is the time-interval between two consecutive measurementsx k ¼1sin X T X 0À1Àcos X T X 0cos X T 0Àsin X T 01Àcos X T X 1sin X T X 0sin X T 0cos X T2666437775x k À1þv k ð95:12ÞWhere the process noise with an unknown covariance as below equations.v k $N 0;Q k À1ðÞ;Q k À1¼diag g M g M ½ ;M ¼T 3=3T 2=2T 2=2T!ð95:13ÞThe scalar parameter g is related to process noise intensities.A radar is fixed at the origin of the target and equipped to measure the range r ,and bearing h .Hence,we write the measurement equation.r k h k ¼ffiffiffiffiffiffiffiffiffiffiffiffiffiffix 2k þy 2k p tan À1x k y k"#þw k ð95:14ÞWhere the measurement noise w k $N 0;R ðÞ,with R ¼diag r 2r r 2h ÂÃDataT ¼1s ;X ¼À3 s À1;r r ¼10m ;r h ¼ffiffiffiffiffi10p mrad95Maneuvering Target Tracking Based on Adaptive Square Root 905True initial statex 0¼1;000m300ms À11;000m 0ms À1ÂÃAnd the associated covarianceP 0=0¼diag 100m 210m 2s À2100m 210m 2s À2ÂÃInitial state estimate^x 0=0$N x 0;P 0=0ÀÁQ k is designed to change according toQ k ¼diag ½g 2M g 2M 1 k 30diag ½g 3M g 3M 31 k 70diag ½g 4M g 4M 71 k 1008<:ð95:15ÞWhere g 2¼10;g 3¼40;g 4¼90;M ¼T 3=3T 2=2T 2=2T !95.5.2Simulation Results and Analysis250independent Monte Carlo runs were made,where the prior process noise covariance Q 0¼g 1M g 1M ½ ,and g 1¼0:1m 2s À3for a more accurate simula-tion.All the filters are initialized with the same condition in each run.The total number of scans per run is 100.To compare various nonlinear filters performances,the root-mean square error (RMSE)of the position and velocity is used.RMSE pos k ðÞ¼ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi1N X N n ¼1x n k À^x n k ÀÁ2þy n k À^y n k ÀÁ2 v u u t ð95:16ÞWhere x n k y n kÀÁand ^x n k ^y n k ÀÁare the true and estimated positions at the n-th Monte Carlo run.Similarly to the RMSE in position,the formula of RMSE in velocity is also written.Figure 95.1shows the trajectory of target and Fig.95.2compares the RMSEs in position and velocity,respectively.In addition,the mean of target state estimation RMSE is also shown in Table 95.1.In Fig.95.2,the RMSEs of various filters are compared across 100time steps,and By reason of the large estimation errors caused by unknown and time variant process noise,it can be seen from the Fig.95.2that RMSE of standard SCKF sometimes deviate from the true RMSE very large,especially when the true process noise is much different from the prior knowledge.Note that as expected in906S.Wang and L.Wang95Maneuvering Target Tracking Based on Adaptive Square Root907Table95.1Mean of target state estimation RMSEParameters SCKF ASCKF Unit Position44.996120.1605m Velocity16.234915.8687ms-1908S.Wang and L.Wang Table95.1,the mean of position and velocity estimations RMSE of ASCKF are obviously less than that of standard SCKF.The better performance of the ASCKF in maneuvering target tracking due to the modified Sage-Husa estimator which can estimate the unknown process noises online,whereas the standard SCKF depends on thefixed prior knowledge about the process noises.In new algorithm,the estimated noise statistics are recursively used by the ASCKF to adaptively compensate the influence caused by inaccurate a priori knowledge and changing statistics of system noise.Ultimately,according to the contents mentioned above,it can be concluded that the performance of the ASCKF is superior to standard SCKF in the condition of unknown and time variant process noises.95.6ConclusionsIn this paper,a new ASCKF,which is based on modified Sage-Husa estimator,has been proposed for maneuvering target tracking with unknown and time variant system noise statistics.The simulation results show that the proposedfilter pro-vides better performance in tracking accuracy than the CKF.References1.Arasara J,Haykin S(2009)Cubature Kalmanfilter.IEEE Trans Autom control54(6):1254–12602.Haykin S(2009).Cubaturefilters:new generation of nonlinearfilters that will impact theliterature.Available via internet.http://soma.mcmaster.ca3.Hu C,Chen W,Chen Y,Liu D(2003)Adaptive Kalmanfiltering for vehicle navigation.J Global Positioning Syst2(1):2–474.Mohamed AH,Schwarz KP(1999)Adaptive Kalmanfiltering for INS/GPS.J Geodesy73:193–2035.Sage A,Husa GW(1969)Adaptivefiltering with unknown prior statistics.In:Proceedings ofjoint automatic control conference,USA,pp760–7696.Bar Shalom Y,Li XR,Kirubarajan T(2001)Estimation with applications to tracking andnavigation.Wiley,New York。