Improved Cubature Kalman Filter for GNSS/INS Based ... - IEEE Xplore

IEEE TRANSACTIONS ON SIGNAL PROCESSING, VOL. 65, NO. 11, JUNE 1, 2017

2975

Improved Cubature Kalman Filter for GNSS/INS Based on Transformation of Posterior Sigma-Points Error Bingbo Cui, Xiyuan Chen, Senior Member, IEEE, and Xinhua Tang

Abstract—Tightly coupled GNSS/INS has been widely approved as a promising substitute for standalone GNSS in urban areas navigation. However, due to the frequent GNSS signal outages, the filter used in GNSS/INS should be insensitive to the less informative observations. In this paper, a novel sigma-points update method is proposed to enhance the robustness of cubature Kalman filter (CKF) under the circumstance of unavailable observations. First, the problems of existing sampling-based filters are analyzed. Then, by transforming the posterior sigma-points error matrix from prediction phase of filtering to the posterior domain of update, the updated sigma-points are expected to capture the covariance more precisely than traditional sigma-points. Finally, an improved CKF (ICKF) is developed by embedding these points into the Bayesian estimation framework, and the upper bounds of error covariance matrices are analyzed theoretically. Signal outages with different durations are simulated and results demonstrate that ICKF outperforms state-of-the-art methods. Index Terms—Deterministic sampling, integrated navigation, cubature Kalman filter, observations missing.

I. INTRODUCTION LOBAL navigation satellite system (GNSS) has been widely applied in vehicle and pedestrians navigation because of good long-term accuracy and all-weather navigation ability. However, GNSS suffers from signal attenuation which often occurs in urban and indoor areas, such as interference, multipath and signal shading. Tightly coupled GNSS/INS has been widely approved as a promising substitute for standalone GNSS in the navigation of urban areas, because of its better stability and reliability. Due to the nonlinearity of measurement function and INS system equation, the data fusion in tightly coupled GNSS/INS is a typical nonlinear filtering problem. Unlike

G

Manuscript received July 25, 2016; revised January 22, 2017 and February 26, 2017; accepted February 27, 2017. Date of publication March 8, 2017; date of current version April 10, 2017. The associate editor coordinating the review of this manuscript and approving it for publication was Prof. Gustau Camps-Valls. This work was supported in part by the National Natural Science Foundation of China (51375087), and in part by the Scientific Research Foundation of Graduate School of Southeast University (YBJJ1574). (Corresponding author: Xiyuan Chen.) The authors are with the Key Laboratory of Micro-Inertial Instrument and Advanced Navigation Technology Ministry of Education, School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China (e-mail: [email protected]; [email protected]; [email protected]). Color versions of one or more of the figures in this paper are available online at http://ieeexplore.ieee.org. Digital Object Identifier 10.1109/TSP.2017.2679685

in linear Gaussian model where Kalman filter (KF) provides optimal solution in the sense of minimum variance criterion, it is intractable to compute the optimal solution in nonlinear cases. In the framework of Bayesian, many sub-optimal approaches have been proposed for the purpose of state estimation. Shortly after the celebrated work of Kalman [1], extended Kalman filter (EKF) was proposed to approximate the integrals involved in the update of the probability density function (pdf) of latent dynamic state [2], [3]. However, EKF may fail when significant nonlinearities or large initial estimation errors are addressed [4], [5]. Julier and Uhlmann proposed unscented Kalman filter (UKF) by using deterministic sampling of previous pdf’s to approximate the posterior pdf [6]. Due to its positive features in better accuracy and similar computational cost compared with EKF, UKF has become a preferable alternative for nonlinear filtering. From the view point of numerical integration, other sampling-based filtering methods have been proposed, e.g. Gauss-Hermite quadrature filter (GHQF) based on GaussHermite quadrature rule [7], [8], cubature Kalman filter (CKF) based on cubature rule [9], etc. It has been approved that CKF has better stability and accuracy than UKF when used in high dimension filtering problems [9], [10]. The above-mentioned filters include sigma-representation of the pdf of random variable, so they belong to a general class of sigma-point Kalman filter (SPKF). Performance comparisons between EKF and SPKF in GNSS/INS have been done in previous works, which however, show inconsistent results [11], [12]. Recently, Zhao studied the performance of CKF in tightly coupled GNSS/IMU based on observability analysis [13], and results indicate that CKF performs better than EKF in case of large initial yaw error and weak observability. Because the typical dimension of tightly coupled GNSS/INS is 17 or higher [11]–[13], we focus on improving the usage of CKF in our work. Differ from SPKF, where the posterior pdf is represented by using deterministically chosen weighted sample points, particle filter (PF) represents the pdf with randomly selected weighted points [14]. PF estimates the whole posterior pdf instead of its first two moments, which can approach optimal Bayesian estimate for nonlinear and non-Gaussian filtering problem [15]. However, besides the inherent problems of PF (e.g., selection of important density and solution to the particle degeneracy) [16], a high computational burden hinders PF from widely application in integration navigation system. It should be noticed that SPKF may fail when frequent signal outages occur or prior information

1053-587X © 2017 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications standards/publications/rights/index.html for more information.

2976

is of large error. Shmaliy proposed a robust Kalman-like filter for state estimation with unknown prior information by utilizing unbiased finite impulse response estimator [17]. By combining strong tracking KF with wavelet neural network, Chen derived a hybrid filter to bridge GPS outages [18]. Besides the state delay in estimation, both of the two methods need careful selection of model parameters, which limits their usages on some special cases. There are three aspects in a sampling-based filter: sigmarepresentation, approximation of the joint pdf, and recursive filters [19]. Most of current sampling-based filters focus on the improvement of the former two aspects, which neglects their intrinsic correlation in the recursive update. It is generally recognized that UKF (or CKF) can reach the accuracy of second order [20] and part of the higher orders in Taylor series sense [21] of the moments of pdf. However, it does not indicate that UKF can give correct mean and covariance for arbitrary nonlinear function, e.g., informative high-order terms are dropped in the covariance series when 3-degree cubature rule is used for tight GNSS/INS. Existing sampling methods, which drop the information of dynamic system except the mean and covariance, may degrade the estimation of non-direct observable states as they can only be corrected by the off-diagonal elements of prediction covariance [22]. When the measurement noise is small, the residual resulting from the approximation of measurement function has a great effect on the posterior pdf [23]. Garc´ıa-Fernández et al. proposed two methods to improve the performance of Bayesian update, one is based on truncating the prior pdf by using the likelihood [24] and the other performs statistical linear regression (SLR) with respect to the posterior pdf [25]. However, the former limits its usage to bijective measurement function and the later needs an iterative procedure to approximate the posterior which increases its complexity. Furthermore, regarding the generation of sample points, it has been reported there is “non-local sampling” problem in CKF when high dimension filtering problems involved [26]. To approximate the posterior pdf, the predicted pdf and the measurement noise are both assumed to be Gaussian distribution, which may introduce errors if there are uncertainties in system models. However, it is generally recognized that a Gaussian state predictive density at each recursive step is closely satisfied in practical application [27]. Because the dynamic model of GNSS/INS is rather accurate, we should expect that the residual of the SLR in approximating process function with respect to the previous posterior pdf could be used in the generation of sigma-points for approximating process function in next filtering period. In a word, in the approximation of intractable integrals consisted in Gaussian filters, the information of propagated sigma-points has not been fully utilized in the two stage Bayesian estimation framework. Inspired by the work of Garc´ıa-Fernández and [28], an efficient sampling framework is proposed for high dimension navigation filtering problems, which is especially useful when the observations noise is sometimes sufficient small and the sensors suffer from frequent signal outages. Based on the truth that the first moment is more accurate than the second moment with symmetric choice of the representative points, an improved CKF


