2.1. Unscented Kalman Filter (UKF)

The UKF is a derivative-free alternative to the EKF, which uses a deterministic sampling approach. The state distribution is represented using a minimal set of carefully chosen sample points, called sigma points. Like EKF, UKF consists of the same two steps: model forecast and data assimilation; except they are now preceded by another step for the selection of sigma points. The UKF is founded on the intuition that, it is easier to approximate a probability distribution than it is to approximate an arbitrary non-linear function or transformation. The sigma points are chosen so that their mean and co-variance are exactly $\bar x$ and P _xx respectively. Each sigma point is then propagated through the non-linear functions yielding a cloud of transformed points at the end. The new estimated mean and co-variance are then computed based on their statistics. This process is called unscented transformation (Haykin, Reference Haykin2001). The n-dimensional random variable with mean $\bar x$ and co-variance P _xx is approximated by (2n+1) weighted points given by

(1)$$\left. \matrix{\chi _0 = \; \bar x,\; \; \; W_0 = \; \kappa /(n + k) \cr \chi _i = \; \bar x + \left( {\sqrt {(n + \kappa )P_{xx}}}\, \right)_i \;, \; W_i = \; 1/2(n + \kappa ) \cr \chi _{i + n} = \; \bar x - \; \left( {\sqrt {\left( {n + \kappa} \right)P_{xx}}}\, \right)_i, \; W_{i + n} = \; 1/2(n + \kappa ) } \right\}$$

In Equation (1), χ denotes the sigma point, W denotes weight, and κ is a constant. The sigma points are passed through the non-linear function to yield the set of transformed sigma points.

(2)$$\varsigma _i = f\,(x_i )$$

The mean and co-variance are given by the weighted average and the weighted outer product of the transformed points.

(3)$$\bar y = \; \mathop \sum \limits_{i = 0}^{2n} w_s^i \varsigma _i $$

(4)$$P_{yy} = \; \mathop \sum \limits_{i = 0}^{2n} w_c^i (\varsigma _i - \bar y)(\varsigma _i - \bar y)^T $$

The time prediction and the measurement update are completed using this mean and co-variance. The time update involves the prediction of next state variables given by

(5)$$\chi _{i,k} = F[\chi _{i,k - 1} ]$$

The mean state space vectors and its priori co-variance are given by

(6)$$\bar x_k^ - = \; \sum\limits_{i = 0}^{2n} {W_i\, \chi _{i,k}} $$

(7)

The measurement model is given by

(8)$$Z_{i,k} = H[\chi _{i,\; k - 1} ]$$

The estimated measurement vector is given by

(9)$$\hat z_k^ - = \; \mathop \sum \limits_{i = 0}^{2n} W_i Z_{i,k} $$

And the co-variance is given by

(10)

The cross co-variance between the state and measurement is given by

(11)

The gain equation is given by

(12)$$k = P_{x_k z_k} P_{z_k z_k} ^{ - 1} $$

The final state and co-variance estimates are given by

(13)$$\hat x_k = \hat x_k^ - \; + k\left( {z_k - \hat z_k^ -} \right)$$

(14)$$P_k = P_k^ - - kP_{x_k z_k} k^T $$

By using the above algorithm, the state of the system can be accurately predicted.

Since the motion model of the vehicle has been changing frequently, more than one estimator is considered to meet the changing environment; IMM has been introduced. As the name implies, multiple models can be utilized simultaneously (Honghui, Reference Honghui2002; Rong and Bar-Shalom, Reference Rong Li and Bar-Shalom1993b; Rong and Vesselin, Reference Rong Li and Vesselin2003). To incorporate soft switching between different motion models, the IMM-UKF is introduced (Qian et al., Reference Qian, An and Xia2010).

2.2. Proposed Method (IMM-UKF-TFS)

