Nonlinear Filtering with IMM Algorithm for Ultra-Tight GPS/INS Integration

Abstract

Abstract 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.

1. 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 state-space 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–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–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–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–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.

2. 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 measurements from channels {1, 2, … N} are integrated with the position, velocity and attitude of the INS in a complementary filter. The code phase and Doppler shift calculated from the INS and satellite ephemeris are used to control both the code numerically controlled oscillator (NCO) and carrier NCO. The number of measurements is twice the number of satellites being used for navigation.

Figure 1.

Illustration of the I and Q components as the measurements of the ultra-tightly coupled GPS/INS integration

The received satellite signal can be presented as:

$\mathrm{y}(\mathrm{t})=\mathrm{A}\cdot CA(\mathrm{t})\cdot \mathrm{D}(\mathrm{t})\hspace{0.17em}\cos \left(2\pi {\mathrm{f}}_0\left(\mathrm{t}-\tau \right)+{\theta }_0\right)+\eta$ (1)

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, f₀ is the carrier frequency, θ₀ 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:

$I={{\displaystyle \int }}_{kT}^{(\mathrm{k}+1)\mathrm{T}}\{\cos (\widehat{\omega }\mathrm{t}+\widehat{\theta })[\mathrm{A}\cos (\omega \mathrm{t}+\theta )+\eta ]\}dt\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}$ (2a) $\mathrm{Q}={{\displaystyle \int }}_{kT}^{(\mathrm{k}+1)\mathrm{T}}\{\sin (\widehat{\omega }\mathrm{t}+\widehat{\theta })[\mathrm{A}\cos (\omega \mathrm{t}+\theta )+\eta ]\}dt\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}$ (2b)

After simplification, we have:

$\mathrm{I}=\frac{\mathrm{A}}{2{\omega }_{\mathrm{e}}}\{\sin ({\omega }_{\mathrm{e}}(\mathrm{k}+1)\mathrm{T}+{\theta }_{\mathrm{e}})-\sin ({\omega }_{\mathrm{e}}kT+{\theta }_{\mathrm{e}})\}+{\eta }_{\mathrm{I}}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}$ (3a) $\mathrm{Q}=\frac{-\mathrm{A}}{2{\omega }_{\mathrm{e}}}\{\cos ({\omega }_{\mathrm{e}}(\mathrm{k}+1)\mathrm{T}+{\theta }_{\mathrm{e}})-\cos ({\omega }_{\mathrm{e}}kT+{\theta }_{\mathrm{e}})\}+{\eta }_{\mathrm{Q}}$ (3b)

where η₁ and η_Q represent the noise components for the I and Q channels, respectively, ${\omega }_{\mathrm{e}}=\widehat{\omega }-\omega =2\pi (\widehat{f}-\mathrm{f})$ is the frequency error tracked by the FLL, ${\theta }_{\mathrm{e}}=\widehat{\theta }-\theta$ is the phase error tracked by the PLL and T is the pre-detection integration time. The expectation of the integral leads to:

$\mathrm{E}[\mathrm{I}]=\frac{\mathrm{A}}{2{\omega }_{\mathrm{e}}}[\sin ({\omega }_{\mathrm{e}}(\mathrm{k}+1)\mathrm{T}+{\theta }_{\mathrm{e}})-\sin ({\omega }_{\mathrm{e}}kT+{\theta }_{\mathrm{e}})]$ (4a) $\mathrm{E}[\mathrm{Q}]=\frac{–A}{2{\omega }_{\mathrm{e}}}[\cos ({\omega }_{\mathrm{e}}(\mathrm{k}+1)\mathrm{T}+{\theta }_{\mathrm{e}})-\cos ({\omega }_{\mathrm{e}}kT+{\theta }_{\mathrm{e}})]$ (4b)

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:

${\omega }_{\mathrm{e}}=\frac{2\pi {\mathrm{f}}_0}{\mathrm{c}}|{\widehat{v}}_{\mathrm{u}}-{\mathbf{v}}_{\mathrm{u}}|=\frac{\omega }{\mathrm{c}}{\mathrm{V}}_{\mathrm{e}}$ (5) ${\theta }_{\mathrm{e}}=\frac{2\pi {\mathrm{f}}_0}{\mathrm{c}}(|{\widehat{v}}_{\mathrm{u}}-{\mathbf{v}}_{\mathrm{u}}|\mathrm{t}-|{\widehat{R}}_{\mathrm{u}}-{\mathbf{R}}_{\mathrm{u}}|)=\frac{\omega }{\mathrm{c}}({\mathrm{V}}_{\mathrm{e}}-{\mathrm{R}}_{\mathrm{e}})$ (6)

where R_u and v_u are the measured position and velocity of the receiver, R̂_u and v̂_u are the receiver estimates of position and velocity and R_e =|R̂_u – R_u| and V_e =|v̂_u – v_u| represent the errors of the position and velocity, given by:

$\begin{array}{l}{\mathrm{R}}_{\mathrm{e}}=\sqrt{{\mathrm{x}}_{\mathrm{e}}^2+{\mathrm{y}}_{\mathrm{e}}^2+{\mathrm{z}}_{\mathrm{e}}^2}=\sqrt{{(\widehat{x}-\mathrm{x})}^2+{(\widehat{y}-\mathrm{y})}^2+{(\widehat{z}-\mathrm{z})}^2}\hspace{0.17em}\\ {\mathrm{V}}_{\mathrm{e}}=\sqrt{{\stackrel{.}{x}}_{\mathrm{e}}^2+{\stackrel{.}{y}}_{\mathrm{e}}^2+{\stackrel{.}{z}}_{\mathrm{e}}^2}=\sqrt{{(\widehat{\stackrel{.}{x}}-\stackrel{.}{x})}^2+{(\widehat{\stackrel{.}{y}}-\stackrel{.}{y})}^2+{(\widehat{\stackrel{.}{z}}-\stackrel{.}{z})}^2}\hspace{0.17em}\end{array}$

