Get 20M+ Full-Text Papers For Less Than $1.50/day. Start a 14-Day Trial for You or Your Team.

Learn More →

A Cubature-Principle-Assisted IMM-Adaptive UKF Algorithm for Maneuvering Target Tracking Caused by Sensor Faults

A Cubature-Principle-Assisted IMM-Adaptive UKF Algorithm for Maneuvering Target Tracking Caused... applied sciences Article A Cubature-Principle-Assisted IMM-Adaptive UKF Algorithm for Maneuvering Target Tracking Caused by Sensor Faults 1 ID 1 , 1 , 2 , 1 Huan Zhou , Hui Zhao *, Hanqiao Huang * and Xin Zhao Aeronautics and Astronautics Engineering College, Air Force Engineering University, Xi’an 710038, China; kgy_zhouh@163.com (H.Z.); bravexin@163.com (X.Z.) Astronautics College, Northwestern Polytechnic University, Xi’an 710072, China * Correspondence: zhaohui_kgy@163.com (H.Z.); cnxahhq@126.com (H.H.); Tel.: +86-029-84787589 (H.Z.); +86-029-84787555 (H.H.) Received: 20 July 2017; Accepted: 26 September 2017; Published: 28 September 2017 Featured Application: The work is very effective and applicable in the fields of the maneuvering target tracking and navigations system design. Abstract: Aimed at solving the problem of decreased filtering precision while maneuvering target tracking caused by non-Gaussian distribution and sensor faults, we developed an efficient interacting multiple model–unscented Kalman filter (IMM-UKF) algorithm. By dividing the IMM-UKF into two links, the algorithm introduces the cubature principle to approximate the probability density of the random variable, after the interaction, by considering the external link of IMM-UKF, which constitutes the cubature-principle-assisted IMM method (CPIMM) for solving the non-Gaussian problem, and leads to an adaptive matrix to balance the contribution of the state. The algorithm provides filtering solutions by considering the internal link of IMM-UKF, which is called a new adaptive UKF algorithm (NAUKF) to address sensor faults. The proposed CPIMM-NAUKF is evaluated in a numerical simulation and two practical experiments including one navigation experiment and one maneuvering target tracking experiment. The simulation and experiment results show that the proposed CPIMM-NAUKF has greater filtering precision and faster convergence than the existing IMM-UKF. The proposed algorithm achieves a very good tracking performance, and will be effective and applicable in the field of maneuvering target tracking. Keywords: maneuvering target tracking; interacting multiple model (IMM); unscented Kalman filter (UKF); Gaussian distribution; sensor fault; cubature principle; adaptive matrix gene 1. Introduction With the continuous development of space technology, maneuvering ability in real time is a key factor in completing complicated space missions for maneuvering targets [1–4]. Performing accurate maneuvering target tracking and precise locating are the core issues in the field of space target surveillance [5–8]. For maneuvering target tracking, the single model filter produces large filtering errors, and it is not appropriate for this application because the filtering algorithms highly depend on the motion model of the targets. The interacting multiple model (IMM) method integrates a variety of models to describe the motion rules of the targets, which improves the filtering precision. IMM was proposed by Blom and Barshalom [9], and consists of three processes: interaction, filtering, and fusion. The interaction process is the core difference between IMM and the first generation of multiple model filters, called autonomous multiple model (AMM) filters [10], which connect all the single filters organically to allow Appl. Sci. 2017, 7, 1003; doi:10.3390/app7101003 www.mdpi.com/journal/applsci Appl. Sci. 2017, 7, 1003 2 of 26 the “soft switching” between models. Due to the ability to automatically adjust the bandwidth of the filter, IMM has been widely used in navigation, positioning, signal processing, maneuvering target tracking, and other fields [11,12]. Based on the standard IMM, increasingly improved IMM algorithms have been created to further increase the filtering accuracy, including the Variable Structure Interactive Multiple Model (VSIMM) [13], Adaptive Interactive Multiple Model (AIMM) [14], and the Adaptive Grid Fuzzy Multiple Model (AGFIMM) [15]. After analysis, the above algorithms are common for all dynamic adjustments of the multi-model set and for real-time conversions between multi-model sets, in order to adapt to the real-time signal change and to improve the filter ’s precision. However, when interacting, the weighted sum of multiple random variables will lead to multiple peaks in the probability density function of the system state, so the sum will not conform to the pure Gaussian distribution [16,17]. As a result, the filtering precision is negatively influenced for the non-Gaussian distribution. Two methods are used to address the problem. The Gaussian distribution can be used to approximate the non-Gaussian distribution [18]. For example, Lainiotis uses the mean square error (MSE) to estimate the error covariance of the interacting random variable [19], but the estimated value is too conservative. Because the MSE estimation results are used in the filter iteration, it artificially increases the system noise in the filtering process, which makes the calculation value of gain matrix too large. Therefore, the filtering precision decreases, especially with a large measured value of noise [20]. The second method is to filter the results of the interaction by using non-Gaussian filters [21]. For instance, the IMM-PF filtering method [22,23] can be used, but the particle filter (PF) causes problems, such as particle degradation and poor real-time performance, in engineering applications [24,25]. The unscented Kalman filter (UKF) is the most widely used algorithm with a single filter in the IMM [26–32], and has many advantages compared to the Kalman Filter (KF), Extended Kalman Filter (EKF) [33,34], and PF. However, the tracking accuracy decreases significantly when sensor faults occur, because model mismatches are produced when the sensor receives measurements with biases [35–37]. Generally, the sensor faults include four varieties: complete failure, fixed deviation, drift deviation, and accuracy decrease. The second and third faults are considered in this research. The current existing adaptive UKF has been proven to be effective in dealing with unknown noise, but no effective adaptive UKF algorithms exist for sensor faults and model mismatches [38,39]. Recently, Arasaratnam et al. [40] proposed the cubature Kalman filter (CKF), based on the cubature principle, and provided rigorous mathematical proof. The cubature principle is the basis of CKF, and it computes the first and second order moments of the finite cubature points through nonlinear propagation, and then estimates the posterior probability density of the nonlinear system. As a typical deterministic sampling filtering algorithm, CKF has fewer sampling points and better numerical stability [41]. As the mixed probability of IMM interaction varies with time, the interaction process is described by nonlinear equations, and the cubature principle in CKF is used to estimate the probability density function of a random variable after the interaction. To address the single model filtering precision problem, a new adaptive matrix gene is used in UKF together with the IMM method, to balance the contribution of the state and filtering results in the presence of sensor faults. This paper develops a new efficient IMM-UKF algorithm for maneuvering target tracking, which addresses the non-Gaussian distribution problem and sensor faults. The IMM-UKF filtering problem is described for maneuvering target tracking with the occurrence of sensor faults. For IMM-UKF external links, the cubature principle (CP) is introduced to IMM to approximate the probability density of the random variable in the interaction, and the precision of the approximation is analyzed, which contributes to CP-assisted IMM (CPIMM). For IMM-UKF internal links, an adaptive matrix gene is integrated into traditional UKF to balance the contribution of the system state and provides filtering solutions, which is called a new adaptive UKF (NAUKF). Simulation and experiment results showed that the proposed cubature principle assisted IMM–adaptive UKF algorithm (CPIMM-NAUKF) has Appl. Sci. 2017, 7, 1003 3 of 26 Appl. Sci. 2017, 7, 1003 3 of 26 algorithm (CPIMM-NAUKF) has high filtering precision and fast convergence, and achieves better tracking performance than the existing IMM-UKF. high filtering precision and fast convergence, and achieves better tracking performance than the The remainder of this paper is organized as follows. The IMM-UKF filtering problem is existing IMM-UKF. described in Section 2. The CPIMM-NAUKF is developed in Section 3. Simulations and practical The remainder of this paper is organized as follows. The IMM-UKF filtering problem is described experiments comparing performance are conducted in Section 4. Finally, the conclusions are in Section 2. The CPIMM-NAUKF is developed in Section 3. Simulations and practical experiments provided in Section 5. comparing performance are conducted in Section 4. Finally, the conclusions are provided in Section 5. 2. Description of the IMM-UKF Filtering Problem 2. Description of the IMM-UKF Filtering Problem 2.1. Maneuvering Target Tracking Dynamic System 2.1. Maneuvering Target Tracking Dynamic System The general IMM-UKF structure diagram, including m models, is shown in Figure 1, where The general IMM-UKF structure diagram, including m models, is shown in Figure 1, where Z represents the unit delay link. The IMM-UKF can be divided to two parts: the external link-IMM represents the unit delay link. The IMM-UKF can be divided to two parts: the external link-IMM structure and the internal link-UKF filter. structure and the internal link-UKF filter. 1,k −1 o 1,k 1,k −1 1,k 1,k −1 P ˆ 1,k −1 X fusion ,k 2,k −1 fusion ,k 2,k −1 o 2,k 2,k −1 2,k 2,k −1 mk,1 − mk,1 − mk , mk,1 − mk , mk,1 − Figure 1. General interacting multiple model-unscented Kalman filter (IMM-UKF) structure. Figure 1. General interacting multiple model-unscented Kalman filter (IMM-UKF) structure. In Figure 1, X and P are the state estimation and covariance matrix, respectively, of the single In Figure 1, i,kX an i,d k P are the state estimation and covariance matrix, respectively, of the ik , ik , UKF filter i (i = 1, 2, . . . , m) at time step k; X and P are the corresponding vector at time i,k1 i,k1 i (1im = ,2,, ) X single UKF fi o lter at time step k ; and P are the corresponding vector o ik,1 − ik,1 − ˆ ˆ step k 1; X and P are the interaction result of X and P , and they are the inputs of i,k1 i,k1 i,k1 i,k1 o o ˆ ˆ at time step ˆ ; X and P are the interaction result of X and P , and they are the k −1 UKF filter i; X and P are the final state estimation and covariance of the IMM-UKF filter, ik,1 − ik,1 − ik,1 − ik,1 − f usion,k f usion,k respectively; and Z denotes the measurement vector for the single UKF filter. inputs of UKF filter i ; X and P are the final state estimation and covariance of the fusion,k fusion,k For single model i, the state and measurement equations of the maneuvering target are [1,4] IMM-UKF filter, respectively; and Z denotes the measurement vector for the single UKF filter. For single model i , the state and measurement equations of the maneuvering target are [1,4] X = f[X , k 1] + G W k k1 k1 k1 (1) Z = h[X , k] + V k Xf = k[,X k k −1]+Γ W kk−− 11k k−1 (1) Zh = [,X k]+V kk k where the subscript i is omitted for the sake of brevity. where the subscript i is omitted for the sake of brevity. Appl. Sci. 2017, 7, 1003 4 of 26 Similarly, the state and measurement equations of the maneuvering target in the presence of sensor faults are [35,37] X = f[X , k 1] + G W k k1 k1 k1 (2) Z = h[X , k] + V + dh k k k out,k where f[] and G represent the state transfer function and interference transfer matrix, respectively; k1 W and V denote the system and measurement noise, respectively, and they are both white Gaussian k1 k noises and mutually uncorrelated; h[] is a nonlinear coordinate transformation function; and dh out,k denotes the model mismatches caused by sensor faults. 2.2. Standard IMM From Figure 1, the standard IMM method can be described in the three following steps [11]: Step 1: Model probability updating. The mixing probability is defined and calculated as u = p u , i, j = 1, 2, . . . , m (3) i j,k1 i j i,k1 where p is the transition probability; u is the correct probability of model i, i = 1, 2, . . . , m at i j i,k1 time step k 1; and c is the prediction probability of model j, j = 1, 2, . . . , m from time step k 1 to time step k [11] which provides c = p u , i, j = 1, 2, . . . , m. (4) j å i j i,k1 i=1 Therefore, we can obtain the updated model probability: u = L c (5) j,k j,k j e e where a = å L c ; L = N(Z, P ); Z represents the innovation residual vector of UKF filter j; j,k j j,k ee ZZ j=1 and P represents the covariance matrix of Z. ee ZZ Step 2: Input interaction. ˆ ˆ X = u X (6) j,k1 å i j,k1 i,k1 i=1 o T e e P = u [P + X X ] (7) å i j,k1 i j,k1 i j,k1 j,k1 i,k1 i=1 e ˆ ˆ where X = X X . i j,k1 i,k1 j,k1 Step 3: Resultant mean and covariance matrix computation. ˆ ˆ X = X u (8) f usion,k å j,k j,k j=1 h ih i ˆ ˆ ˆ ˆ P = u P + X X X X . (9) f usion,k å j,k j,k k j,k k j,k j=1 Appl. Sci. 2017, 7, 1003 5 of 26 2.3. Non-Gaussian Distribution and Sensor Fault Problems for IMM-UKF Appl. Sci. 2017, 7, 1003 5 of 26 2.3.1. Non-Gaussian Distribution 2.3. Non-Gaussian Distribution and Sensor Fault Problems for IMM-UKF For the external IMM-UKF links, X is a weighted sum of m Gaussian random variables jk , 2.3.1. Non-Gaussian Distribution and no longer obeys the pure Gaussian distribution because many peaks will exist in the probability density, which is similar to the probability density of the mixed Gaussian distribution For the external IMM-UKF links, X is a weighted sum of m Gaussian random variables and no j,k [19]. The case when X is scalar and u is constant is explained in this section. It is assumed longer obeys the pure Gaussian distribution because many peaks will exist in the probability density, ik,1 − ik,1 − which is similar to the probability density of the mixed Gaussian distribution [19]. The case when X i,k1 that X is the weighted sum of three Gaussian random variables with normal distribution, and jk,1 − is scalar and u is constant is explained in this section. It is assumed that X is the weighted sum o i,k1 o j,k1 ˆˆ ˆ S is the square root of P , where [,XX ,X ] =−[ 3,0,5] , [,SS ,S ] =[3,2.25,1.8] , o o jk,1 − jk,1 − 12 3 12 3 of three Gaussian random variables with normal distribution, and S is the square root of P , j,k1 j,k1 [uu , ,u ] = [0.3, 0.4, 0.3] . The probability density of the mixed Gaussian distribution is shown in ˆ ˆ ˆ 12 3 where [X , X , X ] = [3, 0, 5], [S , S , S ] = [3, 2.25, 1.8], [u , u , u ] = [0.3, 0.4, 0.3]. The probability 1 2 3 1 2 3 1 2 3 Figure 2. density of the mixed Gaussian distribution is shown in Figure 2. Figure 2. The probability density of the mixed Gaussian distribution. Figure 2. The probability density of the mixed Gaussian distribution. ˆ o In Figure 2, the thick line represents X and has multiple peaks; the mixture no longer follows In Figure 2, the thick line represents X and has multiple peaks; the mixture no longer j,k1 jk,1 − Gaussian distribution. follows Gaussian distribution. After the interaction, Lainiotis completes the approximation of the probability density through After the interaction, Lainiotis completes the approximation of the probability density through the Gaussian distribution [19]: the Gaussian distribution [19]: n o ˆ ˆ ˆ ˆˆ ˆ Pr X = u N X : X , S (S ) (10) PrXX = uN :X ,SS å i,k1 i,k1 i,k1() i,k1 j,k() 1 (10) j,1 k−−  ik,1 { ik,1− ik,1− ik,1− } i=1 i =1 where S is the square root of P . where S is the square root of P . ik,1 − ik,1 − i,k1 i,k1 On the basis of the above distribution, MSE estimates the covariance matrix of the interacting On the basis of the above distribution, MSE estimates the covariance matrix of the interacting random variables, where the mean and covariance matrix are computed by [16] random variables, where the mean and covariance matrix are computed by [16] n o m m o R R o o o o T o oo o o o o ˆ ˆ ˆˆ ˆ ˆ ˆ ˆˆ ˆˆ ˆˆ ˆˆ X = X Pr X dX = å u X N X : X , S (S ) dX XX== PrX du X X NX :X ,S S dX j,k1 j,k1 j,k1 j,k1 i,k1 j,k1 i,k1 i,k1() i,k1 j,k1 ()  jk,1−− jk,1 jk,1− jk,1− i,1 k− jk,1− { i,k−1 i,k−1 i,k−1 } jk,1−  i=1 i =1 (11) (11) m ˆ = u X i,k1 i,k1 = u X i=1  ik,1−− i,k 1 i =1 Appl. Sci. 2017, 7, 1003 6 of 26 o o MSE T ˆ ˆ ˆ ˆ P = u S S + (X X )(X X ) . (12) å i,k1 i,k1 i,k1 j,k1 i,k1 j,k1 j,k1 i,k1 i=1 MSE actual From Equations (11) and (12), we know P is larger than the actual covariance of X (P ). j,k1 j,k1 j,k1 An example of the above one-dimensional case is available, and the Monte Carlo method (MC) is used with 10,000 random sampling points to compute the mean and covariance transferred. The results are shown in Table 1. Table 1. Calculation results of the comparison of the mean and covariance matrix after interaction; MSE: the mean square error; MC: Monte Carlo. Method Mean Covariance MSE 0.6000 3.9417 MC 0.5937 1.3906 MSE actual MSE From Table 1, P > P is verified. Because P will participate in the filtering iteration, j,k1 j,k1 j,k1 it is equivalent to an increase of the system noise in the process of filtering. Then, the gain matrix will be larger, and the filtering effect declines, especially when the actual system noise is small and the measurement noise is large. Therefore, for the external IMM-UKF link, the filtering problem to be solved is how to make a precise estimate of the mean and covariance matrix of the random variables after the interaction for IMM. 2.3.2. Sensor Faults When sensor faults exist, the third-order accuracy condition is no longer met for the standard UKF [42]. Therefore, for the internal IMM-UKF link, the filtering problem to be solved is how to design an effective adaptive UKF algorithm for systems given by Equation (2), when the sensor fault dh exists. out,k 3. Cubature-Principle-Assisted IMM-Adaptive UKF Algorithm (CPIMM-NAUKF) By considering the internal and external link of IMM-UKF respectively, the cubature principle is introduced to the standard IMM, and an adaptive gene matrix is brought into the standard UKF. For IMM, the most important factor on influencing the filtering precision is the non-Gaussian probability density function after the mixing interaction. It is the key to estimating the first moment and second moment of the random variables with high accuracy. For adaptive UKF, one effective method is to balance contribution to UKF filtering solutions of the state information, measurement information, and innovation information. 3.1. CPIMM 3.1.1. Cubature Principle Assuming that c  N c : c ˆ , SS , where the dimension of c is l 1, then the first moment and second moment of any function y = f (c) can be solved by the following integral form: y ˆ = E(y) = f (c)N c : c ˆ , SS dc R (13) P = D(y) = f (c) f (c)N c : c ˆ , SS dc. Equation (13) is a multidimensional integral problem, and it is difficult to solve through analytic methods. The cubature principle states that a cubature point with a weighted value can be chosen to approximate the integral of the multidimensional function, and the method of choosing the cubature point is: Appl. Sci. 2017, 7, 1003 7 of 26 c = c ˆ  Sx , r = 1, 2, . . . , l (14) r r x = le , r = 1, 2, . . . , l (15) r r where e is the rth column of the unit matrix I . ll 3.1.2. Estimating the Interaction by Using CP Since the mixing probability varies with time, the interactive process is abstracted as a nonlinear function, and the cubature principle is used to approximate the probability distribution of the interacting random variables. Firstly, the nonlinear function is used to describe the interaction process. The augmented system c = [x , . . . , x ] is constructed, where x  N x : x ˆ , S S , i = 1, . . . , m is a random variable with 1 i i i the n 1 dimension, and represents the state vector of the model i; l = m n; c ˆ = [x ˆ , . . . , x ˆ ] . 1 m Assuming that x is independent, and we have cov(x ˆ , x ˆ ) = 0, the square root of the covariance i i j i6=j matrix of c is S = diag([S , . . . , S ]), and we have c  N c : c ˆ , SS . 1 m The next step is choosing cubature points for nonlinear propagation. The cubature points are selected by Equation (13), and S is rewritten as S = s V, where V is the upper triangular matrix with all the diagonal elements being 1; s = S(r, r) is the value of the element on the diagonal of S, and the cubature points can be written as c = c ˆ  ls v , r = 1, 2, . . . , l (16) r r r where v is the rth column of V. The cubature points are calculated after the interaction by y = f (c ). (17) Finally, according to the cubature principle, the mean and covariance matrix of y are obtained as y ˆ  w y (18) å r r=l P  w y y ˆ)(y y ˆ) (19) y å r r r r=l where r 6= 0; w = 1/2l. For the external IMM, X shown by Equation (6) can be recalculated by Equation (18), j,k1 and P shown by Equation (7) can be recalculated by Equation (19); then, the newly obtained j,k1 X and P are used in the iteration of the IMM filter, which leads to CP-IMM. j,k1 j,k1 3.1.3. Accuracy Analysis of the CPIMM In the literature [22], the approximate precision of the mean of the Gaussian random variable after nonlinear propagation, by using the cubature principle, has been analyzed. In this section, the accuracy of the covariance matrix of the cubature principle will be analyzed. l n Assume that f (c) 2 R is the nonlinear mapping from R to R ; c  N(c : c ˆ , P); Dc  N(Dc : 0, P). If the Taylor series expansion of y = f (c) is carried out at c ˆ with two derivatives, we have h i 0 T y = f (c ˆ + Dc) = f (c ˆ) + f (c ˆ)Dc + Dc f (c ˆ)Dc (20) 2 i Appl. Sci. 2017, 7, 1003 8 of 26 where f (c ˆ) is the Jacobian matrix with a n l dimension; f (c ˆ) represents n matrixes with n l th dimensions, which are called Hessian matrixes; f (c ˆ) is the i Hessian matrix. Thus, we have ¶ f ¶ f ¶ f i i i f (c ˆ) = , . .., ¶x ¶x ¶x 1 2 l c=c ˆ 2 3 2 2 2 ¶ f ¶ f ¶ f i i i . . . 6 7 ¶x ¶x ¶x ¶x ¶x ¶x 6 1 1 1 2 1 7 6 7 2 2 2 6 7 ¶ f ¶ f ¶ f i i i 6 7 . (21) . . . 6 7 00 ¶x ¶x ¶x ¶x ¶x ¶x 2 1 2 2 2 l 6 7 f (c ˆ) = i 6 7 6 7 . . . . . . . . . . . . 6 7 6 7 6 7 2 2 2 ¶ f ¶ f ¶ f i i i 4 5 . . . ¶x ¶x ¶x ¶x ¶x ¶x l 1 l 2 l l c=c ˆ Calculating the first and second moments of Equation (20), we obtain E(y) = f (c ˆ) + tr( f (c ˆ)P 2 i i h i (22) T 00 00 0 0 1 D(y) = f (c ˆ)P( f (c ˆ)) + tr( f (c ˆ)P f (c ˆ)P) i j i,j where tr() represents the trace of a matrix. Equation (18) can be rewritten as y ˆ = f (c ˆ) + ( f (c ) 2 f (c ˆ) + f (c )). (23) r r 2l r=1 Substituting Equation (23) to (19), and assuming D = ( f (c ) 2 f (c ˆ) + f (c )), we have r r r=1 2l 2l 1 r 1 r T P = ( f (c ) f (c ˆ))() ( f (c ) f (c ˆ))D å å y 2 2l 4l 1 1 2l 1 r 1 T å ( f (c ) f (c ˆ)) D + DD (24) 2 2 4l 4l 1 r 1 T = ( f (c ) f (c ˆ))() DD . 2l 4l r=l When Dc = ls v ! 0 is correct by considering the selection process of cubature points shown r r r in Equation (17), we have f (c ) f (c ˆ) ! f (c ˆ)u ls (25) r r+l f (c ) 2 f (c ˆ) + f (c ) ! [u f (c ˆ)u ] . 2 i ls Substituting Equation (25) into Equations (23) and (24), respectively, we obtain y ˆ ! f (c ˆ) + tr( f (c ˆ)P i i (26) h i T 00 00 0 0 P ! f (c ˆ)P( f (c ˆ)) + tr( f (c ˆ)P f (c ˆ)P) . i j 2 i,j Appl. Sci. 2017, 7, 1003 9 of 26 Equations (22) and (26) show that the mean and covariance estimates can achieve second-order accuracy of a Taylor series expansion using the cubature point to create a random variable through the nonlinear function. The one-dimensional case in Section 2.3 is used to illustrate that the mean value obtained by the cubature principle is 0.6, and the covariance is 1.3826, which is much closer to the results obtained by Monte Carlo method than the MSE method. 3.2. New Adaptive Unscented Kalman Filter (NAUKF) Motivated by the proposed UKF algorithm, we propose a new adaptive UKF (NAUKF) consisting of an adaptive matrix gene. P , P and P represent the corresponding covariance k/k1 (XZ)k/k1 (ZZ)k/k1 e e e computed by the standard UKF, and they are changed into P , P and P in k/k1 (XZ)k/k1 (ZZ)k/k1 the NAUKF. Theorem 1. On the basis of standard UKF, set up a new adaptive matrix geneD as ( ) 2n (c) (i) (i) ˆ ˆ D = P W [g Z ][g Z ] R . (27) k ee å k/k1 k/k1 k (ZZ)k/k1 i k/k1 k/k1 i=0 Let 2n (c) (i) (i) ˆ ˆ P = W [g Z ][g Z ] + D R (28) å k/k1 k/k1 k k (ZZ)k/k1 i k/k1 k/k1 i=0 (i) (c) where g is calculated from the Sigma points in the standard UKF; Z is the innovation; W denotes k/k1 k/k1 i e ˆ e the weighted coefficient [3]; Z = Z Z ; and P is the covariance of Z ; Then the k k/k1 ee k/k1 k/k1 (ZZ)k/k1 NAUKF can address the filtering problem in the presence of sensor faults shown by Equation (2). Proof. Because model mismatches exist in the system shown by Equation (2), based on Theorem 1, the result is P = P = P (29) k/k1 e e k/k1 (XX)k/k1 P = P = P (30) ee (XZ)k/k1 (XZ)k/k1 (XZ)k/k1 e ˆ where X = X X . k/k1 k k/k1 After setting up D shown by Equation (27), we have P = P = P . (31) ee (ZZ)k/k1 (ZZ)k/k1 (ZZ)k/k1 From Equations (29) to (31), we know that the third-order precision conditions can be satisfied, which means that the NAUKF can be used for the system that has sensor faults. This completes the proof. To avoid the divergence problem of NAUKF, a covariance matching criterion is used: e e tr(Z Z )  Y tr(P ) (32) k/k1 k/k1 (ZZ)k/k1 where Y (Y  1) is a preset adjustable coefficient. If Equation (32) is satisfied, the NAUKF has been convergent; P should be updated with an k/k1 adaptive weighted coefficient z by increasing the contribution of Z on the filtering solutions. k k 2n (c) (i) (i) ˆ ˆ P = z W [c X ][c X ] + Q (33) k/k1 kå k/k1 k/k1 k1 i k/k1 k/k1 i=0 where z is set by k Appl. Sci. 2017, 7, 1003 10 of 26 z z  1 0 0 z = (34) Appl. Sci. 2017, 7, 1003 1 z < 1 10 of 26 tr(P R ) ee k tr(PR − ) (ZZ)k/k1  k () ZZ k/ k −1 z = . (35) 0 ζ = 0 2n 2n T (c) (i) (i) (ci ) () ()i T (35) ˆ ˆ ˆ ˆ tr( W [c X ][c X ] ) å tr( W [χ −− X ][χ X ] )  k/k1 k/k1 i i k/kk1/1kk−− /k 1 k k/1 /kk k−− 1 /k 1 i=0 i = 0 3.3. Implementation Steps of the CPIMM-NAUKF Algorithm 3.3. Implementation Steps of the CPIMM-NAUKF Algorithm Based on Sections 3.1 and 3.2, the CPIMM-NAUKF structure is shown in Figure 3. The Based on Sections 3.1 and 3.2, the CPIMM-NAUKF structure is shown in Figure 3. implementation steps of the CPIMM-NAUKF are as follows. The implementation steps of the CPIMM-NAUKF are as follows. Step 1: NAUKF filtering process. Step 1: NAUKF filtering process. First, calculate the covariance by setting X and P of the standard UKF, and compute First, calculate the covariance by setting X and P of the standard UKF, and compute 0 0 00 P , P , P . Next, select the new adaptive gene selection by setting upΔD through PP,,P . Next, select the new adaptive gene selection by setting up through k/k1 (XZ)k/k1 (ZZ)k/k1 k k/1 k−− (XZ)kk/1 (ZZ)kk/1− k Equation (27), and recalculate P with Equation (28) (ZZ)k/k1 Equation (27), and recalculate P with Equation (28) () ZZk/k −1 2n 2n (c) (ci ()i) () ()i (i) T ˆˆ ˆ ˆ P =− W[] γ Z [] γ −Z +Δ R P = W [g Z ][g Z ] . + D R . (36)  (36) (ZZ)k/k1(ZZ)k/1 å k−− i kk/1 kk/1 /k− k1 kk/1− k/1 k− k/k k 1k k k i k/k1 k/k1 i =0 i=0 1,k −1 o 1,k 1,k −1 1,k 1,k −1 P ˆ 1,k −1 X fusion,k 2,k −1 fusion,k 2,k −1 2,k 2,k −1 o P 2,k 2,k −1 mk,1 − mk,1 − o mk , mk,1 − mk , mk,1 − Figure 3. The cubature principle assisted IMM-adaptive UKF algorithm (CPIMM-NAUKF) structure. Figure 3. The cubature principle assisted IMM-adaptive UKF algorithm (CPIMM-NAUKF) structure. To determine the convergence strategy, judge the divergence according to Equation (32). If it is satisfied, To determi go directly ne the convergence stra to the next step; otherwise, tegy, judge the di update Pvergence by using accord Equations ing to Equ (33)–(35). ation (32). If it is k/k1 satisfi Next, ed, go todi obtain rectly to the next step; ot the NUKF filteringherwi results, se, use upda the te following: P by using Equations (33)–(35). kk/1 − Next, to obtain the NUKF filtering results, use the following: K = P (P ) (37) (XZ)k/k1 (ZZ)k/k1 −1 KP = () P (37) k (XZk) /1 k−− (ZZk) /1 k ˆˆ ˆ XX=+K() Z−Z (38) kk/1k−− k k k/1k PP=−KP K (39) kk/1k−− k (ZZ)k/1k k Appl. Sci. 2017, 7, 1003 11 of 26 ˆ ˆ ˆ X = X + K (Z Z ) (38) k k/k1 k k k/k1 P = P K P K (39) k k/k1 k (ZZ)k/k1 k ˆ ˆ ˆ From Equations (38) and (39), obtain the X , P , X , P , . . . , and X , P for UKF filters 1 1,k 1,k 2,k 2,k m,k m,k to m of IMM-UKF. Step 2: The CPIMM filtering process. First, compute the mixing probability. Calculate the mixing probability u according to ij,k1 ˆ ˆ ˆ Equation (3). Next, determine the Interaction by using CP. After X , P , X , P , . . . , and X , P 1,k 1,k 2,k 2,k m,k m,k are obtained based on Step 1, calculate X and P , respectively, with Equations (18) and (19). j,k1 j,k1 Then update the model probability by calculating the mixing probability u according to Equation (5). j,k1 To complete the fusion, calculate resultant mean and covariance matrix with Equations (8) and (9). 4. Experimental Results and Discussion To verify the effectiveness of the maneuvering target tracking of the proposed CPIMM-NAUKF, numerical simulations and practical experiments are both conducted. For the numerical simulation, two sets of experiments were simulated: (1) the standard IMM [9] and CPIMM algorithms; (2) the IMM-UKF [43], IMM-NAUKF connecting the standard IMM with NAUKF, CPIMM-UKF connecting the CPIMM with standard UKF, and CPIMM-NAUKF algorithms. The experimental environment was set as Intel i7 computer with 4-core, 64-bit, 2.4 GHz, 8 GB RAM, and MATLAB R2013a software. For the practical experiments, an integrated navigation system and a synthetic aperture radar (SAR) tracking system using the CPIMM-NAUKF algorithm are, respectively, shown to test the proposed method with real data. 4.1. Numerical Simulations and Analysis . . To design the simulation scenarios, suppose that X = [x, y, x, y] represents the state of the maneuvering target, including the position and velocity. The sensor is the radar, and its position is fixed at the coordinate origin. Thus, the measurement equation is " # " # 2 2 r (x + y ) k k k Z = = + V (40) k k q tan (y /x ) k k k where r and q are the distance and angle measurement components of the radar, respectively. k k The high-speed maneuvering target moves in a two-dimensional scenario with 100 s; the initial position is [0 m, 400 m], and the initial velocity is [10 m/s, 40 m/s]; the target moves at a constant velocity in a straight line from 0 s–20 s, with a constant right turn rate being 0.1 rad/s from 20 s–40 s, constant velocity in a straight line at 40 s–60 s, constant left turn with the turn rate being 0.1 rad/s at 60 s–80 s, and a constant velocity in a straight line at 80 s–100 s. In a Cartesian coordinate system, the true trajectory of the maneuvering target is depicted in Figure 4. Appl. Sci. 2017, 7, 1003 12 of 26 Appl. Sci. 2017, 7, 1003 12 of 26 Destination point Start point 0 200 400 600 800 1000 1200 1400 1600 1800 2000 X/(m) Figure 4. True trajectory of the maneuvering target. Figure 4. True trajectory of the maneuvering target. Both the IMM and CPIMM include three models: one Constant Velocity (CV) model [44] and Both the IMM and CPIMM include three models: one Constant Velocity (CV) model [44] and two two Coordinated Turn (CT) models [44]. Coordinated Turn (CT) models [44]. For the CV model, the state transition matrix is For the CV model, the state transition matrix is 10 T 0   2 3   1 T 0 0 01 0 0   6 7 F = (41) CV 0 1 0 0 6 7 00 1 T F = (41) 6 7 CV   4 0 0 1 T 5 000 1   0 0 0 1 where T is the sampling period, and T = 1 s. where T is the sampling period, and T = 1 s. The process noise is calculated with The process noise is calculated with V = N (.; 0;σ ) (42) CV CV V = N(.; 0; s ) (42) CV CV 2 2 where σ denotes the covariance of the process noise, and σ = 5 . CV CV 2 2 For the CT model, the state transition matrix is where s denotes the covariance of the process noise, and s = 5. CV CV For the CT model, the state transition matrix is sin(ωω TT ) 1 − cos( )  10 −  2 3 ωω sin(wT) 1cos(wT)  1 0 w w 0 cos()ωω TT 0 −sin() 6 7  0 cos(wT) 0 sin(wT) 6 7 F = (43) CT  F = 6 7 (43) CT 1 −1c os( cosωω (wTT T )) sin sin( (wT) ) 4 5 0 1  01 w w ωω  0 sin(wT) 0 cos(wT)  0sin(ωω TT ) 0 cos( )  where w is the turn rate. where ω is the turn rate. The process noise meets with The process noise meets with V = N(.; 0; s ) (44) CT CT V = N (.; 0;σ ) (44) CT CT 2 2 where s denotes the covariance of the process noise, and s = 20. CT CT 2 2 The two models use the following transition and mode probabilities. where σ denotes the covariance of the process noise, and σ = 20 . CT CT The transition probability matrix is The two models use the following transition and mode probabilities. The transition probability matrix is Y/(m) Appl. Sci. 2017, 7, 1003 13 of 26 2 3 0.99 0.005 0.005 6 7 0.005 0.99 0.005 . 4 5 0.005 0.005 0.99 The initial mode probability matrix is [ 0.99 0.005 0.005]. 2 2 The measurement covariance is R = diag s , s = diag(15, 15). w w Case 1 is where the sensor has no faults and is in a normal work situation. Case 2 is when the sensor has faults, and dh = [dr dq ] , where dr and dq represents the distance deviation within out,k k k k k [5 m, 5 m] and the angle drift rate within [0 1.0 10 rad/s], respectively. Every simulation experiment is tested 100 times using Monte Carlo, and algorithms of two sets of comparison simulations are evaluated by root mean square error (RMSE), which is computed through i i i i ˆ ˆ E = X X )(X X (45) i=1 i th where X and X are the estimated and true state in the i simulation, respectively; N is the number of simulations; N = 100. 4.1.1. CPIMM Simulation To validate the effectiveness of CPIMM, the first set of comparisons, including the standard IMM and CPIMM, was simulated. We assumed there were no sensor faults, similar to the Simulation Case 1 in the scenario design section. Figure 5 gives the true and estimated trajectories of the maneuvering target. As seen in Figure 5, both CPIMM and IMM can track the trajectory of the target. Appl. Sci. 2017, 7, 1003 14 of 26 True IMM Destination point CPIMM Start point 0 200 400 600 800 1000 1200 1400 1600 1800 2000 X/(m) Figure 5. True and estimated trajectories of the maneuvering target for CPIMM for Case 1. Figure 5. True and estimated trajectories of the maneuvering target for CPIMM for Case 1. IMM CPIMM 0 10 20 30 40 50 60 70 80 90 100 Time/(sec) Figure 6. Position root mean square error (RMSE) comparison for CPIMM for Case 1. Y/(m) Position RMSE/(m/s) Appl. Sci. 2017, 7, 1003 14 of 26 True IMM Destination point CPIMM Appl. Sci. 2017, 7, 1003 14 of 26 Start point The comparisons of the RMSE position and velocity curves of the two filtering algorithms are shown in Figur 200 es 6 and 7, and the corresponding statistical data of RMSE are listed in Table 2. The filtering precision of the IMM is lower than the CPIMM, although the IMM is not divergent. The CPIMM greatly increases the filtering precision compared with IMM. In addition, this means that 0 200 400 600 800 1000 1200 1400 1600 1800 2000 the tracking performance of CPIMM is higher than X/that (m) of the IMM, because the cubature principle is used to increase the estimation values in the interaction process of the IMM. Figure 5. True and estimated trajectories of the maneuvering target for CPIMM for Case 1. IMM CPIMM 0 10 20 30 40 50 60 70 80 90 100 Time/(sec) Figure 6. Position root mean square error (RMSE) comparison for CPIMM for Case 1. Appl. Sci. 2017, 7, 1003 15 of 26 Figure 6. Position root mean square error (RMSE) comparison for CPIMM for Case 1. IMM CPIMM 0 10 20 30 40 50 60 70 80 90 100 Time/(sec) Figure 7. Velocity RMSE comparison for CPIMM for Case 1. Figure 7. Velocity RMSE comparison for CPIMM for Case 1. Table 2. Performance comparison of the algorithms for CPIMM for Case 1. Position Error (m) Velocity Error (m/s) Algorithm Mean Standard Deviation Mean Standard Deviation IMM 3.8245 2.0377 0.7102 0.6935 CPIMM 2.0630 1.1604 0.4596 0.4187 4.1.2. CPIMM-NAUKF Simulation To validate the effectiveness of CPIMM-NAUKF, the second set of comparisons, including the IMM-UKF, IMM-NAUKF, CPIMM-UKF, and CPIMM-NAUKF were simulated, assuming there are sensor faults, similar to Simulation Case 2 in the scenario design section. Figure 8 gives the true and estimated trajectories of the maneuvering target. Both CPIMM and IMM can track the trajectory of the target, but the estimated trajectory of CPIMM-NAUKF was closest to the true trajectory. Y/(m) Position RMSE/(m/s) Velocity RMSE/(m/s) Appl. Sci. 2017, 7, 1003 15 of 26 Table 2. Performance comparison of the algorithms for CPIMM for Case 1. Position Error (m) Velocity Error (m/s) Algorithm Mean Standard Deviation Mean Standard Deviation IMM 3.8245 2.0377 0.7102 0.6935 CPIMM 2.0630 1.1604 0.4596 0.4187 4.1.2. CPIMM-NAUKF Simulation To validate the effectiveness of CPIMM-NAUKF, the second set of comparisons, including the IMM-UKF, IMM-NAUKF, CPIMM-UKF, and CPIMM-NAUKF were simulated, assuming there are sensor faults, similar to Simulation Case 2 in the scenario design section. Figure 8 gives the true and estimated trajectories of the maneuvering target. Both CPIMM and IMM can track the trajectory of the target, but the estimated trajectory of CPIMM-NAUKF was closest to the true trajectory. Appl. Sci. 2017, 7, 1003 16 of 26 True IMM-UKF Destination point IMM-NAUKF CPIMM-UKF CPIMM-NAUKF Start point 0 200 400 600 800 1000 1200 1400 1600 1800 2000 X/(m) Figure 8. True and estimated trajectories of the maneuvering target for CPIMM-NAUKF for Case 2. Figure 8. True and estimated trajectories of the maneuvering target for CPIMM-NAUKF for Case 2. The comparisons of the RMSE position and velocity curves of four filtering algorithms are The comparisons of the RMSE position and velocity curves of four filtering algorithms are shown in Figures 9 and 10, and the corresponding statistical data of RMSE are listed in Table 3. The shown in Figures 9 and 10, and the corresponding statistical data of RMSE are listed in Table 3. sensor faults negatively influenced the filtering precision of the UKF and the filtering error of the The sensor faults negatively influenced the filtering precision of the UKF and the filtering error of standard IMM-UKF was the largest. The IMM-NAUKF has better filtering accuracy than the the standard IMM-UKF was the largest. The IMM-NAUKF has better filtering accuracy than the IMM-UKF, because the internal link NAUKF effectively handled the sensor fault problem, but the IMM-UKF, because the internal link NAUKF effectively handled the sensor fault problem, but the filtering convergence performance was poor. The CPIMM-UKF also has better filtering accuracy filtering convergence performance was poor. The CPIMM-UKF also has better filtering accuracy than than the IMM-UKF because the internal link CPIMM has a better effect for maneuvering target the IMM-UKF because the internal link CPIMM has a better effect for maneuvering target tracking tracking than IMM, but the filtering accuracy relationship between IMM-NAUKF and CPIMM-UKF than IMM, but the filtering accuracy relationship between IMM-NAUKF and CPIMM-UKF does not does not exist. Ultimately, CPIMM-NAUKF has the best filtering accuracy, since it significantly exist. Ultimately, CPIMM-NAUKF has the best filtering accuracy, since it significantly improves the improves the IMM-UKF through the external and internal links, which makes it extremely effective IMM-UKF through the external and internal links, which makes it extremely effective for maneuvering for maneuvering target tracking when sensor faults occur. target tracking when sensor faults occur. In summary, as a combination of CPIMM and NAUKF, the CPIMM-NAUKF, compared with 1710 .5 the existing IMM-UKF, makes a greater contribution to filtering performance in terms of accuracy IMM-UKF and convergence speed. This is because it includes two important links, the external CPIMM and the IMM-NAUKF CPIMM-UKF internal NAUKF, which work simultaneously and effectively. CPIMM-NAUKF 12.5 7.2 5 2.5 -2 -4 10 40 70 80 0 100 20 20 0 30 30 0 400 50 50 0 60 60 0 700 800 90 90 0 10 1000 0 Time/(sec) Figure 9. Position RMSE comparison for CPIMM-NAUKF for Case 2. Position RMSE/(m) Y/(m) Appl. Sci. 2017, 7, 1003 16 of 26 True IMM-UKF Destination point IMM-NAUKF CPIMM-UKF CPIMM-NAUKF Start point 0 200 400 600 800 1000 1200 1400 1600 1800 2000 X/(m) Figure 8. True and estimated trajectories of the maneuvering target for CPIMM-NAUKF for Case 2. The comparisons of the RMSE position and velocity curves of four filtering algorithms are shown in Figures 9 and 10, and the corresponding statistical data of RMSE are listed in Table 3. The sensor faults negatively influenced the filtering precision of the UKF and the filtering error of the standard IMM-UKF was the largest. The IMM-NAUKF has better filtering accuracy than the IMM-UKF, because the internal link NAUKF effectively handled the sensor fault problem, but the filtering convergence performance was poor. The CPIMM-UKF also has better filtering accuracy than the IMM-UKF because the internal link CPIMM has a better effect for maneuvering target tracking than IMM, but the filtering accuracy relationship between IMM-NAUKF and CPIMM-UKF does not exist. Ultimately, CPIMM-NAUKF has the best filtering accuracy, since it significantly improves the IMM-UKF through the external and internal links, which makes it extremely effective Appl. Sci. 2017, 7, 1003 16 of 26 for maneuvering target tracking when sensor faults occur. 1710 .5 IMM-UKF IMM-NAUKF CPIMM-UKF CPIMM-NAUKF 12.5 7.2 5 2. -2 5 -4 0 10 100 20 20 0 30 30 0 40 400 50 50 0 60 60 0 70 70 0 80 80 0 90 90 0 10 1000 0 Time/(sec) Appl. Sci. 2017, 7, 1003 17 of 26 Figure Figure 9. 9. Position Position RMSE RMSE comparison comparison for C for CPIMM-NAUKF PIMM-NAUKF fo for r CCase ase 2.2. IMM-UKF IMM-NAUKF 3.6 5 CPIMM-UKF CPIMM-NAUKF 2.4 5 3 2 1.2 5 1 1 0.0 5 -1 10 40 70 80 0 40 80 20 12 30 0 160 200 50 240 60 280 320 36 90 0 100 400 Time/(sec) Figure Figure 10. 10. Velo Velocity city RMS RMSE E c comparison omparison for CP for CPIMM-NAUKF IMM-NAUKF fofor r Case 2 Case. 2. Table 3. Performance comparison for CPIMM-NAUKF for Case 2. Table 3. Performance comparison for CPIMM-NAUKF for Case 2. Position Error (m) Velocity Error (m/s) Algorithm Position Error (m) Velocity Error (m/s) Algorithm Mean Standard Deviation Mean Standard Deviation Mean Standard Deviation Mean Standard Deviation IMM-UKF 11.8533 4.3387 3.1026 1.9552 IMM-UKF 11.8533 4.3387 3.1026 1.9552 IMM-NAUKF 7.0473 2.9116 1.2143 1.0348 IMM-NAUKF 7.0473 2.9116 1.2143 1.0348 CPIMM-UKF 9.3558 3.0098 2.4961 1.2246 CPIMM-UKF 9.3558 3.0098 2.4961 1.2246 CPIMM-NAUKF 4.8671 0.9169 0.4294 0.3857 CPIMM-NAUKF 4.8671 0.9169 0.4294 0.3857 In summary, as a combination of CPIMM and NAUKF, the CPIMM-NAUKF, compared with the existing IMM-UKF, makes a greater contribution to filtering performance in terms of accuracy and convergence speed. This is because it includes two important links, the external CPIMM and the internal NAUKF, which work simultaneously and effectively. 4.2. Practical Experiments and Analysis Furthermore, to demonstrate the effectiveness of the proposed CPIMM-NAUKF method for real applications, two practical experiments were conducted. An unmanned aerial vehicle (UAV) was loaded with an Inertial Navigation System (INS)/Global Positioning System (GPS) integrated navigation system for its navigation and positioning, and equipped with an SAR tracking system to track the maneuvering targets. 4.2.1. INS/GPS Integrated Navigation System As shown in Figures 11 and 12, the vehicle loaded integrated navigation system included a NV-IMU300 type INS, a JAVAD Lexon-GGD112T type GPS receiver, and a NovAtel DL-V3 GPS receiver on the vehicle, and another NovAtel DL-V3 GPS receiver was located at the Earth Reference Station. The working frequency of the GPS was 20 Hz. The maximum distance between the vehicle and the Earth Reference Station had to be less than 60 km, which ensured that the positioning precision of the vehicle was better than 0.1 m. Let the geographic coordinate system be the navigation coordinate system with three directions of East, North, Up. Define the system states as Position RMSE/(m) Velocity RMSE/(m) Y/(m) Appl. Sci. 2017, 7, 1003 17 of 26 4.2. Practical Experiments and Analysis Furthermore, to demonstrate the effectiveness of the proposed CPIMM-NAUKF method for real applications, two practical experiments were conducted. An unmanned aerial vehicle (UAV) was loaded with an Inertial Navigation System (INS)/Global Positioning System (GPS) integrated navigation system for its navigation and positioning, and equipped with an SAR tracking system to track the maneuvering targets. 4.2.1. INS/GPS Integrated Navigation System As shown in Figures 11 and 12, the vehicle loaded integrated navigation system included a NV-IMU300 type INS, a JAVAD Lexon-GGD112T type GPS receiver, and a NovAtel DL-V3 GPS receiver on the vehicle, and another NovAtel DL-V3 GPS receiver was located at the Earth Reference Station. The working frequency of the GPS was 20 Hz. The maximum distance between the vehicle and the Earth Reference Station had to be less than 60 km, which ensured that the positioning precision of the vehicle was better than 0.1 m. Let the geographic coordinate system be the navigation coordinate system with three directions of East, North, Up. Define the system states as X(t) = y, q, g, v , v , v , L, l, h, # , # , # ,r ,r ,r (46) E N U x y z x y z where y, q, g represent the heading angle, the pitch angle, and the roll angle of the vehicle, respectively; v , v , v are components of the velocity along the East, North, and Up directions, respectively; E N U L, l, h are components of the position along the East, North, and Up directions, respectively; # , # , # x y z represent the drift of the gyro; r ,r ,r represent the bias of the accelerator. x y z The state space model is X t == f X t + W t (47) ( ) [ ( )] ( ) the concrete equations of which can be found in [45]. Select the velocity and position outputs of the GPS as the measurement variables: Z = [v , v , v , L , l , h ] (48) EG N G U G G G G where v , v , v are components of the velocity measured via GPS along the East, North, and EG N G U G Up directions, respectively; L , l , h are components of the position measured via GPS along East, G G G North, and Up directions, respectively. The velocity and position information measured via GPS can be used as a reference to confirm the position error of the INS/GPS. The measurement equation is Z = H X + V (49) k k k k where H = [O , O , O ]. 63 66 66 In the experiment process, the initial position was as follows: East longitude 108.891227 , North latitude 34.286866 , and an altitude of 437.92 m. Components of the velocity along the East, North, and Up directions were, respectively, 8 m/s, 35 m/s, and 0 m/s; the drift of the gyro was 0.12 /h, and the white noise was 0.02 /h; the bias of the accelerator was 10 g, and the white noise was 10 g; the positioning and position precision of the GPS were 5 m and 0.05 m/s; the other initial parameters were the same as the those described in the simulation section. The experiment test time was designed as 500 s with the sample period being 1 s, and the end point of the vehicle was East longitude 108.995683 , North latitude 34.285292 , and an altitude of 438.01 m. The practical road condition was the urban highway, and the real route of the vehicle is shown in Figure 13. Appl. Sci. 2017, 7, 1003 18 of 26 Appl. Sci. 2017, 7, 1003 18 of 26 Xt,=∇ ψθ,γ,v,v,v,L,λ,h,ε,ε,ε, ,∇,∇ (46) () EN U x y z x y z   Xt,=∇ ψθ,γ,v,v,v,L,λ,h,ε,ε,ε, ,∇,∇ (46) () EN U x y z x y z  where ψ,, θ γ represent the heading angle, the pitch angle, and the roll angle of the vehicle, where ψ,, θ γ represent the heading angle, the pitch angle, and the roll angle of the vehicle, respectively; vv,,v are components of the velocity along the East, North, and Up directions, E NU respectively; vv,,v are components of the velocity along the East, North, and Up directions, E NU respectively; L,, λ h are components of the position along the East, North, and Up directions, respectively; L,, λ h are components of the position along the East, North, and Up directions, respectively; εε,,ε represent the drift of the gyro; represent the bias of the ∇∇,,∇ x yz x yz respectively; εε,,ε represent the drift of the gyro; ∇∇,,∇ represent the bias of the x yz x yz accelerator. accelerator. The state space model is The state space model is Xf (tt ) == X () +W ()t (47)  Xftt == X +Wt ( ) () () (47)  the concrete equations of which can be found in [45]. the concrete equations of which can be found in [45]. Select the velocity and position outputs of the GPS as the measurement variables: Select the velocity and position outputs of the GPS as the measurement variables: Z =[] v,v ,v ,L ,λ ,h (48) EG NG UG G G G Z = v,v ,v ,L ,λ ,h (48) [] EG NG UG G G G where vv,,v are components of the velocity measured via GPS along the East, North, and Up EGNG UG where vv,,v are components of the velocity measured via GPS along the East, North, and Up EGNG UG directions, respectively; L ,, λ h are components of the position measured via GPS along East, GG G directions, respectively; L ,, λ h are components of the position measured via GPS along East, GG G North, and Up directions, respectively. The velocity and position information measured via GPS North, and Up directions, respectively. The velocity and position information measured via GPS can be used as a reference to confirm the position error of the INS/GPS. can be used as a reference to confirm the position error of the INS/GPS. The measurement equation is The measurement equation is ZH=+X V (49) kk k k ZH=+X V (49) kk k k Appl. Sci. 2017, 7, 1003 18 of 26 where H = 00,,0 . [ ] k 63×× 6 6 6×6 where H = [00,,0 ] . k 63×× 6 6 6×6 Figure 11. The experimental vehicle. Figure 11. The experimental vehicle. Figure 11. The experimental vehicle. Appl. Sci. 2017, 7, 1003 19 of 26 North, and Up directions were, respectively, −8 m/s, −35 m/s, and 0 m/s; the drift of the gyro was −3 0.12°/h, and the white noise was 0.02°/h; the bias of the accelerator was 10 g, and the white noise −4 was 10 g; the positioning and position precision of the GPS were 5 m and 0.05 m/s; the other initial parameters were the same as the those described in the simulation section. The experiment test time was designed as 500 s with the sample period being 1 s, and the end point o f the vehicle was East Figure 12. The vehicle loaded navigation equipment. longitude 108.995683°, North latitude 34.285292°, and an altitude of 438.01 m. The practical road Figure 12. The vehicle loaded navigation equipment. Figure 12. The vehicle loaded navigation equipment. condition was the urban highway, and the real route of the vehicle is shown in Figure 13. In the experiment process, the initial position was as follows: East longitude 108.891227°, In the experiment process, the initial position was as follows: East longitude 108.891227°, North latitude 34.286866°, and an altitude of 437.92 m. Components of the velocity along the East, North latitude 34.286866°, and an altitude of 437.92 m. Components of the velocity along the East, Figure 13. The real route of the vehicle in navigation. Figure 13. The real route of the vehicle in navigation. Five filtering methods including standard EKF [33], CKF [40], standard UKF [27], existing adaptive UKF (AUKF) [38], and NAUKF were used in the experiment, and these algorithms were evaluated by the estimation error rather than RMSE, which is computed through (50) E=−XX The comparisons of the longitude and latitude error curves of five filtering algorithms are shown in Figures 14 and 15, and the corresponding statistical data of the mean absolute error are listed in Table 4. We can see that the EKF and CKF have almost the same filtering accuracy, although both EKF and CKF can deal with the nonlinear filtering problems—they only have first order accuracy. The filtering errors are, respectively, [−18.1025, 19.9471] m and [−17.5118, 18.4296] m of the longitude error ranging, and [−20.1143, 16.2477] m and [−17.4154, 14.9953] m of the latitude error ranging. The standard UKF has better filtering precision than does EKF and CKF, because UKF can reach third order accuracy, but it still has substantial oscillation and error due to the longitude error ranging [−11.0549, 10.2178] m and latitude error ranging [−9.1766, 9.9737] m. The AUKF has the adaptive ability of the imprecise noise in the urban complex environment, so it has better filtering accuracy than the UKF with the longitude error ranging [−5.8142, 6.0273] m and latitude error ranging [−8.1184, 6.4153] m. Ultimately, NAUKF has the best filtering accuracy, with the longitude error ranging [−4.1109, 4.3804] m and latitude error ranging [−6.1096, 4.7138] m, since it can not only adapt to the noise but also the sensor faults and model mismatches. Appl. Sci. 2017, 7, 1003 19 of 26 Five filtering methods including standard EKF [33], CKF [40], standard UKF [27], existing adaptive UKF (AUKF) [38], and NAUKF were used in the experiment, and these algorithms were evaluated by the estimation error rather than RMSE, which is computed through E = X X (50) The comparisons of the longitude and latitude error curves of five filtering algorithms are shown in Figures 14 and 15, and the corresponding statistical data of the mean absolute error are listed in Table 4. We can see that the EKF and CKF have almost the same filtering accuracy, although both EKF and CKF can deal with the nonlinear filtering problems—they only have first order accuracy. The filtering errors are, respectively, [18.1025, 19.9471] m and [17.5118, 18.4296] m of the longitude error ranging, and [20.1143, 16.2477] m and [17.4154, 14.9953] m of the latitude error ranging. The standard UKF has better filtering precision than does EKF and CKF, because UKF can reach third order accuracy, but it still has substantial oscillation and error due to the longitude error ranging [11.0549, 10.2178] m and latitude error ranging [9.1766, 9.9737] m. The AUKF has the adaptive ability of the imprecise noise in the urban complex environment, so it has better filtering accuracy than the UKF with the longitude error ranging [5.8142, 6.0273] m and latitude error ranging [8.1184, 6.4153] m. Ultimately, NAUKF has the best filtering accuracy, with the longitude error ranging [4.1109, 4.3804] m and latitude error ranging [6.1096, 4.7138] m, since it can not only adapt to the noise but also the sensor faults and Appl. Sci. 2017, 7, 1003 20 of 26 model mismatches. EK F CK F UK F AUKF NA UK F -5 -10 -15 -20 0 50 100 150 200 250 300 350 400 450 500 Tim e/(s ec ) Figure 14. Longitude error comparison for NAUKF in INS/GPS navigation system. Figure 14. Longitude error comparison for NAUKF in INS/GPS navigation system. EKF CK F UK F AUKF NA UK F -5 -10 -15 -20 0 50 100 150 200 250 300 350 400 450 500 Tim e/(s ec ) Figure 15. Latitude error comparison for NAUKF in INS/GPS navigation system. Longitude E rror/(m ) Latitude E rror/(m ) Appl. Sci. 2017, 7, 1003 20 of 26 EK F CK F UK F AUKF NA UK F -5 -10 -15 -20 0 50 100 150 200 250 300 350 400 450 500 Tim e/(s ec ) Appl. Sci. 2017, 7, 1003 20 of 26 Figure 14. Longitude error comparison for NAUKF in INS/GPS navigation system. EKF CK F UK F AUKF NA UK F -5 -10 -15 -20 0 50 100 150 200 250 300 350 400 450 500 Tim e/(s ec ) Figure 15. Latitude error comparison for NAUKF in INS/GPS navigation system. Figure 15. Latitude error comparison for NAUKF in INS/GPS navigation system. Table 4. Performance comparison for NAUKF in INS/GPS navigation system. Mean Absolute Error of Longitude (m) Mean Absolute Error of Latitude (m) Algorithm Mean Standard Deviation Mean Standard Deviation EKF 8.6533 7.9816 8.2107 7.0044 CKF 8.4154 7.5127 8.0109 6.8135 UKF 6.9018 6.5011 6.9271 6.6102 AUKF 4.4225 4.0118 4.8159 4.6825 NAUKF 3.0156 2.8105 2.8192 2.4947 In summary, UKF has better filtering precision than KF, EKF, and CKF, because UKF can reach third order accuracy. The proposed NAUKF has the best filtering performance in terms of precision and dynamic response for its adaptive ability to both imprecise noise and sensor faults, and it can increase the precision and adaptability of the navigation system effectively. 4.2.2. SAR Tracking System As shown in Figures 16 and 17, the UAV is equipped with one D3160 type SAR. The UAV is a six-rotor UAV with mass being 20 kg and velocity range being 0 m/s–20 m/s. The D3160 type SAR is a kind of micro-SAR system and the parameters are as follows: mass: 4 kg, operating frequency band: X or Ku, detection range: 10 km, resolution: 0.3 m. Longitude E rror/(m ) Latitude E rror/(m ) Appl. Sci. 2017, 7, 1003 21 of 26 Appl. Sci. 2017, 7, 1003 21 of 26 Table 4. Performance comparison for NAUKF in INS/GPS navigation system. Table 4. Performance comparison for NAUKF in INS/GPS navigation system. Mean Absolute Error of Longitude (m) Mean Absolute Error of Latitude (m) Algorithm Mean Absolute Error of Longitude (m) Mean Absolute Error of Latitude (m) Mean Standard Deviation Mean Standard Deviation Algorithm Mean Standard Deviation Mean Standard Deviation EKF 8.6533 7.9816 8.2107 7.0044 EKF 8.6533 7.9816 8.2107 7.0044 CKF 8.4154 7.5127 8.0109 6.8135 CKF 8.4154 7.5127 8.0109 6.8135 UKF 6.9018 6.5011 6.9271 6.6102 UKF 6.9018 6.5011 6.9271 6.6102 AUKF 4.4225 4.0118 4.8159 4.6825 AUKF 4.4225 4.0118 4.8159 4.6825 NAUKF 3.0156 2.8105 2.8192 2.4947 NAUKF 3.0156 2.8105 2.8192 2.4947 In summary, UKF has better filtering precision than KF, EKF, and CKF, because UKF can reach In summary, UKF has better filtering precision than KF, EKF, and CKF, because UKF can reach third order accuracy. The proposed NAUKF has the best filtering performance in terms of precision third order accuracy. The proposed NAUKF has the best filtering performance in terms of precision and dynamic response for its adaptive ability to both imprecise noise and sensor faults, and it can and dynamic response for its adaptive ability to both imprecise noise and sensor faults, and it can increase the precision and adaptability of the navigation system effectively. increase the precision and adaptability of the navigation system effectively. 4.2.2. SAR Tracking System 4.2.2. SAR Tracking System As shown in Figures 16 and 17, the UAV is equipped with one D3160 type SAR. The UAV is a As shown in Figures 16 and 17, the UAV is equipped with one D3160 type SAR. The UAV is a six-rotor UAV with mass being 20 kg and velocity range being 0 m/s–20 m/s. The D3160 type SAR is six-rotor UAV with mass being 20 kg and velocity range being 0 m/s–20 m/s. The D3160 type SAR is a kind of micro-SAR system and the parameters are as follows: mass: 4 kg, operating frequency Appl. Sci. 2017, 7, 1003 21 of 26 a kind of micro-SAR system and the parameters are as follows: mass: 4 kg, operating frequency band: X or Ku, detection range: 10 km, resolution: 0.3 m. band: X or Ku, detection range: 10 km, resolution: 0.3 m. Figure 16. The experimental unmanned aerial vehicle (UAV). Figure 16. The experimental unmanned aerial vehicle (UAV). Figure 16. The experimental unmanned aerial vehicle (UAV). (a) SAR (b) Antenna (a) SAR (b) Antenna Figure 17. The UAV loaded tracking equipment; (a): SAR (synthetic aperture radar); (b): Antenna. Figure 17. The UAV loaded tracking equipment; (a): SAR (synthetic aperture radar); (b): Antenna. Figure 17. The UAV loaded tracking equipment; (a): SAR (synthetic aperture radar); (b): Antenna. The SAR can detect the pitch angle ϕ , azimuth ϕ and distance d of the target vehicle 1 2 ϕ ϕ d The SAR can detect the pitch angle , azimuth and distance of the target vehicle 1 2 relative to the UAV, and the measurement equation is The SAR can detect the pitch angle j , azimuth j and distance d of the target vehicle relative to 1 2 relative to the UAV, and the measurement equation is the UAV, and the measurement equation is 2 3 2 2 2 (x x ) + (y y ) + (z z ) t t t U U U 6 7 0 1 2 3 6 7 6 7 z z 6 U t 7 6 7 @ A arctan q 6 7 Z = j = (51) 4 5 6 7 2 2 6 (x x ) + (y y ) 7 U t U t 2 6 7 4 5 y y U t arctan x x T T where [x , y , z ] is the position of UAV; [x , y , z ] is the position of the target vehicle. t t t U U U In the experiment process, both the pitch angle and azimuth measurement had the drift rate, and the distance measurement had the deviation for the SAR, but the precise statistics about the sensor faults and noises are still unknown. In Figure 18, the target vehicle is driving on the expressway, and the initial position of the UAV and the target vehicle is, respectively, East longitude 108.933195 , North latitude 34.188183 , and an altitude of 1.50 km and East longitude 108.938962 , North latitude 34.187501 , and an altitude of 436.05 m. The experiment test time is designed as 1000 s with the sample period being 1 s, and the end point of the UAV and the target vehicle is, respectively, East longitude 109.130943 , North latitude 34.245275 , and an altitude of 1.50 km and East longitude 108.145231 , North latitude 34.239740 , an altitude of 436.76 m. Appl. Sci. 2017, 7, 1003 22 of 26    22 2 () xx−+(y −y) +(z −z) Ut U t U t      d    zz −  Ut  Z== ϕ arctan (51)     22  xx−+ y −y  () ( )  Ut U t ϕ   2      yy − Ut arctan    xx − Ut    T T where [, x yz, ] is the position of UAV; [, x yz, ] is the position of the target vehicle. UU U tt t In the experiment process, both the pitch angle and azimuth measurement had the drift rate, and the distance measurement had the deviation for the SAR, but the precise statistics about the sensor faults and noises are still unknown. In Figure 18, the target vehicle is driving on the expressway, and the initial position of the UAV and the target vehicle is, respectively, East longitude 108.933195°, North latitude 34.188183°, and an altitude of 1.50 km and East longitude 108.938962°, North latitude 34.187501°, and an altitude of 436.05 m. The experiment test time is designed as 1000 s with the sample period being 1 s, and the end point of the UAV and the target Appl. vehi Sci. cle i 2017 s, ,respecti 7, 1003 vely, East longitude 109.130943°, North latitude 34.245275°, and an altitude of 22 of 26 1.50 km and East longitude 108.145231°, North latitude 34.239740°, an altitude of 436.76 m. Figure 18. The real route of the vehicle in target tracking. Figure 18. The real route of the vehicle in target tracking. When the IMM and CPIMM methods are used, the target model description is the same as the When the IMM and CPIMM methods are used, the target model description is the same as numerical simulation. Four filtering methods including IMM-UKF, IMM-NAUKF, CPIMM-UKF, the numerical simulation. Four filtering methods including IMM-UKF, IMM-NAUKF, CPIMM-UKF, and CPIMM-NAUKF were used in the experiment, and these algorithms were evaluated by the and CPIMM-NAUKF were used in the experiment, and these algorithms were evaluated by the estimation error shown by Equation (50). estimation error shown by Equation (50). The comparisons of the longitude and latitude error curves of five filtering algorithms are shown Thein comparisons Figures 19 and of the 20longitude , and the co and rresp latitude ondinerr g st or atcurves istical d of atfive a of t filtering he mean algorithms absolute er arror e shown are in listed Figur in T es 19 able 5. We c and 20, and an the see that the standar corresponding statistical d IMM-UKF h data of as the bigge the mean st filter absolute ing err erroror wit are listed h the in longitude error ranging [−25.0223, 24.8762] m and latitude error ranging [−23.9917, 24.9834] m. The Table 5. We can see that the standard IMM-UKF has the biggest filtering error with the longitude error IMM-NAUKF has better filtering precision than IMM-UKF because of the adaptive ability to the ranging [25.0223, 24.8762] m and latitude error ranging [23.9917, 24.9834] m. The IMM-NAUKF has better filtering precision than IMM-UKF because of the adaptive ability to the sensor faults with the longitude error ranging [16.1849, 14.0871] m and latitude error ranging [13.8628, 14.0563] m. The CPIMM-UKF has better filtering precision than IMM-UKF because of the adaptive ability to the non-Gaussian distribution with the longitude error ranging [10.0547, 14.2482] m and latitude error ranging [14.7196, 12.2544] m. The CPIMM-NAUKF has the best filtering accuracy, since it can not only adapt to the non-Gaussian but also the sensor faults and model mismatches with the longitude error ranging [5.0018, 6.8623] m and latitude error ranging [4.8007, 4.9106] m. Table 5. Performance comparison for CPIMM-NAUKF in the SAR tracking system. Mean Absolute Error of Longitude (m) Mean Absolute Error of Latitude (m) Algorithm Mean Standard Deviation Mean Standard Deviation IMM-UKF 9.0125 8.2336 9.0058 8.1329 IMM-NAUKF 6.7243 6.0772 6.1174 5.9287 CPIMM-UKF 7.1588 7.0154 7.0542 6.8835 CPIMM-NAUKF 2.8514 2.2259 2.3417 2.0016 Appl. Appl. Sci. Sci. 2017 2017, , 7 7, 1003 , 1003 23 of 23 of 26 26 sensor faults with the longitude error ranging [−16.1849, 14.0871] m and latitude error ranging sensor faults with the longitude error ranging [−16.1849, 14.0871] m and latitude error ranging [−13.8628, 14.0563] m. The CPIMM-UKF has better filtering precision than IMM-UKF because of the [−13.8628, 14.0563] m. The CPIMM-UKF has better filtering precision than IMM-UKF because of the adaptive ability to the non-Gaussian distribution with the longitude error ranging [−10.0547, adaptive ability to the non-Gaussian distribution with the longitude error ranging [−10.0547, 14.2482] m and latitude error ranging [−14.7196, 12.2544] m. The CPIMM-NAUKF has the best 14.2482] m and latitude error ranging [−14.7196, 12.2544] m. The CPIMM-NAUKF has the best filt filter ering ing accu accuracy racy, since , since it it can can not not onl only y ad adapt apt ttoo the the non- non-Ga Gaussi ussiaan n but but al also the sensor fa so the sensor faul ults a ts an nd d model mi model misma smattches wit ches with h t th he long e longit itude e ude error ran rror rang gin ing g [[−−5 5..00 0018 18, , 6 6..862 8623] 3] m m a an nd la d lattiitude error rangi tude error rangin ng g Appl. Sci. 2017, 7, 1003 23 of 26 [−4.8007, 4.9106] m. [−4.8007, 4.9106] m. IM M -UK F IM M -UK F IM M -NA UK F IM M -NA UK F CP IM M -UK F CP IM M -UK F CP IM M -NA UK F CP IM M -NA UK F 10 10 -5 -5 -10 -10 -15 -15 -2 -20 0 -25 -25 0 0 100 100 20 2000 300 300 400 400 500 500 600 600 70 7000 800 800 90 9000 100 10000 Ti Tim me e//((sse ecc)) Figure 19. Longitude error comparison for CPIMM-NAUKF in SAR tracking system. Figure 19. Figure 19. Lon Long gitude error comparison for CP itude error comparison for CPIIMM-N MM-NAUKF i AUKF in n SAR SAR trac tracking s king sy yssttem. em. IMM-UK F IMM-UK F IIM MM M--NA NAUK UKF F CP CPIIM MM M--UK UKF F CP IM M -NA UK F CP IM M -NA UK F 0 0 -5 -5 -10 -10 -15 -15 -20 -20 -25 -25 0 100 200 300 400 500 600 700 800 900 1000 0 100 200 300 400 500 600 700 800 900 1000 Tim e/(sec) Tim e/(sec) Figure 20. Latitude error comparison for CPIMM-NAUKF in the synthetic aperture radar (SAR) tracking system. Figure 20. Latitude error comparison for CPIMM-NAUKF in the synthetic aperture radar (SAR) Figure 20. Latitude error comparison for CPIMM-NAUKF in the synthetic aperture radar (SAR) tracking system. tracking system. In summary, the practical experiments results demonstrate that the proposed CPIMM-NAUKF is obviously superior to the existing IMM and UKF algorithms in terms of filtering precision and dynamic response for its adaptive ability to both non-Gaussian and sensor faults, and it can significantly improve the precision and robustness of the maneuvering target tracking system. 5. Conclusions Maneuvering target tracking is a key technology and research hotspot in the field of navigation, guidance, and control. This paper proposes an efficient IMM-UKF, called CPIMM-NAUKF, for maneuvering target tracking. In the external link of CPIMM-NAUKF, the mean and covariance Latitude E rror/(m ) Latitude E rror/(m ) Longitude E rror/(m ) Longitude E rror/(m ) Appl. Sci. 2017, 7, 1003 24 of 26 matrix can be calculated with high estimation precision by introducing the cubature principle, which solves the non-Gaussian problem. In the internal CPIMM-NAUKF link, the contribution of system states and innovations on the filtering solution can be balanced by introducing an adaptive matrix gene, which addresses the sensor faults. The proposed algorithm was evaluated in a classical maneuvering target tracking numerical simulation and two practical experiments including one navigation experiment with INS/GPS and one target tracking experiment with SAR. Simulation results validate that the proposed CPIMM-NAUKF improves the filtering performance in terms of higher accuracy and faster convergence, and tracks more effectively. In our future research, we will focus on the difference between UKF and other nonlinear filtering algorithms of various noise strengths. Acknowledgments: This work was supported by the National Natural Science Foundation of China (No. 61601505), National Aviation Science Foundation of China (No. 20155196022) and the Shaanxi Natural Science Foundation of China (No. 2016JQ6050). Author Contributions: Huan Zhou and Hui Zhao provided insights into formulating the ideas and designed the algorithm. Hanqiao Huang performed the simulations. Xin Zhao reviewed the manuscript and gave suggestions in the proposed method. Conflicts of Interest: The authors declare no conflict of interest. References 1. Matveev, A.S.; Semakova, A.A.; Savkin, A.V. Tight circumnavigation of multiple moving targets based on a new method of tracking environmental boundaries. Automatica 2017, 79, 52–60. [CrossRef] 2. Fan, Y.; Lu, F.; Zhu, W.; Bai, G.; Yan, L. A Hybrid Model Algorithm for Hypersonic Glide Vehicle Maneuver Tracking Based on the Aerodynamic Model. Appl. Sci. 2017, 7, 159. [CrossRef] 3. Cao, Y.; Wang, G.; Yan, D.; Zhao, Z. Two Algorithms for the Detection and Tracking of Moving Vehicle Targets in Aerial Infrared Image Sequences. Remote Sens. 2016, 8, 28. [CrossRef] 4. Leven, W.F.; Lanterman, A.D. Unscented Kalman Filters for Multiple Target Tracking With Symmetric Measurement Equations. IEEE Trans. Autom. Control 2009, 54, 370–375. [CrossRef] 5. Song, Y.; Zhang, B.; Zhao, K. Indirect neuroadaptive control of unknown MIMO systems tracking uncertain target under sensor failures. Automatica 2017, 77, 103–111. [CrossRef] 6. Wu, J.; Li, K.; Zhang, Q.; An, W.; Jiang, Y.; Ping, X.; Chen, P. Iterative RANSAC based adaptive birth intensity estimation in GM-PHD filter for multi-target tracking. Signal Process. 2017, 131, 412–421. [CrossRef] 7. Dong, P.; Jing, Z.; Gong, D.; Tang, B. Maneuvering multi-target tracking based on variable structure multiple model GMCPHD filter. Signal Process. 2017, 141, 158–167. [CrossRef] 8. Roy, A.; Mitra, D. Unscented-Kalman-Filter-Based Multitarget Tracking Algorithms for Airborne Surveillance Application. J. Guid. Control Dyn. 2016, 39, 1949–1966. [CrossRef] 9. Blom, H.; Barshalom, Y. The Interacting Multiple Model Algorithm for Systems with Markovian Switching Coefficients. IEEE Trans. Autom. Control 1988, 33, 780–783. [CrossRef] 10. Lan, J.; Li, X.R.; Jilkov, V.P.; Mu, C. Second order Markov chain based multi-model algorithm for maneuvering target tracking. IEEE Trans. Aerosp. Electron. Syst. 2013, 49, 3–19. [CrossRef] 11. Khalid, S.S.; Abrar, S. A low-complexity interacting multiple model filter for maneuvering target tracking. Int. J. Electron. Commun. 2017, 73, 157–164. [CrossRef] 12. Zhu, W.; Wang, W.; Yuan, G. An Improved Interacting Multiple Model Filtering Algorithm Based on the Cubature Kalman Filter for Maneuvering Target Tracking. Sensors 2016, 16, 805. [CrossRef] [PubMed] 13. Kim, T.H.; Moon, K.R.; Song, T.L. Variable-structured interacting multiple model algorithm for the ballistic coefficient estimation of a re-entry ballistic target. Int. J. Control Autom. Syst. 2013, 11, 1204–1213. [CrossRef] 14. Zhu, Z. Ship-borne radar maneuvering target tracking based on the variable structure adaptive grid interacting multiple model. J. Zhejiang Univ. Sci. C 2013, 14, 733–742. [CrossRef] 15. Zhang, Y.; Guo, C.; Hu, H.; Liu, S.; Chu, J. An algorithm of the adaptive grid and fuzzy interacting multiple models. J. Mar. Sci. Appl. 2014, 13, 340–345. [CrossRef] 16. Li, X.R.; Jilkov, V.P. Survey of maneuvering target tracking—Part V: Multiple-model methods. IEEE Trans. Aerosp. Electron. Syst. 2005, 41, 1255–1321. Appl. Sci. 2017, 7, 1003 25 of 26 17. Cui, N.; Hong, L.; Layne, J.R. A Comparison of Nonlinear Filtering Approaches with an Application to Ground Target Tracking. Signal Process. 2008, 85, 1469–1492. [CrossRef] 18. Li, W.; Jia, Y. Gaussian Mixture PHD Filter for Jump Markov Models based on Best-fitting Gaussian Approximation. Signal Process. 2011, 91, 1036–1042. [CrossRef] 19. Lainiotis, D.; Sims, F. Performance measure for adaptive Kalman estimators. IEEE Trans. Autom. Control 1970, 15, 249–250. [CrossRef] 20. Kirubarajan, T.; Barshalom, Y. Kalman Filter Versus IMM Estimator: When Do We Need the Latter? IEEE Trans. Aerosp. Electron. Syst. 2000, 39, 1452–1457. [CrossRef] 21. Susan, S.; Sharma, M. Automatic texture defect detection using Gaussian mixture entropy modeling. Neurocomputing 2017, 235, 274–286. [CrossRef] 22. Liang, H.; Kang, F. Tracking UUV based on interacting multiple model unscented particle filter with multi-sensor information fusion. Optik-Int. J. Light Electron. Opt. 2015, 126, 5067–5073. 23. Chang, D.; Fan, M. Interacting multiple model particle filtering using new particle resampling algorithm. In Proceedings of the 2014 IEEE Global Communications Conference, Austin, TX, USA, 8–12 December 2014; pp. 3215–3219. 24. Gordon, N.J.; Salmond, D.J.; Smith, A.F.M. Novel approach to nonlinear and non-Gaussian Bayesian state estimation. IEE Proc. F-Radar Signal Process. 1993, 140, 107–113. [CrossRef] 25. Zhou, H.; Deng, Z.; Xia, Y.; Fu, M. A new sampling method in particle filter based on Pearson correlation coefficient. Neurocomputing 2016, 216, 208–215. [CrossRef] 26. Julier, S.J.; Uhlmann, J.K.; Durrant-Whyte, H.F. A new approach for filtering nonlinear systems. In Proceedings of the American Control Conference, Seattle, WA, USA, 21–23 June 1995; pp. 1628–1632. 27. Julier, S.J.; Uhlmann, J.K.; Durrant-Whyte, H.F. A new method for nonlinear transformation of means and covariances in filters and estimators. IEEE Trans. Autom. Control 2000, 45, 477–482. [CrossRef] 28. Boada, B.L.; Boada, M.J.L.; Diaz, V. Vehicle sideslip angle measurement based on sensor data fusion using an integrated ANFIS and an Unscented Kalman Filter algorithm. Mech. Syst. Signal Process. 2016, 72–73, 832–845. [CrossRef] 29. Wang, D.; Lv, H.; Wu, J. In-flight initial alignment for small UAV MEMS-based navigation via adaptive unscented Kalman filtering approach. Aerosp. Sci. Technol. 2017, 61, 73–84. [CrossRef] 30. Kumar, D.V.A.N.R.; Rao, S.K.; Raju, K.P. Integrated Unscented Kalman filter for underwater passive target tracking with towed array measurements. Optik-Int. J. Light Electron. Opt. 2016, 127, 2840–2847. [CrossRef] 31. Rahimi, A.; Kumar, K.D.; Alighanbari, H. Fault estimation of satellite reaction wheels using covariance based adaptive unscented Kalman filter. Acta Astronaut. 2017, 134, 159–169. [CrossRef] 32. Zheng, X.; Fang, H. An integrated unscented kalman filter and relevance vector regression approach for lithium-ion battery remaining useful life and short-term capacity prediction. Reliab. Eng. Syst. Saf. 2015, 144, 74–82. [CrossRef] 33. Zahedi Ygane, M.H.; Ansarifar, G.R. Extended Kalman filter design to estimate the poisons concentrations in the P.W.R nuclear reactors based on the reactor power measurement. Ann. Nucl. Energy 2017, 101, 576–585. [CrossRef] 34. Kulikova, M.V.; Kulikov, G.Y. NIRK-based accurate continuous–Discrete extended Kalman filters for estimating continuous-time stochastic target tracking models. J. Comput. Appl. Math. 2017, 316, 260–270. [CrossRef] 35. Yang, Y.; Gao, W. A new learning statistic for adaptive filter based on predicted residuals. Prog. Nat. Sci. 2006, 16, 833–837. 36. Yang, Y.; Gao, W. An optimal adaptive Kalman filter. J. Geod. 2006, 80, 177–183. [CrossRef] 37. Soken, H.E.; Hajiyev, C. Pico satellite attitude estimation via Robust Unscented Kalman Filter in the presence of measurement faults. ISA Trans. 2010, 49, 249–256. [CrossRef] [PubMed] 38. Gan, X.; Gao, W.; Dai, Z.; Liu, W. Research on WNN soft fault diagnosis for analog circuit based on adaptive UKF algorithm. Appl. Soft Comput. 2017, 50, 252–259. 39. Ning, X.; Li, Z.; Wu, W.; Yang, Y.; Fang, J.; Liu, G. Recursive adaptive filter using current innovation for celestial navigation during the Mars approach phase. Sci. China 2017, 60, 032205. [CrossRef] 40. Arasaratnam, I.; Haykin, S. Cubature Kalman Filters. IEEE Trans. Autom. Control 2009, 54, 1254–1269. [CrossRef] Appl. Sci. 2017, 7, 1003 26 of 26 41. Arasaratnam, I.; Haykin, S.; Hurd, T.R. Cubature Kalman Filtering for Continuous-Discrete Systems: Theory and Simulations. IEEE Trans. Signal. Process. 2010, 58, 4977–4993. [CrossRef] 42. Zhou, H.; Huang, H.; Zhao, H.; Zhao, X.; Yin, X. Adaptive Unscented Kalman Filter for Target Tracking in othe Presence of Nonlinear Systems Involving Model Mismatches. Remote Sens. 2017, 9, 657. [CrossRef] 43. Elenchezhiyan, M.; Prakash, J. State estimation of stochastic non-linear hybrid dynamic system using an interacting multiple model algorithm. ISA Trans. 2015, 58, 520–532. [CrossRef] [PubMed] 44. Chen, X.; Li, Y.; Li, Y.; Yu, J.; Li, X. A Novel Probabilistic Data Association for Target Tracking in a Cluttered Environment. Sensors 2016, 16, 2180. [CrossRef] [PubMed] 45. Zhao, L.; Qiu, H.; Feng, Y. Analysis of a robust Kalman filter in loosely coupled GPS/INS navigation system. Measurement 2016, 80, 138–147. [CrossRef] © 2017 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/). http://www.deepdyve.com/assets/images/DeepDyve-Logo-lg.png Applied Sciences Multidisciplinary Digital Publishing Institute

A Cubature-Principle-Assisted IMM-Adaptive UKF Algorithm for Maneuvering Target Tracking Caused by Sensor Faults

Zhou, Huan; Zhao, Hui; Huang, Hanqiao; Zhao, Xin
Applied Sciences , Volume 7 (10) – Sep 28, 2017
Download PDF
26 pages

Loading next page...
 
/lp/multidisciplinary-digital-publishing-institute/a-cubature-principle-assisted-imm-adaptive-ukf-algorithm-for-NXrkjGfNyn

References (43)

Publisher
Multidisciplinary Digital Publishing Institute
Copyright
© 1996-2019 MDPI (Basel, Switzerland) unless otherwise stated
ISSN
2076-3417
DOI
10.3390/app7101003
Publisher site
See Article on Publisher Site

Abstract

applied sciences Article A Cubature-Principle-Assisted IMM-Adaptive UKF Algorithm for Maneuvering Target Tracking Caused by Sensor Faults 1 ID 1 , 1 , 2 , 1 Huan Zhou , Hui Zhao *, Hanqiao Huang * and Xin Zhao Aeronautics and Astronautics Engineering College, Air Force Engineering University, Xi’an 710038, China; kgy_zhouh@163.com (H.Z.); bravexin@163.com (X.Z.) Astronautics College, Northwestern Polytechnic University, Xi’an 710072, China * Correspondence: zhaohui_kgy@163.com (H.Z.); cnxahhq@126.com (H.H.); Tel.: +86-029-84787589 (H.Z.); +86-029-84787555 (H.H.) Received: 20 July 2017; Accepted: 26 September 2017; Published: 28 September 2017 Featured Application: The work is very effective and applicable in the fields of the maneuvering target tracking and navigations system design. Abstract: Aimed at solving the problem of decreased filtering precision while maneuvering target tracking caused by non-Gaussian distribution and sensor faults, we developed an efficient interacting multiple model–unscented Kalman filter (IMM-UKF) algorithm. By dividing the IMM-UKF into two links, the algorithm introduces the cubature principle to approximate the probability density of the random variable, after the interaction, by considering the external link of IMM-UKF, which constitutes the cubature-principle-assisted IMM method (CPIMM) for solving the non-Gaussian problem, and leads to an adaptive matrix to balance the contribution of the state. The algorithm provides filtering solutions by considering the internal link of IMM-UKF, which is called a new adaptive UKF algorithm (NAUKF) to address sensor faults. The proposed CPIMM-NAUKF is evaluated in a numerical simulation and two practical experiments including one navigation experiment and one maneuvering target tracking experiment. The simulation and experiment results show that the proposed CPIMM-NAUKF has greater filtering precision and faster convergence than the existing IMM-UKF. The proposed algorithm achieves a very good tracking performance, and will be effective and applicable in the field of maneuvering target tracking. Keywords: maneuvering target tracking; interacting multiple model (IMM); unscented Kalman filter (UKF); Gaussian distribution; sensor fault; cubature principle; adaptive matrix gene 1. Introduction With the continuous development of space technology, maneuvering ability in real time is a key factor in completing complicated space missions for maneuvering targets [1–4]. Performing accurate maneuvering target tracking and precise locating are the core issues in the field of space target surveillance [5–8]. For maneuvering target tracking, the single model filter produces large filtering errors, and it is not appropriate for this application because the filtering algorithms highly depend on the motion model of the targets. The interacting multiple model (IMM) method integrates a variety of models to describe the motion rules of the targets, which improves the filtering precision. IMM was proposed by Blom and Barshalom [9], and consists of three processes: interaction, filtering, and fusion. The interaction process is the core difference between IMM and the first generation of multiple model filters, called autonomous multiple model (AMM) filters [10], which connect all the single filters organically to allow Appl. Sci. 2017, 7, 1003; doi:10.3390/app7101003 www.mdpi.com/journal/applsci Appl. Sci. 2017, 7, 1003 2 of 26 the “soft switching” between models. Due to the ability to automatically adjust the bandwidth of the filter, IMM has been widely used in navigation, positioning, signal processing, maneuvering target tracking, and other fields [11,12]. Based on the standard IMM, increasingly improved IMM algorithms have been created to further increase the filtering accuracy, including the Variable Structure Interactive Multiple Model (VSIMM) [13], Adaptive Interactive Multiple Model (AIMM) [14], and the Adaptive Grid Fuzzy Multiple Model (AGFIMM) [15]. After analysis, the above algorithms are common for all dynamic adjustments of the multi-model set and for real-time conversions between multi-model sets, in order to adapt to the real-time signal change and to improve the filter ’s precision. However, when interacting, the weighted sum of multiple random variables will lead to multiple peaks in the probability density function of the system state, so the sum will not conform to the pure Gaussian distribution [16,17]. As a result, the filtering precision is negatively influenced for the non-Gaussian distribution. Two methods are used to address the problem. The Gaussian distribution can be used to approximate the non-Gaussian distribution [18]. For example, Lainiotis uses the mean square error (MSE) to estimate the error covariance of the interacting random variable [19], but the estimated value is too conservative. Because the MSE estimation results are used in the filter iteration, it artificially increases the system noise in the filtering process, which makes the calculation value of gain matrix too large. Therefore, the filtering precision decreases, especially with a large measured value of noise [20]. The second method is to filter the results of the interaction by using non-Gaussian filters [21]. For instance, the IMM-PF filtering method [22,23] can be used, but the particle filter (PF) causes problems, such as particle degradation and poor real-time performance, in engineering applications [24,25]. The unscented Kalman filter (UKF) is the most widely used algorithm with a single filter in the IMM [26–32], and has many advantages compared to the Kalman Filter (KF), Extended Kalman Filter (EKF) [33,34], and PF. However, the tracking accuracy decreases significantly when sensor faults occur, because model mismatches are produced when the sensor receives measurements with biases [35–37]. Generally, the sensor faults include four varieties: complete failure, fixed deviation, drift deviation, and accuracy decrease. The second and third faults are considered in this research. The current existing adaptive UKF has been proven to be effective in dealing with unknown noise, but no effective adaptive UKF algorithms exist for sensor faults and model mismatches [38,39]. Recently, Arasaratnam et al. [40] proposed the cubature Kalman filter (CKF), based on the cubature principle, and provided rigorous mathematical proof. The cubature principle is the basis of CKF, and it computes the first and second order moments of the finite cubature points through nonlinear propagation, and then estimates the posterior probability density of the nonlinear system. As a typical deterministic sampling filtering algorithm, CKF has fewer sampling points and better numerical stability [41]. As the mixed probability of IMM interaction varies with time, the interaction process is described by nonlinear equations, and the cubature principle in CKF is used to estimate the probability density function of a random variable after the interaction. To address the single model filtering precision problem, a new adaptive matrix gene is used in UKF together with the IMM method, to balance the contribution of the state and filtering results in the presence of sensor faults. This paper develops a new efficient IMM-UKF algorithm for maneuvering target tracking, which addresses the non-Gaussian distribution problem and sensor faults. The IMM-UKF filtering problem is described for maneuvering target tracking with the occurrence of sensor faults. For IMM-UKF external links, the cubature principle (CP) is introduced to IMM to approximate the probability density of the random variable in the interaction, and the precision of the approximation is analyzed, which contributes to CP-assisted IMM (CPIMM). For IMM-UKF internal links, an adaptive matrix gene is integrated into traditional UKF to balance the contribution of the system state and provides filtering solutions, which is called a new adaptive UKF (NAUKF). Simulation and experiment results showed that the proposed cubature principle assisted IMM–adaptive UKF algorithm (CPIMM-NAUKF) has Appl. Sci. 2017, 7, 1003 3 of 26 Appl. Sci. 2017, 7, 1003 3 of 26 algorithm (CPIMM-NAUKF) has high filtering precision and fast convergence, and achieves better tracking performance than the existing IMM-UKF. high filtering precision and fast convergence, and achieves better tracking performance than the The remainder of this paper is organized as follows. The IMM-UKF filtering problem is existing IMM-UKF. described in Section 2. The CPIMM-NAUKF is developed in Section 3. Simulations and practical The remainder of this paper is organized as follows. The IMM-UKF filtering problem is described experiments comparing performance are conducted in Section 4. Finally, the conclusions are in Section 2. The CPIMM-NAUKF is developed in Section 3. Simulations and practical experiments provided in Section 5. comparing performance are conducted in Section 4. Finally, the conclusions are provided in Section 5. 2. Description of the IMM-UKF Filtering Problem 2. Description of the IMM-UKF Filtering Problem 2.1. Maneuvering Target Tracking Dynamic System 2.1. Maneuvering Target Tracking Dynamic System The general IMM-UKF structure diagram, including m models, is shown in Figure 1, where The general IMM-UKF structure diagram, including m models, is shown in Figure 1, where Z represents the unit delay link. The IMM-UKF can be divided to two parts: the external link-IMM represents the unit delay link. The IMM-UKF can be divided to two parts: the external link-IMM structure and the internal link-UKF filter. structure and the internal link-UKF filter. 1,k −1 o 1,k 1,k −1 1,k 1,k −1 P ˆ 1,k −1 X fusion ,k 2,k −1 fusion ,k 2,k −1 o 2,k 2,k −1 2,k 2,k −1 mk,1 − mk,1 − mk , mk,1 − mk , mk,1 − Figure 1. General interacting multiple model-unscented Kalman filter (IMM-UKF) structure. Figure 1. General interacting multiple model-unscented Kalman filter (IMM-UKF) structure. In Figure 1, X and P are the state estimation and covariance matrix, respectively, of the single In Figure 1, i,kX an i,d k P are the state estimation and covariance matrix, respectively, of the ik , ik , UKF filter i (i = 1, 2, . . . , m) at time step k; X and P are the corresponding vector at time i,k1 i,k1 i (1im = ,2,, ) X single UKF fi o lter at time step k ; and P are the corresponding vector o ik,1 − ik,1 − ˆ ˆ step k 1; X and P are the interaction result of X and P , and they are the inputs of i,k1 i,k1 i,k1 i,k1 o o ˆ ˆ at time step ˆ ; X and P are the interaction result of X and P , and they are the k −1 UKF filter i; X and P are the final state estimation and covariance of the IMM-UKF filter, ik,1 − ik,1 − ik,1 − ik,1 − f usion,k f usion,k respectively; and Z denotes the measurement vector for the single UKF filter. inputs of UKF filter i ; X and P are the final state estimation and covariance of the fusion,k fusion,k For single model i, the state and measurement equations of the maneuvering target are [1,4] IMM-UKF filter, respectively; and Z denotes the measurement vector for the single UKF filter. For single model i , the state and measurement equations of the maneuvering target are [1,4] X = f[X , k 1] + G W k k1 k1 k1 (1) Z = h[X , k] + V k Xf = k[,X k k −1]+Γ W kk−− 11k k−1 (1) Zh = [,X k]+V kk k where the subscript i is omitted for the sake of brevity. where the subscript i is omitted for the sake of brevity. Appl. Sci. 2017, 7, 1003 4 of 26 Similarly, the state and measurement equations of the maneuvering target in the presence of sensor faults are [35,37] X = f[X , k 1] + G W k k1 k1 k1 (2) Z = h[X , k] + V + dh k k k out,k where f[] and G represent the state transfer function and interference transfer matrix, respectively; k1 W and V denote the system and measurement noise, respectively, and they are both white Gaussian k1 k noises and mutually uncorrelated; h[] is a nonlinear coordinate transformation function; and dh out,k denotes the model mismatches caused by sensor faults. 2.2. Standard IMM From Figure 1, the standard IMM method can be described in the three following steps [11]: Step 1: Model probability updating. The mixing probability is defined and calculated as u = p u , i, j = 1, 2, . . . , m (3) i j,k1 i j i,k1 where p is the transition probability; u is the correct probability of model i, i = 1, 2, . . . , m at i j i,k1 time step k 1; and c is the prediction probability of model j, j = 1, 2, . . . , m from time step k 1 to time step k [11] which provides c = p u , i, j = 1, 2, . . . , m. (4) j å i j i,k1 i=1 Therefore, we can obtain the updated model probability: u = L c (5) j,k j,k j e e where a = å L c ; L = N(Z, P ); Z represents the innovation residual vector of UKF filter j; j,k j j,k ee ZZ j=1 and P represents the covariance matrix of Z. ee ZZ Step 2: Input interaction. ˆ ˆ X = u X (6) j,k1 å i j,k1 i,k1 i=1 o T e e P = u [P + X X ] (7) å i j,k1 i j,k1 i j,k1 j,k1 i,k1 i=1 e ˆ ˆ where X = X X . i j,k1 i,k1 j,k1 Step 3: Resultant mean and covariance matrix computation. ˆ ˆ X = X u (8) f usion,k å j,k j,k j=1 h ih i ˆ ˆ ˆ ˆ P = u P + X X X X . (9) f usion,k å j,k j,k k j,k k j,k j=1 Appl. Sci. 2017, 7, 1003 5 of 26 2.3. Non-Gaussian Distribution and Sensor Fault Problems for IMM-UKF Appl. Sci. 2017, 7, 1003 5 of 26 2.3.1. Non-Gaussian Distribution 2.3. Non-Gaussian Distribution and Sensor Fault Problems for IMM-UKF For the external IMM-UKF links, X is a weighted sum of m Gaussian random variables jk , 2.3.1. Non-Gaussian Distribution and no longer obeys the pure Gaussian distribution because many peaks will exist in the probability density, which is similar to the probability density of the mixed Gaussian distribution For the external IMM-UKF links, X is a weighted sum of m Gaussian random variables and no j,k [19]. The case when X is scalar and u is constant is explained in this section. It is assumed longer obeys the pure Gaussian distribution because many peaks will exist in the probability density, ik,1 − ik,1 − which is similar to the probability density of the mixed Gaussian distribution [19]. The case when X i,k1 that X is the weighted sum of three Gaussian random variables with normal distribution, and jk,1 − is scalar and u is constant is explained in this section. It is assumed that X is the weighted sum o i,k1 o j,k1 ˆˆ ˆ S is the square root of P , where [,XX ,X ] =−[ 3,0,5] , [,SS ,S ] =[3,2.25,1.8] , o o jk,1 − jk,1 − 12 3 12 3 of three Gaussian random variables with normal distribution, and S is the square root of P , j,k1 j,k1 [uu , ,u ] = [0.3, 0.4, 0.3] . The probability density of the mixed Gaussian distribution is shown in ˆ ˆ ˆ 12 3 where [X , X , X ] = [3, 0, 5], [S , S , S ] = [3, 2.25, 1.8], [u , u , u ] = [0.3, 0.4, 0.3]. The probability 1 2 3 1 2 3 1 2 3 Figure 2. density of the mixed Gaussian distribution is shown in Figure 2. Figure 2. The probability density of the mixed Gaussian distribution. Figure 2. The probability density of the mixed Gaussian distribution. ˆ o In Figure 2, the thick line represents X and has multiple peaks; the mixture no longer follows In Figure 2, the thick line represents X and has multiple peaks; the mixture no longer j,k1 jk,1 − Gaussian distribution. follows Gaussian distribution. After the interaction, Lainiotis completes the approximation of the probability density through After the interaction, Lainiotis completes the approximation of the probability density through the Gaussian distribution [19]: the Gaussian distribution [19]: n o ˆ ˆ ˆ ˆˆ ˆ Pr X = u N X : X , S (S ) (10) PrXX = uN :X ,SS å i,k1 i,k1 i,k1() i,k1 j,k() 1 (10) j,1 k−−  ik,1 { ik,1− ik,1− ik,1− } i=1 i =1 where S is the square root of P . where S is the square root of P . ik,1 − ik,1 − i,k1 i,k1 On the basis of the above distribution, MSE estimates the covariance matrix of the interacting On the basis of the above distribution, MSE estimates the covariance matrix of the interacting random variables, where the mean and covariance matrix are computed by [16] random variables, where the mean and covariance matrix are computed by [16] n o m m o R R o o o o T o oo o o o o ˆ ˆ ˆˆ ˆ ˆ ˆ ˆˆ ˆˆ ˆˆ ˆˆ X = X Pr X dX = å u X N X : X , S (S ) dX XX== PrX du X X NX :X ,S S dX j,k1 j,k1 j,k1 j,k1 i,k1 j,k1 i,k1 i,k1() i,k1 j,k1 ()  jk,1−− jk,1 jk,1− jk,1− i,1 k− jk,1− { i,k−1 i,k−1 i,k−1 } jk,1−  i=1 i =1 (11) (11) m ˆ = u X i,k1 i,k1 = u X i=1  ik,1−− i,k 1 i =1 Appl. Sci. 2017, 7, 1003 6 of 26 o o MSE T ˆ ˆ ˆ ˆ P = u S S + (X X )(X X ) . (12) å i,k1 i,k1 i,k1 j,k1 i,k1 j,k1 j,k1 i,k1 i=1 MSE actual From Equations (11) and (12), we know P is larger than the actual covariance of X (P ). j,k1 j,k1 j,k1 An example of the above one-dimensional case is available, and the Monte Carlo method (MC) is used with 10,000 random sampling points to compute the mean and covariance transferred. The results are shown in Table 1. Table 1. Calculation results of the comparison of the mean and covariance matrix after interaction; MSE: the mean square error; MC: Monte Carlo. Method Mean Covariance MSE 0.6000 3.9417 MC 0.5937 1.3906 MSE actual MSE From Table 1, P > P is verified. Because P will participate in the filtering iteration, j,k1 j,k1 j,k1 it is equivalent to an increase of the system noise in the process of filtering. Then, the gain matrix will be larger, and the filtering effect declines, especially when the actual system noise is small and the measurement noise is large. Therefore, for the external IMM-UKF link, the filtering problem to be solved is how to make a precise estimate of the mean and covariance matrix of the random variables after the interaction for IMM. 2.3.2. Sensor Faults When sensor faults exist, the third-order accuracy condition is no longer met for the standard UKF [42]. Therefore, for the internal IMM-UKF link, the filtering problem to be solved is how to design an effective adaptive UKF algorithm for systems given by Equation (2), when the sensor fault dh exists. out,k 3. Cubature-Principle-Assisted IMM-Adaptive UKF Algorithm (CPIMM-NAUKF) By considering the internal and external link of IMM-UKF respectively, the cubature principle is introduced to the standard IMM, and an adaptive gene matrix is brought into the standard UKF. For IMM, the most important factor on influencing the filtering precision is the non-Gaussian probability density function after the mixing interaction. It is the key to estimating the first moment and second moment of the random variables with high accuracy. For adaptive UKF, one effective method is to balance contribution to UKF filtering solutions of the state information, measurement information, and innovation information. 3.1. CPIMM 3.1.1. Cubature Principle Assuming that c  N c : c ˆ , SS , where the dimension of c is l 1, then the first moment and second moment of any function y = f (c) can be solved by the following integral form: y ˆ = E(y) = f (c)N c : c ˆ , SS dc R (13) P = D(y) = f (c) f (c)N c : c ˆ , SS dc. Equation (13) is a multidimensional integral problem, and it is difficult to solve through analytic methods. The cubature principle states that a cubature point with a weighted value can be chosen to approximate the integral of the multidimensional function, and the method of choosing the cubature point is: Appl. Sci. 2017, 7, 1003 7 of 26 c = c ˆ  Sx , r = 1, 2, . . . , l (14) r r x = le , r = 1, 2, . . . , l (15) r r where e is the rth column of the unit matrix I . ll 3.1.2. Estimating the Interaction by Using CP Since the mixing probability varies with time, the interactive process is abstracted as a nonlinear function, and the cubature principle is used to approximate the probability distribution of the interacting random variables. Firstly, the nonlinear function is used to describe the interaction process. The augmented system c = [x , . . . , x ] is constructed, where x  N x : x ˆ , S S , i = 1, . . . , m is a random variable with 1 i i i the n 1 dimension, and represents the state vector of the model i; l = m n; c ˆ = [x ˆ , . . . , x ˆ ] . 1 m Assuming that x is independent, and we have cov(x ˆ , x ˆ ) = 0, the square root of the covariance i i j i6=j matrix of c is S = diag([S , . . . , S ]), and we have c  N c : c ˆ , SS . 1 m The next step is choosing cubature points for nonlinear propagation. The cubature points are selected by Equation (13), and S is rewritten as S = s V, where V is the upper triangular matrix with all the diagonal elements being 1; s = S(r, r) is the value of the element on the diagonal of S, and the cubature points can be written as c = c ˆ  ls v , r = 1, 2, . . . , l (16) r r r where v is the rth column of V. The cubature points are calculated after the interaction by y = f (c ). (17) Finally, according to the cubature principle, the mean and covariance matrix of y are obtained as y ˆ  w y (18) å r r=l P  w y y ˆ)(y y ˆ) (19) y å r r r r=l where r 6= 0; w = 1/2l. For the external IMM, X shown by Equation (6) can be recalculated by Equation (18), j,k1 and P shown by Equation (7) can be recalculated by Equation (19); then, the newly obtained j,k1 X and P are used in the iteration of the IMM filter, which leads to CP-IMM. j,k1 j,k1 3.1.3. Accuracy Analysis of the CPIMM In the literature [22], the approximate precision of the mean of the Gaussian random variable after nonlinear propagation, by using the cubature principle, has been analyzed. In this section, the accuracy of the covariance matrix of the cubature principle will be analyzed. l n Assume that f (c) 2 R is the nonlinear mapping from R to R ; c  N(c : c ˆ , P); Dc  N(Dc : 0, P). If the Taylor series expansion of y = f (c) is carried out at c ˆ with two derivatives, we have h i 0 T y = f (c ˆ + Dc) = f (c ˆ) + f (c ˆ)Dc + Dc f (c ˆ)Dc (20) 2 i Appl. Sci. 2017, 7, 1003 8 of 26 where f (c ˆ) is the Jacobian matrix with a n l dimension; f (c ˆ) represents n matrixes with n l th dimensions, which are called Hessian matrixes; f (c ˆ) is the i Hessian matrix. Thus, we have ¶ f ¶ f ¶ f i i i f (c ˆ) = , . .., ¶x ¶x ¶x 1 2 l c=c ˆ 2 3 2 2 2 ¶ f ¶ f ¶ f i i i . . . 6 7 ¶x ¶x ¶x ¶x ¶x ¶x 6 1 1 1 2 1 7 6 7 2 2 2 6 7 ¶ f ¶ f ¶ f i i i 6 7 . (21) . . . 6 7 00 ¶x ¶x ¶x ¶x ¶x ¶x 2 1 2 2 2 l 6 7 f (c ˆ) = i 6 7 6 7 . . . . . . . . . . . . 6 7 6 7 6 7 2 2 2 ¶ f ¶ f ¶ f i i i 4 5 . . . ¶x ¶x ¶x ¶x ¶x ¶x l 1 l 2 l l c=c ˆ Calculating the first and second moments of Equation (20), we obtain E(y) = f (c ˆ) + tr( f (c ˆ)P 2 i i h i (22) T 00 00 0 0 1 D(y) = f (c ˆ)P( f (c ˆ)) + tr( f (c ˆ)P f (c ˆ)P) i j i,j where tr() represents the trace of a matrix. Equation (18) can be rewritten as y ˆ = f (c ˆ) + ( f (c ) 2 f (c ˆ) + f (c )). (23) r r 2l r=1 Substituting Equation (23) to (19), and assuming D = ( f (c ) 2 f (c ˆ) + f (c )), we have r r r=1 2l 2l 1 r 1 r T P = ( f (c ) f (c ˆ))() ( f (c ) f (c ˆ))D å å y 2 2l 4l 1 1 2l 1 r 1 T å ( f (c ) f (c ˆ)) D + DD (24) 2 2 4l 4l 1 r 1 T = ( f (c ) f (c ˆ))() DD . 2l 4l r=l When Dc = ls v ! 0 is correct by considering the selection process of cubature points shown r r r in Equation (17), we have f (c ) f (c ˆ) ! f (c ˆ)u ls (25) r r+l f (c ) 2 f (c ˆ) + f (c ) ! [u f (c ˆ)u ] . 2 i ls Substituting Equation (25) into Equations (23) and (24), respectively, we obtain y ˆ ! f (c ˆ) + tr( f (c ˆ)P i i (26) h i T 00 00 0 0 P ! f (c ˆ)P( f (c ˆ)) + tr( f (c ˆ)P f (c ˆ)P) . i j 2 i,j Appl. Sci. 2017, 7, 1003 9 of 26 Equations (22) and (26) show that the mean and covariance estimates can achieve second-order accuracy of a Taylor series expansion using the cubature point to create a random variable through the nonlinear function. The one-dimensional case in Section 2.3 is used to illustrate that the mean value obtained by the cubature principle is 0.6, and the covariance is 1.3826, which is much closer to the results obtained by Monte Carlo method than the MSE method. 3.2. New Adaptive Unscented Kalman Filter (NAUKF) Motivated by the proposed UKF algorithm, we propose a new adaptive UKF (NAUKF) consisting of an adaptive matrix gene. P , P and P represent the corresponding covariance k/k1 (XZ)k/k1 (ZZ)k/k1 e e e computed by the standard UKF, and they are changed into P , P and P in k/k1 (XZ)k/k1 (ZZ)k/k1 the NAUKF. Theorem 1. On the basis of standard UKF, set up a new adaptive matrix geneD as ( ) 2n (c) (i) (i) ˆ ˆ D = P W [g Z ][g Z ] R . (27) k ee å k/k1 k/k1 k (ZZ)k/k1 i k/k1 k/k1 i=0 Let 2n (c) (i) (i) ˆ ˆ P = W [g Z ][g Z ] + D R (28) å k/k1 k/k1 k k (ZZ)k/k1 i k/k1 k/k1 i=0 (i) (c) where g is calculated from the Sigma points in the standard UKF; Z is the innovation; W denotes k/k1 k/k1 i e ˆ e the weighted coefficient [3]; Z = Z Z ; and P is the covariance of Z ; Then the k k/k1 ee k/k1 k/k1 (ZZ)k/k1 NAUKF can address the filtering problem in the presence of sensor faults shown by Equation (2). Proof. Because model mismatches exist in the system shown by Equation (2), based on Theorem 1, the result is P = P = P (29) k/k1 e e k/k1 (XX)k/k1 P = P = P (30) ee (XZ)k/k1 (XZ)k/k1 (XZ)k/k1 e ˆ where X = X X . k/k1 k k/k1 After setting up D shown by Equation (27), we have P = P = P . (31) ee (ZZ)k/k1 (ZZ)k/k1 (ZZ)k/k1 From Equations (29) to (31), we know that the third-order precision conditions can be satisfied, which means that the NAUKF can be used for the system that has sensor faults. This completes the proof. To avoid the divergence problem of NAUKF, a covariance matching criterion is used: e e tr(Z Z )  Y tr(P ) (32) k/k1 k/k1 (ZZ)k/k1 where Y (Y  1) is a preset adjustable coefficient. If Equation (32) is satisfied, the NAUKF has been convergent; P should be updated with an k/k1 adaptive weighted coefficient z by increasing the contribution of Z on the filtering solutions. k k 2n (c) (i) (i) ˆ ˆ P = z W [c X ][c X ] + Q (33) k/k1 kå k/k1 k/k1 k1 i k/k1 k/k1 i=0 where z is set by k Appl. Sci. 2017, 7, 1003 10 of 26 z z  1 0 0 z = (34) Appl. Sci. 2017, 7, 1003 1 z < 1 10 of 26 tr(P R ) ee k tr(PR − ) (ZZ)k/k1  k () ZZ k/ k −1 z = . (35) 0 ζ = 0 2n 2n T (c) (i) (i) (ci ) () ()i T (35) ˆ ˆ ˆ ˆ tr( W [c X ][c X ] ) å tr( W [χ −− X ][χ X ] )  k/k1 k/k1 i i k/kk1/1kk−− /k 1 k k/1 /kk k−− 1 /k 1 i=0 i = 0 3.3. Implementation Steps of the CPIMM-NAUKF Algorithm 3.3. Implementation Steps of the CPIMM-NAUKF Algorithm Based on Sections 3.1 and 3.2, the CPIMM-NAUKF structure is shown in Figure 3. The Based on Sections 3.1 and 3.2, the CPIMM-NAUKF structure is shown in Figure 3. implementation steps of the CPIMM-NAUKF are as follows. The implementation steps of the CPIMM-NAUKF are as follows. Step 1: NAUKF filtering process. Step 1: NAUKF filtering process. First, calculate the covariance by setting X and P of the standard UKF, and compute First, calculate the covariance by setting X and P of the standard UKF, and compute 0 0 00 P , P , P . Next, select the new adaptive gene selection by setting upΔD through PP,,P . Next, select the new adaptive gene selection by setting up through k/k1 (XZ)k/k1 (ZZ)k/k1 k k/1 k−− (XZ)kk/1 (ZZ)kk/1− k Equation (27), and recalculate P with Equation (28) (ZZ)k/k1 Equation (27), and recalculate P with Equation (28) () ZZk/k −1 2n 2n (c) (ci ()i) () ()i (i) T ˆˆ ˆ ˆ P =− W[] γ Z [] γ −Z +Δ R P = W [g Z ][g Z ] . + D R . (36)  (36) (ZZ)k/k1(ZZ)k/1 å k−− i kk/1 kk/1 /k− k1 kk/1− k/1 k− k/k k 1k k k i k/k1 k/k1 i =0 i=0 1,k −1 o 1,k 1,k −1 1,k 1,k −1 P ˆ 1,k −1 X fusion,k 2,k −1 fusion,k 2,k −1 2,k 2,k −1 o P 2,k 2,k −1 mk,1 − mk,1 − o mk , mk,1 − mk , mk,1 − Figure 3. The cubature principle assisted IMM-adaptive UKF algorithm (CPIMM-NAUKF) structure. Figure 3. The cubature principle assisted IMM-adaptive UKF algorithm (CPIMM-NAUKF) structure. To determine the convergence strategy, judge the divergence according to Equation (32). If it is satisfied, To determi go directly ne the convergence stra to the next step; otherwise, tegy, judge the di update Pvergence by using accord Equations ing to Equ (33)–(35). ation (32). If it is k/k1 satisfi Next, ed, go todi obtain rectly to the next step; ot the NUKF filteringherwi results, se, use upda the te following: P by using Equations (33)–(35). kk/1 − Next, to obtain the NUKF filtering results, use the following: K = P (P ) (37) (XZ)k/k1 (ZZ)k/k1 −1 KP = () P (37) k (XZk) /1 k−− (ZZk) /1 k ˆˆ ˆ XX=+K() Z−Z (38) kk/1k−− k k k/1k PP=−KP K (39) kk/1k−− k (ZZ)k/1k k Appl. Sci. 2017, 7, 1003 11 of 26 ˆ ˆ ˆ X = X + K (Z Z ) (38) k k/k1 k k k/k1 P = P K P K (39) k k/k1 k (ZZ)k/k1 k ˆ ˆ ˆ From Equations (38) and (39), obtain the X , P , X , P , . . . , and X , P for UKF filters 1 1,k 1,k 2,k 2,k m,k m,k to m of IMM-UKF. Step 2: The CPIMM filtering process. First, compute the mixing probability. Calculate the mixing probability u according to ij,k1 ˆ ˆ ˆ Equation (3). Next, determine the Interaction by using CP. After X , P , X , P , . . . , and X , P 1,k 1,k 2,k 2,k m,k m,k are obtained based on Step 1, calculate X and P , respectively, with Equations (18) and (19). j,k1 j,k1 Then update the model probability by calculating the mixing probability u according to Equation (5). j,k1 To complete the fusion, calculate resultant mean and covariance matrix with Equations (8) and (9). 4. Experimental Results and Discussion To verify the effectiveness of the maneuvering target tracking of the proposed CPIMM-NAUKF, numerical simulations and practical experiments are both conducted. For the numerical simulation, two sets of experiments were simulated: (1) the standard IMM [9] and CPIMM algorithms; (2) the IMM-UKF [43], IMM-NAUKF connecting the standard IMM with NAUKF, CPIMM-UKF connecting the CPIMM with standard UKF, and CPIMM-NAUKF algorithms. The experimental environment was set as Intel i7 computer with 4-core, 64-bit, 2.4 GHz, 8 GB RAM, and MATLAB R2013a software. For the practical experiments, an integrated navigation system and a synthetic aperture radar (SAR) tracking system using the CPIMM-NAUKF algorithm are, respectively, shown to test the proposed method with real data. 4.1. Numerical Simulations and Analysis . . To design the simulation scenarios, suppose that X = [x, y, x, y] represents the state of the maneuvering target, including the position and velocity. The sensor is the radar, and its position is fixed at the coordinate origin. Thus, the measurement equation is " # " # 2 2 r (x + y ) k k k Z = = + V (40) k k q tan (y /x ) k k k where r and q are the distance and angle measurement components of the radar, respectively. k k The high-speed maneuvering target moves in a two-dimensional scenario with 100 s; the initial position is [0 m, 400 m], and the initial velocity is [10 m/s, 40 m/s]; the target moves at a constant velocity in a straight line from 0 s–20 s, with a constant right turn rate being 0.1 rad/s from 20 s–40 s, constant velocity in a straight line at 40 s–60 s, constant left turn with the turn rate being 0.1 rad/s at 60 s–80 s, and a constant velocity in a straight line at 80 s–100 s. In a Cartesian coordinate system, the true trajectory of the maneuvering target is depicted in Figure 4. Appl. Sci. 2017, 7, 1003 12 of 26 Appl. Sci. 2017, 7, 1003 12 of 26 Destination point Start point 0 200 400 600 800 1000 1200 1400 1600 1800 2000 X/(m) Figure 4. True trajectory of the maneuvering target. Figure 4. True trajectory of the maneuvering target. Both the IMM and CPIMM include three models: one Constant Velocity (CV) model [44] and Both the IMM and CPIMM include three models: one Constant Velocity (CV) model [44] and two two Coordinated Turn (CT) models [44]. Coordinated Turn (CT) models [44]. For the CV model, the state transition matrix is For the CV model, the state transition matrix is 10 T 0   2 3   1 T 0 0 01 0 0   6 7 F = (41) CV 0 1 0 0 6 7 00 1 T F = (41) 6 7 CV   4 0 0 1 T 5 000 1   0 0 0 1 where T is the sampling period, and T = 1 s. where T is the sampling period, and T = 1 s. The process noise is calculated with The process noise is calculated with V = N (.; 0;σ ) (42) CV CV V = N(.; 0; s ) (42) CV CV 2 2 where σ denotes the covariance of the process noise, and σ = 5 . CV CV 2 2 For the CT model, the state transition matrix is where s denotes the covariance of the process noise, and s = 5. CV CV For the CT model, the state transition matrix is sin(ωω TT ) 1 − cos( )  10 −  2 3 ωω sin(wT) 1cos(wT)  1 0 w w 0 cos()ωω TT 0 −sin() 6 7  0 cos(wT) 0 sin(wT) 6 7 F = (43) CT  F = 6 7 (43) CT 1 −1c os( cosωω (wTT T )) sin sin( (wT) ) 4 5 0 1  01 w w ωω  0 sin(wT) 0 cos(wT)  0sin(ωω TT ) 0 cos( )  where w is the turn rate. where ω is the turn rate. The process noise meets with The process noise meets with V = N(.; 0; s ) (44) CT CT V = N (.; 0;σ ) (44) CT CT 2 2 where s denotes the covariance of the process noise, and s = 20. CT CT 2 2 The two models use the following transition and mode probabilities. where σ denotes the covariance of the process noise, and σ = 20 . CT CT The transition probability matrix is The two models use the following transition and mode probabilities. The transition probability matrix is Y/(m) Appl. Sci. 2017, 7, 1003 13 of 26 2 3 0.99 0.005 0.005 6 7 0.005 0.99 0.005 . 4 5 0.005 0.005 0.99 The initial mode probability matrix is [ 0.99 0.005 0.005]. 2 2 The measurement covariance is R = diag s , s = diag(15, 15). w w Case 1 is where the sensor has no faults and is in a normal work situation. Case 2 is when the sensor has faults, and dh = [dr dq ] , where dr and dq represents the distance deviation within out,k k k k k [5 m, 5 m] and the angle drift rate within [0 1.0 10 rad/s], respectively. Every simulation experiment is tested 100 times using Monte Carlo, and algorithms of two sets of comparison simulations are evaluated by root mean square error (RMSE), which is computed through i i i i ˆ ˆ E = X X )(X X (45) i=1 i th where X and X are the estimated and true state in the i simulation, respectively; N is the number of simulations; N = 100. 4.1.1. CPIMM Simulation To validate the effectiveness of CPIMM, the first set of comparisons, including the standard IMM and CPIMM, was simulated. We assumed there were no sensor faults, similar to the Simulation Case 1 in the scenario design section. Figure 5 gives the true and estimated trajectories of the maneuvering target. As seen in Figure 5, both CPIMM and IMM can track the trajectory of the target. Appl. Sci. 2017, 7, 1003 14 of 26 True IMM Destination point CPIMM Start point 0 200 400 600 800 1000 1200 1400 1600 1800 2000 X/(m) Figure 5. True and estimated trajectories of the maneuvering target for CPIMM for Case 1. Figure 5. True and estimated trajectories of the maneuvering target for CPIMM for Case 1. IMM CPIMM 0 10 20 30 40 50 60 70 80 90 100 Time/(sec) Figure 6. Position root mean square error (RMSE) comparison for CPIMM for Case 1. Y/(m) Position RMSE/(m/s) Appl. Sci. 2017, 7, 1003 14 of 26 True IMM Destination point CPIMM Appl. Sci. 2017, 7, 1003 14 of 26 Start point The comparisons of the RMSE position and velocity curves of the two filtering algorithms are shown in Figur 200 es 6 and 7, and the corresponding statistical data of RMSE are listed in Table 2. The filtering precision of the IMM is lower than the CPIMM, although the IMM is not divergent. The CPIMM greatly increases the filtering precision compared with IMM. In addition, this means that 0 200 400 600 800 1000 1200 1400 1600 1800 2000 the tracking performance of CPIMM is higher than X/that (m) of the IMM, because the cubature principle is used to increase the estimation values in the interaction process of the IMM. Figure 5. True and estimated trajectories of the maneuvering target for CPIMM for Case 1. IMM CPIMM 0 10 20 30 40 50 60 70 80 90 100 Time/(sec) Figure 6. Position root mean square error (RMSE) comparison for CPIMM for Case 1. Appl. Sci. 2017, 7, 1003 15 of 26 Figure 6. Position root mean square error (RMSE) comparison for CPIMM for Case 1. IMM CPIMM 0 10 20 30 40 50 60 70 80 90 100 Time/(sec) Figure 7. Velocity RMSE comparison for CPIMM for Case 1. Figure 7. Velocity RMSE comparison for CPIMM for Case 1. Table 2. Performance comparison of the algorithms for CPIMM for Case 1. Position Error (m) Velocity Error (m/s) Algorithm Mean Standard Deviation Mean Standard Deviation IMM 3.8245 2.0377 0.7102 0.6935 CPIMM 2.0630 1.1604 0.4596 0.4187 4.1.2. CPIMM-NAUKF Simulation To validate the effectiveness of CPIMM-NAUKF, the second set of comparisons, including the IMM-UKF, IMM-NAUKF, CPIMM-UKF, and CPIMM-NAUKF were simulated, assuming there are sensor faults, similar to Simulation Case 2 in the scenario design section. Figure 8 gives the true and estimated trajectories of the maneuvering target. Both CPIMM and IMM can track the trajectory of the target, but the estimated trajectory of CPIMM-NAUKF was closest to the true trajectory. Y/(m) Position RMSE/(m/s) Velocity RMSE/(m/s) Appl. Sci. 2017, 7, 1003 15 of 26 Table 2. Performance comparison of the algorithms for CPIMM for Case 1. Position Error (m) Velocity Error (m/s) Algorithm Mean Standard Deviation Mean Standard Deviation IMM 3.8245 2.0377 0.7102 0.6935 CPIMM 2.0630 1.1604 0.4596 0.4187 4.1.2. CPIMM-NAUKF Simulation To validate the effectiveness of CPIMM-NAUKF, the second set of comparisons, including the IMM-UKF, IMM-NAUKF, CPIMM-UKF, and CPIMM-NAUKF were simulated, assuming there are sensor faults, similar to Simulation Case 2 in the scenario design section. Figure 8 gives the true and estimated trajectories of the maneuvering target. Both CPIMM and IMM can track the trajectory of the target, but the estimated trajectory of CPIMM-NAUKF was closest to the true trajectory. Appl. Sci. 2017, 7, 1003 16 of 26 True IMM-UKF Destination point IMM-NAUKF CPIMM-UKF CPIMM-NAUKF Start point 0 200 400 600 800 1000 1200 1400 1600 1800 2000 X/(m) Figure 8. True and estimated trajectories of the maneuvering target for CPIMM-NAUKF for Case 2. Figure 8. True and estimated trajectories of the maneuvering target for CPIMM-NAUKF for Case 2. The comparisons of the RMSE position and velocity curves of four filtering algorithms are The comparisons of the RMSE position and velocity curves of four filtering algorithms are shown in Figures 9 and 10, and the corresponding statistical data of RMSE are listed in Table 3. The shown in Figures 9 and 10, and the corresponding statistical data of RMSE are listed in Table 3. sensor faults negatively influenced the filtering precision of the UKF and the filtering error of the The sensor faults negatively influenced the filtering precision of the UKF and the filtering error of standard IMM-UKF was the largest. The IMM-NAUKF has better filtering accuracy than the the standard IMM-UKF was the largest. The IMM-NAUKF has better filtering accuracy than the IMM-UKF, because the internal link NAUKF effectively handled the sensor fault problem, but the IMM-UKF, because the internal link NAUKF effectively handled the sensor fault problem, but the filtering convergence performance was poor. The CPIMM-UKF also has better filtering accuracy filtering convergence performance was poor. The CPIMM-UKF also has better filtering accuracy than than the IMM-UKF because the internal link CPIMM has a better effect for maneuvering target the IMM-UKF because the internal link CPIMM has a better effect for maneuvering target tracking tracking than IMM, but the filtering accuracy relationship between IMM-NAUKF and CPIMM-UKF than IMM, but the filtering accuracy relationship between IMM-NAUKF and CPIMM-UKF does not does not exist. Ultimately, CPIMM-NAUKF has the best filtering accuracy, since it significantly exist. Ultimately, CPIMM-NAUKF has the best filtering accuracy, since it significantly improves the improves the IMM-UKF through the external and internal links, which makes it extremely effective IMM-UKF through the external and internal links, which makes it extremely effective for maneuvering for maneuvering target tracking when sensor faults occur. target tracking when sensor faults occur. In summary, as a combination of CPIMM and NAUKF, the CPIMM-NAUKF, compared with 1710 .5 the existing IMM-UKF, makes a greater contribution to filtering performance in terms of accuracy IMM-UKF and convergence speed. This is because it includes two important links, the external CPIMM and the IMM-NAUKF CPIMM-UKF internal NAUKF, which work simultaneously and effectively. CPIMM-NAUKF 12.5 7.2 5 2.5 -2 -4 10 40 70 80 0 100 20 20 0 30 30 0 400 50 50 0 60 60 0 700 800 90 90 0 10 1000 0 Time/(sec) Figure 9. Position RMSE comparison for CPIMM-NAUKF for Case 2. Position RMSE/(m) Y/(m) Appl. Sci. 2017, 7, 1003 16 of 26 True IMM-UKF Destination point IMM-NAUKF CPIMM-UKF CPIMM-NAUKF Start point 0 200 400 600 800 1000 1200 1400 1600 1800 2000 X/(m) Figure 8. True and estimated trajectories of the maneuvering target for CPIMM-NAUKF for Case 2. The comparisons of the RMSE position and velocity curves of four filtering algorithms are shown in Figures 9 and 10, and the corresponding statistical data of RMSE are listed in Table 3. The sensor faults negatively influenced the filtering precision of the UKF and the filtering error of the standard IMM-UKF was the largest. The IMM-NAUKF has better filtering accuracy than the IMM-UKF, because the internal link NAUKF effectively handled the sensor fault problem, but the filtering convergence performance was poor. The CPIMM-UKF also has better filtering accuracy than the IMM-UKF because the internal link CPIMM has a better effect for maneuvering target tracking than IMM, but the filtering accuracy relationship between IMM-NAUKF and CPIMM-UKF does not exist. Ultimately, CPIMM-NAUKF has the best filtering accuracy, since it significantly improves the IMM-UKF through the external and internal links, which makes it extremely effective Appl. Sci. 2017, 7, 1003 16 of 26 for maneuvering target tracking when sensor faults occur. 1710 .5 IMM-UKF IMM-NAUKF CPIMM-UKF CPIMM-NAUKF 12.5 7.2 5 2. -2 5 -4 0 10 100 20 20 0 30 30 0 40 400 50 50 0 60 60 0 70 70 0 80 80 0 90 90 0 10 1000 0 Time/(sec) Appl. Sci. 2017, 7, 1003 17 of 26 Figure Figure 9. 9. Position Position RMSE RMSE comparison comparison for C for CPIMM-NAUKF PIMM-NAUKF fo for r CCase ase 2.2. IMM-UKF IMM-NAUKF 3.6 5 CPIMM-UKF CPIMM-NAUKF 2.4 5 3 2 1.2 5 1 1 0.0 5 -1 10 40 70 80 0 40 80 20 12 30 0 160 200 50 240 60 280 320 36 90 0 100 400 Time/(sec) Figure Figure 10. 10. Velo Velocity city RMS RMSE E c comparison omparison for CP for CPIMM-NAUKF IMM-NAUKF fofor r Case 2 Case. 2. Table 3. Performance comparison for CPIMM-NAUKF for Case 2. Table 3. Performance comparison for CPIMM-NAUKF for Case 2. Position Error (m) Velocity Error (m/s) Algorithm Position Error (m) Velocity Error (m/s) Algorithm Mean Standard Deviation Mean Standard Deviation Mean Standard Deviation Mean Standard Deviation IMM-UKF 11.8533 4.3387 3.1026 1.9552 IMM-UKF 11.8533 4.3387 3.1026 1.9552 IMM-NAUKF 7.0473 2.9116 1.2143 1.0348 IMM-NAUKF 7.0473 2.9116 1.2143 1.0348 CPIMM-UKF 9.3558 3.0098 2.4961 1.2246 CPIMM-UKF 9.3558 3.0098 2.4961 1.2246 CPIMM-NAUKF 4.8671 0.9169 0.4294 0.3857 CPIMM-NAUKF 4.8671 0.9169 0.4294 0.3857 In summary, as a combination of CPIMM and NAUKF, the CPIMM-NAUKF, compared with the existing IMM-UKF, makes a greater contribution to filtering performance in terms of accuracy and convergence speed. This is because it includes two important links, the external CPIMM and the internal NAUKF, which work simultaneously and effectively. 4.2. Practical Experiments and Analysis Furthermore, to demonstrate the effectiveness of the proposed CPIMM-NAUKF method for real applications, two practical experiments were conducted. An unmanned aerial vehicle (UAV) was loaded with an Inertial Navigation System (INS)/Global Positioning System (GPS) integrated navigation system for its navigation and positioning, and equipped with an SAR tracking system to track the maneuvering targets. 4.2.1. INS/GPS Integrated Navigation System As shown in Figures 11 and 12, the vehicle loaded integrated navigation system included a NV-IMU300 type INS, a JAVAD Lexon-GGD112T type GPS receiver, and a NovAtel DL-V3 GPS receiver on the vehicle, and another NovAtel DL-V3 GPS receiver was located at the Earth Reference Station. The working frequency of the GPS was 20 Hz. The maximum distance between the vehicle and the Earth Reference Station had to be less than 60 km, which ensured that the positioning precision of the vehicle was better than 0.1 m. Let the geographic coordinate system be the navigation coordinate system with three directions of East, North, Up. Define the system states as Position RMSE/(m) Velocity RMSE/(m) Y/(m) Appl. Sci. 2017, 7, 1003 17 of 26 4.2. Practical Experiments and Analysis Furthermore, to demonstrate the effectiveness of the proposed CPIMM-NAUKF method for real applications, two practical experiments were conducted. An unmanned aerial vehicle (UAV) was loaded with an Inertial Navigation System (INS)/Global Positioning System (GPS) integrated navigation system for its navigation and positioning, and equipped with an SAR tracking system to track the maneuvering targets. 4.2.1. INS/GPS Integrated Navigation System As shown in Figures 11 and 12, the vehicle loaded integrated navigation system included a NV-IMU300 type INS, a JAVAD Lexon-GGD112T type GPS receiver, and a NovAtel DL-V3 GPS receiver on the vehicle, and another NovAtel DL-V3 GPS receiver was located at the Earth Reference Station. The working frequency of the GPS was 20 Hz. The maximum distance between the vehicle and the Earth Reference Station had to be less than 60 km, which ensured that the positioning precision of the vehicle was better than 0.1 m. Let the geographic coordinate system be the navigation coordinate system with three directions of East, North, Up. Define the system states as X(t) = y, q, g, v , v , v , L, l, h, # , # , # ,r ,r ,r (46) E N U x y z x y z where y, q, g represent the heading angle, the pitch angle, and the roll angle of the vehicle, respectively; v , v , v are components of the velocity along the East, North, and Up directions, respectively; E N U L, l, h are components of the position along the East, North, and Up directions, respectively; # , # , # x y z represent the drift of the gyro; r ,r ,r represent the bias of the accelerator. x y z The state space model is X t == f X t + W t (47) ( ) [ ( )] ( ) the concrete equations of which can be found in [45]. Select the velocity and position outputs of the GPS as the measurement variables: Z = [v , v , v , L , l , h ] (48) EG N G U G G G G where v , v , v are components of the velocity measured via GPS along the East, North, and EG N G U G Up directions, respectively; L , l , h are components of the position measured via GPS along East, G G G North, and Up directions, respectively. The velocity and position information measured via GPS can be used as a reference to confirm the position error of the INS/GPS. The measurement equation is Z = H X + V (49) k k k k where H = [O , O , O ]. 63 66 66 In the experiment process, the initial position was as follows: East longitude 108.891227 , North latitude 34.286866 , and an altitude of 437.92 m. Components of the velocity along the East, North, and Up directions were, respectively, 8 m/s, 35 m/s, and 0 m/s; the drift of the gyro was 0.12 /h, and the white noise was 0.02 /h; the bias of the accelerator was 10 g, and the white noise was 10 g; the positioning and position precision of the GPS were 5 m and 0.05 m/s; the other initial parameters were the same as the those described in the simulation section. The experiment test time was designed as 500 s with the sample period being 1 s, and the end point of the vehicle was East longitude 108.995683 , North latitude 34.285292 , and an altitude of 438.01 m. The practical road condition was the urban highway, and the real route of the vehicle is shown in Figure 13. Appl. Sci. 2017, 7, 1003 18 of 26 Appl. Sci. 2017, 7, 1003 18 of 26 Xt,=∇ ψθ,γ,v,v,v,L,λ,h,ε,ε,ε, ,∇,∇ (46) () EN U x y z x y z   Xt,=∇ ψθ,γ,v,v,v,L,λ,h,ε,ε,ε, ,∇,∇ (46) () EN U x y z x y z  where ψ,, θ γ represent the heading angle, the pitch angle, and the roll angle of the vehicle, where ψ,, θ γ represent the heading angle, the pitch angle, and the roll angle of the vehicle, respectively; vv,,v are components of the velocity along the East, North, and Up directions, E NU respectively; vv,,v are components of the velocity along the East, North, and Up directions, E NU respectively; L,, λ h are components of the position along the East, North, and Up directions, respectively; L,, λ h are components of the position along the East, North, and Up directions, respectively; εε,,ε represent the drift of the gyro; represent the bias of the ∇∇,,∇ x yz x yz respectively; εε,,ε represent the drift of the gyro; ∇∇,,∇ represent the bias of the x yz x yz accelerator. accelerator. The state space model is The state space model is Xf (tt ) == X () +W ()t (47)  Xftt == X +Wt ( ) () () (47)  the concrete equations of which can be found in [45]. the concrete equations of which can be found in [45]. Select the velocity and position outputs of the GPS as the measurement variables: Select the velocity and position outputs of the GPS as the measurement variables: Z =[] v,v ,v ,L ,λ ,h (48) EG NG UG G G G Z = v,v ,v ,L ,λ ,h (48) [] EG NG UG G G G where vv,,v are components of the velocity measured via GPS along the East, North, and Up EGNG UG where vv,,v are components of the velocity measured via GPS along the East, North, and Up EGNG UG directions, respectively; L ,, λ h are components of the position measured via GPS along East, GG G directions, respectively; L ,, λ h are components of the position measured via GPS along East, GG G North, and Up directions, respectively. The velocity and position information measured via GPS North, and Up directions, respectively. The velocity and position information measured via GPS can be used as a reference to confirm the position error of the INS/GPS. can be used as a reference to confirm the position error of the INS/GPS. The measurement equation is The measurement equation is ZH=+X V (49) kk k k ZH=+X V (49) kk k k Appl. Sci. 2017, 7, 1003 18 of 26 where H = 00,,0 . [ ] k 63×× 6 6 6×6 where H = [00,,0 ] . k 63×× 6 6 6×6 Figure 11. The experimental vehicle. Figure 11. The experimental vehicle. Figure 11. The experimental vehicle. Appl. Sci. 2017, 7, 1003 19 of 26 North, and Up directions were, respectively, −8 m/s, −35 m/s, and 0 m/s; the drift of the gyro was −3 0.12°/h, and the white noise was 0.02°/h; the bias of the accelerator was 10 g, and the white noise −4 was 10 g; the positioning and position precision of the GPS were 5 m and 0.05 m/s; the other initial parameters were the same as the those described in the simulation section. The experiment test time was designed as 500 s with the sample period being 1 s, and the end point o f the vehicle was East Figure 12. The vehicle loaded navigation equipment. longitude 108.995683°, North latitude 34.285292°, and an altitude of 438.01 m. The practical road Figure 12. The vehicle loaded navigation equipment. Figure 12. The vehicle loaded navigation equipment. condition was the urban highway, and the real route of the vehicle is shown in Figure 13. In the experiment process, the initial position was as follows: East longitude 108.891227°, In the experiment process, the initial position was as follows: East longitude 108.891227°, North latitude 34.286866°, and an altitude of 437.92 m. Components of the velocity along the East, North latitude 34.286866°, and an altitude of 437.92 m. Components of the velocity along the East, Figure 13. The real route of the vehicle in navigation. Figure 13. The real route of the vehicle in navigation. Five filtering methods including standard EKF [33], CKF [40], standard UKF [27], existing adaptive UKF (AUKF) [38], and NAUKF were used in the experiment, and these algorithms were evaluated by the estimation error rather than RMSE, which is computed through (50) E=−XX The comparisons of the longitude and latitude error curves of five filtering algorithms are shown in Figures 14 and 15, and the corresponding statistical data of the mean absolute error are listed in Table 4. We can see that the EKF and CKF have almost the same filtering accuracy, although both EKF and CKF can deal with the nonlinear filtering problems—they only have first order accuracy. The filtering errors are, respectively, [−18.1025, 19.9471] m and [−17.5118, 18.4296] m of the longitude error ranging, and [−20.1143, 16.2477] m and [−17.4154, 14.9953] m of the latitude error ranging. The standard UKF has better filtering precision than does EKF and CKF, because UKF can reach third order accuracy, but it still has substantial oscillation and error due to the longitude error ranging [−11.0549, 10.2178] m and latitude error ranging [−9.1766, 9.9737] m. The AUKF has the adaptive ability of the imprecise noise in the urban complex environment, so it has better filtering accuracy than the UKF with the longitude error ranging [−5.8142, 6.0273] m and latitude error ranging [−8.1184, 6.4153] m. Ultimately, NAUKF has the best filtering accuracy, with the longitude error ranging [−4.1109, 4.3804] m and latitude error ranging [−6.1096, 4.7138] m, since it can not only adapt to the noise but also the sensor faults and model mismatches. Appl. Sci. 2017, 7, 1003 19 of 26 Five filtering methods including standard EKF [33], CKF [40], standard UKF [27], existing adaptive UKF (AUKF) [38], and NAUKF were used in the experiment, and these algorithms were evaluated by the estimation error rather than RMSE, which is computed through E = X X (50) The comparisons of the longitude and latitude error curves of five filtering algorithms are shown in Figures 14 and 15, and the corresponding statistical data of the mean absolute error are listed in Table 4. We can see that the EKF and CKF have almost the same filtering accuracy, although both EKF and CKF can deal with the nonlinear filtering problems—they only have first order accuracy. The filtering errors are, respectively, [18.1025, 19.9471] m and [17.5118, 18.4296] m of the longitude error ranging, and [20.1143, 16.2477] m and [17.4154, 14.9953] m of the latitude error ranging. The standard UKF has better filtering precision than does EKF and CKF, because UKF can reach third order accuracy, but it still has substantial oscillation and error due to the longitude error ranging [11.0549, 10.2178] m and latitude error ranging [9.1766, 9.9737] m. The AUKF has the adaptive ability of the imprecise noise in the urban complex environment, so it has better filtering accuracy than the UKF with the longitude error ranging [5.8142, 6.0273] m and latitude error ranging [8.1184, 6.4153] m. Ultimately, NAUKF has the best filtering accuracy, with the longitude error ranging [4.1109, 4.3804] m and latitude error ranging [6.1096, 4.7138] m, since it can not only adapt to the noise but also the sensor faults and Appl. Sci. 2017, 7, 1003 20 of 26 model mismatches. EK F CK F UK F AUKF NA UK F -5 -10 -15 -20 0 50 100 150 200 250 300 350 400 450 500 Tim e/(s ec ) Figure 14. Longitude error comparison for NAUKF in INS/GPS navigation system. Figure 14. Longitude error comparison for NAUKF in INS/GPS navigation system. EKF CK F UK F AUKF NA UK F -5 -10 -15 -20 0 50 100 150 200 250 300 350 400 450 500 Tim e/(s ec ) Figure 15. Latitude error comparison for NAUKF in INS/GPS navigation system. Longitude E rror/(m ) Latitude E rror/(m ) Appl. Sci. 2017, 7, 1003 20 of 26 EK F CK F UK F AUKF NA UK F -5 -10 -15 -20 0 50 100 150 200 250 300 350 400 450 500 Tim e/(s ec ) Appl. Sci. 2017, 7, 1003 20 of 26 Figure 14. Longitude error comparison for NAUKF in INS/GPS navigation system. EKF CK F UK F AUKF NA UK F -5 -10 -15 -20 0 50 100 150 200 250 300 350 400 450 500 Tim e/(s ec ) Figure 15. Latitude error comparison for NAUKF in INS/GPS navigation system. Figure 15. Latitude error comparison for NAUKF in INS/GPS navigation system. Table 4. Performance comparison for NAUKF in INS/GPS navigation system. Mean Absolute Error of Longitude (m) Mean Absolute Error of Latitude (m) Algorithm Mean Standard Deviation Mean Standard Deviation EKF 8.6533 7.9816 8.2107 7.0044 CKF 8.4154 7.5127 8.0109 6.8135 UKF 6.9018 6.5011 6.9271 6.6102 AUKF 4.4225 4.0118 4.8159 4.6825 NAUKF 3.0156 2.8105 2.8192 2.4947 In summary, UKF has better filtering precision than KF, EKF, and CKF, because UKF can reach third order accuracy. The proposed NAUKF has the best filtering performance in terms of precision and dynamic response for its adaptive ability to both imprecise noise and sensor faults, and it can increase the precision and adaptability of the navigation system effectively. 4.2.2. SAR Tracking System As shown in Figures 16 and 17, the UAV is equipped with one D3160 type SAR. The UAV is a six-rotor UAV with mass being 20 kg and velocity range being 0 m/s–20 m/s. The D3160 type SAR is a kind of micro-SAR system and the parameters are as follows: mass: 4 kg, operating frequency band: X or Ku, detection range: 10 km, resolution: 0.3 m. Longitude E rror/(m ) Latitude E rror/(m ) Appl. Sci. 2017, 7, 1003 21 of 26 Appl. Sci. 2017, 7, 1003 21 of 26 Table 4. Performance comparison for NAUKF in INS/GPS navigation system. Table 4. Performance comparison for NAUKF in INS/GPS navigation system. Mean Absolute Error of Longitude (m) Mean Absolute Error of Latitude (m) Algorithm Mean Absolute Error of Longitude (m) Mean Absolute Error of Latitude (m) Mean Standard Deviation Mean Standard Deviation Algorithm Mean Standard Deviation Mean Standard Deviation EKF 8.6533 7.9816 8.2107 7.0044 EKF 8.6533 7.9816 8.2107 7.0044 CKF 8.4154 7.5127 8.0109 6.8135 CKF 8.4154 7.5127 8.0109 6.8135 UKF 6.9018 6.5011 6.9271 6.6102 UKF 6.9018 6.5011 6.9271 6.6102 AUKF 4.4225 4.0118 4.8159 4.6825 AUKF 4.4225 4.0118 4.8159 4.6825 NAUKF 3.0156 2.8105 2.8192 2.4947 NAUKF 3.0156 2.8105 2.8192 2.4947 In summary, UKF has better filtering precision than KF, EKF, and CKF, because UKF can reach In summary, UKF has better filtering precision than KF, EKF, and CKF, because UKF can reach third order accuracy. The proposed NAUKF has the best filtering performance in terms of precision third order accuracy. The proposed NAUKF has the best filtering performance in terms of precision and dynamic response for its adaptive ability to both imprecise noise and sensor faults, and it can and dynamic response for its adaptive ability to both imprecise noise and sensor faults, and it can increase the precision and adaptability of the navigation system effectively. increase the precision and adaptability of the navigation system effectively. 4.2.2. SAR Tracking System 4.2.2. SAR Tracking System As shown in Figures 16 and 17, the UAV is equipped with one D3160 type SAR. The UAV is a As shown in Figures 16 and 17, the UAV is equipped with one D3160 type SAR. The UAV is a six-rotor UAV with mass being 20 kg and velocity range being 0 m/s–20 m/s. The D3160 type SAR is six-rotor UAV with mass being 20 kg and velocity range being 0 m/s–20 m/s. The D3160 type SAR is a kind of micro-SAR system and the parameters are as follows: mass: 4 kg, operating frequency Appl. Sci. 2017, 7, 1003 21 of 26 a kind of micro-SAR system and the parameters are as follows: mass: 4 kg, operating frequency band: X or Ku, detection range: 10 km, resolution: 0.3 m. band: X or Ku, detection range: 10 km, resolution: 0.3 m. Figure 16. The experimental unmanned aerial vehicle (UAV). Figure 16. The experimental unmanned aerial vehicle (UAV). Figure 16. The experimental unmanned aerial vehicle (UAV). (a) SAR (b) Antenna (a) SAR (b) Antenna Figure 17. The UAV loaded tracking equipment; (a): SAR (synthetic aperture radar); (b): Antenna. Figure 17. The UAV loaded tracking equipment; (a): SAR (synthetic aperture radar); (b): Antenna. Figure 17. The UAV loaded tracking equipment; (a): SAR (synthetic aperture radar); (b): Antenna. The SAR can detect the pitch angle ϕ , azimuth ϕ and distance d of the target vehicle 1 2 ϕ ϕ d The SAR can detect the pitch angle , azimuth and distance of the target vehicle 1 2 relative to the UAV, and the measurement equation is The SAR can detect the pitch angle j , azimuth j and distance d of the target vehicle relative to 1 2 relative to the UAV, and the measurement equation is the UAV, and the measurement equation is 2 3 2 2 2 (x x ) + (y y ) + (z z ) t t t U U U 6 7 0 1 2 3 6 7 6 7 z z 6 U t 7 6 7 @ A arctan q 6 7 Z = j = (51) 4 5 6 7 2 2 6 (x x ) + (y y ) 7 U t U t 2 6 7 4 5 y y U t arctan x x T T where [x , y , z ] is the position of UAV; [x , y , z ] is the position of the target vehicle. t t t U U U In the experiment process, both the pitch angle and azimuth measurement had the drift rate, and the distance measurement had the deviation for the SAR, but the precise statistics about the sensor faults and noises are still unknown. In Figure 18, the target vehicle is driving on the expressway, and the initial position of the UAV and the target vehicle is, respectively, East longitude 108.933195 , North latitude 34.188183 , and an altitude of 1.50 km and East longitude 108.938962 , North latitude 34.187501 , and an altitude of 436.05 m. The experiment test time is designed as 1000 s with the sample period being 1 s, and the end point of the UAV and the target vehicle is, respectively, East longitude 109.130943 , North latitude 34.245275 , and an altitude of 1.50 km and East longitude 108.145231 , North latitude 34.239740 , an altitude of 436.76 m. Appl. Sci. 2017, 7, 1003 22 of 26    22 2 () xx−+(y −y) +(z −z) Ut U t U t      d    zz −  Ut  Z== ϕ arctan (51)     22  xx−+ y −y  () ( )  Ut U t ϕ   2      yy − Ut arctan    xx − Ut    T T where [, x yz, ] is the position of UAV; [, x yz, ] is the position of the target vehicle. UU U tt t In the experiment process, both the pitch angle and azimuth measurement had the drift rate, and the distance measurement had the deviation for the SAR, but the precise statistics about the sensor faults and noises are still unknown. In Figure 18, the target vehicle is driving on the expressway, and the initial position of the UAV and the target vehicle is, respectively, East longitude 108.933195°, North latitude 34.188183°, and an altitude of 1.50 km and East longitude 108.938962°, North latitude 34.187501°, and an altitude of 436.05 m. The experiment test time is designed as 1000 s with the sample period being 1 s, and the end point of the UAV and the target Appl. vehi Sci. cle i 2017 s, ,respecti 7, 1003 vely, East longitude 109.130943°, North latitude 34.245275°, and an altitude of 22 of 26 1.50 km and East longitude 108.145231°, North latitude 34.239740°, an altitude of 436.76 m. Figure 18. The real route of the vehicle in target tracking. Figure 18. The real route of the vehicle in target tracking. When the IMM and CPIMM methods are used, the target model description is the same as the When the IMM and CPIMM methods are used, the target model description is the same as numerical simulation. Four filtering methods including IMM-UKF, IMM-NAUKF, CPIMM-UKF, the numerical simulation. Four filtering methods including IMM-UKF, IMM-NAUKF, CPIMM-UKF, and CPIMM-NAUKF were used in the experiment, and these algorithms were evaluated by the and CPIMM-NAUKF were used in the experiment, and these algorithms were evaluated by the estimation error shown by Equation (50). estimation error shown by Equation (50). The comparisons of the longitude and latitude error curves of five filtering algorithms are shown Thein comparisons Figures 19 and of the 20longitude , and the co and rresp latitude ondinerr g st or atcurves istical d of atfive a of t filtering he mean algorithms absolute er arror e shown are in listed Figur in T es 19 able 5. We c and 20, and an the see that the standar corresponding statistical d IMM-UKF h data of as the bigge the mean st filter absolute ing err erroror wit are listed h the in longitude error ranging [−25.0223, 24.8762] m and latitude error ranging [−23.9917, 24.9834] m. The Table 5. We can see that the standard IMM-UKF has the biggest filtering error with the longitude error IMM-NAUKF has better filtering precision than IMM-UKF because of the adaptive ability to the ranging [25.0223, 24.8762] m and latitude error ranging [23.9917, 24.9834] m. The IMM-NAUKF has better filtering precision than IMM-UKF because of the adaptive ability to the sensor faults with the longitude error ranging [16.1849, 14.0871] m and latitude error ranging [13.8628, 14.0563] m. The CPIMM-UKF has better filtering precision than IMM-UKF because of the adaptive ability to the non-Gaussian distribution with the longitude error ranging [10.0547, 14.2482] m and latitude error ranging [14.7196, 12.2544] m. The CPIMM-NAUKF has the best filtering accuracy, since it can not only adapt to the non-Gaussian but also the sensor faults and model mismatches with the longitude error ranging [5.0018, 6.8623] m and latitude error ranging [4.8007, 4.9106] m. Table 5. Performance comparison for CPIMM-NAUKF in the SAR tracking system. Mean Absolute Error of Longitude (m) Mean Absolute Error of Latitude (m) Algorithm Mean Standard Deviation Mean Standard Deviation IMM-UKF 9.0125 8.2336 9.0058 8.1329 IMM-NAUKF 6.7243 6.0772 6.1174 5.9287 CPIMM-UKF 7.1588 7.0154 7.0542 6.8835 CPIMM-NAUKF 2.8514 2.2259 2.3417 2.0016 Appl. Appl. Sci. Sci. 2017 2017, , 7 7, 1003 , 1003 23 of 23 of 26 26 sensor faults with the longitude error ranging [−16.1849, 14.0871] m and latitude error ranging sensor faults with the longitude error ranging [−16.1849, 14.0871] m and latitude error ranging [−13.8628, 14.0563] m. The CPIMM-UKF has better filtering precision than IMM-UKF because of the [−13.8628, 14.0563] m. The CPIMM-UKF has better filtering precision than IMM-UKF because of the adaptive ability to the non-Gaussian distribution with the longitude error ranging [−10.0547, adaptive ability to the non-Gaussian distribution with the longitude error ranging [−10.0547, 14.2482] m and latitude error ranging [−14.7196, 12.2544] m. The CPIMM-NAUKF has the best 14.2482] m and latitude error ranging [−14.7196, 12.2544] m. The CPIMM-NAUKF has the best filt filter ering ing accu accuracy racy, since , since it it can can not not onl only y ad adapt apt ttoo the the non- non-Ga Gaussi ussiaan n but but al also the sensor fa so the sensor faul ults a ts an nd d model mi model misma smattches wit ches with h t th he long e longit itude e ude error ran rror rang gin ing g [[−−5 5..00 0018 18, , 6 6..862 8623] 3] m m a an nd la d lattiitude error rangi tude error rangin ng g Appl. Sci. 2017, 7, 1003 23 of 26 [−4.8007, 4.9106] m. [−4.8007, 4.9106] m. IM M -UK F IM M -UK F IM M -NA UK F IM M -NA UK F CP IM M -UK F CP IM M -UK F CP IM M -NA UK F CP IM M -NA UK F 10 10 -5 -5 -10 -10 -15 -15 -2 -20 0 -25 -25 0 0 100 100 20 2000 300 300 400 400 500 500 600 600 70 7000 800 800 90 9000 100 10000 Ti Tim me e//((sse ecc)) Figure 19. Longitude error comparison for CPIMM-NAUKF in SAR tracking system. Figure 19. Figure 19. Lon Long gitude error comparison for CP itude error comparison for CPIIMM-N MM-NAUKF i AUKF in n SAR SAR trac tracking s king sy yssttem. em. IMM-UK F IMM-UK F IIM MM M--NA NAUK UKF F CP CPIIM MM M--UK UKF F CP IM M -NA UK F CP IM M -NA UK F 0 0 -5 -5 -10 -10 -15 -15 -20 -20 -25 -25 0 100 200 300 400 500 600 700 800 900 1000 0 100 200 300 400 500 600 700 800 900 1000 Tim e/(sec) Tim e/(sec) Figure 20. Latitude error comparison for CPIMM-NAUKF in the synthetic aperture radar (SAR) tracking system. Figure 20. Latitude error comparison for CPIMM-NAUKF in the synthetic aperture radar (SAR) Figure 20. Latitude error comparison for CPIMM-NAUKF in the synthetic aperture radar (SAR) tracking system. tracking system. In summary, the practical experiments results demonstrate that the proposed CPIMM-NAUKF is obviously superior to the existing IMM and UKF algorithms in terms of filtering precision and dynamic response for its adaptive ability to both non-Gaussian and sensor faults, and it can significantly improve the precision and robustness of the maneuvering target tracking system. 5. Conclusions Maneuvering target tracking is a key technology and research hotspot in the field of navigation, guidance, and control. This paper proposes an efficient IMM-UKF, called CPIMM-NAUKF, for maneuvering target tracking. In the external link of CPIMM-NAUKF, the mean and covariance Latitude E rror/(m ) Latitude E rror/(m ) Longitude E rror/(m ) Longitude E rror/(m ) Appl. Sci. 2017, 7, 1003 24 of 26 matrix can be calculated with high estimation precision by introducing the cubature principle, which solves the non-Gaussian problem. In the internal CPIMM-NAUKF link, the contribution of system states and innovations on the filtering solution can be balanced by introducing an adaptive matrix gene, which addresses the sensor faults. The proposed algorithm was evaluated in a classical maneuvering target tracking numerical simulation and two practical experiments including one navigation experiment with INS/GPS and one target tracking experiment with SAR. Simulation results validate that the proposed CPIMM-NAUKF improves the filtering performance in terms of higher accuracy and faster convergence, and tracks more effectively. In our future research, we will focus on the difference between UKF and other nonlinear filtering algorithms of various noise strengths. Acknowledgments: This work was supported by the National Natural Science Foundation of China (No. 61601505), National Aviation Science Foundation of China (No. 20155196022) and the Shaanxi Natural Science Foundation of China (No. 2016JQ6050). Author Contributions: Huan Zhou and Hui Zhao provided insights into formulating the ideas and designed the algorithm. Hanqiao Huang performed the simulations. Xin Zhao reviewed the manuscript and gave suggestions in the proposed method. Conflicts of Interest: The authors declare no conflict of interest. References 1. Matveev, A.S.; Semakova, A.A.; Savkin, A.V. Tight circumnavigation of multiple moving targets based on a new method of tracking environmental boundaries. Automatica 2017, 79, 52–60. [CrossRef] 2. Fan, Y.; Lu, F.; Zhu, W.; Bai, G.; Yan, L. A Hybrid Model Algorithm for Hypersonic Glide Vehicle Maneuver Tracking Based on the Aerodynamic Model. Appl. Sci. 2017, 7, 159. [CrossRef] 3. Cao, Y.; Wang, G.; Yan, D.; Zhao, Z. Two Algorithms for the Detection and Tracking of Moving Vehicle Targets in Aerial Infrared Image Sequences. Remote Sens. 2016, 8, 28. [CrossRef] 4. Leven, W.F.; Lanterman, A.D. Unscented Kalman Filters for Multiple Target Tracking With Symmetric Measurement Equations. IEEE Trans. Autom. Control 2009, 54, 370–375. [CrossRef] 5. Song, Y.; Zhang, B.; Zhao, K. Indirect neuroadaptive control of unknown MIMO systems tracking uncertain target under sensor failures. Automatica 2017, 77, 103–111. [CrossRef] 6. Wu, J.; Li, K.; Zhang, Q.; An, W.; Jiang, Y.; Ping, X.; Chen, P. Iterative RANSAC based adaptive birth intensity estimation in GM-PHD filter for multi-target tracking. Signal Process. 2017, 131, 412–421. [CrossRef] 7. Dong, P.; Jing, Z.; Gong, D.; Tang, B. Maneuvering multi-target tracking based on variable structure multiple model GMCPHD filter. Signal Process. 2017, 141, 158–167. [CrossRef] 8. Roy, A.; Mitra, D. Unscented-Kalman-Filter-Based Multitarget Tracking Algorithms for Airborne Surveillance Application. J. Guid. Control Dyn. 2016, 39, 1949–1966. [CrossRef] 9. Blom, H.; Barshalom, Y. The Interacting Multiple Model Algorithm for Systems with Markovian Switching Coefficients. IEEE Trans. Autom. Control 1988, 33, 780–783. [CrossRef] 10. Lan, J.; Li, X.R.; Jilkov, V.P.; Mu, C. Second order Markov chain based multi-model algorithm for maneuvering target tracking. IEEE Trans. Aerosp. Electron. Syst. 2013, 49, 3–19. [CrossRef] 11. Khalid, S.S.; Abrar, S. A low-complexity interacting multiple model filter for maneuvering target tracking. Int. J. Electron. Commun. 2017, 73, 157–164. [CrossRef] 12. Zhu, W.; Wang, W.; Yuan, G. An Improved Interacting Multiple Model Filtering Algorithm Based on the Cubature Kalman Filter for Maneuvering Target Tracking. Sensors 2016, 16, 805. [CrossRef] [PubMed] 13. Kim, T.H.; Moon, K.R.; Song, T.L. Variable-structured interacting multiple model algorithm for the ballistic coefficient estimation of a re-entry ballistic target. Int. J. Control Autom. Syst. 2013, 11, 1204–1213. [CrossRef] 14. Zhu, Z. Ship-borne radar maneuvering target tracking based on the variable structure adaptive grid interacting multiple model. J. Zhejiang Univ. Sci. C 2013, 14, 733–742. [CrossRef] 15. Zhang, Y.; Guo, C.; Hu, H.; Liu, S.; Chu, J. An algorithm of the adaptive grid and fuzzy interacting multiple models. J. Mar. Sci. Appl. 2014, 13, 340–345. [CrossRef] 16. Li, X.R.; Jilkov, V.P. Survey of maneuvering target tracking—Part V: Multiple-model methods. IEEE Trans. Aerosp. Electron. Syst. 2005, 41, 1255–1321. Appl. Sci. 2017, 7, 1003 25 of 26 17. Cui, N.; Hong, L.; Layne, J.R. A Comparison of Nonlinear Filtering Approaches with an Application to Ground Target Tracking. Signal Process. 2008, 85, 1469–1492. [CrossRef] 18. Li, W.; Jia, Y. Gaussian Mixture PHD Filter for Jump Markov Models based on Best-fitting Gaussian Approximation. Signal Process. 2011, 91, 1036–1042. [CrossRef] 19. Lainiotis, D.; Sims, F. Performance measure for adaptive Kalman estimators. IEEE Trans. Autom. Control 1970, 15, 249–250. [CrossRef] 20. Kirubarajan, T.; Barshalom, Y. Kalman Filter Versus IMM Estimator: When Do We Need the Latter? IEEE Trans. Aerosp. Electron. Syst. 2000, 39, 1452–1457. [CrossRef] 21. Susan, S.; Sharma, M. Automatic texture defect detection using Gaussian mixture entropy modeling. Neurocomputing 2017, 235, 274–286. [CrossRef] 22. Liang, H.; Kang, F. Tracking UUV based on interacting multiple model unscented particle filter with multi-sensor information fusion. Optik-Int. J. Light Electron. Opt. 2015, 126, 5067–5073. 23. Chang, D.; Fan, M. Interacting multiple model particle filtering using new particle resampling algorithm. In Proceedings of the 2014 IEEE Global Communications Conference, Austin, TX, USA, 8–12 December 2014; pp. 3215–3219. 24. Gordon, N.J.; Salmond, D.J.; Smith, A.F.M. Novel approach to nonlinear and non-Gaussian Bayesian state estimation. IEE Proc. F-Radar Signal Process. 1993, 140, 107–113. [CrossRef] 25. Zhou, H.; Deng, Z.; Xia, Y.; Fu, M. A new sampling method in particle filter based on Pearson correlation coefficient. Neurocomputing 2016, 216, 208–215. [CrossRef] 26. Julier, S.J.; Uhlmann, J.K.; Durrant-Whyte, H.F. A new approach for filtering nonlinear systems. In Proceedings of the American Control Conference, Seattle, WA, USA, 21–23 June 1995; pp. 1628–1632. 27. Julier, S.J.; Uhlmann, J.K.; Durrant-Whyte, H.F. A new method for nonlinear transformation of means and covariances in filters and estimators. IEEE Trans. Autom. Control 2000, 45, 477–482. [CrossRef] 28. Boada, B.L.; Boada, M.J.L.; Diaz, V. Vehicle sideslip angle measurement based on sensor data fusion using an integrated ANFIS and an Unscented Kalman Filter algorithm. Mech. Syst. Signal Process. 2016, 72–73, 832–845. [CrossRef] 29. Wang, D.; Lv, H.; Wu, J. In-flight initial alignment for small UAV MEMS-based navigation via adaptive unscented Kalman filtering approach. Aerosp. Sci. Technol. 2017, 61, 73–84. [CrossRef] 30. Kumar, D.V.A.N.R.; Rao, S.K.; Raju, K.P. Integrated Unscented Kalman filter for underwater passive target tracking with towed array measurements. Optik-Int. J. Light Electron. Opt. 2016, 127, 2840–2847. [CrossRef] 31. Rahimi, A.; Kumar, K.D.; Alighanbari, H. Fault estimation of satellite reaction wheels using covariance based adaptive unscented Kalman filter. Acta Astronaut. 2017, 134, 159–169. [CrossRef] 32. Zheng, X.; Fang, H. An integrated unscented kalman filter and relevance vector regression approach for lithium-ion battery remaining useful life and short-term capacity prediction. Reliab. Eng. Syst. Saf. 2015, 144, 74–82. [CrossRef] 33. Zahedi Ygane, M.H.; Ansarifar, G.R. Extended Kalman filter design to estimate the poisons concentrations in the P.W.R nuclear reactors based on the reactor power measurement. Ann. Nucl. Energy 2017, 101, 576–585. [CrossRef] 34. Kulikova, M.V.; Kulikov, G.Y. NIRK-based accurate continuous–Discrete extended Kalman filters for estimating continuous-time stochastic target tracking models. J. Comput. Appl. Math. 2017, 316, 260–270. [CrossRef] 35. Yang, Y.; Gao, W. A new learning statistic for adaptive filter based on predicted residuals. Prog. Nat. Sci. 2006, 16, 833–837. 36. Yang, Y.; Gao, W. An optimal adaptive Kalman filter. J. Geod. 2006, 80, 177–183. [CrossRef] 37. Soken, H.E.; Hajiyev, C. Pico satellite attitude estimation via Robust Unscented Kalman Filter in the presence of measurement faults. ISA Trans. 2010, 49, 249–256. [CrossRef] [PubMed] 38. Gan, X.; Gao, W.; Dai, Z.; Liu, W. Research on WNN soft fault diagnosis for analog circuit based on adaptive UKF algorithm. Appl. Soft Comput. 2017, 50, 252–259. 39. Ning, X.; Li, Z.; Wu, W.; Yang, Y.; Fang, J.; Liu, G. Recursive adaptive filter using current innovation for celestial navigation during the Mars approach phase. Sci. China 2017, 60, 032205. [CrossRef] 40. Arasaratnam, I.; Haykin, S. Cubature Kalman Filters. IEEE Trans. Autom. Control 2009, 54, 1254–1269. [CrossRef] Appl. Sci. 2017, 7, 1003 26 of 26 41. Arasaratnam, I.; Haykin, S.; Hurd, T.R. Cubature Kalman Filtering for Continuous-Discrete Systems: Theory and Simulations. IEEE Trans. Signal. Process. 2010, 58, 4977–4993. [CrossRef] 42. Zhou, H.; Huang, H.; Zhao, H.; Zhao, X.; Yin, X. Adaptive Unscented Kalman Filter for Target Tracking in othe Presence of Nonlinear Systems Involving Model Mismatches. Remote Sens. 2017, 9, 657. [CrossRef] 43. Elenchezhiyan, M.; Prakash, J. State estimation of stochastic non-linear hybrid dynamic system using an interacting multiple model algorithm. ISA Trans. 2015, 58, 520–532. [CrossRef] [PubMed] 44. Chen, X.; Li, Y.; Li, Y.; Yu, J.; Li, X. A Novel Probabilistic Data Association for Target Tracking in a Cluttered Environment. Sensors 2016, 16, 2180. [CrossRef] [PubMed] 45. Zhao, L.; Qiu, H.; Feng, Y. Analysis of a robust Kalman filter in loosely coupled GPS/INS navigation system. Measurement 2016, 80, 138–147. [CrossRef] © 2017 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

Journal

Applied Sciences – Multidisciplinary Digital Publishing Institute

Published: Sep 28, 2017

There are no references for this article.