International Journal of Advanced Robotic Systems Nonlinear Filtering with Imm Algorithm for Ultra-tight Gps/ins Integration Regular Paper

This paper conducts a performance evaluation for the ultra‐tight integration of a Global positioning system (GPS) and an inertial navigation system (INS), using nonlinear filtering approaches with an interacting multiple model (IMM) algorithm. An ultra‐tight GPS/INS architecture involves the integration of in‐phase and quadrature components from the correlator of a GPS receiver with INS data. An unscented Kalman filter (UKF), which employs a set of sigma points by deterministic sampling, avoids the error caused by linearization as in an extended Kalman filter (EKF). Based on the filter structural adaptation for describing various dynamic behaviours, the IMM nonlinear filtering provides an alternative for designing the adaptive filter in the ultra‐tight GPS/INS integration. The use of IMM enables tuning of an appropriate value for the process of noise covariance so as to maintain good estimation accuracy and tracking capability. Two examples are provided to illustrate the effectiveness of the design and demonstrate the effective improvement in navigation estimation accuracy. A performance comparison among various filtering methods for ultra‐tight integration of GPS and INS is also presented. The IMM based nonlinear filtering approach demonstrates the effectiveness of the algorithm for improved positioning performance.


Introduction
Global positioning systems (GPS) [1] are capable of providing accurate position information. Unfortunately, the data is prone to jamming or being lost due to the limitations of electromagnetic waves, which form the fundamental of their operation. The inertial navigation system (INS) is a self-contained system that integrates three acceleration components and three angular velocity components. However, the error in position coordinates increases unboundedly as a function of time. Due to the inherent complementary operational characteristics of GPS and INS systems, the synergy of both systems has been widely explored [1,2]. The GPS/INS integration gives an adequate solution to provide a navigation system that has superior performance in comparison with either a GPS or an INS stand-alone system. The design of GPS/INS integrated navigation systems heavily depends on the design of the sensor fusion method.
The Kalman filter (KF) [2,3] is a well-known sequential data assimilation scheme for solving the Wiener problem.
The Kalman filter not only works well in practice, but also is theoretically attractive because it has been shown that the filter that minimizes the variance of the estimation mean square error. While employed as the navigational state estimator, the extended Kalman filter (EKF), the nonlinear version of the KF, has been one of the promising approaches for estimating vehicle state variables and suppressing navigation measurement noise. In designing a navigation filter, there exists some model uncertainty, which cannot be expressed by a linear statespace model. The linear model includes modelling errors since the actual vehicle motions are non-linear processes.
It should be noted that the Kalman filter attempts to minimize the variance of the estimation errors and provides an optimal result only if the system model, the system initial conditions and the noise characteristics can be specified a priori, the perfect a priori knowledge of both the process noise and measurement noise are Gaussian white processes and the noise statistics for both disturbances are completely known. In Kalman filter designs, cases where the theoretical behaviour of a filter and its actual behaviour do not agree may lead to divergence problems. In various circumstances where there are uncertainties in the system model and noise description, and the assumptions on the statistics of disturbances are violated due to the fact that in a number of practical situations, the availability of a precisely known model is unrealistic. It is very often the case that little a priori knowledge is available concerning the manoeuvre. One way to take them into account is to consider a nominal model affected by uncertainty.
As for system nonlinearity, the EKF might suffer from performance degradation and divergence problems due to the linearization process. To address the nonlinearity, other filters such as the unscented Kalman filter (UKF) [4][5][6][7] are becoming popular. Proposed by Julier et al. [4] to address nonlinear state estimation in the context of control theory, the UKF is a nonlinear, distribution approximation method that uses a finite number of carefully chosen sigma points to propagate the probability of state distribution. Unlike the conventional EKF, which achieves first-order accuracy with the linearization process using Jacobian matrices, the UKF employs a minimal set of sigma points using a deterministic sampling approach and at least the second order accuracy of the posterior mean and covariance can be captured. The UKF approximates the Gaussian distribution by a set of deterministically selected samples called sigma points, which are propagated through the true non-linear models to capture the true mean and covariance of the transformed distribution. The UKF makes a Gaussian approximation with a limited number of sigma points through the Unscented Transform (UT) [5,6]. Through the nonlinear dynamics of the system, the true mean and covariance of the Gaussian random variable (GRV) are completely captured with a minimal set of samples. The basic premise behind the UKF is it is easier to approximate a Gaussian distribution than it is to approximate an arbitrary nonlinear function. The posterior mean and covariance can be captured accurately to the second order of Taylor series expansion for any nonlinear system. One of the remarkable merits is that the overall computational complexity of the UKF is the same as that of the EKF.
An adaptive mechanism that dynamically identifies uncertainties or modelling errors can be adopted for the various circumstances where there are uncertainties in the system model and noise description and the assumptions of the statistics of disturbances are violated due to the fact that in a number of practical situations, the availability of a precisely known model is unrealistic. For example, when in the modelling step, some phenomena are disregarded. The suboptimal configuration is typically based on a simplified error state dynamic/measurement model. One way to take this into account is to consider a nominal model affected by uncertainty. To fulfil the requirement of achieving filter optimality or to prevent the divergence problems of the Kalman filter, the so-called adaptive Kalman filter (AKF) approach [8][9][10] has been a promising strategy and has been widely explored for dynamically adjusting the parameters of the supposedly optimum filter, based on the estimates of the unknown parameters for on-line estimation of motion, as well as the available signal and noise statistics data. Many efforts have been made to improve the estimation of the covariance matrices based on the innovation-based estimation approach. The two major approaches that have been proposed for AKF are the multiple model adaptive estimate (MMAE) and the innovation adaptive estimation (IAE).
The AKF approach is based on parametric adaptation, while the interacting multiple model (IMM) approach [11] is based on filter structural adaptation (model switching). The IMM algorithm has a configuration that runs several model-matched state estimation filters in parallel, which exchange information (interact) at each sampling time. In the IMM approach, multiple models are developed. In each model a filter runs and the IMM algorithm makes uses of model probabilities to weight the inputs and output of a bank of parallel filters at each time instant. Based on the filter structural adaptation (model switching) for describing various dynamic behaviours, the IMM algorithm was originally applied to target tracking [11,12] and was only recently extended to GPS navigation [13,14] and the integration of GPS and INS [15][16][17][18][19][20][21]. The IMM algorithm based on the augmented Kalman filter (AUKF) has been proposed to increase the accuracy of IMM algorithm in the presence of low and medium manoeuvres [19]. The AUKF considers the acceleration as an additive state term in the state space equation and estimates the acceleration and the original states simultaneously. Jo, Chu and Sunwoo [20] presented a positioning algorithm based on an IMM filter that integrates GPS and in-vehicle sensors to adapt the vehicle model to various driving conditions. In work by Deepalakshmi and Malleswaran [21], the output of a Kalman filter using a Rauch-Tung-Striebel smoother and the output of a Kalman filter using a two-filter smoothing algorithm are given to the IMM algorithm, to improve the performance of the navigation solution of the land vehicles. A model probability evaluator calculates the current probability of the vehicle being in each of the possible modes. A global estimate of the vehicle's state is computed using the latest mode probabilities. This algorithm carries out soft switching between the various modes by adjusting the probabilities of each mode, which are used as weightings in the combined global state estimate. The covariance matrix associated with this combined estimate takes into account the covariances of the mode-conditioned estimates, as well as the differences between these estimates.
In traditional GPS/INS integration designs, the loosely coupled integration uses the GPS derived position and the velocity as the measurements. This architecture is sub-optimal from the standpoint of preventing GPS outages (i.e., with less than four available satellites). The tightly coupled GPS/INS navigation filter blends the GPS pseudorange and inertial measurements and obtains the vehicle navigation solution. The ultra-tightly coupled (UTC) architecture combines the I (in-phase) and Q (quadrature) accumulator outputs in the receiver signal tracking loops and the INS navigation filter function into a single Kalman filter [19][20][21][22][23][24][25]. The other potential alternatives for GPS/INS integration include the unscented Kalman filter (UKF) [26,27].
The remainder of this paper is organized as follows. In Section 2, the preliminary background on ultra-tightly coupled GPS/INS integration is reviewed. Section 3 introduces the interacting multiple model unscented Kalman filter. In Section 4, two illustrative examples are provided for performance assessment for various filtering approaches. Conclusions are given in Section 5.