The INS equations describing the three-dimensional inertial navigation state are:

${\stackrel{.}{V}}_{\mathrm{e}}^{\mathrm{n}}={\mathbf{C}}_{\mathrm{b}}^{\mathrm{n}}{\mathbf{f}}^{\mathrm{n}}-[\mathrm{\Omega }({\omega }_{en}^{\mathrm{n}})+2\mathrm{\Omega }({\omega }_{ie}^{\mathrm{n}})]{\mathbf{V}}_{\mathrm{e}}^{\mathrm{n}}+{\mathbf{g}}_{\mathrm{l}}^{\mathrm{n}}$ (7)

The error model employed for INS is a terrestrial INS psi-angle error model [22]:

$\begin{array}{l}\delta \stackrel{.}{r}=-{\omega }_{en}\times \delta \mathbf{r}+\delta \mathbf{v}\\ \delta \stackrel{.}{v}=-({\omega }_{ie}+{\omega }_{in})\times \delta \mathbf{v}-\psi \times \mathbf{f}+{\epsilon }_{\mathrm{a}}\\ \stackrel{.}{\psi }=-{\omega }_{in}\times \psi +{\epsilon }_{\mathrm{g}}\end{array}$ (8)

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:

${\mathbf{x}}_{\mathrm{k}}=[\mathrm{x},\mathrm{y},\mathrm{z},\stackrel{.}{x},\stackrel{.}{y},\stackrel{.}{z},{\psi }_{\mathrm{x}},{\psi }_{\mathrm{y}},{\psi }_{\mathrm{z}},{\mathrm{a}}_{\mathrm{x}},{\mathrm{a}}_{\mathrm{y}},{\mathrm{a}}_{\mathrm{z}},{\mathrm{g}}_{\mathrm{x}},{\mathrm{g}}_{\mathrm{y}},{\mathrm{g}}_{\mathrm{z}},\mathrm{b},\mathrm{d}{]}^T$

As shown in Figure 1, the signals from the receiver correlators, I and Q, form the measurements of the filters. The parameters I/Q and the position/velocity can be related by the following expressions:

$dE[\mathrm{I}]=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial \mathrm{x}}+\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \mathrm{x}}\right]dx$ (9a) $dE[\mathrm{Q}]=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial \stackrel{.}{x}}+\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \stackrel{.}{x}}\right]d\stackrel{.}{x}$ (9b)

In the case that EKF is utilized, the measurements are:

${\mathrm{z}}_{\mathrm{k}}={\{\mathrm{I}+dI,\mathrm{Q}+dQ\}}_{\mathrm{i}}-{\{\mathrm{I}-{\eta }_{\mathrm{I}},\mathrm{Q}-{\eta }_{\mathrm{Q}}\}}_{\mathrm{i}}={\{dI+{\eta }_{\mathrm{I}},dQ+{\eta }_{\mathrm{Q}}\}}_{\mathrm{i}}$ (10)

with the corresponding measurement matrix:

${\mathbf{H}}_{\mathrm{k}}={\left[\begin{array}{ccccccccccc}{\mathrm{h}}_{Ix1}& {\mathrm{h}}_{Iy1}& {\mathrm{h}}_{Iz1}& {\mathrm{h}}_{I\stackrel{.}{x}1}& {\mathrm{h}}_{I\stackrel{.}{y}1}& {\mathrm{h}}_{I\stackrel{.}{z}1}& 0& \cdots & 0& 1& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& \cdots & ⋮& ⋮& ⋮\\ {\mathrm{h}}_{Ixm}& {\mathrm{h}}_{Iym}& {\mathrm{h}}_{Izm}& {\mathrm{h}}_{I\stackrel{.}{x}m}& {\mathrm{h}}_{I\stackrel{.}{y}m}& {\mathrm{h}}_{I\stackrel{.}{z}m}& 0& \cdots & 0& 1& 0\\ {\mathrm{h}}_{Qx1}& {\mathrm{h}}_{Qy}& {\mathrm{h}}_{Qz1}& {\mathrm{h}}_{Q\stackrel{.}{x}1}& {\mathrm{h}}_{Q\stackrel{.}{y}1}& {\mathrm{h}}_{Q\stackrel{.}{z}1}& 0& \cdots & 0& 0& 1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& \cdots & ⋮& ⋮& ⋮\\ {\mathrm{h}}_{Qxm}& {\mathrm{h}}_{Qym}& {\mathrm{h}}_{Qzm}& {\mathrm{h}}_{Q\stackrel{.}{x}m}& {\mathrm{h}}_{Q\stackrel{.}{y}m}& {\mathrm{h}}_{Q\stackrel{.}{z}m}& 0& \cdots & 0& 0& 1\end{array}\right]}_{2\mathrm{m}\times 17}$ (11)

In Equation (10), dI and dQ denote the deviations in the INS predicted I and Q measurements caused by the inertial sensor errors, η₁ and η_Q are the noise components of I and Q in the GPS signal measurements and i (i = 1 … m) represents the number of channels being tracked. The derivatives in the Jacobian matrix of Equation (11) are given by:

$\begin{array}{l}{\mathrm{h}}_{Ixi}=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial \mathrm{x}}+\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \mathrm{x}}\right];{\mathrm{h}}_{I\stackrel{.}{x}i}=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial \stackrel{.}{x}}+\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \stackrel{.}{x}}\right]\\ {\mathrm{h}}_{Iyi}=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial \mathrm{y}}+\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \mathrm{y}}\right];{\mathrm{h}}_{I\stackrel{.}{y}i}=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial \stackrel{.}{y}}+\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \stackrel{.}{y}}\right]\\ {\mathrm{h}}_{Izi}=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial \mathrm{z}}+\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \mathrm{z}}\right];{\mathrm{h}}_{I\stackrel{.}{z}i}=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial {\stackrel{.}{z}}_{\mathrm{z}}}+\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \stackrel{.}{z}}\right]\end{array}$