(ICKF) is developed by using the 3-degree cubature rule. Unlike the work in [28], we consider the stochastic uncertainty in the implementation of recursive cubature points update. That is, the cubature points are initialized based on Gaussian assumption, and then their re-generation is substituted by a linear transformation of the residual of the SLR of process function without constructing prior pdf. The benefits of ICKF compared with other Gaussian filters are analyzed in numerical example. The rest of this paper is arranged as follows. Section II analyzes the accuracy of CKF and points out the problems of existing sampling-based filters. The ICKF and its accuracy analysis are given in Section III. Section IV validates the theoretical analysis and illustrates the proposed algorithm with numerical simulation. Finally, conclusions are given. II. PROBLEMS FORMULATION Considering a random variable x has mean x ¯ and covariance Pxx , and a second random variable z is related to x by z = h(x). Let n be the dimension size of x, the problem is to calculate the estimation of z with mean ¯ z and covariance Pz z . Expanding z into Taylor series around x ¯ as z = h(¯ x + e) ∞

= h(¯ x) + De h +

D2e h D3e h Die h + + , 2! 3! i! i=4

(1)

where e is the perturbation terms. Let ej be the jth component of e, then we have ⎤i ⎡ n ∂ ⎦ i ⎣ De h = ej h(x) . (2) ∂xj j =1 x= x ¯

The expected value of z can be expressed as [20] ∞ Di h e ¯ z = h(¯ x) + E i! i=1 = h(¯ x) +

∞ 1 ∂i h ∂i h + m + · · · , m1···11 1···12 i! ∂x1 i ∂x1 i−1 ∂x2 i=1 (3)

where mc 1 c 2 ···c i is the ith-order moment of e. To capture the mth order of the mean in Taylor series, both the moments of e and Die h must be known to mth order. The perturbation terms of original CKF are selected as

√ √ nPxx − nPxx i . ex i = (4) Because of the symmetrical distribution of ex i all the odd moments sum up to zero. Rewriting (3) as

2

4 De h De h D6e h + + ··· ¯ z = h(¯ x) + E +E 2! 4! 6! T ∇ Pxx ∇ hx= x¯ = h(¯ x) + + r(n), (5) 2

CUI et al.: IMPROVED CUBATURE KALMAN FILTER FOR GNSS/INS BASED ON TRANSFORMATION OF POSTERIOR SIGMA-POINTS ERROR

where ∇T = [ ∂/∂x1 · · · ∂/∂xn ], and the fourth and higherorder terms can be expressed as [26] ⎡ ⎤ 2n ∞ 1 ⎣ r(n) = (n)l−1 P l (i, j)⎦, (6) (2l)! j =1 xx l=2

where l is the order of Taylor series, Pxx (i, j) is the covariance x) = of ex i , j , and ex i , j is the jth component of ex i . Define F (¯ ∂h | ¯ , the covariance is calculated as ∂ x T x= x z) (z − ¯ z)T Pz z = E (z − ¯ = F (¯ x)Pxx F T (¯ x) T T D2e h(D2e h) D3e h(De h)T De h(D3e h) + + +E 3! 2 × 2! 3!

T 1 T ∇ Pxx ∇ hx= x¯ ∇T Pxx ∇ hx= x¯ + · · · . − 4 (7) Noting (6), the approximation error of ¯ z is enlarged with the increase of n, which degrades the accuracy of covariance further. If ¯ z is known up to the third order, the covariance only known to the first term, which however, yields more accurate estimation than EKF as there is a minus positive term in (7). Remark 2.1: When the symmetric perturbation terms in (4) are used, there would be inconsistence in covariance due to the error in Pxx and the time-varying nonlinearity in Die h. A. Previous Work Consider a nonlinear discrete-time system xk = f(xk −1 ) + wk −1 ,

(8)

zk = h(xk ) + νk ,

(9)

where xk ∈ n ×1 , zk ∈ p×1 are the state and measurement vector at time k, wk −1 ∈ n ×1 and νk ∈ p×1 are additive Gaussian white noise with covariance Qk −1 and Rk , f (·) and h(·) are the system and measurement function. The initial state x0 and wk −1 , νk are mutually independent. Both UKF and CKF can be considered as linear regression Kalman filters (LRKFs) whose implementation contains two phases: prediction and update. Let N = 2n, the prediction phase can be summarized as x ˆk |k −1 =

N

ωik −1 f xik −1 ,

(10)

2977

of likelihood and posterior pdf are computed as follows: ˆ zk |k −1 =

N

ωik |k −1 h xik |k −1 ,

(12)

i=1

Pz z =

N

zk |k −1 ωik |k −1 h xik |k −1 − ˆ

i=1

T zk |k −1 × h xik |k −1 − ˆ + Rk , Pxz =

N

(13)

zk |k −1 , ωik |k −1 xik |k −1 − x ˆk |k −1 h xik |k −1 − ˆ

i=1

(14)

−1

zz Kk = Pxz , k |k −1 Pk |k −1 x ˆk |k = x ˆk |k −1 + Kk zk − ˆ zk |k −1 ,

(16)

ˆ k |k = Pk |k −1 − Kk Pz z KTk . P k |k −1

(17)

(15)

Many LRKFs have been proposed based on Gaussian assumptions and different choices of representative points xik −1 and weights ωik −1 . Despite the use of high-degree numerical integration rules can improve the performance marginally, such as 5-degree cubature rule [9] or sparse-grid quadrature rule [29], which however, will increase the computational cost and may bring numerical stability problem. It has been reported in recently published literatures that the selection of prior pdf in the SLR process has a great effect on the Bayesian update framework, especially when the measurement noise is sufficient small [23]–[25]. What is more, for tightly coupled GNSS/INS the nonlinearity is mild and it has no obvious benefits to capture the high-order information of system models by sacrificing the stability and computational cost. In a word, by improving the approximation accuracy for numerical integration would not necessary mean a performance improvement of filtering. Remark 2.2: It should be noted that most of the previˆk −1|k −1 + ous LRKFs generate the sigma-points by xik −1 = x ˆk |k −1 + Gi (n) Pk −1|k −1 for prediction phase, and xik |k −1 = x Gi (n) Pk |k −1 for update phase, where n is the dimension of system state. In CKF Gi (n) is defined as √ nei , i = 1, 2, · · · , n Gi (n) = (18) √ − nei−n , i = n + 1, n + 2, · · · , 2n. with ei ∈ n ×1 denotes the ith elementary column vector. Notice that, the distances of sigma-points from central point depend on n.

i=1

Pk |k −1 =

N

ωik −1

i T ˆk |k −1 f xik −1 − x ˆk |k −1 f xk −1 − x

+ Qk −1 ,

(11)

i=1

where the sequence {(xik −1 , ωik −1 )}N i=1 represent the posterior ˆk −1|k −1 , Pk −1|k −1 ) and the predicated state pdf pdf N (xk −1 ; x ˆk |k −1 , Pk |k −1 ). The approximation is approximated by N (xk ; x

III. IMPROVED CUBATURE KALMAN FILTER In Section III-A, we present the motivation of using posterior sigma-points error to update sigma-points, instead of the covariance. In Section III-B, the novel cubature points update algorithm is derived and its novelty is remarked. Section IIIC analyzes the upper bounds of error covariance matrices for ICKF with model uncertainties in measurement equation. Finally, some aspects on ICKF are discussed.

2978


Fig. 1. Prior and posterior pdf for z = 1.5. Red solid line indicates the posterior pdf calculated by using a dense grid of points. CKF approximates the covariance with non-negligible error.

Fig. 2. Approximations by using traditional and novel sigma-points with different Q. Result indicates ICKF performs much better than CKF when Q value is small.

A. Motivation of the ICKF

p0 (x) = N (3, 4), where N (¯ x, P) represents a Gaussian density with mean x ¯ and covariance P. The measurement equation is

As we have demonstrated in previous section, the accuracy of existing sampling-based methods may be degraded when high-dimensional problem involved. Furthermore, the covariance may contain non-negligible error when the polynomial order of (8) or (9) is of degree m and m-degree cubature rule is employed. When there are measurement uncertainties in the update phase of CKF (e.g., multipath and disturbances in GNSS measurement), which make the assumed Gaussian distribution of measurement noise invalid, the re-generation of sigma-points based on the construction of pdf would lead to a quickly divergence. However, the process model is often rather accurate, if the re-generation of sigma-points for next filtering period is weighted more on the predicted sigma-points, the filter will keep on tracking system state for longer duration even if no innovation is provided. When no sufficient measurement information is provided, the prediction phase of the filter works well, which can be written as p (xk |Zk −1 ) = p (xk |xk −1 ) p (xk −1 |Zk −1 )dxk −1 , (19) where Zk −1 denote the measurement sequence (z1 , · · · , zk −1 ). In the update phase, except for the prior pdf the sigma-points and the values of its diffusion in process function can give extra information to reduce the accumulated error in the calculation of (19). The information of predicted sigma-points can be utilized in two ways: 1) the sigma-points error from prediction phase can be used to generate posterior sigma-points by linear transformation, 2) the values from the diffusion of sigma-points in process function, which capture the high-order terms of moments and non-Gaussian information can be used for the diffusion in nonlinear measurement function. 1) Illustrative Example: The example used in [25] is applied to verify the conclusions of previous discussion. The prior pdf