The Ultra-Tightly Coupled GPS/INS Integration
The ultra-tightly coupled GPS/INS integration architecture is shown in Figure 1. In the UTC mode, the I and Q accumulator outputs from the GPS correlators form the observational measurements of the filters. These The received satellite signal can be presented as: where A is the signal amplitude, CA is the C/A code sequence, D(t) is the navigation data,  is the propagation delay between the satellite and receiver, 0 f is the carrier frequency, 0  is the initial carrier phase and  is the Gaussian noise. The signals from the receiver correlator, in-phase (I) and quadrature (Q), form the measurements of the filters: After simplification, we have: where I  and Q  represent the noise components for the I and Q channels, respectively, e2 (f f) [sin( (k 1)T ) sin( kT )] 2 Equation (4) shows that E[I] and E[Q] depend on the errors of the carrier frequency and phase. These errors can be described in terms of the position and velocity: where u R and u v are the measured position and velocity of the receiver, û R and û v are the receiver estimates of position and velocity and e v represent the errors of the position and velocity, given by: The INS equations describing the three-dimensional inertial navigation state are: n n n n n n n e b en ie e l [ ( The error model employed for INS is a terrestrial INS psiangle error model [22]: where  r is the position error vector,  v is the velocity error vector, ψ is the attitude error vector, en ω is the true rate of the local geographic frame with respect to the Earth frame, ie ω is the earth rate vector with respect to the local geographic frame, in ω is the true 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: 3 inertial error states each in position, velocity, attitude, accelerometer bias, gyro bias and 1 state each for receiver clock bias and drift: As shown in Figure In the case that EKF is utilized, the measurements are: with the corresponding measurement matrix: In Equation (10) where the following derivatives can be calculated:    The nonlinear system governed by the nonlinear stochastic difference equations can be written as: where k x and k w represent the state vector and process noise vector, respectively and 3. Multiply prediction error vector by Kalman gain matrix to obtain state correction vector and update state vector: 4. Update error covariance 5. Predict new state vector and state covariance matrix where the linear approximation equations for system and measurement matrices are obtained through the relations: Equations (17)(18)(19) are measurement update equations and Equations (20)(21) are the time update equations of the algorithm from k to step k 1  . These equations incorporate a measurement value into a priori estimation to obtain an improved a posteriori estimation. In the above equations, k P is the error covariance matrix defined by , in which k x is an estimation of the system state vector k x and the weighting matrix k K is generally referred to as the Kalman gain matrix. The Kalman filter algorithm starts with an initial condition value, 0  x and 0  P . When new measurement k z becomes available with the progression of time, the estimation of states and the corresponding error covariance would follow recursively ad infinity. Further detailed discussion on the EKF can be referred to in [2,3].

The Unscented Kalman Filter A The unscented transformation
The first step in the UKF is to sample the prior state distribution, i.e., generate the sigma points through unscented transformation (UT). Figure 2 illustrates the true means and covariances as compared to those obtained by the mapping of the UKF versus that of the EKF. The dot-line ellipse represents the true covariance. The UKF is implemented through the transformation of the nonlinear function ( ) Several UT's are available. One of the popular 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 2n 1  sigma vectors X (a capital letter) and weighted points W , given by: set to a small positive (e.g., 1e 4 1     ),  is a secondly scaling parameter (usually set as 0) and β is used to incorporate prior knowledge of the distribution of x (when x is normally distributed, β 2  is an optimal value). is the weight for the covariance associated with the ith point, as follows: The sigma vectors are propagated through the nonlinear function to yield a set of transformed sigma points, The mean and covariance of i y are approximated by a weighted average mean and covariance of the transformed sigma points as follows: Figure 2. Illustration of properties of UKF and EKF [27] The first step in the UKF is to sample the prior state distribution, i.e., generate the sigma points through the UT, which is a method for calculating the statistics of a random variable, which undergoes a nonlinear transformation. The basic premise is that to approximate a probability distribution is easier than to approximate an arbitrary nonlinear transformation. The samples are propagated through true nonlinear equations and linearization of the model is not required.
Suppose the mean x and covariance P of vector x are known, a set of deterministic vectors called sigma points can then be found. The ensemble mean and covariance of the sigma points are equal to x and P . The nonlinear function f( )  y x is applied to each deterministic vector to obtain transformed vectors. The ensemble mean and covariance of the transformed vectors will give a good estimate of the true mean and covariance of y , which is the key to the unscented transformation. The UKF approach estimates are expected to be closer to the true values than the EKF approach. As compared to the EKF's linear approximation, the unscented transformation is accurate to the second order for any nonlinear function.

B The unscented Kalman filter
The implementation algorithm of UKF is summarized as follows: 1. The transformed set is given by instantiating each point through the process model

The Interacting Multiple Model Unscented Kalman Filter
The IMM approach takes into account a set of models to represent the system behaviour patterns or system model. The estimator carries out soft switching among various models by the model probability. The overall estimate is obtained by a combination of the estimates from the filters running in parallel, based on the individual models that match the system modes. The measurements can be obtained from one or more sensors and the model-matched filters can be linear or nonlinear. The algorithm of IMMnonlinear filters is introduced to deal with the noise uncertainty and system nonlinearity simultaneously. Let a general system for multiple models in discrete time be described by where ( )  f and ( )  h are the parameterized state transition and measurement functions, k x and k z are the dynamical state and measurement of the system in model k M and k w , k v are the process noise and measurement noise with covariances k Q and k R , respectively.
The IMMUKF algorithm uses model (Markov chain state) probabilities to weight the input and output of a bank of parallel UKFs at each time instant. The approach takes into account a set of models to represent the system behaviour patterns or system model. The overall estimate is obtained by a combination of the estimates from the filters running in parallel, based on the individual models that match the system modes. An IMM cycle consists of four major stages: interaction (mixing), filtering, model probability calculation and estimate combination. Figure 3 shows a block diagram of the IMMUKF algorithm with two models. One cycle of IMMUKF can be written as follows.

Model interaction/mixing
For given states x with corresponding covariances j j k 1 k 1|k 1     P P and mixing probabilities i|j k 1|k 1   μ for every model, the initial condition for the model j is: The covariance corresponding to the above is: The model transition probabilities, which are related to Markov chain, are defined as: 11 12 1j where i, j 1,2,..., r  and r is the number of sub-models. The mixing probabilities with mode switching probability matrix ij  and the Gaussian mixing probabilities are computed via the equation: where j c is a normalization factor:

Model individual filtering -
Step 1 in UKF loop. The unscented transform creates 2n 1  sigma vectors X (a capital letter) and weighted points W . For state estimation at instant k 1  , sigma points are generated according to: Step 2 in UKF loop. Time update (prediction steps): The samples are propagated through true nonlinear equations; the linearization is unnecessary (calculation of the Jacobian matrix is not required).

Model probabilities update
The model probability j k μ is updated according to the model likelihood and model transition probability governed by the finite-state Markov chain: where the normalized constant: and j k Λ is a likelihood function given by:

Combination of state estimation and covariance combination
The model-individual estimates and covariances are combined to an overall state and covariance.

Results and discussion
Simulation examples were carried out to evaluate the performance of the proposed approach in comparison with the conventional approaches. The computer codes were developed by the authors using Matlab software. The commercial software Satellite Navigation (SATNAV) toolbox [28] by GPSoft LLC was employed for generating the satellite positions and pseudoranges. 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 mode is used and most of the errors can be corrected, but the multipath and receiver thermal noise cannot be eliminated. 4.1 Scenario 1 (Example 1) Figure 4 shows the schematic illustration of a test trajectory for Scenario 1. The trajectory can be divided into ten intervals according to their dynamic characteristics.   Figure 5 shows the accuracy comparison for EKF and UKF in the north, east and altitude components, respectively. Figure 6 shows the position errors in the north, east and altitude components, respectively for IMMEKF as compared to EKF and for IMMUKF as compared to UKF. The use of UKF demonstrates performance improvement when compared to the EKF, due to better nonlinearity treatment. Comparisons of RMSEs are illustrated in Figure 7 for the designs without and with the IMM algorithm. It can be seen that the incorporation of the IMM mechanism can remarkably improve the filter estimation accuracy. Among the four approaches, the IMMUKF algorithm demonstrates superior navigation estimation accuracy as compared to the other three approaches.
The execution times for the four approaches are given in Table 3. The execution times for UKF and IMMEKF are about twice that of EKF; the execution time required for IMMUKF is also approximately twice those of UKF and IMMEKF and is about four times as much as EKF. The information is useful for making trade-offs between estimation accuracy and computational time when selecting a suitable algorithm for a specific application. The model probabilities calculated using the IMMEKF and IMMUKF are provided for inspecting the filter switching capability, as shown in Figure 8.  Table 3. Comparison of execution time for various approaches (in seconds) -Scenario 1

Scenario 2 (Example 2)
Another example is provided for further confirmation of the effectiveness and justification of the performance. The test trajectory is shown in Figure 9. Table 4 provides a description of the vehicle motion. The trajectory can be divided into nine segments, where the vehicle conducts constant acceleration and level flight from 0 to 25 seconds, counter-clockwise turns from 51 to 231 seconds and clockwise circular motion from 283 to 463 seconds. In the three segments, highly dynamic manoeuvring is involved. Constant-velocity straight-line flight is involved in all the other segments, where low dynamic motion is considered.
The results are shown in Figures 10-12. The performance improvement by the IMM algorithm is essentially similar to that obtained in the previous example. Figure 10 provides the accuracy comparison for EKF and UKF and Figure 11 shows the position errors for IMMEKF as compared to EKF and for IMMUKF as compared to UKF. Comparisons of RMSEs are illustrated in Figure 12. Among the four approaches, the IMMUKF algorithm demonstrates superior navigation estimation accuracy. The model probabilities calculated using the IMMEKF and IMMUKF are depicted in Figure 13. When a designer lacks sufficient information to develop a precise model or the parameters change with time, the IMM approach provides a feasible alternative approach that can be adopted for designing an adaptive filter in the ultra-tight GPS/INS integration design.

Conclusions
The IMM-based architecture has been presented to improve the estimation accuracy. The problem of filter parametric adaptation can be regarded as the generalization of structural adaptation (model switching). Featured in structural adaptation, the interacting multiple model (IMM) nonlinear filtering approach has demonstrated good potential as an alternative state estimation technique for the ultra-tightly coupled GPS/INS navigation design. The IMM algorithm is employed for dynamically adjusting process noise by defining different models according to the dynamic situation in which the vehicle is involved and accordingly enhancing the estimation accuracy. The approach is designed to solve the possible degradation problems caused by noise uncertainty and modelling error, so as to improve the navigation accuracy in highly dynamic regions without sacrificing precision in the lower dynamic regions.
An unscented Kalman filtering approach has been demonstrated to be superior to the EKF, due to the fact that the UKF is able to deal with the nonlinear formulation, while the linear model does not reflect the actual dynamic behaviour when the vehicle is manoeuvring. The nonlinear filters have been incorporated into the IMM framework, resulting in the IMMEKF, IMMUKF algorithms. The IMM algorithm has been be employed for dynamically adjusting the process noise. The use of an IMM method allows the exploitation of the benefits of highly dynamic models in the problem of vehicle navigation. Two examples have been provided to illustrate the effectiveness of the design and demonstrate effective improvement in navigation estimation accuracy. Performance comparisons for various approaches: EKF, UKF, IMMEKF and IMMUKF have been presented. Among the four approaches, IMMUKF leads to very promising results in navigational accuracy improvement for an ultra-tightly coupled GPS/INS integrated navigation system. Nevertheless, selection of the nonlinear filters in the IMM framework leads to different levels of computational burden. The information is also provided, which is useful for making trade-offs between estimation accuracy and computational time when selecting a suitable algorithm for a specific application.