and

$\begin{array}{l}{\mathrm{h}}_{Qxi}=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial \mathrm{x}}+\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \mathrm{x}}\right];{\mathrm{h}}_{Q\stackrel{.}{x}i}=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial \stackrel{.}{x}}+\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \stackrel{.}{x}}\right]\\ {\mathrm{h}}_{Qyi}=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial \mathrm{y}}+\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \mathrm{y}}\right];{\mathrm{h}}_{Q\stackrel{.}{y}i}=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial \stackrel{.}{y}}+\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \stackrel{.}{y}}\right]\\ {\mathrm{h}}_{Qzi}=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial \mathrm{z}}+\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \mathrm{z}}\right];{\mathrm{h}}_{Q\stackrel{.}{z}i}=\frac{1}{2}\left[\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\theta }_{\mathrm{e}}}\frac{\partial {\theta }_{\mathrm{e}}}{\partial \stackrel{.}{z}}+\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\omega }_{\mathrm{e}}}\frac{\partial {\omega }_{\mathrm{e}}}{\partial \stackrel{.}{z}}\right]\end{array}$

where the following derivatives can be calculated:

$\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\theta }_{\mathrm{e}}}=\frac{\mathrm{A}}{2{\omega }_{\mathrm{e}}}[\cos ({\omega }_{\mathrm{e}}(\mathrm{k}+1)\mathrm{T}+{\theta }_{\mathrm{e}})-\cos ({\omega }_{\mathrm{e}}kT+{\theta }_{\mathrm{e}})]$ (12) $\begin{array}{l}\frac{\partial \mathrm{E}[\mathrm{I}]}{\partial {\omega }_{\mathrm{e}}}=\frac{-\mathrm{A}}{2{\omega }_{\mathrm{e}}^2}[\sin ({\omega }_{\mathrm{e}}(\mathrm{k}+1)\mathrm{T}+{\theta }_{\mathrm{e}})-\sin ({\omega }_{\mathrm{e}}kT+{\theta }_{\mathrm{e}})]\\ +\frac{\mathrm{A}}{2{\omega }_{\mathrm{e}}}[(\mathrm{k}+1)\mathrm{T}\cdot \cos ({\omega }_{\mathrm{e}}(\mathrm{k}+1)\mathrm{T}+{\theta }_{\mathrm{e}})-kT\cdot \cos ({\omega }_{\mathrm{e}}kT+{\theta }_{\mathrm{e}})]\end{array}$ (13) $\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\theta }_{\mathrm{e}}}=\frac{-\mathrm{A}}{2{\omega }_{\mathrm{e}}}[-\sin ({\omega }_{\mathrm{e}}(\mathrm{k}+1)\mathrm{T}+{\theta }_{\mathrm{e}})+\sin ({\omega }_{\mathrm{e}}kT+{\theta }_{\mathrm{e}})]$ (14) $\begin{array}{l}\frac{\partial \mathrm{E}[\mathrm{Q}]}{\partial {\omega }_{\mathrm{e}}}=\frac{\mathrm{A}}{2{\omega }_{\mathrm{e}}^2}[\cos ({\omega }_{\mathrm{e}}(\mathrm{k}+1)\mathrm{T}+{\theta }_{\mathrm{e}})-\cos ({\omega }_{\mathrm{e}}kT+{\theta }_{\mathrm{e}})]\\ +\frac{\mathrm{A}}{2{\omega }_{\mathrm{e}}}[(\mathrm{k}+1)\mathrm{T}\cdot \cos ({\omega }_{\mathrm{e}}(\mathrm{k}+1)\mathrm{T}+{\theta }_{\mathrm{e}})+kT\cdot \sin ({\omega }_{\mathrm{e}}kT+{\theta }_{\mathrm{e}})]\end{array}$ (15)

Taking derivatives of θ_e and ω_e with respect to the three components of position gives:

$\frac{\partial {\theta }_{\mathrm{e}}}{\partial \mathrm{x}}=\frac{\omega }{\mathrm{c}}\frac{{\mathrm{x}}_{\mathrm{e}}}{{\mathrm{R}}_{\mathrm{e}}};\frac{\partial {\theta }_{\mathrm{e}}}{\partial \mathrm{y}}=\frac{\omega }{\mathrm{c}}\frac{{\mathrm{y}}_{\mathrm{e}}}{{\mathrm{R}}_{\mathrm{e}}};\frac{\partial {\theta }_{\mathrm{e}}}{\partial \mathrm{z}}=\frac{\omega }{\mathrm{c}}\frac{{\mathrm{z}}_{\mathrm{e}}}{{\mathrm{R}}_{\mathrm{e}}}.$

and:

$\frac{\partial {\omega }_{\mathrm{e}}}{\partial \mathrm{x}}=0;\frac{\partial {\omega }_{\mathrm{e}}}{\partial \mathrm{y}}=0;\frac{\partial {\omega }_{\mathrm{e}}}{\partial \mathrm{z}}=0$

respectively. Furthermore, taking derivatives of θ_e and ω_e with respect to the velocities gives:

$\frac{\partial {\theta }_{\mathrm{e}}}{\partial \stackrel{.}{x}}=\frac{-\omega }{\mathrm{c}}\frac{{\stackrel{.}{x}}_{\mathrm{e}}\mathrm{T}}{{\mathrm{V}}_{\mathrm{e}}};\frac{\partial {\theta }_{\mathrm{e}}}{\partial \stackrel{.}{y}}=\frac{-\omega }{\mathrm{c}}\frac{{\stackrel{.}{y}}_{\mathrm{e}}\mathrm{T}}{{\mathrm{V}}_{\mathrm{e}}};\frac{\partial {\theta }_{\mathrm{e}}}{\partial \stackrel{.}{y}}=\frac{-\omega }{\mathrm{c}}\frac{{\stackrel{.}{y}}_{\mathrm{e}}\mathrm{T}}{{\mathrm{V}}_{\mathrm{e}}}$