To improve positional accuracy, IMM-based fixed interval Two Filter Smoothing is introduced in this paper. The proposed method incorporates a smoothing algorithm into the Interacting Multiple Model approach as shown in Figure 1. The inputs to the IMM-UKF-TFS are the modelled state space and measurement vectors. The motion segmented initial state vectors are input in the IMM algorithm. The IMM algorithm provides a soft switching between the different inputs. The IMM algorithm switches the vectors ($\hat x_{k - 1}^{01} $ , $\hat x_{k - 1}^{02} )$ to the appropriate manoeuvring models. These segmented inputs are then fed to the UKF-TFS filter. The Two Filter Smoother (TFS) has two filters, one running forward in time and the other running backward in time (Liu et al., Reference Liu, Nassar and El-Sheimy2010). The idea is to obtain a smooth improved estimate by fusion of these forward and backward estimates, and its associated co-variances (Helmik et al., Reference Helmik, Blair and Hoffman1996). The forward and backward estimates produced by the filter are combined to form a suitable estimate in TFS (Fraser and Potter, Reference Fraser and Potter1969). The measurement vector Z _k comprising the position and velocity updates is given to the UKF filter. The IMM smoothed estimate $\hat x_{k,s}^i $ of $x_k^i $ is thus estimated as a linear combination of two IMM filters $\hat x_{k,f\;} ^i $ and $\hat x_{k,b}^i $. Let $\tilde x_{k,s}^i $ be the IMM smoothed estimate error given by:

(15)$$\tilde x_{k,s}^i = \hat x_{k,s}^i - x_k^i $$

The IMM smoothed estimate is given by

(16)$$\hat x_{k,s}^i = k_1 \hat x_{k,f}^i + k_2 \hat x_{k,b}^i $$

From Equation (15),

(17)$$x_k^i + \tilde x_{k,s}^i = k_1 (x_k^i + \tilde x_{k,f}^i ) + k_2 (x_k^i + \tilde x_{k,b}^i )$$

In Equation (17), $\tilde x_{k,f}^i, \tilde x_{k,b}^i $ are the estimated errors of the IMM forward and backward filter respectively.

(18)$$\tilde x_{k,s}^i = (k_1 + k_2 - I)x_k^i + k_1 \tilde x_{k,f}^i + k_2 \tilde x_{k,b}^i $$

For an unbiased estimate, $E(\tilde x_{k,s}^i ) = 0$,

(19)$$k_1 + k_2 - I = 0$$

(20)$$k_2 = I - k_1 $$

Substituting k ₂ in Equation (16), the smoothed estimate is given by

(21)$$\hat x_{k,s}^i = k_1. \hat x_{k,f}^i + (I - k_1 )\hat x_{k,b}^i $$

Also it can be written as

(22)$$\hat x_{k,s}^i = \hat x_{k,b}^i + k_1 (\hat x_{k,f}^i - \hat x_{k,b}^i )$$

Next, the error co-variance of the smoothed estimate is obtained by

(23)$$\tilde x_{k,s}^i = k_1 \tilde x_{k,f}^i + k_2 \tilde x_{k,b}^i = k_1 \tilde x_{k,f}^i + (I - k_1 )\tilde x_{k,b}^i $$

(24)$$P_{k,s}^i = k_1 P_{k,f}^i. k_1^T + (I - k_1 )P_{k,b}^i (I - k_1 )^T $$

By minimizing the Equation (24) for gain K ₁

(25)$$2k_1 P_{k,f}^i - 2(I - k_1 )P_{k,b}^i = 0$$

(26)$$k_1 = P_{k,b}^i (P_{k,f}^i + P_{k,b}^i )^{ - 1} $$

(27)$$I - k_1 = I - P_{k,b}^i (P_{k,f}^i + P_{k,b}^i )^{ - 1} $$

(28)$$I - k_1 = P_{k,f}^i (P_{k,f}^i + P_{k,b}^i )^{ - 1} $$

Substituting Equations (26) and (28) in the Equation (24)

(29)$${P_{k,s}^i} ^{ - 1} = {P_{k,f}^i} ^{ - 1} + {P_{k,b}^i} ^{ - 1} $$

Thus, the equation for smoothed co-variance of forward and backward IMM filter can be written as,

(30)$$P_{k,s}^i = [{P_{k,f}^i} ^{ - 1} + {P_{k,b}^i} ^{ - 1} ]^{ - 1} $$

Equation (30) proves that the smoothed uncertainty co-variance is less than the uncertainty co-variance of both forward and backward IMM-UKF. The equation for the IMM smoothed estimate is obtained by substituting (26) and (28) in Equation (21)

(31)$$\hat x_{k,s}^i = P_{k,b}^i (P_{k,f}^i + P_{k,b}^i )^{ - 1}. \hat x_{k,f}^i + P_{k,f}^i. (P_{k,f}^i + P_{k,b}^i )^{ - 1}. \hat x_{k,b}^i $$

