Next Article in Journal
Autonomous Underwater Navigation and Optical Mapping in Unknown Natural Environments
Next Article in Special Issue
Stochastic Modeling and Analysis of Multiple Nonlinear Accelerated Degradation Processes through Information Fusion
Previous Article in Journal
Recent Progress on Cellulose-Based Electro-Active Paper, Its Hybrid Nanocomposites and Applications
Previous Article in Special Issue
State Estimation for a Class of Non-Uniform Sampling Systems with Missing Measurements

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 third-degree spherical-radial 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 logic

1. 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 stand-alone 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 miss-modeling. In addition, the complete calculation of the Jacobian matrix is cumbersome and time-consuming [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 first-order 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 non-negative 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 sigma-point 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 third-degree spherical-radial 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 spherical-radial 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 spherical-radial cubature rule is composed of two different integrals, spherical and radial integrals. This is based on the spherical-radial 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 UKF-calculated 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 round-off 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 land-based 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 high-dynamic vehicle maneuvers, no matter what kind of filter: the Taylor series-based EKF, the unscented transformation-based UKF, or the spherical-radial transformation-based 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 Takagi-Sugeno (T-S) model [20] is used in the FLAS. The fuzzy inference system (FIS) is constructed for obtaining the suitable weighting factor according to the time-varying 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:
x k + 1 = f ( x k , k ) + w k
z k = h ( x k , k ) + v k
where the state vector is x k n , process noise vector is w k n , measurement vector is z k m , and measurement noise vector is v k m . The vectors w k and v k are zero mean Gaussian white sequences having zero cross-correlation with each other:
E [ w k w i T ] = { Q k , i = k 0 , i k ;   E [ v k v i T ] = { R k , i = k 0 , i k ;   E [ w k v i T ] = 0 f o r   a l l   i   a n d   k
where Q k is the process noise covariance matrix, and R k is the measurement noise covariance matrix.

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 x , having the mean x ^ and covariance P , and suppose that it propagates through an arbitrary nonlinear function f . The unscented transform creates 2 n + 1 sigma vectors X (a capital letter) and weighted points W , given by
X ( 0 ) = x ^ ;   X ( i ) = x ^ + ( ( n + λ ) P ) i T ;   X ( i + n ) = x ^ ( ( n + λ ) P ) i T ,   i = 1 , , n
W 0 ( m ) = λ ( n + λ ) ;   W 0 ( c ) = W 0 ( m ) + ( 1 α 2 + β ) ;   W i ( m ) = W i ( c ) = 1 2 ( n + λ ) ,   i = 1 , , 2 n
where ( ( n + λ ) P ) i is the ith row of the matrix square root. ( n + λ ) P can be obtained from the lower-triangular matrix of the Cholesky factorization; λ = α 2 ( n + κ ) n is a scaling parameter; α determines the spread of the sigma points around x ; κ is a secondly scaling parameter; β is used to incorporate prior knowledge of the distribution of 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.
The sigma vectors are propagated through the nonlinear function to yield a set of transformed sigma points,
y i = f ( X i ) ,   i = 0 , , 2 n
The mean and covariance of y i are approximated by a weighted average mean and covariance of the transformed sigma points as follows:
y ¯ u = i = 0 2 n W i ( m ) y i
P u = i = 0 2 n W i ( c ) ( y i y ¯ u ) ( y i 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 multi-dimensional integrals, whose integrands are of the form nonlinear function × Gaussian density. The CKF utilizes the property of the efficient numerical integration method known as spherical-radial cubature rule for those multi-dimensional integrals [22]. Based on the third-degree cubature rule, a set of 2 n points are selected in the CKF to capture the mean and covariance in each update cycle. The cubature rule to approximate an n-dimensional Gaussian weighted integral is
R n f ( x ) Ν ( x ; m x , P ) d x 1 2 n i = 1 2 n f ( m x + P ζ i )
where f ( x ) is the arbitrary function, R n is domain of integration. m x is the mean of state x . P is a square root factor of the covariance matrix P satisfying the relation P = P P T and the set of 2 n cubature point. ξ i is the ith cubature point.
The CKF algorithm involves the following stages: Firstly, it approximates the mean and variance of the probability distribution through a set of 2 n 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 2 n cubature points is given by [ ξ i , ω i ] , where ξ i is the ith cubature point and ω i is the corresponding weight:
ξ i = { n [ 1 ] i , i = 1 , 2 , , n n [ 1 ] i n , i = n + 1 , n + 2 , , 2 n
ω i = 1 2 n ,   i = 1 , 2 , , 2 n
where [ 1 ] i n denotes the ith column vector of the identity matrix I n × n .
Under the assumption that the posterior density at time k 1 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 spherical-radial 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 spherical-radial cubature rule for numerically computing Gaussian-weighted 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 numerical-integration perspective of Gaussian filters, employs a third-degree spherical-radical cubature rule to compute Gaussian-weighted integrals numerically and is a relatively derivative-free 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 sigma-point 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 FLAS-assisted CKF Strategy

The fuzzy logic adaptive system is introduced to the CKF framework to enhance improvement for vehicular navigation in high-dynamic 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 cubature-based 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 IF-THEN statements involving fuzzy sets, fuzzy logic, and fuzzy inference. There are two major types of fuzzy rules exist, namely, Mamdani fuzzy rules and Takagi-Sugeno (T-S) fuzzy rules. The designed FLAS utilizes a fuzzy inference system of Takagi-Sugeno 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 T-S fuzzy model represents the conclusion by functions. A typical rule in the T-S 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 , , x n ) = C k 0 + C k 1 x 1 + + C k n x n .
where F is a triangle-shaped membership function of the input variable vector, C k i ( i = 0 ~ n ) are constants in the k-th rule. For a zero-order model, the output level is a constant:
  • 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 first-order 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 .
where F 1 1 and F 2 1 are fuzzy sets and C 10 , C 11 and C 12 are constants.
In the FLAS, the weighted average method of defuzzification to find the crisp output. The weighted average defuzzification method can be expressed as:
y = k = 1 M w k . y k
where the weights w k are computed as:
w k = i = 1 n μ F i k ( x i ) j = 1 M [ i = 1 n μ F i j ( x i ) ]
with i = 1 M w i = 1 , and the μ ’s represent the membership function.

3.2. FLAS-Assisted CKF for Vehicle Navigation

In processing navigation states using the model-based 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 on-line. The T-S 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 ε , 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 zero-mean 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:
υ i = z k z ^ k | k 1
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 μ 1 defined as the averaged magnitude of innovation at the present epoch can be employed for timely reflection of the time-varying vehicle dynamics:
μ 1 = 1 m i = 1 m | υ i |
where υ i = [ υ 1 υ 2 υ m ] T , and m is the number of measurements (e.g., number of satellites in the tightly-coupled 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:
μ 2 = υ i T υ 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 on-line tune the weighting factor according to the innovation information. In this work, the fuzzy logic used to perform adaptation involves two parameters, μ 1 and μ 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 ε as the scaling factor of the process noise covariance, in the following way:
Q k ε Q k
The FLAS is used to identify the appropriate weighting factor so as to keep the innovation sequence toward to the zero-mean white sequence. Two DOD parameters based on the innovation are chosen as the input variables for timely reflection of the time-varying 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 first-order T-S 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 μ 1 is zero and μ 2 is zero THEN ε is C 10
  • IF μ 1 is zero and μ 2 is small THEN ε is C 20
  • IF μ 1 is zero and μ 2 is large THEN ε is C 30
  • IF μ 1 is small and μ 2 is zero THEN ε is C 40 + C 41 μ 1 + C 42 μ 2
  • IF μ 1 is small and μ 2 is small THEN ε is C 50 + C 51 μ 1 + C 52 μ 2
  • IF μ 1 is small and μ 2 is large THEN ε is C 60 + C 61 μ 1 + C 62 μ 2
  • IF μ 1 is large and μ 2 is zero THEN ε is C 70 + C 71 μ 1 + C 72 μ 2
  • IF μ 1 is large and μ 2 is small THEN ε is C 80 + C 81 μ 1 + C 82 μ 2
  • IF μ 1 is large and μ 2 is large THEN ε is C 90 + C 91 μ 1 + C 92 μ 2
where C k i ( i = 0 ~ 2 ) are constants in the k-th rule. Parameters C k i can be tuned for different rules based on a value calculated by the combination of the DOD parameters, μ 1 and μ 2 . The parameter values selected in this work are as follows: C 10 = C 20 = C 30 = 5 ; C 40 = C 50 = C 60 = 10 ; C 70 = C 80 = C 90 = 20 ; C 41 = C 51 = C 61 = 5 ; C 71 = C 81 = C 91 = 10 ; C 42 = C 52 = C 62 = C 72 = C 82 = C 92 = 20 .
Figure 3 shows the sensor fusion architecture of the tightly-coupled GPS/INS navigation based on the FLAS-assisted 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 tightly-coupled configuration. For the loosely-coupled case, the measurement then is the residual of the position/velocity instead of pseudoranges/pseudorange rates. The block indicated by the dashed-line on the right-hand 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 p i are assumed a priori known, which is set as 9 m2. Let each of the white-noise spectral amplitudes that drive the random walk position states be S p = 0.003 ( m / s 2 ) / rad / s . Furthermore, let the clock model spectral amplitudes be S f = 0.4 ( 10 18 ) s and S g = 1.58 ( 10 18 ) 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 two-dimensional loose integration and three-dimensional 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 α = 2.5 , β = 2 , κ = 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 loosely-coupled 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 two-dimensional simulation. The vehicle is simulated to conduct constant-velocity, straight-line 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 π m/s. Furthermore, the higher dynamic maneuvering conducted counter-clockwise 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 × 10 4 m/s2 for the accelerometers and 9 × 10 4 rad/s for the gyroscope, respectively.
The differential equations describing the two-dimensional inertial navigation state, where two accelerometers and one gyroscope are involved as follows:
[ n ˙ e ˙ v ˙ n v ˙ e ψ ˙ ] = [ v n v e a n a e ω r ] = [ v n v e cos ( ψ ) a u sin ( ψ ) a v sin ( ψ ) a u + cos ( ψ ) a v ω r ]
where [ a u , a v ] are the measured acceleration in the body frame, and ω 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:
d d t [ δ n δ e δ v n δ v e δ ψ δ a u δ a v δ ω r ] = [ 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 a e cos ( ψ ) sin ( ψ ) 0 0 0 0 0 a n sin ( ψ ) cos ( ψ ) 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 ] [ δ n δ e δ v n δ v e δ ψ δ a u δ a v δ ω r ] + [ 0 0 u a c c u a c c u g y r o u a c c b u a c c b u g y r o b ]
which is utilized in the integration navigation filter as the inertial error model. In Equation (43), δ n and δ e represent the east and north position errors, respectively; δ v n and δ v e denote the east and north velocity errors, respectively; δ ψ indicate yaw angle; and δ a u , δ a v and δ ω r are the accelerometer biases and gyroscope drift, respectively. The measurement model is written as
[ Δ n Δ e ] = [ n I N S e I N S ] [ n G P S e G P S ] = [ 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 ] [ δ n δ e δ v n δ v e δ ψ δ a u δ a v δ ω r ] + [ v n v e ]
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 on-line 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 FLAS-assisted 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 three-dimensional 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 three-dimensional vehicle trajectory for Scenario 2 is shown in Figure 10. Table 6 presents description of the three-dimensional vehicle motion. Figure 11 shows the Euler angles for the case of three-dimensional 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, counter-clockwise 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 constant-velocity straight-line flight is involved in all the other segments, where the low dynamic motion is considered.
The INS equations describing the three-dimensional inertial navigation state are:
V ˙ e n = C b n f n [ Ω ( ω e n n ) + 2 Ω ( ω i e n ) ] V e n + g l n
The error model employed for INS is a terrestrial INS psi angle error model [24]:
δ R ˙ = ω e n × δ R + δ V
δ V ˙ = ( ω i e + ω i n ) × δ V ψ × f + ε a
ψ ˙ = ω i n × ψ + ε g
where δ R is the position error vector, δ V is the velocity error vector, ψ is the attitude error vector, ω e n refers to the rotation rate of the local geographic frame relative to the earth frame, ω i e refers to the earth rate vector, ω i n refers to the rotation rate of the local geographic frame with respect to the inertial frame, f is the specific force vector, ε a is the accelerometer error vector and ε 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:
x k = [ x , y , z , x ˙ , y ˙ , z ˙ , ψ x , ψ y , ψ z , a x , a y , a z , g x , g y , g z , b , d ] T
A tightly-coupled 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:
[ ρ 1 ρ 2 ρ n ] = [ ρ ^ 1 ρ ^ 2 ρ ^ n ] + [ h x ( 1 ) h y ( 1 ) h z ( 1 ) 1 0 h x ( 2 ) h y ( 2 ) h z ( 2 ) 1 0 h x ( n ) h y ( n ) h z ( n ) 1 0 ] x k + [ v ρ 1 v ρ 2 v ρ n ]
where x k is shown as in Equation (49) and v ρ is the measurement noise.
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 UKF-based 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 three-dimensional 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 FLAS-assisted 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 T-S 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 two-dimensional land vehicle navigation using the loosely-coupled configuration; the other for the three-dimensional navigation using the tightly-coupled 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 101-2221-E-019-027-MY3 and MOST 104-2221-E-019-026-MY2.

Author Contributions

All authors were involved in the study presented in this manuscript. Dah-Jing Jwo determined the research framework, proposed the main idea and performed mathematical modeling. He also contributed to revising and proofreading of the article. Chien-Hao Tseng performed implementation of algorithm and simulations, and wrote the original manuscript of the paper. Sheng-Fuu 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

  1. Farrell, J.; Barth, M. The Global Positioning System and Inertial Navigation; McCraw-Hill Professional: New York, NY, USA, 1999. [Google Scholar]
  2. 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]
  3. Gelb, A. Applied Optimal Estimation; M.I.T. Press: Cambridge, MA, USA, 1974. [Google Scholar]
  4. Psiaki, M.L. Backward smoothing extended Kalman Filter. J. Guid. Control Dyn. 2005, 28, 885–894. [Google Scholar]
  5. Julier, S.J.; Uhlmann, J.K.; Durrant-whyte, 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]
  6. Julier, S.J. The scaled unscented transformation. In Proceedings of the American Control Conference, Anchorage, AK, USA, 8–10 May 2002; pp. 4555–4559.
  7. Julier, S.J.; Uhlmann, J.K.; Durrant-whyte, 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.
  8. Crassidis, J.L. Sigma-Point Kalman Filtering for integrated GPS and inertial navigation. IEEE Trans. Aerosp. Electron. Syst. 2006, 42, 750–756. [Google Scholar]
  9. Van der Merwe, R.; Wan, E.A. Sigma-Point Kalman Filters for Nonlinear Estimation and Sensor-Fusion: 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.
  10. Arasaratnam, I.; Haykin, S.; Hurd, T.R. Cubature Kalman Filtering for Continuous-Discrete Systems: Theory and Simulations. IEEE Trans. Signal Process. 2010, 58, 4977–4993. [Google Scholar]
  11. Arasaratnam, I.; Haykin, S. Cubature Kalman filters. IEEE Trans. Autom. Control 2009, 54, 1254–1269. [Google Scholar]
  12. Jia, B.; Xin, M.; Cheng, Y. High-degree cubature Kalman filter. Automatica 2013, 49, 510–518. [Google Scholar]
  13. Duan, J.; Shi, H.; Liu, D.; Yu, H. Square root cubature Kalman filter-Kalman filter algorithm for intelligent vehicle position estimate. Proc. Eng. 2016, 137, 267–276. [Google Scholar]
  14. Chang, L.; Hu, B.; Li, A.; Qin, F. Transformed unscented Kalman filter. IEEE Trans. Autom. Control 2013, 58, 252–257. [Google Scholar]
  15. Fernandez-Prades, C.; Vila-Valls, 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.
  16. Liu, M.; Lai, J.; Li, Z.; Liu, J. An adaptive cubature Kalman filter algorithm for inertial and land-based navigation system. Aerosp. Sci. Technol. 2016, 51, 52–60. [Google Scholar]
  17. 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]
  18. 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]
  19. 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.
  20. Takagi, T.; Sugeno, M. Fuzzy identification of systems and its application to modelling and control. IEEE Trans. Syst. Man Cybern. 1985, SMC-15, 116–132. [Google Scholar] [CrossRef]
  21. 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]
  22. Zhang, X.C.; Teng, Y.L. A New Derivation of the Cubature Kalman Filters. Asian J. Control 2014, 16, 1501–1510. [Google Scholar]
  23. Cloud Cap Technology: Crista IMU Specifications 2011. Available online: http://www.cloudcaptech.com/ (accessed on 26 June 2015).
  24. Bar-Itzhack, I.Y.; Berman, N. Control Theoretic Approach to Inertial Navigation Systems. AIAA J. Guid. Control Dyn. 1988, 11, 237–245. [Google Scholar]