and:

$\frac{\partial {\omega }_{\mathrm{e}}}{\partial \stackrel{.}{x}}=\frac{-\omega }{\mathrm{c}}\frac{{\stackrel{.}{x}}_{\mathrm{e}}}{{\mathrm{V}}_{\mathrm{e}}};\frac{\partial {\omega }_{\mathrm{e}}}{\partial \stackrel{.}{y}}=\frac{-\omega }{\mathrm{c}}\frac{{\stackrel{.}{y}}_{\mathrm{e}}}{{\mathrm{V}}_{\mathrm{e}}};\frac{\partial {\omega }_{\mathrm{e}}}{\partial \stackrel{.}{z}}=\frac{-\omega }{\mathrm{c}}\frac{{\stackrel{.}{z}}_{\mathrm{e}}}{{\mathrm{V}}_{\mathrm{e}}}$

respectively.

3. The Interacting Multiple Model Unscented Kalman Filter

The nonlinear system governed by the nonlinear stochastic difference equations can be written as:

${\mathbf{x}}_{\mathrm{k}+1}=\mathbf{f}({\mathbf{x}}_{\mathrm{k}},\mathrm{k})+{\mathbf{w}}_{\mathrm{k}}$ (16a) ${\mathbf{z}}_{\mathrm{k}}=\mathbf{h}({\mathbf{x}}_{\mathrm{k}},\mathrm{k})+{\mathbf{v}}_{\mathrm{k}}$ (16b)

where x_k and w_k represent the state vector and process noise vector, respectively and z_k and v_k are the measurement vector and the measurement noise vector, respectively. The vectors w_k and v_k are zero mean Gaussian white sequences and have zero cross-correlation with each other:

$\begin{array}{c}\mathrm{E}[{\mathbf{w}}_{\mathrm{k}}{\mathbf{w}}_{\mathrm{i}}^{\mathrm{T}}]={\mathbf{Q}}_{\mathrm{k}}{\delta }_{ik};\mathrm{E}[{\mathbf{v}}_{\mathrm{k}}{\mathbf{v}}_{\mathrm{i}}^{\mathrm{T}}]={\mathbf{R}}_{\mathrm{k}}{\delta }_{ik};\\ \mathrm{E}[{\mathbf{w}}_{\mathrm{k}}{\mathbf{v}}_{\mathrm{i}}^{\mathrm{T}}]=0\hspace{0.17em}for\hspace{0.17em}all\hspace{0.17em}\mathrm{i}\hspace{0.17em}and\hspace{0.17em}\mathrm{k}\end{array}$

where E[·] represents expectation and superscript “T” denotes matrix transpose. Q_k is the process noise covariance matrix and R_k is the measurement noise covariance matrix. The symbol δ_ik stands for the Kronecker delta function:

${\delta }_{ik}=\{\begin{array}{c}1,\begin{array}{cc}& \mathrm{i}=\mathrm{k}\end{array}\\ 0,\begin{array}{cc}& \mathrm{i}\ne \mathrm{k}\end{array}\end{array}$

The discrete-time extended Kalman filter algorithm is summarized as follows.

Initialize state vector and state covariance matrix: ${\widehat{\mathbf{x}}}_0^{-}$ and P₀⁻

Compute Kalman gain matrix from state covariance and estimated measurement covariance: ${\mathbf{K}}_{\mathrm{k}}={\mathbf{P}}_{\mathrm{k}}^{-}{\mathbf{H}}_{\mathrm{k}}^{\mathrm{T}}{[{\mathbf{H}}_{\mathrm{k}}{\mathbf{P}}_{\mathrm{k}}^{-}{\mathbf{H}}_{\mathrm{k}}^{\mathrm{T}}+{\mathbf{R}}_{\mathrm{k}}]}^{-1}$ (17)

Multiply prediction error vector by Kalman gain matrix to obtain state correction vector and update state vector: ${\widehat{\mathbf{x}}}_{\mathrm{k}}={\widehat{\mathbf{x}}}_{\mathrm{k}}^{-}+{\mathbf{K}}_{\mathrm{k}}[{\mathbf{z}}_{\mathrm{k}}-{\widehat{z}}_{\mathrm{k}}^{-}],\hspace{0.17em}\mathrm{with}\hspace{0.17em}{\widehat{z}}_{\mathrm{k}}^{-}=\mathbf{h}({\widehat{\mathbf{x}}}_{\mathrm{k}}^{-},\mathrm{k})$ (18)

Update error covariance ${\mathbf{P}}_{\mathrm{k}}=\left[\mathbf{I}-{\mathbf{K}}_{\mathrm{k}}{\mathbf{H}}_{\mathrm{k}}\right]{\mathbf{P}}_{\mathrm{k}}^{-}$ (19)

Predict new state vector and state covariance matrix ${\widehat{\mathbf{x}}}_{\mathrm{k}+1}^{-}=\mathbf{f}({\widehat{\mathbf{x}}}_{\mathrm{k}},\mathrm{k})$ (20) ${\mathbf{P}}_{\mathrm{k}+1}^{-}={\mathrm{\Phi }}_{\mathrm{k}}{\mathbf{P}}_{\mathrm{k}}{\mathrm{\Phi }}_{\mathrm{k}}^{\mathbf{T}}+{\mathbf{Q}}_{\mathrm{k}}$ (21) where the linear approximation equations for system and measurement matrices are obtained through the relations: ${\mathrm{\Phi }}_{\mathrm{k}}\approx {\frac{\partial {\mathbf{f}}_{\mathrm{k}}}{\partial \mathbf{x}}|}_{\mathbf{x}={\widehat{\mathbf{x}}}_{\mathrm{k}}^{-}};{\mathbf{H}}_{\mathrm{k}}\approx {\frac{\partial {\mathbf{h}}_{\mathrm{k}}}{\partial \mathbf{x}}|}_{\mathbf{x}={\widehat{\mathbf{x}}}_{\mathrm{k}}^{-}}$ (22)

Equations (17–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, P_k is the error covariance matrix defined by E[(x_k – ○_k)(x_k – ○_k)^T], in which ○_k is an estimation of the system state vector x_k 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, ${\widehat{\mathbf{x}}}_0^{-}$ and P₀⁰. When new measurement z_k 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].

3.1 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 f(·), shown as the solid-line ellipse in the top portion of the figure. The EKF is accomplished through the Jacobian F = ∂Sf∂ _x , shown as the solid-line ellipse in the bottom portion of the figure. The UKF approach estimates are expected to be closer to the true values than the EKF approach.

Figure 2.

Illustration of properties of UKF and EKF [27]

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 ○ 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:

$\begin{array}{c}{\mathbf{X}}_{(0)}=\widehat{\mathbf{x}}\\ {\mathbf{X}}_{(\mathrm{i})}=\widehat{\mathbf{x}}+{\left(\sqrt{(\mathrm{n}+\lambda )\mathbf{P}}\right)}_{\mathrm{i}}^{\mathrm{T}},\hspace{0.17em}\mathrm{i}=1,\mathrm{\dots },n\\ {\mathbf{X}}_{(\mathrm{i}+\mathrm{n})}=\widehat{\mathbf{x}}-{\left(\sqrt{(\mathrm{n}+\lambda )\mathbf{P}}\right)}_{\mathrm{i}}^{\mathrm{T}},\hspace{0.17em}\mathrm{i}=1,\mathrm{\dots },n\end{array}$ (23)

where ${(\sqrt{(\mathrm{n}+\lambda )P})}_{\mathrm{i}}$ is the i th row of the matrix square root. $\sqrt{(\mathrm{n}+\lambda )P}$ can be obtained from the lower-triangular matrix of the Cholesky factorization. λ = α²(n + κ) – n is a scaling parameter, α determines the spread of the sigma points around x́ and is usually 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 ̄ (when x is normally distributed, β = 2 is an optimal value). 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, as follows:

${\mathrm{W}}_0^{(m)}=\frac{\lambda }{(\mathrm{n}+\lambda )}$ (24a) ${\mathrm{W}}_0^{(c)}={\mathrm{W}}_0^{(m)}+(1-{\alpha }^2+\beta )$ (24b) ${\mathrm{W}}_{\mathrm{i}}^{(m)}={\mathrm{W}}_{\mathrm{i}}^{(c)}=\frac{1}{2(\mathrm{n}+\lambda )},\mathrm{i}=1,\mathrm{\dots },2\mathrm{n}$ (24c)

The sigma vectors are propagated through the nonlinear function to yield a set of transformed sigma points,

${\mathbf{y}}_{\mathrm{i}}=\mathrm{f}({\mathbf{X}}^{(\mathrm{i})}),\mathrm{i}=0,\mathrm{\dots },2\mathrm{n}$ (25)

The mean and covariance of y_i are approximated by a weighted average mean and covariance of the transformed sigma points as follows:

${\bar{y}}_{\mathrm{u}}={\displaystyle \sum _{\mathrm{i}=0}^{2\mathrm{n}}{\mathrm{W}}_{\mathrm{i}}^{(\mathrm{m})}}{\mathbf{y}}_{\mathrm{i}}$ (26) ${\bar{P}}_{\mathrm{u}}={\displaystyle \sum _{\mathrm{i}=0}^{2\mathrm{n}}{\mathrm{W}}_{\mathrm{i}}^{(\mathrm{c})}}({\mathbf{y}}_{\mathrm{i}}-{\bar{y}}_{\mathrm{u}}){({\mathbf{y}}_{\mathrm{i}}-{\bar{y}}_{\mathrm{u}})}^{\mathrm{T}}$ (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 y = f(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:

The transformed set is given by instantiating each point through the process model ${\zeta }_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}=\mathrm{f}({\mathbf{X}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}),\hspace{0.17em}\mathrm{i}=0,\mathrm{\dots },2\mathrm{n}$ (28)

Predicted mean ${\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}-1}={\displaystyle \sum _{\mathrm{i}=0}^{2\mathrm{n}}{\mathrm{W}}_{\mathrm{i}}^{(m)}{\zeta }_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}}$

Predicted covariance ${\mathbf{P}}_{\mathrm{k}|\mathrm{k}-1}={\displaystyle \sum _{\mathrm{i}=0}^{2\mathrm{n}}{\mathrm{W}}_{\mathrm{i}}^{(\mathrm{c})}[{\zeta }_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}-{\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}-1}]{[{\zeta }_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}-{\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}-1}]}^{\mathrm{T}}}+{\mathbf{Q}}_{\mathrm{k}-1}$ (29)

Instantiate each of the prediction points through the observation model ${\mathbf{Z}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}=\mathbf{h}({\zeta }_{\mathrm{i},\mathrm{k}|\mathrm{k}-1})$

Predicted observation ${\widehat{z}}_{\mathrm{k}|\mathrm{k}-1}={\displaystyle \sum _{\mathrm{i}=0}^{2\mathrm{n}}{\mathrm{W}}_{\mathrm{i}}^{(\mathrm{m})}{\mathbf{Z}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}}$

Innovation covariance ${\mathbf{P}}_{zz}={\displaystyle \sum _{\mathrm{i}=0}^{2\mathrm{n}}{\mathrm{W}}_{\mathrm{i}}^{(\mathrm{c})}[{\mathbf{Z}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}-{\widehat{z}}_{\mathrm{k}|\mathrm{k}-1}]{[{\mathbf{Z}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}-{\widehat{z}}_{\mathrm{k}|\mathrm{k}-1}]}^{\mathrm{T}}+{\mathbf{R}}_{\mathrm{k}}}$ (30)