(32)$$\hat x_{k,s}^i = \displaystyle{{P_{k,b}^i. P_{k,f}^i} \over {(P_{k,f}^i + P_{k,b}^i )}}[(P_{k,f}^i )^{ - 1}. \hat x_{k,f}^i + (P_{k,b}^i )^{ - 1}. \hat x_{k,b}^i ]$$

Thus, the IMM smoothed estimate is obtained by using the forward and backward uncertainty co-variance, which is given by

(33)$$\hat x_{k,s}^i = P_{k,s}^i [(P_{k,f}^i )^{ - 1} \cdot \hat x_{k,f}^i + (P_{k,b}^i )^{ - 1} \cdot \hat x_{k,b}^i ]$$

The model conditioned smoothed estimate x _k,s^j and its uncertainty co-variance P _k,s^j is given by the equation

(34)$$x_{k,s}^j = \mathop \sum \limits_{i = 1}^n \mu _{k + 1}^{s,i/j} \hat x_{k,s}^i $$

(35)$$P_{k,s}^j = \mathop \sum \limits_{i = 1}^n \mu _{k + i}^{s,i/j}. \left[ {P_{k,s}^i + \left( {\hat x_{k,s}^i - x_{k,s}^j} \right)\left( {\hat x_{k,s}^i - x_{k,s}^j} \right)^T} \right]$$

The mixed smoothed probability is calculated by

(36)$$\mu _{k + 1}^{s,i/j} = \displaystyle{1 \over {r_j}} p_{\,ji} \Lambda _k^{\,ji} $$

The term r_j is computed by

(37)$${\rm r}_j = \mathop \sum \limits_1^n p_{\,ji} \Lambda _k^{\,ji} $$

The likelihood Λ_k^ji is given by

(38)$$\Lambda _k^{\,ji} = N(\Delta _k^{\,ji}, D_k^{\,ji} )$$

Where,

• N () is the probability function of measurement in innovation distribution.

• $\Delta _k^{\,ji} = \hat x_k^{b,i} - x_k^{\,f,i} $ is equivalent to measurement innovation.

• $D_k^{\,ji} = \hat P_k^{b,i} + P_k^{\,f,i} $ is combined co-variance of forward and backward filter.

• $\hat x_k^{b,i} $, $\hat P_k^{b,i} $ are model conditioned backward-time one-step predicted mean and co-variances.

• x _k^f,i, P_k^f,i are model-conditioned forward-time filtered mean and co-variances.

The overall optimal smoothed estimate and its uncertainty co-variance is given by

(39)$$\hat x_k^s = \mathop \sum \limits_{i = 1}^n \mu _k^{s,j} x_{k,s}^j $$

(40)$$\hat P_k^s = \mathop \sum \limits_{i = 1}^n \mu _k^{s,j}. \left[ {P_{k,s}^j + \left( {x_{k,s}^j - \hat x_k^s} \right)\left( {x_{k,s}^j - \hat x_k^s} \right)^T} \right]$$

The smoothed model probabilities are computed as

(41)$$\mu _k^{s,j} = \displaystyle{1 \over r}r_j \mu _k^j $$

In Equation (41), μ _k^j is the forward time filtered model probability and the term ‘r’ is the normalization constant that is given by

(42)$$r = \mathop \sum \limits_{\,j = 1}^n r_j \mu _k^j $$

Figure 1.

Block diagram of IMM-UKF-TFS.

2.3. GPS-INS Integration

GPS-INS integration can be achieved by combining the GPS and INS state vectors (Hide et al., Reference Hide, Moore and Smith2003; Almagbile and Wang, Reference Almagbile and Wang2010) as shown in Figure 2. The difficulty in the modelling of the INS sensor errors has led to the integration of INS and GPS sensors. The difference (δZ _k) between the output of INS and the output of GPS (Z _k) is given to the input of the IMM-UKF-TFS to calculate the gain factor. The gain factor is used to update the INS position vectors to obtain the corrected INS position.

Figure 2.

GPS-INS integrated navigation using IMM-UKF-TFS.

3.1. Constant Velocity Model

The Constant Velocity model has position and velocity components of the vehicle along x and y directions. The state vector of the Constant Velocity model is given by

