Open Access
This article is
 freely available
 reusable
Sensors 2016, 16(8), 1167; https://doi.org/10.3390/s16081167
Article
Fuzzy Adaptive Cubature Kalman Filter for Integrated Navigation Systems
^{1}
Institute of Electrical Control Engineering, National Chiao Tung University, Hsinchu 300, Taiwan
^{2}
Department of Communications, Navigation and Control Engineering, National Taiwan Ocean University, Keelung 202, Taiwan
^{*}
Author to whom correspondence should be addressed.
Academic Editor:
Leonhard M. Reindl
Received: 13 April 2016 / Accepted: 16 July 2016 / Published: 26 July 2016
Abstract
:This paper presents a sensor fusion method based on the combination of cubature Kalman filter (CKF) and fuzzy logic adaptive system (FLAS) for the integrated navigation systems, such as the GPS/INS (Global Positioning System/inertial navigation system) integration. The thirddegree sphericalradial cubature rule applied in the CKF has been employed to avoid the numerically instability in the system model. In processing navigation integration, the performance of nonlinear filter based estimation of the position and velocity states may severely degrade caused by modeling errors due to dynamics uncertainties of the vehicle. In order to resolve the shortcoming for selecting the process noise covariance through personal experience or numerical simulation, a scheme called the fuzzy adaptive cubature Kalman filter (FACKF) is presented by introducing the FLAS to adjust the weighting factor of the process noise covariance matrix. The FLAS is incorporated into the CKF framework as a mechanism for timely implementing the tuning of process noise covariance matrix based on the information of degree of divergence (DOD) parameter. The proposed FACKF algorithm shows promising accuracy improvement as compared to the extended Kalman filter (EKF), unscented Kalman filter (UKF), and CKF approaches.
Keywords:
integrated navigation; cubature Kalman filter; unscented Kalman filter; fuzzy logic1. Introduction
The Global Positioning System (GPS) and inertial navigation system (INS) have complementary operational characteristics and the synergy of both systems has been widely explored [1,2,3]. The GPS/INS integration is the adequate solution to provide a navigation system that has superior performance in comparison with either a GPS or an INS standalone system. The performance of GPS/INS integrated navigation system depends heavily on the design of sensor fusion, which is typically carried out through the extended Kalman filter (EKF) to estimate the state variables of a vehicle system and suppress the navigation measurement noise. Since most of the GPS/INS integration is based on the complimentary filter architecture, the feedback configuration leads to an EKF mode of operation in contrast to an ordinary linearized Kalman filter (LKF) that is associated with the feedforward case. Due to this reason, the system nonlinearity is remained for the feedback case as in the EKF. Although the Kalman filter has been shown to be a minimum mean square error (MMSE) estimator, the fact that EKF relies on the first order linearization of the system model to propagate the mean and covariance of the state might suffer from the performance degradation and divergence problem due to the linearization process and system missmodeling. In addition, the complete calculation of the Jacobian matrix is cumbersome and timeconsuming [4].
To better treat the nonlinearity, the unscented Kalman filter (UKF) [5,6,7] has been developed to address nonlinear state estimation in the context of control theory, which uses a finite number of sigma points to propagate the probability of state distribution through the nonlinear dynamics of system. Unlike the conventional EKF achieving firstorder accuracy where the linearization process using the Jacobian matrices is involved, the UKF employs a minimal set of sigma points by deterministic sampling approach and at least the second order accuracy of the posterior mean and covariance can be captured. The UKF makes a Gaussian approximation with a limited number sigma points through the unscented transform (UT) [8,9]. When the sample points are propagated through the true nonlinear system, the posterior mean and covariance can be captured accurately to the second order of Taylor series expansion for any nonlinear system. However, the rounding errors of numerical calculation for UKF may destroy the nonnegative and asymmetry of covariance matrix, therefore, the convergence rate of the UKF approach is slow and the system may also be unstable. Investigation of the nonlinear filtering approach to the integrated navigation problem has been seen using the sigmapoint filters (SPFs) (e.g., [8,9]).
Under the Bayesian filtering framework, the CKF [10,11,12] provides a new direction to realize the nonlinear transformation. As opposed to the unscented transformation applied in UKF approach, the thirddegree sphericalradial cubature rule is employed in the CKF to compute numerical integration encountered in the nonlinear Bayesian filter. The CKF can be treated as a special case of the UKF, which can be seen in some of the existing literature [13,14]. It is claimed in [14] that the UT is essentially a Gauss quadrature rule and other similar rules can also be applied. The sphericalradial cubature rule used in CKF is a special case of the quadrature rules. Although the positioning performance of the UKF and the CKF might be similar or even identical if the parameters of the UKF are well tuned, the possible negative weights of the center point in UT can be avoided through tuning the parameters [5]. Comparing with the UKF, there are no parameters to tune in CKF approximating nonlinear functions of the system and measurement. In general, using the additional tuning parameters of UKF compared to CKF is not a demerit. They provide flexibility. If one does not want to bother to tune them, just fix them as their default values, or just exclude the center point, and the CKF is automatically obtained.
The sphericalradial cubature rule is composed of two different integrals, spherical and radial integrals. This is based on the sphericalradial transformation and generates an even number of equally weighted cubature points. However, these integrals are then numerically computed by the spherical cubature rule and the Gaussian quadrature rule, respectively. The performance can be improved in terms of estimation accuracy, numerical stability and computational costs [11,12,13,14]. On the other hand, The UKFcalculated estimation covariance matrix is not always guaranteed to be positive definite, which it should be for correct functioning of the UKF. In view of the stability of the nonlinear filter, the CKF can effectively avoid roundoff errors of numerical computation, and possesses more stable performance than the UKF and EKF [12]. The applicability and effectiveness of CKF based positioning system for sensor data fusion is presented in [15,16]. Liu et al. [16] presents an adaptive cubature Kalman filter (ACKF) algorithm based on maximum a posterior estimation and fading factor. In their research, the integration of inertial with distance measuring equipment (DME) was presented for the landbased navigation system. The adaptation of the measurement noise covariance matrix was introduced into the algorithm to enhance the applicability, where the adaptation module is based on the fading factor configuration with fuzzy logic.
There are basically two main factors that influence the estimation performance. First, there is model uncertainty because the adopted system model may not satisfy the actual state transition process. Second, there exist parameter uncertainties in the system model. The fixed value of covariance matrix cannot reflect the actual characteristics of the system error and measurement error. For a nonlinear system, the nonlinearity approximating capability may not be satisfied during the highdynamic vehicle maneuvers, no matter what kind of filter: the Taylor seriesbased EKF, the unscented transformationbased UKF, or the sphericalradial transformationbased CKF is used. Using a nonlinear filter with fixed values of parameters in the varying dynamic environment has a risk on divergence due to modeling errors. To overcome the problem, an adaptive mechanism that dynamically identifies modeling errors can be adopted.
To deal with the system nonlinearity as well as the noise uncertainty, the fuzzy logic adaptive system (FLAS) is introduced [16,17,18,19] into the CKF to form the fuzzy adaptive cubature Kalman filter (FACKF), in which the FLAS is employed to continually adjust the noise strength in the internal model of the CKF framework, and tune the filter as well as possible. The FACKF scheme is applied to the GPS/INS navigation to improve the navigation estimation accuracy. In FACKF, the CKF is involved in the algorithm to estimate the nonlinear system states, while the process noise covariance is adjusted through the FLAS. The fuzzy logic reasoning system [19] based on the TakagiSugeno (TS) model [20] is used in the FLAS. The fuzzy inference system (FIS) is constructed for obtaining the suitable weighting factor according to the timevarying change in dynamics. In addition, degree of divergence (DOD) parameters [21] are employed to identify the degree of change in vehicle dynamics. By monitoring the innovation information, the FLAS, as the filter’s internal module, is employed for dynamically adjusting the weighting factor based on the fuzzy rules so as to enhance the estimation performance and tracking capability.
The remainder of this paper is organized as follows. In Section 2, the preliminary background on the nonlinear filter is introduced, where the UKF and the CKF will be reviewed. The proposed sensor fusion strategy, FACKF approach, is introduced in Section 3. In Section 4, two illustrative examples are provided to evaluate the sensor fusion performance of the integrated navigation systems for the FACKF approach in comparison to those by conventional approaches. Conclusions are given in Section 5.
2. The Nonlinear Filters
Kalman filtering has been recognized as one of the most powerful state estimation techniques. The purpose of the Kalman filter is to provide the estimation with minimum error variance. The UKF is a nonlinear version of the Kalman filter and is widely used for the navigation sensor fusion. The unscented Kalman filtering deals with the case governed by the nonlinear stochastic difference equations:
where the state vector is ${\mathbf{x}}_{k}\in {\Re}^{n}$, process noise vector is ${\mathbf{w}}_{k}\in {\Re}^{n}$, measurement vector is ${\mathbf{z}}_{k}\in {\Re}^{m}$, and measurement noise vector is ${\mathbf{v}}_{k}\in {\Re}^{m}$. The vectors ${\mathbf{w}}_{k}$ and ${\mathbf{v}}_{k}$ are zero mean Gaussian white sequences having zero crosscorrelation with each other:
where ${\mathbf{Q}}_{k}$ is the process noise covariance matrix, and ${\mathbf{R}}_{k}$ is the measurement noise covariance matrix.
$${\mathbf{x}}_{k+1}=\mathbf{f}({\mathbf{x}}_{k},k)+{\mathbf{w}}_{k}$$
$${\mathbf{z}}_{k}=\mathbf{h}({\mathbf{x}}_{k},k)+{\mathbf{v}}_{k}$$
$$\begin{array}{c}\mathbf{E}[{\mathbf{w}}_{k}{\mathbf{w}}_{i}^{\mathrm{T}}]=\{\begin{array}{ll}{\mathbf{Q}}_{k},& i=k\\ 0,& i\ne k\end{array};\mathbf{E}[{\mathbf{v}}_{k}{\mathbf{v}}_{i}^{\mathrm{T}}]=\{\begin{array}{ll}{\mathbf{R}}_{k},& i=k\\ 0,& i\ne k\end{array};\\ \begin{array}{ccc}\mathbf{E}[{\mathbf{w}}_{k}{\mathbf{v}}_{i}^{\mathrm{T}}]=\mathbf{0}& & foralliandk\end{array}\end{array}$$
2.1. The Unscented Kalman Filter
The UKF was first proposed by Julier et al. [7] to address nonlinear state estimation in the context of control theory. The first step in the UKF is to generate the sigma points through the UT. One of the popular UT approaches is the scaled unscented transformation. Consider an $n$ dimensional random variable $\mathbf{x}$, having the mean $\widehat{x}$ and covariance $\mathbf{P}$, and suppose that it propagates through an arbitrary nonlinear function $\mathbf{f}$. The unscented transform creates $2n+1$ sigma vectors $\mathbf{X}$ (a capital letter) and weighted points $W$, given by
where ${\mathbf{(}\sqrt{\mathbf{(}n+\lambda \mathbf{)}\mathbf{P}}\mathbf{)}}_{i}$ is the ith row of the matrix square root. $\sqrt{\mathbf{(}n+\lambda \mathbf{)}\mathbf{P}}$ can be obtained from the lowertriangular matrix of the Cholesky factorization; $\lambda ={\alpha}^{2}(n+\kappa )n$ is a scaling parameter; $\alpha $ determines the spread of the sigma points around $\stackrel{\u2322}{x}$; $\kappa $ is a secondly scaling parameter; $\beta $ is used to incorporate prior knowledge of the distribution of $\overline{x}$; ${W}_{i}^{(m)}$ is the weight for the mean associated with the ith point; and ${W}_{i}^{(c)}$ is the weight for the covariance associated with the ith point.
$${\mathbf{X}}_{(0)}=\widehat{x};{\mathbf{X}}_{(i)}=\widehat{x}+{(\sqrt{(n+\lambda )\mathbf{P}})}_{i}^{T};{\mathbf{X}}_{(i+n)}=\widehat{x}{(\sqrt{(n+\lambda )\mathbf{P}})}_{i}^{T},i=1,\dots ,n$$
$${W}_{0}^{(m)}=\frac{\lambda}{(n+\lambda )};{W}_{0}^{(c)}={W}_{0}^{(m)}+(1{\alpha}^{2}+\beta );{W}_{i}^{(m)}={W}_{i}^{(c)}=\frac{1}{2(n+\lambda )},i=1,\dots ,2n$$
The sigma vectors are propagated through the nonlinear function to yield a set of transformed sigma points,
$${\mathbf{y}}_{i}=f({\mathbf{X}}_{i}),i=0,\dots ,2n$$
The mean and covariance of ${\mathbf{y}}_{i}$ are approximated by a weighted average mean and covariance of the transformed sigma points as follows:
$${\overline{y}}_{u}={\displaystyle \sum _{i=0}^{2n}{W}_{i}^{(m)}}{\mathbf{y}}_{i}$$
$${\mathbf{P}}_{u}={\displaystyle \sum _{i=0}^{2n}{W}_{i}^{(c)}}({\mathbf{y}}_{i}{\overline{y}}_{u}){({\mathbf{y}}_{i}{\overline{y}}_{u})}^{T}$$
As compared to the EKF’s linear approximation, the unscented transformation is accurate to the second order for any nonlinear function. Table 1 shows the algorithm for implementation of the UKF, given as in Equations (9)–(18).
2.2. The Cubature Kalman Filter
A nonlinear Bayesian filter called the cubature Kalman filter (CKF) was presented by Arasaratnam and Haykin [9]. Proposed for nonlinear state estimation, the CKF is a Gaussian approximation of a Bayesian filter that provides a more accurate filtering estimate then existing Gaussian filters.
The Bayesian filter solution reduces to computing multidimensional integrals, whose integrands are of the form nonlinear function × Gaussian density. The CKF utilizes the property of the efficient numerical integration method known as sphericalradial cubature rule for those multidimensional integrals [22]. Based on the thirddegree cubature rule, a set of $2n$ points are selected in the CKF to capture the mean and covariance in each update cycle. The cubature rule to approximate an ndimensional Gaussian weighted integral is
where $\mathbf{f}(\mathbf{x})$ is the arbitrary function, ${\mathrm{R}}^{n}$ is domain of integration. ${m}_{\mathrm{x}}$ is the mean of state $\mathbf{x}$. $\sqrt{\mathbf{P}}$ is a square root factor of the covariance matrix $\mathbf{P}$ satisfying the relation $\mathbf{P}=\sqrt{\mathbf{P}}{\sqrt{\mathbf{P}}}^{T}$ and the set of $2n$ cubature point. ${\mathsf{\xi}}_{i}$ is the ith cubature point.
$${\int}_{{\mathrm{R}}^{\mathrm{n}}}\mathbf{f}(\mathbf{x})}{\rm N}(\mathbf{x};{m}_{x},\mathbf{P})d\mathbf{x}\approx \frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}\mathbf{f}({m}_{\mathrm{x}}+\sqrt{\mathbf{P}}{\mathsf{\zeta}}_{i})$$
The CKF algorithm involves the following stages: Firstly, it approximates the mean and variance of the probability distribution through a set of $2n$ cubature points with the same weight, propagates the cubature points through the nonlinear functions, and then calculates the mean and variance of the current approximate Gaussian distribution by the propagated cubature points. A set of $2n$ cubature points is given by $[{\mathsf{\xi}}_{i},{\omega}_{i}]$, where ${\mathsf{\xi}}_{i}$ is the ith cubature point and ${\omega}_{i}$ is the corresponding weight:
where ${\left[1\right]}_{i}\in {\Re}^{n}$ denotes the ith column vector of the identity matrix ${\mathrm{I}}_{n\times n}$.
$${\mathsf{\xi}}_{i}=\{\begin{array}{lll}\sqrt{n}{[1]}_{i},& & i=1,2,\dots ,n\\ \sqrt{n}{[1]}_{in},& & i=n+1,n+2,\dots ,2n\end{array}$$
$${\omega}_{i}=\frac{1}{2n},i=1,2,\dots ,2n$$
Under the assumption that the posterior density at time $k1$ is known, the steps involved in the time and measurement updates of the CKF are derived, given by Equations (22)–(35), summarized in Table 2. The kernel method of the CKF is that the mean and variance of probability distribution can be approximated by cubature points without any linearization of the system model. Thus, the CKF algorithm does not demand to calculate Jacobian matrices so that the truncation errors can be avoided.
Like the UKF, the CKF is another type of nonlinear filtering approach without linearization of nonlinear model. The CKF and UKF are distinguished in several aspects: (1) there has been emphasis on sphericalradial integration rule of the CKF approach which has desirable numerical accuracy criterion than on the efficiency; (2) the approaches to perform the Cholesky factorization on the error covariance matrix as the first step of both the time and measurement updates in each time step; (3) the CKF follows directly from the sphericalradial cubature rule for numerically computing Gaussianweighted integrals whose important property is that it does not entail any free parameters, whereas the UKF introduces a nonzero scaling parameter; and (4) the covariance matrix in the UKF is not always guaranteed to be positive definite, hence, the decomposition of the covariance matrix is unavailable. In view of numerical stability, the use of UT in the design of the UKF may marginally improve its performance at expense of a reduced numerical stability because the estimation error covariance matrix is not always guaranteed to be positive semidefinite. Compared to the CKF, which builds on the numericalintegration perspective of Gaussian filters, employs a thirddegree sphericalradical cubature rule to compute Gaussianweighted integrals numerically and is a relatively derivativefree nonlinear filter with improved performance over the UKF in terms of numerical stability. In addition, they are fundamentally different in sampling point set: for the sigmapoint set, the stem at the center is highly significant as it carries more weight, whereas the cubature point set does not have a stem at the center and thus does not have the numerical instability problem of UKF. To avoid the numerical instability problem, the CKF can effectively improve filtering stability [11,12,13,14].
3. The FLASassisted CKF Strategy
The fuzzy logic adaptive system is introduced to the CKF framework to enhance improvement for vehicular navigation in highdynamic environments. There are many state estimation methods that were found to have practical applications for vehicle positioning and navigation in various dynamic environments, with a pursuit of estimation accuracy and adaptive capability. Although the cubaturebased CKF solution solves the nonlinear approximation issue in a different way from UKF, it still has to meet the requirements of robust estimation and performance stability in high dynamic environments. In the design of CKF, tolerance to the uncertainty factors, including the noise uncertainty and system modeling inaccuracy, is not a high concern, which require the development of robust demand for CKF filtering scheme. The problem in this work is described as a strategy to adaptively adjust the weighting factor in CKF framework and achieve a balance between robustness and estimation performance.
3.1. The Fuzzy Logic Adaptive System (FLAS)
Fuzzy modeling is the method of describing the characteristics of a system using fuzzy rules, which are linguistic IFTHEN statements involving fuzzy sets, fuzzy logic, and fuzzy inference. There are two major types of fuzzy rules exist, namely, Mamdani fuzzy rules and TakagiSugeno (TS) fuzzy rules. The designed FLAS utilizes a fuzzy inference system of TakagiSugeno type, which has special properties since it represents the nonlinear systems in the form of an interpolation between local linear models. A typical fuzzy system consists of three components: fuzzification, fuzzy reasoning (fuzzy inference), and fuzzy defuzzification, as shown in Figure 1. The fuzzification process converts a crisp input value to a fuzzy value, the fuzzy inference is responsible for drawing calculations from the knowledge base, and the fuzzy defuzzification process converts the fuzzy actions into a crisp action.
The TS fuzzy model represents the conclusion by functions. A typical rule in the TS fuzzy model has the form:
 Model Rule $i$: IF Input ${x}_{1}$ is ${F}_{1}^{1}$ and Input ${x}_{2}$ is ${F}_{2}^{1}$ and Input ${x}_{n}$ is ${F}_{n}^{1}$
 THEN Output ${y}_{k}={f}_{k}({x}_{1},{x}_{2},\dots ,{x}_{n})={C}_{k0}+{C}_{k1}{x}_{1}+\cdots +{C}_{kn}{x}_{n}$.
 Model Rule $i$: IF Input ${x}_{1}$ is ${F}_{1}^{1}$ and Input ${x}_{2}$ is ${F}_{2}^{1}$; THEN Output ${y}_{k}={C}_{10}$.
For the firstorder model, the fuzzy rule can be expressed in the form:
 Model Rule $i$: IF Input ${x}_{1}$ is ${F}_{1}^{1}$ and Input ${x}_{2}$ is ${F}_{2}^{1}$
 THEN Output ${y}_{k}={C}_{10}+{C}_{11}{x}_{1}+{C}_{12}{x}_{2}$.
In the FLAS, the weighted average method of defuzzification to find the crisp output. The weighted average defuzzification method can be expressed as:
where the weights ${w}_{k}$ are computed as:
with $\sum _{i=1}^{M}{w}_{i}=1$, and the $\mu $’s represent the membership function.
$$y={\displaystyle \sum _{k=1}^{M}{w}_{k}.{y}_{k}}$$
$${w}_{k}=\frac{{\displaystyle \prod _{i=1}^{n}{\mu}_{{F}_{i}^{k}}({x}_{i})}}{{\displaystyle \sum _{j=1}^{M}\left[{\displaystyle \prod _{i=1}^{n}{\mu}_{{F}_{i}^{j}}({x}_{i})}\right]}}$$
3.2. FLASAssisted CKF for Vehicle Navigation
In processing navigation states using the modelbased filters as discussed in this paper, the time varying parameters are considered uncertainties to exist in the covariance matrices. The FLAS module in the CKF is employed to adapt the filter online. The TS fuzzy model was used to directly estimate the variance and covariance components for the measurements and adapt the CKF. Generally, when the covariance is becoming large and is deviating from zero mean, the filter will be toward to the instability. To avoid filter divergence and improve the robustness, the fuzzy logic system is used to adapt the CKF by selecting the appropriate weighting factor $\epsilon $, which is used to adaptively adjust the process noise covariance based on a degree of divergence (DOD) parameters.
The innovation information is a critical factor for the filter, including the CKF. For the filter to be optimal, the innovation is a zeromean Gaussian white noise. Therefore, the performance of the CKF can be monitored using the value of the innovation information. It is defined as the discrepancy between actual measurements and predicted measurements:
$${\mathsf{\upsilon}}_{i}={\mathbf{z}}_{k}{\widehat{z}}_{kk1}$$
The DOD parameters for identifying the degree of change in vehicle dynamics need to be defined. Examples of possible approaches are given as follows. The innovation information at the present epoch is employed for timely reflect the change in vehicle dynamics. The DOD parameter ${\mu}_{1}$ defined as the averaged magnitude of innovation at the present epoch can be employed for timely reflection of the timevarying vehicle dynamics:
where ${\mathsf{\upsilon}}_{i}={[{\upsilon}_{1}{\upsilon}_{2}\dots {\upsilon}_{m}]}^{T}$, and $m$ is the number of measurements (e.g., number of satellites in the tightlycoupled configuration). Furthermore, the trace of innovation covariance matrix at present epoch divided by the number of measurements employed for navigation processing can also be used:
$${\mu}_{1}=\frac{1}{m}{\displaystyle \sum _{i=1}^{m}\left{\mathsf{\upsilon}}_{i}\right}$$
$${\mu}_{2}=\frac{{\mathsf{\upsilon}}_{i}^{T}{\mathsf{\upsilon}}_{i}}{m}$$
In the FLAS, the DOD parameters are employed as the inputs for the fuzzy inference engines. By monitoring the DOD parameters, the FLAS is able to online tune the weighting factor according to the innovation information. In this work, the fuzzy logic used to perform adaptation involves two parameters, ${\mu}_{1}$ and ${\mu}_{2}$ [21]. For this reason, this scheme can adjust the process noise covariance adaptively and therefore improves estimation performance. The adjustments are performed simply introducing a weighting factor $\epsilon $ as the scaling factor of the process noise covariance, in the following way:
$${\mathbf{Q}}_{k}\to \epsilon \cdot {\mathbf{Q}}_{k}$$
The FLAS is used to identify the appropriate weighting factor so as to keep the innovation sequence toward to the zeromean white sequence. Two DOD parameters based on the innovation are chosen as the input variables for timely reflection of the timevarying vehicle dynamics. The output is the weighting factor for tuning the process noise covariance matrix. Generally, when the covariance is becoming large and is deviating from zero mean, the filter will be toward to the instability. In such cases, the process noise covariance needs to be increased by applying a larger weighting factor for compensating the modeling error. When the innovation mean and covariance are small, the residual between actual and predicted measurements are small as well, meaning that the two measurements match adequately well.
In this paper, the firstorder TS fuzzy model has been employed. The membership functions (MFs) of input fuzzy variables as shown in Figure 2, where the triangle MFs are involved. The presented FLAS has the IF–THEN form and consists of 9 rules:
 IF ${\mu}_{1}$ is zero and ${\mu}_{2}$ is zero THEN $\epsilon $ is ${C}_{10}$
 IF ${\mu}_{1}$ is zero and ${\mu}_{2}$ is small THEN $\epsilon $ is ${C}_{20}$
 IF ${\mu}_{1}$ is zero and ${\mu}_{2}$ is large THEN $\epsilon $ is ${C}_{30}$
 IF ${\mu}_{1}$ is small and ${\mu}_{2}$ is zero THEN $\epsilon $ is ${C}_{40}+{C}_{41}{\mu}_{1}+{C}_{42}{\mu}_{2}$
 IF ${\mu}_{1}$ is small and ${\mu}_{2}$ is small THEN $\epsilon $ is ${C}_{50}+{C}_{51}{\mu}_{1}+{C}_{52}{\mu}_{2}$
 IF ${\mu}_{1}$ is small and ${\mu}_{2}$ is large THEN $\epsilon $ is ${C}_{60}+{C}_{61}{\mu}_{1}+{C}_{62}{\mu}_{2}$
 IF ${\mu}_{1}$ is large and ${\mu}_{2}$ is zero THEN $\epsilon $ is ${C}_{70}+{C}_{71}{\mu}_{1}+{C}_{72}{\mu}_{2}$
 IF ${\mu}_{1}$ is large and ${\mu}_{2}$ is small THEN $\epsilon $ is ${C}_{80}+{C}_{81}{\mu}_{1}+{C}_{82}{\mu}_{2}$
 IF ${\mu}_{1}$ is large and ${\mu}_{2}$ is large THEN $\epsilon $ is ${C}_{90}+{C}_{91}{\mu}_{1}+{C}_{92}{\mu}_{2}$
Figure 3 shows the sensor fusion architecture of the tightlycoupled GPS/INS navigation based on the FLASassisted CKF filtering mechanism. In this research, the error state integrated navigation model with feedback configuration is used. The residual between GPS pseudorange and INS predicted range is used as the measurement of the CKF in the tightlycoupled configuration. For the looselycoupled case, the measurement then is the residual of the position/velocity instead of pseudoranges/pseudorange rates. The block indicated by the dashedline on the righthand side is the fuzzy logic adaptive system (FLAS), which determines the value of weighting factor ε.
4. Results and Discussion
Numerical simulations have been carried out to evaluate the performance of the FACKF approach in comparison with those of EKF, UKF and CKF approaches for the integrated navigation data fusion. The commercial software Satellite Navigation (SATNAV) Toolbox by GPSoft LLC was employed for generating the satellite positions and pseudoranges after the test trajectory has been defined. It is assumed that the differential GPS mode is used and only the multipath and receiver thermal noise are included.
Assume that the differential GPS (DGPS) mode is used and most of the errors can be corrected, but the multipath and receiver thermal noise cannot be eliminated. The measurement noise variances ${r}_{pi}$ are assumed a priori known, which is set as 9 m^{2}. Let each of the whitenoise spectral amplitudes that drive the random walk position states be ${S}_{p}=0.003({\mathrm{m}/\mathrm{s}}^{2})/\text{rad}/\mathrm{s}$. Furthermore, let the clock model spectral amplitudes be ${S}_{f}=0.4({10}^{18})\mathrm{s}$ and ${S}_{g}=1.58({10}^{18}){\mathrm{s}}^{1}$. In this paper, two illustrative examples are given to confirm the effectiveness of the FACKF approach by valuation of the performance for the various approaches including EKF, UKF, CKF, and FACKF. The simulation tests involve the scenarios of twodimensional loose integration and threedimensional tight integration. For both examples, several sets of parameters have been tested and two sets of parameters for the UKF are presented. The UKF parameters are set as $\alpha =2.5$, $\beta =2$, $\kappa =0$.
4.1. Scenario 1 (Example for 2D Land Vehicle Navigation)
For the first test, a simulated vehicle originates from the (0, 0) m location in the ENU coordinate frame. A looselycoupled GPS/INS integration configuration is used for this two dimensional case. The trajectory can be divided mainly into thirteen time intervals (or segments) according the dynamic characteristics, as indicated in Figure 4. Figure 5 shows the yaw angle of the vehicle for twodimensional simulation. The vehicle is simulated to conduct constantvelocity, straightline moving during seven time intervals, 0–300, 501–600, 701–1000, 1101–1400, 1501–1600, 1701–1800 and 1901–2000 s, all at a speed of $10\pi $ m/s. Furthermore, the higher dynamic maneuvering conducted counterclockwise circular and turn motion during 301–500, 601–700, 1001–1100, 1401–1500, 1601–1700 and 1801–1900 s. Table 3 presents description of the vehicle motion for providing better insight into vehicle dynamic information in each time interval. The standard deviations of inertial sensors are $9\times {10}^{4}$ m/s^{2} for the accelerometers and $9\times {10}^{4}$ rad/s for the gyroscope, respectively.
The differential equations describing the twodimensional inertial navigation state, where two accelerometers and one gyroscope are involved as follows:
where $[{a}_{u},{a}_{v}]$ are the measured acceleration in the body frame, and ${\omega}_{r}$ is the measured yaw rate in the body frame. The error model for INS is constructed by the navigation states augmented by the accelerometer biases and gyroscope drift:
which is utilized in the integration navigation filter as the inertial error model. In Equation (43), $\delta n$ and $\delta e$ represent the east and north position errors, respectively; $\delta {v}_{n}$ and $\delta {v}_{e}$ denote the east and north velocity errors, respectively; $\delta \psi $ indicate yaw angle; and $\delta {a}_{u}$, $\delta {a}_{v}$ and $\delta {\omega}_{r}$ are the accelerometer biases and gyroscope drift, respectively. The measurement model is written as
$$\left[\begin{array}{c}\dot{n}\\ \dot{e}\\ {\dot{v}}_{n}\\ {\dot{v}}_{e}\\ \dot{\psi}\end{array}\right]=\left[\begin{array}{c}{v}_{n}\\ {v}_{e}\\ {a}_{n}\\ {a}_{e}\\ {\omega}_{r}\end{array}\right]=\left[\begin{array}{c}{v}_{n}\\ {v}_{e}\\ \mathrm{cos}(\psi ){a}_{u}\mathrm{sin}(\psi ){a}_{v}\\ \mathrm{sin}(\psi ){a}_{u}+\mathrm{cos}(\psi ){a}_{v}\\ {\omega}_{r}\end{array}\right]$$
$$\frac{d}{dt}\left[\begin{array}{c}\delta n\\ \delta e\\ \delta {v}_{n}\\ \delta {v}_{e}\\ \delta \psi \\ \delta {a}_{u}\\ \delta {a}_{v}\\ \delta {\omega}_{r}\end{array}\right]=\left[\begin{array}{cccccccc}0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& {a}_{e}& \mathrm{cos}(\psi )& \mathrm{sin}(\psi )& 0\\ 0& 0& 0& 0& {a}_{n}& \mathrm{sin}(\psi )& \mathrm{cos}(\psi )& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\delta n\\ \delta e\\ \delta {v}_{n}\\ \delta {v}_{e}\\ \delta \psi \\ \delta {a}_{u}\\ \delta {a}_{v}\\ \delta {\omega}_{r}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ {u}_{acc}\\ {u}_{acc}\\ {u}_{gyro}\\ {u}_{acc}^{b}\\ {u}_{acc}^{b}\\ {u}_{gyro}^{b}\end{array}\right]$$
$$\left[\begin{array}{c}\Delta n\\ \Delta e\end{array}\right]=\left[\begin{array}{c}{n}_{INS}\\ {e}_{INS}\end{array}\right]\left[\begin{array}{c}{n}_{GPS}\\ {e}_{GPS}\end{array}\right]=\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\delta n\\ \delta e\\ \delta {v}_{n}\\ \delta {v}_{e}\\ \delta \psi \\ \delta {a}_{u}\\ \delta {a}_{v}\\ \delta {\omega}_{r}\end{array}\right]+\left[\begin{array}{c}{v}_{n}\\ {v}_{e}\end{array}\right]$$
Figure 6 shows the position errors based on the three filters: EKF, UKF, and CKF. The results for EKF versus UKF are presented by the two plots shown in the left column while those for UKF versus CKF in the right column. Figure 7 provides the comparison of position errors for the UKF, CKF and FACKF. In additions, the estimation performance for the Euler angles among various approaches is presented. The results for the yaw angle errors based on the EKF, UKF, and CKF are shown in Figure 8 and Figure 9. In Figure 8, the result for EKF versus UKF is shown in the left plot while that for UKF versus CKF in the right plot. Comparison of yaw angle errors for UKF, CKF, and FACKF is presented in Figure 9. The FACKF has demonstrated performance improvement capability when compared to the CKF and UKF, due to better treatment on nonlinearity caused by vehicle maneuvers. The FLAS is adopted to online determine the weighting factor in the FACKF, hence prevents the divergence problem, and results in improved navigation accuracy. Table 4 provides the summary of root mean square (RMS) errors and the time consumption for all the four approaches: EKF, UKF, CKF and FACKF. It can be seen that the incorporation of the FLAS mechanism can remarkably improve the estimation accuracy of the navigation states. Among the four approaches, the FLASassisted CKF strategy demonstrates superior navigation accuracy performance as compared to the other three approaches.
4.2. Scenario 2 (Example for 3D Navigation Environment)
The second example for the threedimensional navigation case is presented for further confirmation of the robustness and effectiveness of the proposed method. The data for INS error specifications are taken from Crista IMU specifications [23], as shown in Table 5. The GPS data were generated at 1 Hz and the IMU has a data rate of 10 Hz.
The threedimensional vehicle trajectory for Scenario 2 is shown in Figure 10. Table 6 presents description of the threedimensional vehicle motion. Figure 11 shows the Euler angles for the case of threedimensional simulation. The trajectory for this example can be mainly divided into nine time intervals according to the vehicle dynamic characteristics. The vehicle was simulated to conduct constant acceleration and level flight from 0 to 250 s, counterclockwise turns from 501 to 2310 s and clockwise circular motion from 2821–4630 s. In the three time intervals, highly dynamic maneuvering is involved. The constantvelocity straightline flight is involved in all the other segments, where the low dynamic motion is considered.
The INS equations describing the threedimensional inertial navigation state are:
$${\dot{V}}_{e}^{n}={\mathbf{C}}_{b}^{n}{\mathbf{f}}^{n}[\mathbf{\Omega}({\mathsf{\omega}}_{en}^{n})+2\mathbf{\Omega}({\mathsf{\omega}}_{ie}^{n})]{\mathbf{V}}_{e}^{n}+{\mathbf{g}}_{l}^{n}$$
The error model employed for INS is a terrestrial INS psi angle error model [24]:
where $\delta \mathbf{R}$ is the position error vector, $\delta \mathbf{V}$ is the velocity error vector, $\mathsf{\psi}$ is the attitude error vector, ${\mathsf{\omega}}_{en}$ refers to the rotation rate of the local geographic frame relative to the earth frame, ${\mathsf{\omega}}_{ie}$ refers to the earth rate vector, ${\mathsf{\omega}}_{in}$ refers to the rotation rate of the local geographic frame with respect to the inertial frame, $\mathbf{f}$ is the specific force vector, ${\mathsf{\epsilon}}_{a}$ is the accelerometer error vector and ${\mathsf{\epsilon}}_{g}$ is the gyro drift rate vector. The state vector include 17 states: three inertial error states each in position, velocity, attitude, accelerometer bias, and gyro bias, and one state each for receiver clock bias and drift:
$$\delta \dot{R}={\mathsf{\omega}}_{en}\times \delta \mathbf{R}+\delta \mathbf{V}$$
$$\delta \dot{V}=({\mathsf{\omega}}_{ie}+{\mathsf{\omega}}_{in})\times \delta \mathbf{V}\mathsf{\psi}\times \mathbf{f}+{\mathsf{\epsilon}}_{a}$$
$$\dot{\mathsf{\psi}}={\mathsf{\omega}}_{in}\times \mathsf{\psi}+{\mathsf{\epsilon}}_{g}$$
$${\mathbf{x}}_{k}={[x,y,z,\dot{x},\dot{y},\dot{z},{\psi}_{x},{\psi}_{y},{\psi}_{z},{a}_{x},{a}_{y},{a}_{z},{g}_{x},{g}_{y},{g}_{z},b,d]}^{\mathrm{T}}$$
A tightlycoupled GPS/INS integration configuration is used. The states and the measurements are related nonlinearly. The nonlinear pseudorange equation can be linearized by expanding Taylor’s series around the approximate (or nominal) user position and neglecting the higher terms. If only the pseudorange observables are available, the linearized measurement equation based on $n$ observables can be written as given by:
where ${\mathbf{x}}_{k}$ is shown as in Equation (49) and ${v}_{\rho}$ is the measurement noise.
$$\left[\begin{array}{c}{\rho}_{1}\\ {\rho}_{2}\\ \vdots \\ {\rho}_{n}\end{array}\right]=\left[\begin{array}{c}{\widehat{\rho}}_{1}\\ {\widehat{\rho}}_{2}\\ \vdots \\ {\widehat{\rho}}_{n}\end{array}\right]+\left[\begin{array}{cccccc}{h}_{x}^{(1)}& {h}_{y}^{(1)}& {h}_{z}^{(1)}& \cdots & 1& 0\\ {h}_{x}^{(2)}& {h}_{y}^{(2)}& {h}_{z}^{(2)}& \cdots & 1& 0\\ \vdots & \vdots & \vdots & \cdots & \vdots & \vdots \\ {h}_{x}^{(n)}& {h}_{y}^{(n)}& {h}_{z}^{(n)}& \cdots & 1& 0\end{array}\right]{\mathbf{x}}_{k}+\left[\begin{array}{c}{v}_{{\rho}_{1}}\\ {v}_{{\rho}_{2}}\\ \vdots \\ {v}_{{\rho}_{n}}\end{array}\right]$$
The effectiveness of the proposed method for Scenario 2 is given by the results shown from Figure 12, Figure 13, Figure 14 and Figure 15. Figure 12 provides the position errors based on the three nonlinear filter approaches: UKF, CKF and FACKF. It can be seen that the state estimate of the conventional CKF has deviated from the true state in high dynamic segments, while the FACKF provides remarkable improvement. Comparison of Euler angle estimation results for UKF versus CKF is shown in Figure 13; and comparison of Euler angle estimation results for CKF versus FACKF is shown in Figure 14. The estimation results of Euler angles based on the UKF are unreliable, while the CKF is able to provide acceptable results. When using the FACKF, the results are further improved. The reason is that the FACKF involves the use of appropriate weighting factor online whereas the other three approaches do not offer this flexibility and depend mainly on the fixed parameters of process noise covariance matrix based on the prior knowledge. Figure 15 presents the comparison of Euler angle errors for UKF, CKF, and FACKF, in which the results for the three approaches are shown as in the three plots on the left column and, for better readability, CKF versus FACKF on the right column. Table 7 provides the summary of RMS errors and the time consumption for the 3D navigation case. Comparison of RMS errors is illustrated in Figure 11, which demonstrates that FACKF outperforms significantly the other approaches: EKF, UKF, and CKF.
Based on the results, several important remarks are given as follows:
 (1)
 During the time intervals the vehicle is conducting maneuvering, i.e., circular motion, turn motion, constant acceleration, or variable acceleration, the model mismatch to the actual situation leads to increased errors, as can be seen from the solution obtained by the conventional EKF. The CKF is about to converge in the high dynamic regions where there still exist noticeably large errors for the UKFbased solutions.
 (2)
 Since the use of fixed value of covariance matrix cannot reflect the realistic vehicle dynamics, the performance of the nonlinear filters degrades due to such uncertainties on parameter values. To improve the performance of the CKF, a robust technique that the FLAS mechanism is incorporated for dynamically tuning the process noise covariance in the CKF framework. Remarkable improvement in estimation accuracy is obtained using the FACKF algorithm.
 (3)
 For the threedimensional case, the estimation results of Euler angles based on the UKF are unreliable, while the CKF is able to provide acceptable results. When using the FACKF, the results are further improved.
 (4)
 The results from the two illustrative examples demonstrate that, by monitoring the innovation information based on DOD parameters, the FACKF possesses superior capability to detect the change in vehicle dynamics and adjust the scaling factor so as to prevent the divergence and remain better navigation accuracy. For the segments with sharp turns or abrupt maneuvers involved, the performance improvement becomes obvious.
 (5)
 The results show that through tuning of the process noise covariance, the FLAS helps to resolve the problem on noise uncertainty in the CKF for fusing the integrated navigation system data. Therefore, when a designer does not have sufficient information to develop the precise model, the proposed approach provides a useful alternative for designing the GPS/INS integration.