Cross covariance ${\mathbf{P}}_{\text{xz}}={\displaystyle \sum _{\mathrm{i}=0}^{2\mathrm{n}}{\mathrm{W}}_{\mathrm{i}}^{(\mathrm{c})}\left[{\zeta }_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}-{\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}-1}\right]{\left[{\mathbf{Z}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}-{\widehat{z}}_{\mathrm{k}|\mathrm{k}-1}\right]}^{\mathrm{T}}}$ (31)

Performing update $\begin{array}{l}{\mathbf{K}}_{\mathrm{k}}={\mathbf{P}}_{\text{xz}}{\mathbf{P}}_{zz}^{-1}\\ {\widehat{\mathbf{x}}}_{\mathrm{k}}={\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}-1}+{\mathbf{K}}_{\mathrm{k}}({\mathbf{z}}_{\mathrm{k}}-{\widehat{z}}_{\mathrm{k}|\mathrm{k}-1})\\ {\mathbf{P}}_{\mathrm{k}}={\mathbf{P}}_{\mathrm{k}}^{-}-{\mathbf{K}}_{\mathrm{k}}{\mathbf{P}}_{zz}{\mathbf{K}}_{\mathrm{k}}^{\mathrm{T}}\end{array}$ (32)

3.2 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 IMM-nonlinear 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

${\mathbf{x}}_{\mathrm{k}+1}=\mathbf{f}({\mathbf{x}}_{\mathrm{k}},\mathrm{k},{\mathrm{M}}_{\mathrm{k}})+{\mathbf{w}}_{\mathrm{k}}({\mathbf{x}}_{\mathrm{k}},{\mathrm{M}}_{\mathrm{k}})$ (33a) ${\mathbf{z}}_{\mathrm{k}}=\mathbf{h}({\mathbf{x}}_{\mathrm{k}},\mathrm{k},{\mathrm{M}}_{\mathrm{k}})+{\mathbf{v}}_{\mathrm{k}}({\mathbf{x}}_{\mathrm{k}},{\mathrm{M}}_{\mathrm{k}})$ (33b)

where f(·) and h(·) are the parameterized state transition and measurement functions, x_k and z_k are the dynamical state and measurement of the system in model M_k and w_k, v_k are the process noise and measurement noise with covariances Q_k and R_k, 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.

Figure 3.

Block diagram of the IMMUKF algorithm (one cycle with two models)

1. Model interaction/mixing

For given states x_k-1^j=x_k-1|k-1^j with corresponding covariances P_k-1^j=P_k-1|k-1^j and mixing probabilities μ_k-1|k-1^i|j for every model, the initial condition for the model j is:

${\widehat{\mathbf{x}}}_{\mathrm{k}-1|\mathrm{k}-1}^{0\mathrm{j}}={\displaystyle \sum _{\mathrm{i}=1}^{\mathrm{r}}{\widehat{\mathbf{x}}}_{\mathrm{k}-1|\mathrm{k}-1}^{\mathrm{i}}{\mu }_{\mathrm{k}-1|\mathrm{k}-1}^{\mathrm{i}|\mathrm{j}}},\hspace{0.17em}\mathrm{j}=1,2,\mathrm{\dots },\mathrm{r}$ (34)

The covariance corresponding to the above is:

$\begin{array}{l}{\mathbf{P}}_{k-1|k-1}^{0j}={\displaystyle \sum _{i=1}^r{\mu }_{k-1|k-1}^{i|j}\{{\mathbf{P}}_{k-1|k-1}^i+[{\widehat{\mathbf{x}}}_{k-1|k-1}^i\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}}\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}-{\widehat{\mathbf{x}}}_{k-1|k-1}^{0j}]{\left[{\widehat{\mathbf{x}}}_{k-1|k-1}^i-{\widehat{\mathbf{x}}}_{k-1|k-1}^{0j}\right]}^T\}\end{array}$ (35)

The model transition probabilities, which are related to Markov chain, are defined as:

${\pi }_{ij}=\mathrm{p}\{{\mathrm{M}}_{\mathrm{k}}^{\mathrm{j}}|{\mathrm{M}}_{\mathrm{k}-1}^{\mathrm{i}}\}=\left[\begin{array}{cccc}{\pi }_{11}& {\pi }_{12}& \dots & {\pi }_{1\mathrm{j}}\\ {\pi }_{21}& {\pi }_{22}& \dots & {\pi }_{2\mathrm{j}}\\ ⋮& ⋮& \ddots & ⋮\\ {\pi }_{\mathrm{i}1}& {\pi }_{\mathrm{i}2}& \dots & {\pi }_{ij}\end{array}\right]$ (36)

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:

${\mu }_{\mathrm{k}-1|\mathrm{k}-1}^{\mathrm{i}|\mathrm{j}}=\frac{1}{{\bar{c}}_{\mathrm{j}}}{\pi }_{ij}{\mu }_{\mathrm{k}-1}^{\mathrm{i}}$ (37)

where ${\bar{c}}_{\mathrm{j}}$ is a normalization factor:

${\bar{c}}_{\mathrm{j}}={\displaystyle \sum _{\mathrm{i}=1}^{\mathrm{r}}{\pi }_{ij}{\mu }_{\mathrm{k}-1}^{\mathrm{i}}}$ (38)

2. 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:

$\{\begin{array}{l}{\mathbf{X}}_{0,\mathrm{k}-1}^{\mathrm{j}}={\widehat{\mathbf{x}}}_{\mathrm{k}-1}^{0\mathrm{j}}\\ {\mathbf{X}}_{\mathrm{i},\mathrm{k}-1}^{\mathrm{j}}={\widehat{\mathbf{x}}}_{\mathrm{k}-1}^{0\mathrm{j}}+{\left(\sqrt{(\mathrm{n}+\lambda ){\mathbf{P}}_{\mathrm{k}-1}^{0\mathrm{j}}}\right)}_{\mathrm{i}}^{\mathrm{T}}\\ {\mathbf{X}}_{\mathrm{i}+\mathrm{n},\mathrm{k}-1}^{\mathrm{j}}={\widehat{\mathbf{x}}}_{\mathrm{k}-1}^{0\mathrm{j}}-{\left(\sqrt{(\mathrm{n}+\lambda ){\mathbf{P}}_{\mathrm{k}-1}^{0\mathrm{j}}}\right)}_{\mathrm{i}}^{\mathrm{T}}\end{array},\mathrm{i}=1,\mathrm{\dots },\mathrm{n}$ (39)

- Step 2 in UKF loop. Time update (prediction steps):

${\zeta }_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}={\mathrm{f}}_{\mathrm{j}}({\mathbf{X}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}),\hspace{0.17em}\mathrm{i}=0,\mathrm{\dots },2\mathrm{n}$ (40) ${\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}={\displaystyle \sum _{\mathrm{i}=0}^{2\mathrm{n}}{\mathrm{W}}_{\mathrm{i}}^{(\mathrm{m})}{\zeta }_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}}$ (41) ${\mathbf{P}}_{\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}={\displaystyle \sum _{\mathrm{i}=0}^{2\mathrm{n}}{\mathrm{W}}_{\mathrm{i}}^{(\mathrm{c})}[{\zeta }_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}-{\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}]{[{\zeta }_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}-{\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}]}^{\mathrm{T}}+{\mathbf{Q}}_{\mathrm{k}-1}^{\mathrm{j}}}$ (42) ${\mathbf{Z}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}=\mathrm{h}({\zeta }_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}})$ (43) ${\widehat{z}}_{\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}={\displaystyle \sum _{\mathrm{i}=0}^{2\mathrm{n}}{\mathrm{W}}_{\mathrm{i}}^{(\mathrm{m})}{\mathbf{Z}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}}$ (44)

- Step 3 in UKF loop. Measurement update (correction steps):

${\mathbf{P}}_{zz}^{\mathrm{j}}={\displaystyle \sum _{\mathrm{i}=0}^{2\mathrm{n}}{\mathrm{W}}_{\mathrm{i}}^{(\mathrm{c})}[{\mathbf{Z}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}-{\widehat{z}}_{\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}]{[{\mathbf{Z}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}-{\widehat{z}}_{\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}]}^{\mathrm{T}}+{\mathbf{R}}_{\mathrm{k}}^{\mathrm{j}}}$ (45) ${\mathbf{P}}_{xz}^{\mathrm{j}}={\displaystyle \sum _{\mathrm{i}=0}^{2\mathrm{n}}{\mathrm{W}}_{\mathrm{i}}^{(\mathrm{c})}[{\zeta }_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}-{\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}]{[{\mathbf{Z}}_{\mathrm{i},\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}-{\widehat{z}}_{\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}]}^{\mathrm{T}}}$ (46) ${\mathbf{K}}_{\mathrm{k}}^{\mathrm{j}}={\mathbf{P}}_{xz}^{\mathrm{j}}{({\mathbf{P}}_{zz}^{\mathrm{j}})}^{-1}$ (47) ${\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}}^{\mathrm{j}}={\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}+{\mathbf{K}}_{\mathrm{k}}^{\mathrm{j}}{\upsilon }_{\mathrm{k}}^{\mathrm{j}},\hspace{0.17em}\hspace{0.17em}where\hspace{0.17em}\hspace{0.17em}{\upsilon }_{\mathrm{k}}^{\mathrm{j}}={\mathbf{z}}_{\mathrm{k}}-{\widehat{z}}_{\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}$ (48) ${\mathbf{P}}_{\mathrm{k}|\mathrm{k}}^{\mathrm{j}}={\mathbf{P}}_{\mathrm{k}|\mathrm{k}-1}^{\mathrm{j}}-{\mathbf{K}}_{\mathrm{k}}^{\mathrm{j}}{\mathbf{P}}_{zz}^{\mathrm{j}}{\mathbf{K}}_{\mathrm{k}}^{\mathrm{j}}{}^{\mathrm{T}}$ (49)

The samples are propagated through true nonlinear equations; the linearization is unnecessary (calculation of the Jacobian matrix is not required).

3. Model probabilities update

The model probability μ_k^j is updated according to the model likelihood and model transition probability governed by the finite-state Markov chain:

${\mu }_{\mathrm{k}}^{\mathrm{j}}=\frac{1}{\mathrm{c}}{\mathrm{\Lambda }}_{\mathrm{k}}^{\hspace{0.17em}\hspace{0.17em}\mathrm{j}}{\overline{\mathrm{c}}}_{\mathrm{j}}$ (50)

where the normalized constant:

$\mathrm{c}=\sum _{\mathrm{j}=1}^{\mathrm{r}}{\overline{\mathrm{c}}}_{\mathrm{j}}{\mathrm{\Lambda }}_{\mathrm{k}}^{\hspace{0.17em}\mathrm{j}}$ (51)

and Λ_k^j is a likelihood function given by:

${\mathrm{\Lambda }}_{\mathrm{k}}^{\mathrm{j}}=\frac{1}{\sqrt{2\pi |{\mathbf{P}}_{zz}^{\mathrm{j}}|}}\exp \left[-\frac{1}{2}{\upsilon }_{\mathrm{k}}^{\mathrm{j}}{}^{\mathrm{T}}{({\mathbf{P}}_{zz}^{\mathrm{j}})}^{-1}{\upsilon }_{\mathrm{k}}^{\mathrm{j}}\right]$ (52)

4. Combination of state estimation and covariance combination

The model-individual estimates and covariances are combined to an overall state and covariance.