(46)$$x(k) = [x_e (k)y_n (k) \dot x_e (k)\dot y_n (k)]^T $$

The state space representation of the Constant Velocity model is

(47)$$\left[{\matrix{ {x_e (k)} \cr {y_n (k)} \cr {\dot x_e (k)} \cr {\dot y_n (k)} \cr}} \right] = \; \left[ {\matrix{ 1 & 0 & T & O \cr 0 & 1 & 0 & T \cr 0 & 0 & 1 & 0 \cr 0 & 0 & 0 & 1 \cr}} \right]\left[{\matrix{ {x_e (k - 1)} \cr {y_n (k - 1)} \cr {\dot x_e (k - 1)} \cr {\dot y_n (k - 1)} \cr}} \right] + \left[{\matrix {1/2 T^2 & 0 \cr T & 0 \cr 0 & 1/2 T^2 \cr 0 & T \cr}} \right]w(k - 1)$$

The observation matrix is given by

(48)$$H = \left( {\matrix{ 1 & {\matrix{ 0 & 0 & 0 \cr}} \cr 0 & {\matrix{ 1 & 0 & 0 \cr}} \cr}} \right)$$

3.2. Coordinated Turn Model (Circular Turn Model)

The Coordinated Turn model has position and velocity components of the vehicle along x and y directions and a turn rate parameter \rmOmega. The state vector of the Coordinated Turn model is given by,

(49)$$x(k) = [x_e (k)y_n (k)\dot x_e (k)\dot y_n (k)\; \; \Omega (k)]^T $$

The state space representation of the Coordinated Turn model is given by,

(50)$$ \eqalign{ \left[ {\matrix{ {x_e (k)} \cr {y_n (k)} \cr {\dot x_e (k)} \cr {\dot y_n (k)} \cr {\Omega (k)} \cr}} \right] = & \; \left[ {\matrix{ 1 & 0 & {\displaystyle{{\sin \Omega T} \over \Omega}} & { - \displaystyle{{1 - \cos\Omega T} \over \Omega}} & 0 \cr 0 & 0 & {\cos\Omega T} & {\sin \Omega T} & 0 \cr 0 & 1 & {\displaystyle{{1 - \cos\Omega T} \over \Omega}} & {\displaystyle{{\sin \Omega T} \over \Omega}} & 0 \cr 0 & 0 & {\sin \Omega T} & {\cos\Omega T} & 0 \cr 0 & 0 & 0 & 0 & 1 \cr}} \right]\left[ {\matrix{ {\matrix{ {\matrix{ {x_e (k - 1)} \cr {y_n (k - 1)} \cr}} \cr {\dot x_e (k - 1)} \cr {\dot y_n (k - 1)} \cr}} \cr {\Omega (k - 1)} \cr}} \right] \cr & + \left[ {\matrix {1/2 T^2 & 0 \cr T & 0 \cr 0 & 1/2 T^2 \cr 0 & T \cr 0 & 0 \cr}} \right]w\left( {k - 1} \right)} $$

The observation matrix is given by

(51)$$H = \left( {\matrix{ {\matrix{ 1 & 0 & 0 \cr}} & {\matrix{ 0 & 0 \cr}} \cr {\matrix{ 0 & 1 & 0 \cr}} & {\matrix{ 0 & 0 \cr}} \cr}} \right)$$

3.3. Constant Acceleration Model

The Constant Acceleration model has position, velocity and acceleration components of the vehicle along x and y directions. The state vector of the Constant Acceleration model is given by

(52)$$x(k) = [x_e (k)y_n (k)\dot x_e (k)\dot y_n (k)\ddot x_e (k)\ddot y_n (k)]^T $$

In Equations (46) to (50), x _e(k),y _n(k) represent position parameters in east and north directions, $\dot x_e (k),\ \dot y_n (k)\; $ are the velocity parameters in east and north directions and $\ddot x_e (k),\ddot y_n (k)$ are the acceleration parameters in east and north directions respectively. The state space representation of the Constant Acceleration model is given by,