zk = axk 3 + ηk , with measurement noise ηk ∼ N (0, 0.1), a = 0.01 and the posterior pdf can be calculated analytically. Suppose zk = 1.5, then we approximates the posterior PDF by using CKF based on 3degree cubature rule, and the result is shown in Fig. 1. Notice that, CKF solves the moment computation problem contained in (5), but approximates the covariance in (7) with non-negligible error. Without doubt, in case this covariance is applied to generate sigma points for next filtering period, the residual of SLR for process function will increase, which degrades the accuracy of likelihood approximation. Let the process function be xk = xk −1 + wk −1 , and wk −1 ∼ N (0, Q). The result of using CKF and ICKF after two recursive steps with different Q value is shown in Fig. 2, based on which we can conclude that when posterior pdf is markedly narrower than the prior pdf, the sampling method of CKF down-weights the new observation, making the approximation of posterior pdf presents bias. By inheriting extra information from the cubature points that generated in prediction phase, the ICKF can reduce the bias and in turn slow down the divergence of filtering. B. Novel Cubature Points Update xk −1|k −1 , Pk −1|k −1 ) be the estimated state at Let xk −1 ∼ N (ˆ xk |k −1 , Pk |k −1 ) be the predicted state at time k − 1, xk ∼ N (ˆ time k, and factorize Pk −1|k −1 and Pk |k −1 as Pk −1|k −1 = Sk −1|k −1 SkT−1|k −1 , Pk |k −1 = Sk |k −1 SkT|k −1 .

(20) (21)


Algorithm 1: Cubature Kalman Filter. ˆ k −1|k −1 , Pk−1|k−1 Require: x for i = 1, · · · , N do Compute the cubature points xik−1 using (22) Assign weights : ωik −1 = 1/N end for Prediction phase: Compute the predicted mean x ˆk|k−1 using (10) Compute the predicted covariance Pk|k−1 using (11) Update phase: for i = 1, · · · , N do Compute the cubature points Xik|k−1 using (23) Assign weights: ωik |k −1 = ωik −1 end for Compute the estimated measurement ˆ zik|k−1 using (12) Estimate the Kalman gain Kk using (13), (14), (15) Estimate the update state x ˆk|k and covariance Pk|k using (16), (17)

The sigma-points generated for the prediction and update phase are xik −1 xik |k −1

= Sk−1|k −1 ξi + x ˆk −1|k −1 ,

(22)

= Sk|k −1 ξi + x ˆk |k −1 .

(23)

where ξi is defined as (18). The approximation of predicted state and posterior state densities in CKF is presented in Algorithm 1. Noting that with the increase of n, the distances of cubature points are away from the central point, and the approximation error is increased for high-dimensional filtering problem. Define the error matrices and weights 1 N ˜− − x ˆ · · · x − x ˆ x , (24) = X k |k −1 k |k −1 k |k −1 k |k −1 k |k −1

˜ + = x1 − x ˆk |k · · · xN ˆk |k , X (25) k k −x k |k

ω = ωk1 · · · ωkN , (26) W = diag(ω),

(27)

where xik |k −1 = f(xik −1 ) is the predicted cubature points. Notice that, in Algorithm 1 the prediction phase and update phase share the same weights ω, and diag(ω) is a function that build diagonal matrix with the main diagonal being ω, N is the number of cubature points. Some useful relationships are ˜− X k |k −1 ω = 0, T ˜− ˜− X k |k −1 W(Xk |k −1 ) = Pk |k −1 − Qk −1 ,

(28) (29)

which indicates that the cubature points should at least account for the mean and covariance of the approximation error of ˜+ process function. Analogously, the updated error matrix X k |k should satisfy

2979

where Pk |k − ΔE represents the uncertainties that the sigmapoints error matrix must account for in the update phase of filtering, and ΔE is a positive definite matrix that will be discussed ˜ + is directly generated by in III-D. Assuming the updated X k |k − ˜ + = BX ˜ k |k −1 , where B is a transformation matrix. We can X k |k easily conclude that ˜ + ω = BX ˜− X k |k −1 ω = 0, k |k T T ˜− ˜− Pk |k − ΔE = BX k |k −1 W(Xk |k −1 ) B = B Pk |k −1 − Qk −1 BT .

(32)

(33)

If Pk |k −1 − Qk −1 , Pk |k − ΔE are positive definite matrix, we can factorize them as − T Pk |k −1 − Qk −1 = L− , k Lk + T . Pk |k − ΔE = L+ k Lk

(34) (35)

It follows − T T Pk |k − ΔE = BL− B , k Lk

(36)

− T where B matrix satisfies L+ k = BLk Ξ, with ΞΞ = I and I is the identity matrix of appropriate dimension, we have

− −1 T Lk . B = L+ k Ξ

(37)

Then, by setting Ξ as the identity matrix we can get the simplest − −1 ˜+ solution B = L+ k (Lk ) . After we get Xk |k , the posterior cuba˜ + (i), where X ˜ + (i) ˆk |k + X ture points is generated by xi = x k

k |k

k |k

˜ + . The CKF using this novel cubature is the ith column of X k |k points update method is presented in Algorithm 2. Remark 3.1: Unlike the work of [26], where an optimal rotation angle is employed to transform the sigma-points for different filtering problems, our novel sigma-points depends on the approximation error of predicted cubature points only. The updated sigma-points can be written as − −1 − ˜ X ˆk |k + L+ xik = x k Lk k |k −1 ,

which does not depend on system state dimension n and solves the “non-local sampling” problem efficiently. Remark 3.2: The positive definiteness of Pk |k −1 − Qk −1 can be ensured by (11), where the weights of 3-degree cubature rule are always positive. Remark 3.3: The updated cubature points depend on the accuracy of the moments only, which means that the limitation on Gaussian distribution in the sigma-points update of LRKFs is removed.

˜ + ω = 0, X k |k

(30)

C. Boundedness of Error Covariance Matrices

˜ + W(X ˜ + )T = Pk |k − ΔE, X k |k k |k

(31)

ˆk |k −1 be the prediction error, x ˜k |k = Let x ˜k |k −1 = xk − x ˆk |k be the filtering error. Subtracting (10) from (8), and xk − x

2980


Algorithm 2: Improved Cubature Kalman Filter. Require: x ˆk−1|k−1 , Pk−1|k−1 Run Algorithm 1 to initialize sigma-points error matrix ˜+ X k−1|k−1 for i = 1, · · · , N do Compute the cubature points: ˜+ ˆk−1|k−1 + X xik−1 = x k−1|k−1 (i) i Assign weights : ωk −1 = 1/N end for Prediction phase: Compute the predicted mean x ˆk|k−1 using (10) Compute the predicted covariance Pk|k−1 using (11) ˜− Compute predicted sigma error matrix X k|k−1 using (24) − Compute Lk by factorizing Pk|k−1 − Qk −1 Update phase: for i = 1, · · · , N do Compute the cubature points: xik|k−1 = f (xik−1 ) Assign weights: ωik |k −1 = ωik −1 end for Compute the estimated measurement ˆ zik|k−1 using (12) Estimate the Kalman gain Kk using (13), (14), (15) Estimate the update state x ˆk|k and covariance Pk|k using (16), (17) Compute L+ k by factorizing Pk|k − ΔE Compute posterior sigma error matrix: ˜ + = L+ L− −1 X ˜− X k k k|k k|k−1 using the Taylor series expansion around x ˆk −1|k −1 , we have x ˜k |k−1 = f(ˆ xk −1|k−1)+ Φk |k −1 x ˜k −1|k −1 + o x ˜k −1|k −1 + wk−1 −

N

˜+ ˆk −1|k −1 + X ωik −1 f x k −1|k −1 (i)

i=1

˜k −1|k −1 + wk −1 ˜k −1|k −1 + o x = Φk |k −1 x − Φk |k −1

N

˜+ ωik −1 X k −1|k −1

−

ωik −1 o

Φk |k −1

N

ωik |k −1 h xik |k −1

i=1

˜k |k −1 + Bk ΔB k EB k xk + vk = Hk |k −1 + Ck +1 N1,k+1 Θk+1 x N ˜− − Hk |k −1 + Mk +1 N2,k +1 Θk +1 ωik |k −1 X k |k −1 (i) , i=1

(41) with Hk |k −1 =

∂h(xk ) , ∂xk x k = xˆ k |k −1

Ck +1 and Mk +1 are problem dependent scaling matrices, and Ni,k +1 ,Θk +1 are similar to the matrices definition in (39).Bk ΔB k EB k xk represents the parameter uncertainties, where Bk is known scaling matrix of appropriate dimensions, EB k ∈ n ×n is known and can be set as I, ΔB k ∈ n ×n is unknown matrix satisfying ΔB k ΔTB k ≤ I [31]. Substitute (28), (40) and (41) into (16), we obtain x ˜k |k = xk − (ˆ xk |k −1 + Kk ˜ zk |k −1 ) = I − Kk Hk |k −1 + Ck +1 N1,k +1 Θk +1 x ˜k |k −1 − Kk (Bk ΔB k EB k xk + vk ) .

(42)

Theorem 3.1: The prediction error covariance Pk |k −1 and the filtering error covariance Pk |k can be formulated as Pk |k −1 = Φk |k −1 + Ak N1,k Θk Pk −1|k −1 T × Φk |k −1 + Ak N1,k Θk + Qk −1 , (43)

+ Kk (ΔPk + Rk ) KTk ,

˜+ Xk −1|k −1 (i) ,

∂f(xk −1 ) = ∂xk −1

˜ zk |k −1 = h(xk) + Bk ΔB k EB k xk + vk −

Pk |k = I − Kk Hk |k −1 + Ck +1 N1,k +1 Θk +1 Pk |k −1 [♦]T

(i)

(38)

i=1

with

By considering the uncertainties presented in measurement model, we can expand h(xk ) around x ˆk |k −1 as

and

i=1 N

linearization error of system model and both of them satisfy Nj,k (Nj,k )T ≤ I, with j = 1, 2. Substitute (30) into (38), we have ˜k −1|k −1 + wk −1 . (40) x ˜k |k −1 = Φk |k −1 + Ak N1,k Θk x

with

ΔPk = Bk ΔB k EB k E xk xTk ETB k ΔTB k BTk ,

x k −1 = x ˆ k −1 |k −1

The high order terms of Taylor series are formulated as [30] ˜k −1|k −1 = Ak N1,k Θk x ˜k −1|k −1 , (39a) o x ˜+ ˜+ (39b) o X i,k −1|k −1 = Dk N2,k Θk Xk −1|k −1 (i) , where Ak and Dk are problem dependent scaling matrices, Θk is an extra degree of freedom to tune the filter, N1,k and N2,k are unknown time-dependent matrices accounting for the

(45)

where [Ω]Pk |k −1 [♦] represents [Ω]Pk |k −1 [Ω] . Proof: It can be shown that (43), (44) follow directly from (40) and (42), respectively, so the proof is omitted for conciseness. Remark 3.4: Noting (43) and (44), we can conclude that due to consideration of nonlinear errors and parameter uncertainties in measurement model, there are unknown matrices Ak , Ck and ΔB k in the construction of Riccati equation. These matrices are often not easy to tune in practice, so there is no close-form solution to the nonlinear filtering. However, a suitable filter gain Kk should be designed to guarantee an upper bound covariance T

.

(44)

T


of the filtering error and also to minimize the upper bound. The boundedness proof of filtering error covariance corresponding to Theorem 3.1 is given in Appendix A. D. Discussion on the ICKF The novel sigma-points generation method is based on the moments error in approximating the intractable integrals of nonlinear filter with bounded uncertainties. Noting (44), similar to the computation of L− k we define ΔE in (31) as ΔE = Λ ·

Kk Rk KTk

,

(46)

where Λ is a weight matrix depends on the observability of state, e.g., for GNSS/INS we set the diagonal of Λ as 1 except the elements corresponding to non-direct observed state, and set the off-diagonal elements of Λ as 0 to make the correction for attitude and sensor errors unaffected. By separating the stochastic uncertainties from Pk |k −1 and Pk |k as in (29) and (31), the transformation of sigma-points error matrix can account for the bounded uncertainties of system models more precisely. Once ˜− the update phase with no innovation is provided, X k |k −1 captures more prior information than existing sampling methods. The predicted cubature points xik |k −1 can also provide useful information accounting for high-order terms of moments and non-Gaussian information of dynamic model. Based on above analysis, we can conclude that when additional noise is considered, in addition to removing the Gaussian assumptions, ICKF reaches more accurate estimation compared with CKF. A local convergence proof of ICKF is given in Appendix B, and result indicates when some conditions are satisfied x ˜k |k would converge to zero asymptotically. Noting (66) and (67), the convergence is affected by the accuracy of prior pdf, however when Qk is larger this influence become less. Notice that, in Algorithm 2 the initialization of sigma-points error matrix is done by using CKF. However, we can employ other complicated but efficient algorithms, such as PF, Gaussian sum filter and the algorithm proposed in [17], to reduce the initial error of prior pdf. Clearly, when Qk is positive and large enough the estimation error is bounded even for bad approximation to the nonlinear model. The difference of computational cost between ICKF and CKF lies in the generation of cubature points. For ICKF, the computational cost of float multiplication in cubature points update is 53 n3 + 5n2 , and for CKF this value is 23 n3 + 4n2 . As the novel sigma-points update needs one more matrix inversion and one more matrix multiplication consisted in (37), it is slightly more computational demanding than traditional sigma-points generation method. It should be mentioned that similar works have been done in previous reports, which update the points directly without constructing the pdf [28], [32]. However, the motivation of our work is different from their approaches. Unlike [28], we focus on the high dimension filtering problems, and by uncoupling the dependence of approximation error on the dimension of system model, the new sigma-points update is more efficient for highdimensional filtering problem. In [32], the error matrix from predicted observations is also involved in the update of sigma-

2981

points, which increases the computational cost obviously and makes it not suitable for high dimension filtering problems. As there are often model uncertainties in measurement function, the extra information from the sigma-points error of likelihood approximation is more involved. We bring uncertainty reduction into the transform of sigma-points error based on the analysis of model uncertainties, and the above-mentioned methods do not try to address the model uncertainties problem. IV. NUMERICAL RESULTS AND DISCUSSION The filtering model of tightly coupled GNSS/INS is given and its nonlinearity is analyzed in Appendix C. Simulation results based on the data set generated by Spirent SimGEN are reported. A. Filtering Model of Tight GNSS/INS The body frame (b-frame) is selected as Right-Front-Up, local navigation frame (n-frame) is selected as East-North-Up, and the details of geocentric inertial frame (i-frame) and earth-centered earth-fixed frame (e-frame) can be found in [22]. The state vector of tightly coupled GNSS/INS is

T x = ψ δv δp ba bg δtu δtr u , where ψ is attitude vector, δv is the error vector of velocity resolving in n-frame, δp is position error vector, ba and bg are the biases of the accelerometers and gyros in three axes expressed in b-frame, δtu and δtr u are the offset and drift of receiver clock expressed in meters and m/s. The nonlinear system model of INS can be formulated as

˙ = Cω (I − Cpn ) ωnin + δωnin − Cpb δωbib , (47) ψ δvn = I − Cnp Cpb fibb + Cpb δfibb + δvn × (2ωnie + ωnen ) + vn × (2δωnie + δωnen ) , δ L˙ =

δvN , δ h˙ = δvU , RN + h

δ λ˙ =

vE sec L δvE sec L + δL tan L, RN + h RN + h

(48)

(49)

where Cω is the transformation matrix from angular velocity to Euler angles,Cba represents the rotation matrix from a-frame to b-frame, ωcba represents the rotation of a-frame with respect to b-frame resolved about c-frame, δωcba is the corresponding error vector. fibb is the measured specific force resolved about bframe, δfibb is corresponding error vector. L,λ, h are the latitude, longitude and height, vE , vN , vU are the east, north and up velocity, and δvE , δvN , δvU , δL, δλ, δh are corresponding error vector. RM and RN are the median radius and normal radius, respectively. B. Results and Discussion The duration of trajectory is 418 s, and the aircraft runs at a speed of 200 m/s with two 45° turns in opposite directions. The initial attitude error is (−0.05, 0.04, 5) in degree, the update rates of GNSS and INS are 2 Hz and 100 Hz. The bias of accelerometer is 1 mg, and the noise of accelerometer is

2982

Fig. 3. Attitude error of different algorithms with no signal outage. CKF performs slightly better than EKF, and ICKF outperforms the other two filters.

√ 0.1 mg/√ Hz. The bias of gyro is 10 ◦ /h, and the noise of gyro is 0.1 ◦ / h. The equivalent pseudo-range error of receiver clock is 10 km, and the equivalent pseudo-range rate error is 100 m/s. The root mean square error (RMSE) is taken as the metric to evaluate the performance of different filtering methods. The attitude result of different algorithms is shown in Fig. 3. It can be clearly seen that ICKF outperforms EKF and CKF. Although CKF reaches better approximation accuracy than EKF, CKF shows almost similar performance with EKF in terms of convergence rate and stability accuracy. As the only difference between CKF and ICKF is the prior information provided to measurement update, we can conclude that the approximation error is negligible compared with the error due to less informative prior distribution. It is notable that both EKF and CKF produce biases when the filter is stable, which can be further alleviated by reducing the system uncertainty. However, it is always the case that a large Qk is initialized to account for the unknown prior information at first, and it is tedious to tune this value in the design of KF. Notice that, ICKF shows fast convergence rate in terms of yaw than EKF and CKF with the same filter parameters including Qk and initial filtering error covariance. More detailed comparison is given in Fig. 4, where the RMSEs of attitude, velocity (V) and position (P) are computed. The results in Fig. 4 confirm that CKF shows slight better accuracy than EKF in tight GNSS/INS even if a large initial yaw error is given, and ICKF achieves much more accurate estimation than CKF. To verify the performance of ICKF with observations missing, outages are simulated by setting Kk = 0. The results of different filters with outages started at the 30th epoch and with duration 60 s (Outage#2) are shown in Fig. 5, Fig. 6 and Fig. 7. Notice that, CKF shows slight better result than EKF when outage occurs, and converges faster than EKF when the signal appears again. This coincides with the fact that CKF performs SLR with respect to sigma-points located at the prior distribution region, and EKF only performs SLR with respect to a single point in this region. Once the signal appears again,


Fig. 4. RMSEs of different algorithms with no signal outage. Notice that, ICKF performs much better than the other two filters.

Fig. 5. Attitude error of different algorithms with 60 s signal outage. There are biases for EKF and CKF when they are stable. On the contrary, ICKF reduces the biases and performs the best among the filters.

in case the undergoing filter model does not far away from the actual system model, the convergence of filter would be accelerated. Notice that, ICKF performs better than CKF which coincides with our previous analysis. Both CKF and ICKF perform SLR with respect to the sigma-points, the difference lies in that CKF inherits information from prior pdf by mean and covariance only, whereas ICKF introduces the posterior sigma-points error matrix without constructing prior pdf, which removes the dependence of cubature points update on Gaussian assumption. By comparing Fig. 7 with Fig. 4, we can conclude that the states with strong observability (e.g., position and velocity) are largely degraded when there is no innovation input to (16). However, the estimation of states that with weak observability, which depends on the error covariance of predicted state is less sensitive to the observation missing. Furthermore, if we can slow down the varying of error covariance, these states


Fig. 6. Velocity error of different algorithms with 60 s signal outage. CKF performs slightly better result than EKF, and ICKF performs the best among the filters.

2983

Fig. 8. Trajectory of simulated sensor outages. All the outages start at the 30th epoch represented in pentagram symbol, and the ends of the outages are indicated by circular symbols marked in corresponding colors.

TABLE I POSITION RESULT OF DIFFERENT ALGORITHMS

Fig. 7. RMSEs of different algorithms with 60 s signal outage. Notice that, the position results of EKF and CKF are degraded seriously.

can be tracked for long duration until the change of system uncertainty (i.e., Qk ) cannot be neglected any more. However, as there are no effective tools to evaluate the uncertainty of filtering model online, it is advisable to separate it from the generation of cubature points. When the value of Qk is large enough, the bounded uncertainty contained in system models can be neglected and ICKF may reduce to CKF (see Fig. 2). The main concern of vehicle navigation system in urban or indoor areas is the position error. Define following equation to evaluate the accuracy of position result RM SEpos

M 1 = (RM SEer + RM SEnr ), M r =1

where M is the number of Monte Carlo (MC) run, RM SEer and RM SEnr are the RMSE of east and north position of the rth run, which are expressed in meters. As we have stated, the performance of ICKF depends on the uncertainty of system model defined by (8). However, as the filter does not know what

the dynamic of vehicle will be, it is useful to study its sensitivity to different dynamic conditions. In Fig. 8, another three outages are simulated, all of which start at the 30th epoch with durations of 30 s (Outage #1), 90 s (Outage #3) and 120 s (Outage #4), respectively. By setting M = 20, the RMSEp os of different filtering methods are listed in Table I. Notice that, during outage #1 and outage #2 the vehicle turns in the same direction, so the system uncertainty does not change very much, both EKF and CKF can produce degraded position result. Although the position error of ICKF in outage #3 increases when the vehicle turns in opposite direction, it keeps on tracking the state with acceptable precision. However, the results of EKF and CKF are largely degraded in outage #3, and both of the two algorithms diverge in most of the runs when the duration of outage is increased to 120 s. In a word, with the present of short outages, ICKF has much smaller error than EKF and CKF, which demonstrates that SLR with respect to the novel cubature points is insensitive to observations missing. To verify the superiority of ICKF in statistical sense, Fig. 9 shows the boxplot of 50 MC runs for 60 s outage case. The index represents the state of navigation system, e.g., 1-7 implies yaw, pitch, roll, velocity and position in east and north directions, respectively. It is notable that, the result of ICKF shows much better robustness than CKF when different noises with the same statistical property are used. Furthermore, the attitude result of ICKF shows less dispersion and outliers than CKF, which indicates that ICKF has more consistent covariance than CKF. As the correction for attitude come from the off-diagonal elements

2984


First, rewrite (43) and (44) as follows: Pk |k −1 Pk −1|k −1 = Φk |k −1 + Ak N1,k Θk Pk −1|k −1 (♦)T

Pk |k Pk |k −1

+ Qk −1 , (50) = I − Kk Hk |k −1 + Ck +1 N1,k +1 Θk +1 Pk |k −1[♦]T + Kk Bk ΔB k EB k E xk xTk ETB k ΔTB k BTk + Rk KTk ,

(51)

where T ˆk |k −1 + x E xk xTk = E x ˆk |k −1 + x ˜k |k −1 x ˜k |k −1 . Fig. 9. Boxplot of the RMSEs for 50 runs with 60 s signal outage. ICKF shows better robustness than EKF and CKF in terms of all the navigation parameters, and the superiorities in position and velocity are significant.

of prediction error covariance, we can conclude that the novel cubature points update method is more efficient than EKF and CKF in computing the covariance of prior pdf.

We have the following elementary inequality

1

1

ε2 x ˜k |k −1 − ε− 2 x ˆk |k −1

T 1 1 ˜k |k −1 − ε− 2 x ˆk |k −1 ≥ 0, ε2 x

yields ˆTk |k −1 + x ˆk |k −1 x ˜Tk |k −1 ≤ ε˜ xk |k −1 x ˜Tk |k −1 x ˜k |k −1 x + ε−1 x ˆk |k −1 x ˆTk |k −1 ,

V. CONCLUSION In this paper, a novel cubature points update method is proposed, based on which an improved cubature Kalman filter (ICKF) is developed. Theoretical analysis and simulation results reveal that ICKF shows better accuracy and faster convergence rate than CKF and EKF when observation missing occurs in high-dimensional filtering problem. Although CKF is slightly better than EKF when there are large initial yaw error and uncertainties in filtering model, it has no benefits to replace EKF with CKF as the later increases the computational cost but without improving the filtering accuracy obviously. The advantages of ICKF can be summarized as follows: r the generation of cubature points does not depend on the dimension of system state, making it suitable for highdimensional filtering problem. r posterior cubature points, which includes the diffused points and sigma-points error matrix are useful for the approximation of likelihood when large prior error is provided. r estimation of non-direct observed state can be improved by designing ΔE which is intuitively understood based on the model uncertainties analysis. We provide a general class of sigma-points update framework that works in an implicit way. Future work would focus on designing the prior pdf in an explicit form [33]. The usage of ICKF on real land vehicle navigation is also under consideration. APPENDIX A The proof of the bounded error covariance is provided in this appendix. This proof is a resemblance to the proof in [30] with some simplifications.

where ε is a positive scalar, then we get E xk xTk ˆTk |k −1 + x ˆk |k −1 x ˜Tk |k −1 + x ˜k |k −1 x ˆTk |k −1 =E x ˆk |k −1 x +x ˜k |k −1 x ˜Tk |k −1 ˆk |k −1 x ≤ E (1 + ε) x ˜k |k −1 x ˜Tk |k −1 + 1 + ε−1 x ˆTk |k −1 , (52) By considering (51) and (52) we have Pk |k ≤ I − Kk Hk |k −1 + Ck +1 N1,k +1 Θk +1 Pk |k −1 [♦]T + Kk Bk ΔB k EB k Tk |k −1 ETB k ΔTB k BTk + Rk KTk . (53) where ˆk |k −1 x ˆTk |k −1 . Tk |k −1 = (1 + ε) Pk |k −1 + 1 + ε−1 x We introduce two discrete-time Riccati-like difference equations: Σk |k −1 Σk −1|k −1 T T = Φk |k −1 Σ−1 k −1|k −1 − γ1,k Θk Θk Φk |k −1 −1 + γ1,k Ak ATk + Qk −1 ,

(54)


Σk |k Σk |k −1

where Q∗k = αk Fk Kk (ΔPk + Rk )(αk Fk Kk )T + Qk . Let the following assumptions hold: 1) There are real positive num∗ bers fm in , hm in , βm in , αm in , αm ax , qm in , qm ax , qm in , rm ax , pm ax , pm in such that the following bounds on various matrices are satisfied for every k ≥ 0:

−1 T − γ Θ Θ = I − Kk Hk |k −1 Σ−1 2,k +1 k +1 k +1 k |k −1 T I − Kk Hk |k −1 −1 T T + γ2,k +1 Kk Ck +1 Ck +1 Kk + Kk Bk ΔB k EB k Tk |k −1 ETB k ΔTB k BTk + Rk KTk , (55)

where Σ0|0 = P0|0 > 0,γ1,k , γ2,k +1 are positive scalars, and the following constraints −1 γ1,k I − Θk Σk −1|k −1 ΘTk > 0,

fm2 in I ≤ Φk ΦTk , h2m in I ≤ Hk HTk , βm in I ≤ βk , αm in I ≤ αk ≤ αm ax I, pm in I ≤ Pk ≤ pm ax I, ∗ ∗ qm in I ≤ Qk ≤ qm ax I, Rk ≤ rm ax I, qm in I ≤ Qk .

are satisfied. Based on the Lemma 2 and 3 in [30], we can conclude that Pk |k ≤ Σk |k . Then, we would like to design Kk that minimizes the upper bound of Σk |k . By letting the partial derivative of Σk |k with respect to Kk be zero, we obtain ∂tr Σk |k ∂Kk −1 T = −2 I − Kk Hk |k −1 Σ−1 − γ Θ Θ 2,k +1 k +1 k +1 k |k −1 HTk |k −1 + 2Kk −1 T T T T × γ2,k +1 Ck +1 Ck +1 + Bk ΔB k EB k Tk |k −1 EB k ΔB k Bk + Rk = 0. Base on the above equation, the optimal Kk is computed as −1 T Kk = Σ−1 − γ Θ Θ HTk |k −1 2,k +1 k +1 k +1 k |k −1

−1 T × Hk |k −1 Σ−1 − γ Θ Θ HTk |k −1 2,k +1 k +1 k +1 k |k −1 −1 T T T T + γ2,k +1Ck +1 Ck +1 + Bk ΔB k EB k Tk |k −1 EB k ΔB k Bk

(56)

In this section, we provide a local convergence analysis for the ICKF by following [34] and [35]. First, we define

=x ˜Tk |k −1 [αk Φk (I − Kk βk Hk )]T P−1 k +1|k × [αk Φk (I − Kk βk Hk )] x ˜k |k −1 + E (Bk ΔB k EB k xk + vk )T (αk Φk Kk )T P−1 k +1|k

x ˜ . ×αk Φk Kk (Bk ΔB k EB k xk + vk ) + wkT P−1 w k k |k −1 k +1|k (62)

From (58), we have Pk +1|k

= αk Φk (I − Kk βk Hk) Pk |k −1 + [αk Φk (I − Kk βk Hk)]−1

× Q∗k [αk Φk (I − Kk βk Hk )]−T

[αk Φk (I − Kk βk Hk )]T . (63)

On the basis of (60), we get [35] T αk Φk (I − Kk βk Hk ) Q∗−1 k [αk Φk (I − Kk βk Hk )] , (64) ≤ (αm ax fm ax + αm ax fm ax K∗ βm ax hm ax )2 Q∗−1 k where K∗ is the upper bound of Kk . Substituting (64) into (63), the function becomes

Φk |k −1 + Ak N1,k Θk = αk Φk , Hk |k −1 + Ck +1 N1,k +1 Θk +1 = βk Hk , where αk =diag([α1k , · · · , αn k]), βk =diag([β1k , · · · , βpk]). Follow from (40)-(44) we have x ˜k +1|k = αk Φk x ˜k |k −1 − αk Φk Kk ˜ zk |k −1 + wk ,

(60)

˜k |k = 0. and we want to prove limk →∞ x Define Vk +1 (˜ xk +1|k ) = x ˜Tk +1|k P−1 ˜k +1|k , by noting k +1|k x (58) it is clear that 2 2 x x ˜k +1|k ˜k +1|k ˜k+1|k ≤ ≤ Vk+1 x . 2 2 2 2 pm ax αm pm ax αm ax fm ax I+ qm ax I ax fm ax I (61) Take the conditional expectation yields ˜k +1|k x ˜k |k −1 E Vk +1 x

when Θk +1 , Bk and Ck +1 are equal to zero, (56) reduce to (15). APPENDIX B

(59)

2) There exist real constants fm ax , hm ax , βm ax such that the following bounds are satisfied:

αk Φk ≤ fm ax , βk Hk ≤ hm ax , βk ≤ βm ax ,

−1 T γ2,k +1 I − Θk +1 Σk |k −1 Θk +1 > 0,

+ Rk }−1 ,