${\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}}={\displaystyle \sum _{\mathrm{j}=1}^{\mathrm{r}}{\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}}^{\mathrm{j}}{\mu }_{\mathrm{k}}^{\mathrm{j}}}$ (53) ${\mathbf{P}}_{\mathrm{k}|\mathrm{k}}={\displaystyle \sum _{\mathrm{j}=1}^{\mathrm{r}}{\mu }_{\mathrm{k}}^{\mathrm{j}}\{{\mathbf{P}}_{\mathrm{k}|\mathrm{k}}^{\mathrm{j}}+[{\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}}^{\mathrm{j}}-{\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}}]{[{\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}}^{\mathrm{j}}-{\widehat{\mathbf{x}}}_{\mathrm{k}|\mathrm{k}}]}^{\mathrm{T}}\}}$ (54)

4. 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. The measurement noise variances ${\mathrm{r}}_{{\rho }_{\mathrm{i}}}$ are set at 15m². Let each of the white-noise spectral amplitudes that drive the random walk position states be S_p = 0.003(m / sec²)rad / sec. Furthermore, let the clock model spectral amplitudes be S_f =0.4(10⁻¹⁸)sec and S_g = 1.58(10⁻¹⁸)sec⁻¹. The data for INS error specifications is taken from Crista IMU specifications [29], summarized in Table 1. For the two models of the IMM algorithms, two process noise covariance matrices are used. Model 1 represents the system with smaller process noise (Q_k^small, while the system with larger process noise (Q_k^large) is represented by Model 2. In this paper, the ratio of process noise is 100, i.e., Q_k^large=100Q_k^small. This ratio depends on the designer's knowledge of the dynamic characteristics. For a very high dynamic manoeuvring situation, a larger ratio should be used. The model transition probability matrix is given by:

Table 1.

INS error specifications (from Crista IMU Specifications) [29]

	Gyros	Accelerometers
Range	±300 º /sec	±10 G
Scale Factor Error	< 3 º/sec (@ 25°C)	< 100 mG (0.98 m/s2) (@ 25 ºC)
In-Run Bias Error:
Fixed temperature	< 0.2 º/sec (warmed up)	< 25 mG (0.245 m/s∧2)
Over temperature	< 0.6 º/sec	< 51 mG (0.500 m/s∧2)
Noise (1σ, no over-samples)	< ± 0.7 º /sec	< ± 12 mG (0.120 m/s∧2)

$\pi =\left[\begin{array}{cc}0.99& 0.01\\ 0.01& 0.99\end{array}\right]$ (55)

The UKF parameters are set as α = 1, κ = 0, β = 2.

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. Table 2 presents a description of the vehicle motion to provide a better insight into vehicle dynamic information in each time interval. The trajectory can be divided into ten time intervals (or segments) according to the dynamic characteristics. The vehicle was simulated to conduct constant acceleration and level flight from 0 to 25 seconds, clockwise circular motion with a radius of 750m from 41 to 231 seconds and counter-clockwise turns from 283 to 374 seconds, where highly dynamic manoeuvring is involved. For all the other segments, constant-velocity straight-line flight is carried out.

Table 2.

Description of the vehicle motion for Scenario 1

Segment number	Time interval (sec)	Motion
1	[0–25]	Constant acceleration level flight
2	[26–40]	Climbing
3	[41–231]	Counter-clockwise circular motion
4	[232–266]	Climbing
5	[267–282]	Constant velocity level flight
6	[283–374]	Constant acceleration clockwise turn
7	[375–389]	Descending
8	[390–424]	Constant velocity level flight
9	[425–449]	Descending
10	[450–484]	Constant velocity level flight

Figure 4.

The three-dimensional test trajectory for Scenario 1

Figures 5–7 provide the position accuracy for the results based on various approaches. 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.

Figure 5.

Comparison of position error for EKF and UKF - Scenario 1

Figure 6.

Position errors for IMMEKF as compared to EKF (left) and for IMMUKF as compared to UKF (right) - Scenario 1

Figure 7.

Comparison of RMSE for EKF and UKF based results (left) and for IMMEKF and IMMUKF based results (right) - Scenario 1

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. For the intervals where highly dynamic manoeuvres are involved, the model probability of Model 2 becomes larger than that of Model 1. In contrast, in the lower dynamic intervals, the model probability of Model 1 is larger than that of Model 2, confirming that Model 2 is more appropriate for highly dynamic regions. Moreover, the model probability for the IMMUKF demonstrates a stronger capability to accurately capture the dynamic behaviours than the IMMEKF, resulting in better estimation accuracy.

Table 3.

Comparison of execution time for various approaches (in seconds) - Scenario 1

Approach	Time
EKF	8.784
UKF	16.152
IMMEKF	17.821
IMMUKF	33.056

Figure 8.

Comparison of model probability for IMMEKF (left) and IMMUKF (right) - Scenario 1

4.2 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.

Table 4.

Description of the vehicle motion for Scenario 2

Segment number	Time interval (sec)	Motion
1	[0–25]	Constant acceleration level flight
2	[26–50]	Climbing
3	[51–231]	Counter-clockwise circular motion
4	[232–282]	Climbing
5	[283–463]	Clockwise circular motion
6	[464–488]	Descending
7	[489–512]	Constant velocity level flight
8	[513–547]	Descending
9	[548–573]	Constant velocity level flight

Figure 9.

The test trajectory for Scenario 2

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.

Figure 10.

Comparison of position error for EKF and UKF - Scenario 2

Figure 11.

Position errors for IMMEKF as compared to EKF (left) and for IMMUKF as compared to UKF (right) - Scenario 2

Figure 12.

Comparison of RMSE for EKF and UKF based results (left) and for IMMEKF and IMMUKF based results (right) - Scenario 2

Figure 13.

Comparison of model probability for IMMEKF (left) and IMMUKF (right) - Scenario 2

5. 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.

6. Acknowledgements

Funding for this work was provided by the National Science Council of the Republic of China under grant numbers NSC 98-2221-E-019-021-MY3 and NSC 101-2221-E-019-027-MY3.