(53)$$\left[ {\matrix{ {x_e (k)} \cr {y_n (k)} \cr {\dot x_e (k)} \cr {\dot y_n (k)} \cr {\ddot x_e (k)} \cr {\ddot y_n (k)} \cr}} \right] = \left[ {\matrix{ {\matrix{ {\matrix{ 1 \cr 0 \cr 0 \cr}} \cr 0 \cr {\matrix{ 0 \cr 0 \cr}} \cr}} & {\matrix{ {\matrix{ 0 \cr 1 \cr 0 \cr}} \cr 0 \cr {\matrix{ 0 \cr 0 \cr}} \cr}} & {\matrix{ {\matrix{ {\matrix{ T \cr 0 \cr 1 \cr}} \cr 0 \cr {\matrix{ 0 \cr 0 \cr}} \cr}} & {\matrix{ {\matrix{ 0 \cr T \cr 0 \cr}} \cr 1 \cr {\matrix{ 0 \cr 0 \cr}} \cr}} & {\matrix{ {\matrix{ {\matrix{ {T^2 /2} \cr 0 \cr T \cr}} \cr 0 \cr {\matrix{ 1 \cr 0 \cr}} \cr}} & {\matrix{ {\matrix{ 0 \cr {T^2 /2} \cr 0 \cr}} \cr T \cr {\matrix{ 0 \cr 0 \cr}} \cr}} \cr}} \cr}} \cr}} \right]\left[ {\matrix{ {x_e (k - 1)} \cr {y_n (k - 1)} \cr {\dot x_e (k - 1)} \cr {\dot y_n (k - 1)} \cr {\ddot x_e (k - 1)} \cr {\ddot y_n (k - 1)} \cr}} \right] + \left[ {\matrix{ {T^3 /6} & 0 \cr {T^2 /2} & 0 \cr T & 0 \cr 0 & {T^3 /6} \cr 0 & {T^2 /2} \cr 0 & T \cr} \;} \right]w(k - 1)$$

The observation matrix is given by

(54)$$H = \; \left( {\matrix{ {\matrix{ 1 & 0 & 0 \cr}} & {\matrix{ 0 & 0 & 0 \cr}} \cr {\matrix{ 0 & 1 & 0 \cr}} & {\matrix{ 0 & 0 & 0 \cr}} \cr}} \right)$$

4.1. Constant Velocity (CV)-Coordinated Turn (CT) Model

The simulated trajectory of the CV-CT model is shown in Figure 3. The simulation scenario is explained as follows. The trajectory of the vehicle motion is modelled to cover up dynamic characteristics such as Constant Velocity and Coordinated Turn as shown in Table 1. The vehicle starts from the origin. At 4^ths, the vehicle starts to turn left with turn rate Ω=1. At 9^ths, the vehicle stops turning and moves in a linear manner for 2 seconds. At 11^ths, the vehicle starts to turn right with rate Ω=−1. At 16^ths, the vehicle stops turning and moves linear manner for 4 seconds with the same velocity. The position estimates of the vehicle using the different estimation methods are plotted in Figure 4. The Mean Square Errors (MSEs) of position estimates are listed in Table 4.

Figure 3.

Two-dimensional trajectory of simulated vehicle.

Figure 4.

Estimates of the vehicle position.

Table 1.

CV-CT modelled vehicle trajectory.

In the time interval, where the vehicle follows the CV model, the estimates of the corresponding UKF are approximately equal. But the multiple model-smoothed estimation utilizes more measurements in forward as well as backward estimation; the resultant smoothed estimates of IMM-UKF-TFS are better than IMM-UKF and UKF. In the time interval where the vehicle follows the Coordinated Turn model, the mismatches of the model leads to a reduction in performance of both single model filters such as UKF and multiple model filters such as IMM-UKF. However, IMM-UKF-TFS is more efficient in detecting the changes in vehicle dynamics, prevents the divergence and achieves better positional accuracy.

Figures 5 and 6 give the variation of CV model and CT model probability for both the IMM filter and IMM smoothed filter. Figures 5 and 6 give a clear idea about the changes in model probability between the CV and CT models with respect to the actual vehicle dynamics. The probability weight value gives higher priority to the CV model when the vehicle is in the non-manoeuvring region, and gives higher priority to the CT model when the vehicle is in the manoeuvring region. Figures 7 and 8 give the positional error of the vehicle using UKF, IMM-UKF, RTSS and IMM-UKF-TFS approaches in north and east directions. This shows that the estimate of IMM-UKF-TFS converge to the true value so that the error becomes less.