2985

(57)

Pk +1|k = [αk Φk (I − Kk βk Hk )] Pk |k −1 [♦]T + Q∗k , (58)

−T P−1 k +1|k ≤ [αk Φk (I − Kk βk Hk )] −1 ∗ qm in I × Pk |k−1 + (αm ax fm ax + αm ax fm ax K∗βm ax hm ax)2

× [αk Φk (I − Kk βk Hk )]−1 ,

(65)

2986


then the first term in (62) can be written as [αk Φk (I − Kk βk Hk )]

T

P−1 k +1|k

Equal (70) with (71) we get − Cnp (ωnn p ×)Cpb + (I − Cnp )Cpb (ωbpb ×)

[αk Φk (I − Kk βk Hk )]

∗ pm in qm in I ≤ 1− ∗ I ∗ (αm ax fm ax + αm ax fm ax K βm ax hm ax)2 + pm ax qm in

× P−1 k |k −1 .

(66)

vk∗

Define = Bk ΔB k EB k xk + vk , then under the assumptions (59) and (60) we obtain ∗ E (vk∗ )T (αk Φk Kk )T P−1 k +1|k αk Φk Kk vk ˜k |k −1 + wkT P−1 k +1|k wk x ≤

2 2 1 (K∗ )2 αm ax fm ax tr (vk∗ )T vk∗ + tr wkT wk = μ. pm in pm in (67) p