5. Conclusions
This paper has presented a sensor fusion method for the GPS/INS integrated navigation systems. The proposed approach is based on the combination of cubature Kalman filter (CKF) to treat the system nonlinearity, and fuzzy logic adaptive system (FLAS) to tune the covariance matrix of the process noise during the vehicle maneuvering.
The proposed FLASassisted CKF enhances the robustness of CKF with better treatment to system models including statistical uncertainties (mainly for adjusting the covariance matrix of the process noise) through the FLAS. If the nonlinear filter does not perform satisfactorily well, the FLAS would provide an appropriate weighting factor to improve the navigation accuracy of the CKF. The FLAS adaptive mechanism provides adequate tuning of the weighting factor, which enables the system to achieve a balance between estimation accuracy and robustness. Based on the TS fuzzy model, the proposed FACKF scheme timely performs effective detection of vehicle maneuvering motion and exhibits robustness against the divergence problem. The FACKF method is designed to overcome the possible degradation problems caused by modeling errors on noise uncertainty, so as to improve the navigation accuracy in highly dynamic segments without sacrificing precision in the other regions.
To validate the effectiveness of the proposed method, two illustrative examples for the GPS/INS integration have been presented, one for the twodimensional land vehicle navigation using the looselycoupled configuration; the other for the threedimensional navigation using the tightlycoupled configuration. Evaluation of navigation performance among various nonlinear filters, including EKF, UKF, CKF, and FACKF, has been presented. As an enhanced version of CKF, the FACKF possesses superior performance improvement in navigational accuracy and reveals very good potential as an alternative navigation state estimator for the GPS/INS navigation design.
Acknowledgments
This work has been supported in part by the Ministry of Science and Technology, Taiwan, under grant numbers NSC 1012221E019027MY3 and MOST 1042221E019026MY2.
Author Contributions
All authors were involved in the study presented in this manuscript. DahJing Jwo determined the research framework, proposed the main idea and performed mathematical modeling. He also contributed to revising and proofreading of the article. ChienHao Tseng performed implementation of algorithm and simulations, and wrote the original manuscript of the paper. ShengFuu Lin reviewed the manuscript and provided vital comments, interpreted the results and provided scientific advising to this study.
Conflicts of Interest
The authors declare no conflict of interest.
References
 Farrell, J.; Barth, M. The Global Positioning System and Inertial Navigation; McCrawHill Professional: New York, NY, USA, 1999. [Google Scholar]
 Brown, R.G.; Hwang, P.Y.C. Introduction to Random Signals and Applied Kalman Filtering; John Wiley & Sons: New York, NY, USA, 1997. [Google Scholar]
 Gelb, A. Applied Optimal Estimation; M.I.T. Press: Cambridge, MA, USA, 1974. [Google Scholar]
 Psiaki, M.L. Backward smoothing extended Kalman Filter. J. Guid. Control Dyn. 2005, 28, 885–894. [Google Scholar]
 Julier, S.J.; Uhlmann, J.K.; Durrantwhyte, H.F. A new method for the nonlinear transformation of means and covariances in filters and estimators. IEEE Trans. Autom. Control 2000, 5, 477–482. [Google Scholar]
 Julier, S.J. The scaled unscented transformation. In Proceedings of the American Control Conference, Anchorage, AK, USA, 8–10 May 2002; pp. 4555–4559.
 Julier, S.J.; Uhlmann, J.K.; Durrantwhyte, H.F. A new approach for filtering nonlinear system. In Proceedings of the American Control Conference, Seattle, WA, USA, 21–23 June 1995; pp. 1628–1632.
 Crassidis, J.L. SigmaPoint Kalman Filtering for integrated GPS and inertial navigation. IEEE Trans. Aerosp. Electron. Syst. 2006, 42, 750–756. [Google Scholar]
 Van der Merwe, R.; Wan, E.A. SigmaPoint Kalman Filters for Nonlinear Estimation and SensorFusion: Applications to Integrated Navigation. In Proceedings of the AIAA Guidance, Navigation & Control Conference, Providence, RI, USA, 16–19 August 2004; Volume 10, pp. 16–19.
 Arasaratnam, I.; Haykin, S.; Hurd, T.R. Cubature Kalman Filtering for ContinuousDiscrete Systems: Theory and Simulations. IEEE Trans. Signal Process. 2010, 58, 4977–4993. [Google Scholar]
 Arasaratnam, I.; Haykin, S. Cubature Kalman filters. IEEE Trans. Autom. Control 2009, 54, 1254–1269. [Google Scholar]
 Jia, B.; Xin, M.; Cheng, Y. Highdegree cubature Kalman filter. Automatica 2013, 49, 510–518. [Google Scholar]
 Duan, J.; Shi, H.; Liu, D.; Yu, H. Square root cubature Kalman filterKalman filter algorithm for intelligent vehicle position estimate. Proc. Eng. 2016, 137, 267–276. [Google Scholar]
 Chang, L.; Hu, B.; Li, A.; Qin, F. Transformed unscented Kalman filter. IEEE Trans. Autom. Control 2013, 58, 252–257. [Google Scholar]
 FernandezPrades, C.; VilaValls, J. Bayesian nonlinear filtering using quadrature and cubature rules applied to sensor data fusion for positioning. In Proceedings of the IEEE International Conference on Communications, Cape Town, South Africa, 23–27 May 2010; pp. 1–5.
 Liu, M.; Lai, J.; Li, Z.; Liu, J. An adaptive cubature Kalman filter algorithm for inertial and landbased navigation system. Aerosp. Sci. Technol. 2016, 51, 52–60. [Google Scholar]
 Tseng, C.H.; Chang, C.W.; Jwo, D.J. Fuzzy adaptive interacting multiple model nonlinear filter for integrated navigation sensor fusion. Sensors 2011, 11, 2090–2111. [Google Scholar] [PubMed]
 Jwo, D.J.; Lai, S.Y. Navigation integration using the fuzzy strong tracking unscented Kalman filter. J. Navig. 2009, 62, 303–322. [Google Scholar] [CrossRef]
 Sasiadek, J.Z.; Wang, Q.; Zeremba, M.B. Fuzzy Adaptive Kalman filtering for INS/GPS data fusion. In Proceedings of the IEEE International Symposium on Intelligent Control, Rio Patras, Greece, 17–19 July 2000; pp. 181–186.
 Takagi, T.; Sugeno, M. Fuzzy identification of systems and its application to modelling and control. IEEE Trans. Syst. Man Cybern. 1985, SMC15, 116–132. [Google Scholar] [CrossRef]
 Jwo, D.J.; Wang, S.H. Adaptive fuzzy strong tracking extended Kalman filter for GPS navigation. IEEE Sens. J. 2007, 7, 778–789. [Google Scholar] [CrossRef]
 Zhang, X.C.; Teng, Y.L. A New Derivation of the Cubature Kalman Filters. Asian J. Control 2014, 16, 1501–1510. [Google Scholar]
 Cloud Cap Technology: Crista IMU Specifications 2011. Available online: http://www.cloudcaptech.com/ (accessed on 26 June 2015).
 BarItzhack, I.Y.; Berman, N. Control Theoretic Approach to Inertial Navigation Systems. AIAA J. Guid. Control Dyn. 1988, 11, 237–245. [Google Scholar]