Figure 1. A fuzzy system.
Figure 1. A fuzzy system.
Sensors 16 01167 g001
Figure 2. Membership functions of input fuzzy variables μ 1 (a) and μ 2 (b).
Figure 2. Membership functions of input fuzzy variables μ 1 (a) and μ 2 (b).
Sensors 16 01167 g002
Figure 3. Architecture of the tightly-coupled GPS/INS (Global Positioning System/inertial navigation system) integrated navigation using the FACKF (fuzzy adaptive cubature Kalman filter)-feedback configuration.
Figure 3. Architecture of the tightly-coupled GPS/INS (Global Positioning System/inertial navigation system) integrated navigation using the FACKF (fuzzy adaptive cubature Kalman filter)-feedback configuration.
Sensors 16 01167 g003
Figure 4. Two-dimensional vehicle trajectory for Scenario 1.
Figure 4. Two-dimensional vehicle trajectory for Scenario 1.
Sensors 16 01167 g004
Figure 5. The yaw angle of the vehicle for two-dimensional simulation.
Figure 5. The yaw angle of the vehicle for two-dimensional simulation.
Sensors 16 01167 g005
Figure 6. Position errors for EKF, UKF, and CKF—Scenario 1: (a,c) EKF versus UKF; and (b,d) UKF versus CKF.
Figure 6. Position errors for EKF, UKF, and CKF—Scenario 1: (a,c) EKF versus UKF; and (b,d) UKF versus CKF.
Sensors 16 01167 g006
Figure 7. Comparison of position errors based on the UKF, CKF, and FACKF—Scenario 1. (a) East position error; (b) North position error.
Figure 7. Comparison of position errors based on the UKF, CKF, and FACKF—Scenario 1. (a) East position error; (b) North position error.
Sensors 16 01167 g007
Figure 8. Comparison of yaw angle errors for EKF, UKF, and CKF—Scenario 1: (a) EKF versus UKF; and (b) UKF versus CKF.
Figure 8. Comparison of yaw angle errors for EKF, UKF, and CKF—Scenario 1: (a) EKF versus UKF; and (b) UKF versus CKF.
Sensors 16 01167 g008
Figure 9. Comparison of yaw angle errors for UKF, CKF, and FACKF—Scenario 1.
Figure 9. Comparison of yaw angle errors for UKF, CKF, and FACKF—Scenario 1.
Sensors 16 01167 g009
Figure 10. Three-dimensional vehicle trajectory for Scenario 2.
Figure 10. Three-dimensional vehicle trajectory for Scenario 2.
Sensors 16 01167 g010
Figure 11. The Euler angles for the case of three-dimensional simulation. (a) Roll angle; (b) Pitch angle; (c) Yaw angle.
Figure 11. The Euler angles for the case of three-dimensional simulation. (a) Roll angle; (b) Pitch angle; (c) Yaw angle.
Sensors 16 01167 g011aSensors 16 01167 g011b
Figure 12. Position errors for UKF, CKF, and FACKF—Scenario 2. (a) East position error; (b) North position error; (c) Altitude error.
Figure 12. Position errors for UKF, CKF, and FACKF—Scenario 2. (a) East position error; (b) North position error; (c) Altitude error.
Sensors 16 01167 g012aSensors 16 01167 g012b
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 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.
Sensors 16 01167 g013
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 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.
Sensors 16 01167 g014aSensors 16 01167 g014b
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.
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.
Sensors 16 01167 g015
Table 1. Implementation algorithm for the unscented Kalman filter.
Table 1. Implementation algorithm for the unscented Kalman filter.
- Initialization: Initialize state vector x ^ 0 | 0 and state covariance matrix P 0 | 0
- Time update
(1) The transformed set is given by instantiating each point through the process model
ζ i , k | k 1 = f ( X i , k 1 ) ,   i = 0 , , 2 n
(2) Predicted mean
x ^ k | k 1 = i = 0 2 n W i ( m ) ζ i , k | k 1
(3) Predicted covariance
P k | k 1 = i = 0 2 n W i ( c ) [ ζ i , k | k 1 x ^ k | k 1 ] [ ζ i , k | k 1 x ^ k | k 1 ] T + Q k 1
(4) Instantiate each of the prediction points through observation model
Z i , k | k 1 = h ( ζ i , k | k 1 )
(5) Predicted observation
z ^ k | k 1 = i = 0 2 n W i ( m ) Z i , k | k 1
-Measurement update
(6) Innovation covariance
P zz = i = 0 2 n W i ( c ) [ Z i , k | k 1 z ^ k | k 1 ] [ Z i , k | k 1 z ^ k | k 1 ] T + R k
(7) Cross covariance
P x z = i = 0 2 n W i ( c ) [ ζ i , k | k 1 x ^ k | k 1 ] [ Z i , k | k 1 z ^ k | k 1 ] T
(8) Performing update
K k = P xz P zz 1
x ^ k | k = x ^ k | k 1 + K k ( z k z ^ k | k 1 )
P k | k = P k | k 1 K k P zz K k T
Table 2. Implementation algorithm for the cubature Kalman filter.
Table 2. Implementation algorithm for the cubature Kalman filter.
- Initialization: Initialize state vector x ^ 0 | 0 and state covariance matrix P 0 | 0
- Time update
(1) Factorize the covariance
P k 1 | k 1 = S k 1 | k 1 S k 1 | k 1 T
(2) Evaluate the cubature points
X i , k 1 | k 1 = S k 1 | k 1 ξ i + x ^ k 1 | k 1
(3) Evaluate the propagated cubature points through the process model
X i , k | k 1 * = f ( X i , k 1 | k 1 )
(4) Estimate the predicted mean
x ^ k | k 1 = i = 1 2 n ω i X i , k | k 1 *
(5) Estimate the predicted error covariance
P k | k 1 = i = 1 2 n ω i X i , k | k 1 * X i , k | k 1 * T x ^ k | k 1 x ^ k | k 1 T + Q k 1
- Measurement update
(6) Factorize the covariance
P k | k 1 = S k | k 1 S k | k 1 T
(7) Evaluate the cubature points
X i , k | k 1 = S k | k 1 ξ i + x ^ k | k 1
(8) Evaluate the propagated cubature points through observation model
Z i , k | k 1 = h ( X i , k | k 1 )
(9) Evaluate the propagated observation
z ^ k | k 1 = i = 1 2 n ω i Z i , k | k 1
(10) Estimate the innovation covariance
P z z = i = 1 2 n ω i Z i , k | k 1 Z i , k | k 1 T z ^ k | k 1 z ^ k | k 1 T + R k
(11) Estimate the cross-covariance
P x z = i = 1 2 n ω i X i , k | k 1 Z i , k | k 1 T x ^ k | k 1 z ^ k | k 1 T
(12) Perform update state vector x ^ k | k and its covariance matrix P k | k
K k = P x z P z z 1
x ^ k | k = x ^ k | k 1 + K k ( z k z ^ k | k 1 )
P k | k = P k | k 1 K k P z z K k T
Table 3. Description of the vehicle motion for Scenario 1.
Table 3. Description of the vehicle motion for Scenario 1.
Segment NumberTime Interval (s)Motion
1(0–300)Constant velocity straight line
2(301–500)Counter-clockwise circular motion
3(501–600)Constant velocity straight line
4(601–700)Counter-clockwise turn
5(701–1000)Constant velocity straight line
6(1001–1100)Counter-clockwise turn
7(1101–1400)Constant velocity straight line
8(1401–1500)Counter-clockwise 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
Table 4. Summary of RMS errors and the time consumption for Scenario 1.
Table 4. Summary of RMS errors and the time consumption for Scenario 1.
RMS Errors (in Units of m for Positions and Rad for Angles, Respectively)Time Consumption (s)
EastNorthYaw
EKF10.24008.89170.05116.144
UKF2.39663.16360.023612.702
CKF1.45241.61610.016511.872
FACKF0.36340.23120.009613.162
Table 5. INS error specifications (from Crista IMU Specifications [23]).
Table 5. INS error specifications (from Crista IMU Specifications [23]).
GyrosAccelerometers
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/s2)
In-Run Bias Error
   Fixed temperature<0.2°/s (warmed up)<25 mG (0.245 m/s2)
   Over temperature<0.6°/s<51 mG (0.500 m/s2)
Noise (1σ, no oversamples)<± 0.7°/s<±12 mG (0.120 m/s2)
Table 6. Description of the vehicle motion for Scenario 2.
Table 6. Description of the vehicle motion for Scenario 2.
Segment NumberTime Interval (s)Motion
1(0–250)Constant acceleration level flight
2(251–500)Climbing
3(501–2310)Counter-clockwise 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
Table 7. Summary of RMS errors and the time consumption for Scenario 2.
Table 7. Summary of RMS errors and the time consumption for Scenario 2.
RMS Errors (in Units of m for Positions and Rad for Angles, Respectively)Time Consumption (s)
EastNorthAltitudeRollPitchYaw
EKF9.01127.61606.36855.15439.24976.715132.287
UKF3.77764.45024.08341.13801.41991.094138.418
CKF1.79211.40481.17360.00470.00270.096537.742
FACKF0.80750.59330.66980.00050.00040.034640.106
Back to TopTop