q∗

I

Define λ = (α f +α f m iKn ∗mβ i n h ) 2 +p q ∗ I , and it m ax m ax m ax m ax m ax m ax m a x m in is clear that μ > 0 and 0 < λ < 1. By inserting (67) into (62), we have ˜k +1|k x ˜k |k −1 ≤ x ˜Tk |k −1 (1 − λ) x ˜k |k −1 + μ. E Vk +1 x Apply Lemma 1 of [34] we can conclude that x ˜k +1|k is bounded in mean square. Then, on the basis of (57) we obtain 2 ˜k +1|k − wk

˜ xk 2 = (αk Φk )−2 x 2 ˜k +1|k + wk ≤ (αk Φk )−2 x 2 −2 −2 + wk 2 . x ˜ (68) ≤ 2αm f k +1|k in m in Take the expectation of (68) yields 2 2 −2 −2 x E ˜ xk 2 ≤ 2αm E ˜ + E

w . f

k k +1|k in m in As both x ˜k +1|k and wk are bounded in mean square, we can ˜k |k = 0. conclude limk →∞ x

The nonlinearity of system model for GNSS/INS is analyzed in this section. Because of the misalignment error and random drift of gyro and accelerometer, the actual navigation frame (p-frame) does not coincide with ideal navigation coordinate n-frame. Let Cpn be the rotation matrix from n-frame to pframe, which is also the attitude error matrix, ωpn b and ωnn b are the rotation velocity of b-frame with respect to n-frame resolved about computation frame and ideal navigation frame, respectively. Then we have (69)