Figure 3.
Architecture of the tightlycoupled GPS/INS (Global Positioning System/inertial navigation system) integrated navigation using the FACKF (fuzzy adaptive cubature Kalman filter)feedback configuration.
Figure 6.
Position errors for EKF, UKF, and CKF—Scenario 1: (a,c) EKF versus UKF; and (b,d) UKF versus CKF.
Figure 7.
Comparison of position errors based on the UKF, CKF, and FACKF—Scenario 1. (a) East position error; (b) North position error.
Figure 8.
Comparison of yaw angle errors for EKF, UKF, and CKF—Scenario 1: (a) EKF versus UKF; and (b) UKF versus CKF.
Figure 11.
The Euler angles for the case of threedimensional simulation. (a) Roll angle; (b) Pitch angle; (c) Yaw angle.
Figure 12.
Position errors for UKF, CKF, and FACKF—Scenario 2. (a) East position error; (b) North position error; (c) Altitude error.
Figure 13.
Comparison of Euler angle estimation results: UKF (in green) versus CKF (in blue)—Scenario 2. (a) Roll angle; (b) Pitch angle; (c) Yaw angle.
Figure 14.
Comparison of Euler angle estimation results: CKF (in green) versus FACKF (in blue)—Scenario 2. (a) Roll angle; (b) Pitch angle; (c) Yaw angle.
Figure 15.
Comparison of Euler angle errors for UKF, CKF, and FACKF—Scenario 2: (a,c,e) UKF, CKF and FACKF; and (b,d,f) CKF versus FACKF.
 Initialization: Initialize state vector ${\widehat{x}}_{00}$ and state covariance matrix ${\mathbf{P}}_{00}$ 
 Time update 