Figure 5.

Constant Velocity model probability.

Figure 6.

Coordinated Turn model probability.

Figure 7.

Vehicle position estimate error in north.

Figure 8.

Vehicle position estimate error in east.

4.2. Constant Velocity (CV)-Constant Acceleration (CA) Model

The simulated trajectory of the CV-CA model is shown in Figure 9. The simulation scenario is as follows: The trajectory of the vehicle motion is modelled to cover dynamic characteristics such as Constant Velocity and Constant Acceleration as shown in Table 2. The trajectory is simulated for 200 time steps with step size T=0·1. The movement of the vehicle is described below. The vehicle starts from origin with velocity and acceleration $\left( {\dot x,\dot y,\ddot x,\ddot y} \right)$=(0, 1, 0, 1). At 4^ths, the vehicle starts to move in the east direction with a constant velocity. At 10^ths, the vehicle moves in the north direction for 2 seconds with a constant total velocity of one. At 12^ths, the vehicle starts move in a northeast direction with constant velocity for 6 seconds. At 19^ths, the vehicle moves with the constant acceleration in east direction. Figure 10 shows the estimates of different position estimation techniques.

Figure 9.

Two-dimensional trajectory of simulated vehicle.

Figure 10.

Estimates of vehicle position.

Table 2.

CV-CA modelled vehicle trajectory.

Figures 11 and 12 give a clear idea about the changes in the model probability between the CV and CA models with respect to the actual vehicle dynamics. The probability weight value of the CV model is more when the vehicle is in the non-manoeuvring region and the weight value of the CA model is more when the vehicle is in the manoeuvring region. Figures 13 and 14 show the positional error of the vehicle in the north and east directions. The vehicle's position error estimate shows that the proposed approach gives the most accurate navigation estimate in the vehicle's manoeuvring and non-manoeuvring region, when compared to the other estimates.

Figure 11.

Constant Velocity model probability.

Figure 12.

Constant Acceleration model probability.

Figure 13.

Vehicle position estimate error in north.

Figure 14.

Vehicle position estimate error in east.

4.3. Constant Acceleration (CA) and Coordinated Turn (CT) Model

The simulated trajectory of the CA-CT model is shown in Figure 15. The simulation scenario is explained as follows. The trajectory of the vehicle motion is modelled to cover dynamic characteristics such as Constant Acceleration and Coordinated Turn as shown in Table 3. The trajectory is simulated for 200 time steps with step size T=0·1. The movement of vehicle is as follows: The vehicle starts from origin with acceleration $(\ddot x,\ddot y)$=(1, 0). At 2 s, the vehicle starts to turn left with rate Ω=1. At 9^ths, vehicle stops turning and moves for 4 seconds with a constant total acceleration of one. At 13^ths, vehicle starts to turn left with rate Ω=1. At 19^ths, vehicle stops turning and moves for 2 seconds with the same acceleration. Figure 16 shows the estimates of different position estimation techniques.

Figure 15.

Two dimensional trajectory of simulated vehicle.

Figure 16.

Estimates of vehicle position.

Table 3.

CA-CT modelled vehicle trajectory.

Figures 17 and 18 give an idea of the changes in model probability between the Constant Acceleration and Coordinated Turn models with respect to the actual vehicle dynamics. The probability weight value gives higher priority to the CA model when the vehicle is in the non-manoeuvring region and gives higher priority to the CT model when the vehicle is in the manoeuvring region. Figures 19 and 20 show the positional error of the vehicle in north and east directions respectively. The vehicle's positional error estimate shows that IMM-UKF-TFS approach gives the most accurate navigation estimate of the vehicle's accelerating and turning region compared to the other techniques. In Table 4, the MSEs of position estimates produced by the tested methods are listed. It can be seen that the estimates produced by the proposed IMM-UKF-TFS using CA-CT model are better than those produced by the IMM-UKF and single model UKF.

Figure 17.

Constant Acceleration model probability.

Figure 18.

Coordinated Turn model probability.

Figure 19.

Vehicle position estimate error in north.

Figure 20.

Vehicle position estimate error in east.

Table 4.

Comparison of Mean Square Error of different estimation methods for CV-CT, CA-CT and CV-CA models.

The comparison of Mean Square Error estimates of various combinations of motion models are given in Table 4.