By taking derivation of (69) with respect to the rotation velocity, we get ˙ np Cp + (I − Cnp )C ˙ p, ˙ pn = −C C b b

(72)

Left multiple (72) by Cpn , right multiple (72) by Cbp gives (ωnn p ×) + Cpb (ωbpb ×)Cbp − Cpb (ωbn b ×)Cbp = 0.

(70)

(73)

By substituting Cpb (ωbpb ×)Cbp = (ωppb ×), Cpb (ωbn b ×)Cbp = (ωpn b ×), into (73), and using the mapping of vector and its skewsymmetric matrix we get ωnn p = ωpn b − ωppb ˆ pin ) = Cpb (ωbib − Cbn ωnin ) − (ˆ ωbib − Cbp ω

= Cpb (ωbib − Cbn ωnin ) − (ωbib + δωbib − Cbp (ωnin + δωnin )) = (I − Cpn )ωnin − Cpb δωbib + δωnin .

(74)

Define trigonometric function cos = C, sin = S, and a series of rotation is performed to align the navigation axis, which is expressed in vector as ψ = [ ϕ θ ψ ]T , then we get Cpn = Ry (θ)Rx (ϕ)Rz (ψ) ⎡ ⎤ CθCψ − SθSϕSψ CθSψ + SθSϕCψ −SθCϕ ⎢ ⎥ −CϕSψ CϕCψ Sϕ ⎦, =⎣ SθCψ + CθSϕSψ SθSψ − CθSϕCψ

CθCϕ (75)

where Rα (β) stand for the rotation axis is α and the angular is β. Assume all the Euler platform error angles are of small values, i.e., Cω ≈ 1, and the biases are the only gyro errors modeled as filter states, i.e., δωbib ≈ bg then ˙ = (ψ×)ωnin − Cpb bg + δωnin . ωnn p ≈ ψ

APPENDIX C

Cpn = Cpb − Cnb = (I − Cnp )Cpb .

− Cpb (ωbpb ×) + Cnb (ωbn b ×) = 0.

(76)

When there is large error in attitude, e.g.,ψ is large, which is often the case in land vehicle navigation with low cost inertial sensors, then ⎡ ⎤ 1 − Cψ −Sψ θ ⎢ ⎥ Sψ 1 − Cψ −ϕ ⎦ . (77) I − Cpn = ⎣ −θCψ − ϕSψ

−θSψ + ϕCψ

0

Under the assumption that there are no observations errors or model uncertainties, an ideal velocity can be computed. However, there are always sensor errors and model uncertainties that generate attitude error. When the biases of accelerometer are the only errors modeled as filter states, i.e., δfibb ≈ ba , and large ψ is assumed, the difference between ideal velocity and actual velocity satisfies δv˙ = (I − (Cpn )T )Cpb fibb + Cpb ba + δv × (2ωnie + ωnen )

and ˙ pn = Cp (ωbpb ×) − Cnb (ωbn b ×). C b

(71)

+ v × (2δωnie + δωnen ).

(78)


Noticed that, the attitude and velocity error function have items that contain the multiplication of states and their trigonometric function, so the filter must at least approximates the process function at the accuracy of 2nd-order in mean Taylor series and 4th-order in covariance Taylor series. REFERENCES [1] R. E. Kalman, “A new approach to linear filtering and prediction problems,” J. Basic Eng., vol. 82, pp. 35–45, Mar. 1960. [2] H. Cox, “On the estimation of state variables and parameters for noisy dynamic systems,” IEEE Trans. Autom. Control, vol. 9, no. 1, pp. 5–12, Jan. 1964. [3] M. Athans, R. Wisher, and A. Bertolini, “Suboptimal state estimation for continuous-time nonlinear systems from discrete noisy measurements,” IEEE Trans. Autom. Control, vol. 13, no. 5, pp. 504–514, Oct. 1968. [4] J. L., Crassidis and F. L. Markley, “Unscented filtering for spacecraft attitude estimation,” J. Guid., Control, Dyn., vol. 26, no. 4, pp. 536–542, Jul. 2003. [5] Y. X. Wu, D. W. Hu, and X. P. Hu, “A numerical-integration perspective on Gaussian filters,” IEEE Trans. Signal Process., vol. 54, no. 8, pp. 2910–2921, Aug. 2006. [6] S. J. Julier, J. K. Uhlman, and H. F. Durrant-Whyte, “A new approach for filtering nonlinear systems,” in Proc. IEEE Amer. Control Conf., Jun. 1995, pp. 1628–1632. [7] W. I. Tam and D. Hatzinakos, “An efficient radar tracking algorithm using multidimensional Gauss-Hermite quadratures,” in Proc. IEEE Int. Conf. Acoust., Speech Signal Process., Apr. 1997, vol. 5, pp. 3777–3780. [8] K. Ito and K. Xiong, “Gaussian filtering for nonlinear filtering problems,” IEEE Trans. Autom. Control, vol. 45, no. 5, pp. 910–927, May 2000. [9] I. Arasaratnam and S. Haykin, “Cubature Kalman filters,” IEEE Trans. Autom. Control, vol. 54, no. 6, pp. 1254–1269, Jun. 2009. [10] B. Xu, P. Zhang, H. Z. Wen, and X. Wu, “Stochastic stability and performance analysis of cubature Kalman filter,” Neurocomput., vol. 186, pp. 218–227, 2016. [11] J. L. Crassidis, “Sigma-point Kalman filtering for integrated GPS and inertial navigation,” IEEE Trans. Aerosp. Electron. Syst., vol. 42, no. 2, pp. 750–756, Apr. 2006. [12] J. Wendel, J. Metzger, R. Moenikes, A. Maier, and G. F. Trommer, “A performance comparison of tightly coupled GPS/INS navigation systems based on extended and sigma point Kalman filters,” Navigation, vol. 53, no. 1, pp. 21–31, 2006. [13] Y. W. Zhao, “Performance evaluation of cubature Kalman filter in a GPS/IMU tightly-coupled navigation system,” Signal Process., vol. 119, pp. 67–79, 2016. [14] M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking,” IEEE Trans. Signal Process., vol. 50, no. 2, pp. 174–188, Feb. 2002. [15] M. F. Bugallo, S. Xu, and P. M. Djuric, “Performance comparison of EKF and particle filtering methods for maneuvering targets,” Digit. Signal Process., vol. 17, pp. 774–786, 2007. [16] P. M. Djuric et al., “Particle filtering,” IEEE Signal Process. Mag., vol. 20, no. 5, pp. 19–38, Sep. 2003. [17] Y. S. Shmaliy, “An iterative Kalman-like algorithm ignoring noise and initial conditions,” IEEE Trans. Signal Process., vol. 59, no. 6, pp. 2465–2473, Jun. 2011. [18] X. Y. Chen, C. Shen, W. B. Zhang, M. Tomizuka, Y. Xu, and K. L. Chiu, “Novel hybrid of strong Kalman filter and wavelet neural network for GPS/INS during GPS outages,” Measurement, vol. 46, no. 10, pp. 3847–3854, 2013. [19] M. T. Menegaz, J. Y. Ishihara, G. A. Borges, and A. N. Vargas, “A systematization of the unscented Kalman filter theory,” IEEE Trans. Autom. Control, vol. 60, no. 10, pp. 2583–2598, Oct. 2015. [20] S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear estimation,” Proc. IEEE, vol. 92, no. 3, pp. 401–422, Mar. 2004. [21] S. J. Julier and J. K. Uhlmann, “The scaled unscented transformation,” in Proc. IEEE Amer. Control Conf., May 2002, pp. 4555–4559. [22] P. D. Groves, Principles of GNSS, Intertial, and Multisensory Integrated Navigation Systems, 3rd ed. Boston, MA, USA: Artech House, 2013, pp. 584–606. ´ Garc´ıa-Fernández, “Analysis of Kalman filter ap[23] M. R. Morelande and A. proximations for nonlinear measurements,” IEEE Trans. Signal Process., vol. 61, no. 22, pp. 5477–5483, Nov. 2013.

2987

´ Garc´ıa-Fernández, M. R. Morelande and J. Grajal, “Truncated un[24] A. scented Kalman filtering,” IEEE Trans. Signal Process., vol. 60, no. 7, pp. 3372–3386, Jul. 2012. ´ Garc´ıa-Fernández, L. Svensson, M. R. Morelande and S. Sarkka, [25] A. “Posterior linearization filter: Principles and implementation using sigma points,” IEEE Trans. Signal Process., vol. 63, no. 20, pp. 5561–5573, Oct. 2015. [26] L. B. Chang, B. Q. Hu, A. Li, and F. J. Qin, “Transformed unscented Kalman filter,” IEEE Trans. Autom. Control, vol. 58, no. 1, pp. 252–257, Jan. 2013. [27] C. J. Masrelied, “Approximate non-Gaussian filtering with linear state and observation relations,” IEEE Trans. Autom. Control, vol. 20, no. 1, pp. 107–110, Feb. 1975. [28] Y. Tian and Y. Cheng, “Novel measurement update method for quadraturebased Gaussian filters,” in Proc. AIAA Guid., Navig., Control Conf., Aug. 2013. [29] B. Jia, M. Xin, and Y. Cheng, “Sparse-grid quadrature nonlinear filtering,” Automatica, vol. 48, no. 2, pp. 327–341, 2012. [30] J. Hu, Z. D. Wang, H. J. Gao, and L. K. Stergioulas, “Extended Kalman filtering with stochastic nonlinearities and multiple missing measurements,” Automatica, vol. 48, no. 9, pp. 2007–2015, 2012. [31] K. Xiong, C. L. Wei, and L. D. Liu, “Robust Kalman filtering for discretetime nonlinear systems with parameter uncertainties,” Aerosp. Sci. Technol., vol. 18, no. 1 pp. 15–24, 2012. [32] Y. L. Huang, Y. G. Zhang, N. Li, and L. Zhao, “An improved Gaussian approximate filtering method,” Acta Autom. Sinica, vol. 42, no. 3, pp. 385– 401, Mar. 2016. [33] F. Sandblom and L. Svensson, “Moment estimation using a marginalized transform,” IEEE Trans. Signal Process., vol. 60, no. 12, pp. 6138–6150, Dec. 2012. [34] K. Xiong, H. Y. Zhang, and C. W. Chan, “Performance evaluation of UKF-based nonlinear filter,” Automatica, vol. 42, no. 2, pp. 261–270, 2006. [35] L. Li and Y. Q. Xia, “Stochastic stability of the unscented Kalman filter with intermittent observations,” Automatica, vol. 48, no. 5, pp. 978–981, 2012.

Bingbo Cui received the B.S. degree in electrical engineering and the M.S. degree from Chongqing University of Technology, Chongqing, China, in 2005 and 2012, respectively. He is currently working toward the Ph.D. degree at the Faculty of Precision Instrument and Machinery, Southeast University, Nanjing, China. His research interests include inertial navigation, nonlinear filtering, and integrated navigation in particular.

Xiyuan Chen (M’10–SM’13) received the Ph.D. degree in precision instrument and machinery from Southeast University, Nanjing, China, in 1998. He is currently a Professor in the School of Instrument Science and Engineering, Southeast University. His research interests include fiber optic sensors, inertial navigation, GNSS software receiver and wireless location technologies, integrated navigation, and related application.

Xinhua Tang received the Ph.D. degree from Politecnico di Torino, Torino, Italy. He worked in the Istituto Superiore Mario Boella, Turin, Italy, in 2014. He is currently an Assistant Professor in Southeast University, Nanjing, China. His interest focuses on the development of advanced GNSS techniques including new discriminator design, vector tracking architecture, and ultratight integration.