(1) The transformed set is given by instantiating each point through the process model 
$${\mathsf{\zeta}}_{i,kk1}=\mathbf{f}({\mathbf{X}}_{i,k1}),i=0,\dots ,2n$$

(2) Predicted mean 
$${\widehat{x}}_{kk1}={\displaystyle \sum _{i=0}^{2n}{W}_{i}^{(m)}{\mathsf{\zeta}}_{i,kk1}}$$

(3) Predicted covariance 
$${\mathbf{P}}_{kk1}={\displaystyle \sum _{i=0}^{2n}{W}_{i}^{(c)}[{\mathsf{\zeta}}_{i,kk1}{\widehat{x}}_{kk1}]{[{\mathsf{\zeta}}_{i,kk1}{\widehat{x}}_{kk1}]}^{T}}+{\mathbf{Q}}_{k1}$$

(4) Instantiate each of the prediction points through observation model 
$${\mathbf{Z}}_{i,kk1}=\mathbf{h}({\mathsf{\zeta}}_{i,kk1})$$

(5) Predicted observation 
$${\widehat{z}}_{kk1}={\displaystyle \sum _{i=0}^{2n}{W}_{i}^{\left(m\right)}{\mathbf{Z}}_{i,kk1}}$$

Measurement update 
(6) Innovation covariance 
$${\mathbf{P}}_{\text{zz}}={\displaystyle \sum _{i=0}^{2n}{W}_{i}^{(c)}[{\mathbf{Z}}_{i,kk1}{\widehat{z}}_{kk1}]{[{\mathbf{Z}}_{i,kk1}{\widehat{z}}_{kk1}]}^{T}+{\mathbf{R}}_{k}}$$

(7) Cross covariance 
$${\mathbf{P}}_{\mathrm{x}\mathbf{z}}={\displaystyle \sum _{i=0}^{2n}{W}_{i}^{(c)}[{\mathsf{\zeta}}_{i,kk1}{\widehat{x}}_{kk1}]{[{\mathbf{Z}}_{i,kk1}{\widehat{z}}_{kk1}]}^{T}}$$

(8) Performing update 
$${\mathbf{K}}_{k}={\mathbf{P}}_{\text{xz}}{\mathbf{P}}_{\text{zz}}^{\mathbf{1}}$$

$${\widehat{x}}_{kk}={\widehat{x}}_{kk1}+{\mathbf{K}}_{k}({\mathbf{z}}_{k}{\widehat{z}}_{kk1})$$

$${\mathbf{P}}_{kk}={\mathbf{P}}_{kk1}{\mathbf{K}}_{k}{\mathbf{P}}_{\text{zz}}{\mathbf{K}}_{k}^{T}$$

 Initialization: Initialize state vector ${\widehat{x}}_{00}$ and state covariance matrix ${\mathbf{P}}_{00}$ 
 Time update 
(1) Factorize the covariance 
$${\mathbf{P}}_{k1k1}={\mathbf{S}}_{k1k1}{\mathbf{S}}_{k1k1}^{T}$$

(2) Evaluate the cubature points 
$${\mathbf{X}}_{i,k1k1}={\mathbf{S}}_{k1k1}{\mathsf{\xi}}_{i}+{\widehat{x}}_{k1k1}$$

(3) Evaluate the propagated cubature points through the process model 
$${\mathbf{X}}_{i,kk1}^{*}=\mathbf{f}({\mathbf{X}}_{i,k1k1})$$

(4) Estimate the predicted mean 
$${\widehat{x}}_{kk1}={\displaystyle \sum _{i=1}^{2n}{\omega}_{i}}{\mathbf{X}}_{i,kk1}^{*}$$

(5) Estimate the predicted error covariance 
$${\mathbf{P}}_{kk1}={\displaystyle \sum _{i=1}^{2n}{\omega}_{i}{\mathbf{X}}_{i,kk1}^{*}{\mathbf{X}}_{i,kk1}^{{*}^{T}}}{\widehat{x}}_{kk1}{\widehat{x}}_{kk1}^{T}+{\mathbf{Q}}_{k1}$$

 Measurement update 
(6) Factorize the covariance 
$${\mathbf{P}}_{kk1}={\mathbf{S}}_{kk1}{\mathbf{S}}_{kk1}^{T}$$

(7) Evaluate the cubature points 
$${\mathbf{X}}_{i,kk1}={\mathbf{S}}_{kk1}{\mathsf{\xi}}_{i}+{\widehat{x}}_{kk1}$$

(8) Evaluate the propagated cubature points through observation model 
$${\mathbf{Z}}_{i,kk1}=\mathbf{h}({\mathbf{X}}_{i,kk1})$$

(9) Evaluate the propagated observation 
$${\widehat{z}}_{kk1}={\displaystyle \sum _{i=1}^{2n}{\omega}_{i}}{\mathbf{Z}}_{i,kk1}$$

(10) Estimate the innovation covariance 
$${\mathbf{P}}_{zz}={\displaystyle \sum _{i=1}^{2n}{\omega}_{i}{\mathbf{Z}}_{i,kk1}{\mathbf{Z}}_{i,kk1}^{T}}{\widehat{z}}_{kk1}{\widehat{z}}_{kk1}^{T}+{\mathbf{R}}_{k}$$

(11) Estimate the crosscovariance 
$${\mathbf{P}}_{xz}={\displaystyle \sum _{i=1}^{2n}{\omega}_{i}{\mathbf{X}}_{i,kk1}{\mathbf{Z}}_{i,kk1}^{T}}{\widehat{x}}_{kk1}{\widehat{z}}_{kk1}^{T}$$

(12) Perform update state vector ${\widehat{x}}_{kk}$ and its covariance matrix ${\mathbf{P}}_{kk}$ 
$${\mathbf{K}}_{k}={\mathbf{P}}_{xz}{\mathbf{P}}_{zz}^{1}$$

$${\widehat{x}}_{kk}={\widehat{x}}_{kk1}+{\mathbf{K}}_{k}({\mathbf{z}}_{k}{\widehat{z}}_{kk1})$$

$${\mathbf{P}}_{kk}={\mathbf{P}}_{kk1}{\mathbf{K}}_{k}{\mathbf{P}}_{zz}{\mathbf{K}}_{k}^{T}$$

Segment Number  Time Interval (s)  Motion 

1  (0–300)  Constant velocity straight line 
2  (301–500)  Counterclockwise circular motion 
3  (501–600)  Constant velocity straight line 
4  (601–700)  Counterclockwise turn 
5  (701–1000)  Constant velocity straight line 
6  (1001–1100)  Counterclockwise turn 
7  (1101–1400)  Constant velocity straight line 
8  (1401–1500)  Counterclockwise turn 
9  (1501–1600)  Constant velocity straight line 
10  (1601–1700)  Clockwise turn 
11  (1701–1800)  Constant velocity straight line 
12  (1801–1900)  Clockwise turn 
13  (1901–2000)  Constant velocity straight line 
RMS Errors (in Units of m for Positions and Rad for Angles, Respectively)  Time Consumption (s)  

East  North  Yaw  
EKF  10.2400  8.8917  0.0511  6.144 
UKF  2.3966  3.1636  0.0236  12.702 
CKF  1.4524  1.6161  0.0165  11.872 
FACKF  0.3634  0.2312  0.0096  13.162 
Table 5.
INS error specifications (from Crista IMU Specifications [23]).
Gyros  Accelerometers  

Range  ±300°/s  ±10 G 
Scale Factor Error  <1% (@ 25 °C)  <1% (@ 25 °C) 
(i.e., < 3°/s)  (i.e., < 100 mG or 0.98 m/s^{2})  
InRun Bias Error  
Fixed temperature  <0.2°/s (warmed up)  <25 mG (0.245 m/s^{2}) 
Over temperature  <0.6°/s  <51 mG (0.500 m/s^{2}) 
Noise (1σ, no oversamples)  <± 0.7°/s  <±12 mG (0.120 m/s^{2}) 
Segment Number  Time Interval (s)  Motion 

1  (0–250)  Constant acceleration level flight 
2  (251–500)  Climbing 
3  (501–2310)  Counterclockwise circular motion 
4  (2311–2820)  Climbing 
5  (2821–4630)  Clockwise circular motion 
6  (4631–4880)  Descending 
7  (4881–5120)  Constant velocity level flight 
8  (5121–5470)  Descending 
9  (5471–5740)  Constant velocity level flight 
RMS Errors (in Units of m for Positions and Rad for Angles, Respectively)  Time Consumption (s)  

East  North  Altitude  Roll  Pitch  Yaw  
EKF  9.0112  7.6160  6.3685  5.1543  9.2497  6.7151  32.287 
UKF  3.7776  4.4502  4.0834  1.1380  1.4199  1.0941  38.418 
CKF  1.7921  1.4048  1.1736  0.0047  0.0027  0.0965  37.742 
FACKF  0.8075  0.5933  0.6698  0.0005  0.0004  0.0346  40.106 
© 2016 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 (CCBY) license (http://creativecommons.org/licenses/by/4.0/).