Loosely Coupled INS/GPS Integration with Constant Lever Arm using Marginal Unscented Kalman Filter

Abstract

A loosely coupled Inertial Navigation System (INS) and Global Positioning System (GPS) are studied, particularly considering the constant lever arm effect. A five-element vector, comprising a craft's horizontal velocities in the navigation frame and its position in the earth-centred and earth-fixed frame, is observed by GPS, and in the presence of lever arm effect, the nonlinear observation equation from the state vector to the observation vector is established and addressed by the correction stage of an unscented Kalman filter (UKF). The conditionally linear substructure in the nonlinear observation equation is exploited, and a computationally efficient refinement of the UKF called marginalized UKF (MUKF) is investigated to incorporate this substructure where fewer sigma points are needed, and the computational expense is cut down while the high accuracy and good applicability of the UKF are retained. A performance comparison between UKF and MUKF demonstrates that the MUKF can achieve, if not better, at least a comparable performance to the UKF, but at a lower computational expense.

1. INTRODUCTION

An Inertial Navigation System (INS) is entirely self-contained within the vehicle, in the sense that it is not dependent on the transmission of signals from the vehicle or reception from an external source. However the performance of an INS is characterised by an unbounded drift in the error of the navigation results. This problem can be addressed by using sensors that are more accurate, but are, as a result, much more expensive. Low cost is thus a basic requirement along with high accuracy, and often these requirements conflict. Thus it is often not economic to use more accurate sensors to get accurate long-term navigation results (Titterton and Weston, 2004). Alternatively, an external source of navigation information can be employed to aid the INS. The Global Positioning System (GPS), or other global navigation satellite systems (GNSS) (Yang et al., 2011), can provide drift-free position and velocity with good accuracy. However, the main drawbacks of GPS are threefold: the signal can be jammed; the outputs are at a relatively low rate; and accurate attitude information cannot be provided through single-antenna GPS receivers. However GPS can be used to aid INS due to their complementary attributes (Jwo et al., 2012). It can be expected to obtain improved navigation results with better reliability, latency, bandwidth and update rate through the integration of INS and GPS.

The celebrated Kalman filter (KF) is often chosen as the information fusion algorithm in integrated navigation. If the phase centre of the GPS antenna and the geometrical centre of the IMU do not coincide with each other, which is often the case, the lever arm effect must be considered. The observation equation considering the lever arm effect is very nonlinear (Geng et al., 2011). Nonlinearity in the state and/or measurement equations can be addressed by the extended Kalman filter (EKF). However the simple “first order Taylor series truncation” in the EKF has several flaws which have been revealed by many scholars and engineers, see e.g. (Julier et al., 2000; Jwo and Lai, 2009).

There has been intensive interest in proposing new nonlinear filters to address the aforementioned flaws of EKF since around 2000. Many of these new filters fall into the class of sample-based ones in which the derivatives or the Jacobian matrices of the nonlinear equations are no longer needed. One of these filters is called the unscented Kalman filter (UKF) which is based on the intuition that it is easier to approximate a Gaussian distribution than it is to approximate an arbitrary nonlinear function or transformation (Julier et al., 2000). The finite-difference-based central difference filter (CDF) (Ito and Xiong, 2000) and Sterling-polynomial-interpolation-based divided difference filter (DDF) (Norgaard et al., 2000) were proposed independently and almost simultaneously. In Van Der Merwe (2004), UKF, CDF, and DDF are treated as a family of algorithms called sigma point Kalman filters (SPKF). Lefebvre et al. (2002), revisited the UKF from the perspective of statistical linear regression through sampling. Lerner reveals that the core of UKF, called the unscented transformation (UT), is essentially the Gaussian quadrature rule for third order monomials (Lerner, 2002), and the fifth order UKF is proposed implying that the standard UKF is just the third order one. A review of nonlinear filters from the perspective of numerical integrations can be found in Wu et al. (2006). Arasaratnam and Hawkin (2009) proposed the so-called cubature Kalman filter (CKF) through the third order spherical-radial cubature rule. It has been pointed out in several papers that CKF can be seen, in some sense, as a special form of UKF, see e.g. Duník et al. (2012). Quite recently a high-degree CKF was proposed by Jia et al. (2012) based on the fifth order spherical-radial cubature rule which is similar to the fifth order UKF in Lerner (2002) and Wu et al. (2006).

The superiority of these SPKFs are at some cost of heavier computational load, because square roots of matrices are needed to generate sigma points whose number is usually 2n+1 with n being the dimension of the state, and moreover nonlinear function evaluations should be carried out at all these points, as opposed to EKF in which these evaluations are carried out only once in one recursion. So if both UKF and EKF can meet the accuracy requirement, the latter is preferred (Daum, 2005). The dimension related problems can be addressed to some extent with the simplex UKF whose sigma points are fewer but can only give degraded performance (Julier, 2003).

In many cases there is some linear substructure in the nonlinear function under consideration, i.e. the function is nonlinear only in some of the state elements while the remaining part is (conditionally) linearly mapped to the functional values. By exploiting this special structure in the nonlinear function, a refined form of UKF called marginalized UKF (MUKF) can be adopted without degrading the performance, where sigma points are generated only to capture the “nonlinear” part of the state elements, and a reduced number of sigma points can result in a reduction in the computation burden (Chang, 2012; Morelande & Moran, 2007).

In an integrated INS/GPS system, the accumulating INS errors are effectively restrained through periodical integration with GPS observations. Also, the INS calculation is at a high rate, with a small update interval within which the errors can be small values. So the linear error equation based on small angle errors is constructed. The lever arm effects on position and velocity are expressed in earth-centred-earth-fixed and navigation frames, respectively (Geng et al., 2011). Under refined perceptions, it is pointed out that the observations are a nonlinear function of only eight elements of the 13-element state vector while the remaining ones, the north and east velocity errors and the three accelerometer biases, are conditionally linearly mapped to the observation vector. The nonlinearity in the observation equation due to the lever arm effect is addressed by the MUKF instead of the standard UKF and the computational cost is relatively lower while attaining the high accuracy and easy applicability desired.

The remainder of the paper is organised as follows. The state equation and observation equation are constructed in Section 2 and Section 3 respectively. The implementation of the MUKF in INS/GPS is presented in Section 4. In Section 5, the performance of MUKF and UKF is compared in simulation studies, and the performance of the former is validated. Some concluding remarks are given in Section 6.

The reference frames used in this paper are briefly introduced here. All the frames are orthogonal, right-handed Cartesian. The inertial frame (i-frame) is an approximate one with its origin at the centre of the earth and axes non-rotating with respect to the fixed stars. The earth-centred-earth-fixed frame (e-frame) has its origin at the centre of the earth and is fixed to the earth. The first axis is along the earth pole and the second axis lies along the intersection of the plane of the Greenwich meridian with the earth's equatorial plane. The navigation frame (n-frame) is a local horizontal one with three axes along north, east, and downward respectively. The body frame (b-frame) is fixed to the vehicle, particularly with the origin at the location of the IMU. Three axes point to forward, rightward, and downward respectively.

2. STATE SPACE MODELLING

The state space dynamic model is composed of the INS error model and inertial sensor error model. In this section INS attitude, velocity and position error equations and inertial sensor bias and noise models are presented, and the discretisation of the continuous model is also studied at the end of this section.

2.1. INS error models

For the sake of simplicity, the INS error equations are listed as follows. Details about the derivation of these equations can be found in many textbooks on INS, see e.g. Titterton and Weston, (2004. P.342). This has

(1)$$\dot \phi {\rm = -} \omega _{in}^n \times \phi - \delta \omega _{in}^n - C_b^n \delta \omega _{ib}^b $$

(2)$$\delta \dot v{\rm =} C_b^n f^b \times \phi + C_b^n \delta f^b - (2\omega _{ie}^n + \omega _{en}^n ) \times \delta v - (2\delta \omega _{ie}^n + \delta \omega _{en}^n ) \times v - \delta g$$

(3)$$\delta \varphi = \displaystyle{{\delta v_N} \over {R + h}},\;\delta \lambda = \displaystyle{{\delta v_E} \over {(R + h)\cos \varphi}} + \displaystyle{{v_E \sin \varphi \delta \varphi} \over {(R + h)\cos ^2 \varphi}} $$

where ϕ is the angle error vector, whose elements can be considered as roll, pitch and yaw; ω _xy^z denotes the angular velocity of the y frame with respect to the x frame expressed in the z frame; C _x^y, called the attitude matrix or direction cosine matrix (DCM), can be used to transform the coordinates of one vector in the x frame to the coordinates of this same vector in the y frame; v, called ground velocity, is the velocity of the b frame with respect to the e frame expressed in the n frame; f^b is the specific force expressed in the b frame; g is the gravity vector expressed in the n frame; φ is the latitude; λ is the longitude; h is the height; R is the radius of the earth; a dot above a value denotes the time derivative and a delta before a value denote the errors in this value. Note that ω _ib^b and f^b can be measured by the gyro-triad and accelerometer-triad in the inertial measurement unit and the earth is assumed to be a sphere for the sake of simplicity.

2.2. Inertial sensor models

δω _ib^b and δf^b represent the errors in the gyro-measured angular rate and the accelerometer-measured specific force respectively. These errors are due to the imperfections in the inertial sensors. There are many error sources in inertial sensors including the fixed biases, in-run random biases, g-dependent biases, scale-factor errors and misalignment errors. Part of these error sources can be modelled and calibrated through testing and the measurement of these sensors can be compensated with the testing results. The residual errors after calibration and compensation can be effectively modelled as the sum of one constant term and one random term, known as biases and noises respectively, i.e.

(4)$$\delta \omega _{ib}^b = \varepsilon + n_g, \;\delta f^b = {\it\Delta} + n_a $$

(5)$$\dot \varepsilon = 0,\;\dot \Delta = 0$$

where ε and Δ are biases in the measurements of gyro and accelerometer respectively. Similarly n _g and n _a are noises in these measurements which are always modelled as white, zero-mean, and normally distributed noises.

Since it is usually more intuitive to know how the noise affects the calculated angle and velocity, it is common for manufacturers to specify noises using angle random walk (ARW) or velocity random walk (VRW) (Woodman, 2007). The relation between the random walk and the noise, taking the ARW as an example, is as follows

(6)$$\sigma _\theta (t) = \sqrt {{\mathop{\rm var}} \left( {\int_0^t {\varepsilon (\tau )d\tau}} \right)} \approx \sqrt {{\mathop{\rm var}} \left( {\delta t\sum\limits_{i = 1}^n {N_i}} \right)} = \sqrt {(\delta t)^2 n\;{\mathop{\rm var}} (N)} = \sigma \sqrt {\delta t \cdot t} $$

(7)$$ARW = \sigma _\theta (1) = \sigma \sqrt {\delta t} $$

where N _i is the ith random variable in the white noise sequence with σ as its standard deviation, δt is the sample interval in the sensor testing. The relation between the ARW and the power spectral density (PSD) of ε is

(8)$$PSD = 3600(ARW)^2 $$

with (°/h)²/Hz and ${\rm {\curr { \char "000B0}} {\char "0002F}}\sqrt {\rm h} $ as the units of PSD and ARW respectively.

2.3. State space models and discretisation

Equations (1)–(5) can be combined to form a single error equation in the vector form as

(9)$$\dot x = Ax + Gu$$

where x and u are the state vector and noise vector respectively,

(10)$$x = [\matrix{ {\phi _N} & {\phi _E} & {\phi _D} & {\delta v_N} & {\delta v_E} & {\delta \varphi} & {\delta \lambda} & {\varepsilon _x} & {\varepsilon _y} & {\varepsilon _z} & {{\it\Delta} _x} & {{\it\Delta} _y} & {{\it\Delta} _z} \cr} ]^T $$

(11)$$u = [\matrix{ {n_{g,x}} & {n_{g,y}} & {n_{g,z}} & {n_{a,x}} & {n_{a,y}} & {n_{a,z}} \cr} ]^T $$

The details of the corresponding matrices A and G can be found in textbooks such as Titterton and Weston (2004. P. 345).

Note that the vertical components of velocity and position errors are not included. The pure inertial vertical channel is numerically unstable and consequently certain height or barometric measurements should be blended with the INS data, so these downward velocity errors and the height or depth error are not included in the state vector (Bekir, 2007). It should also be noted that all the values in Equation (9) are essentially time dependent values, and the time indices are omitted for the sake of simplicity.

Equation (9) can be converted into discrete time form as in Bekir (2007):

(12)$$x_{k + 1} = F_k x_k + u_k $$

(13)$$F_k = \exp (A\delta t) \approx I + A\delta t + 0 {\cdot} 5A^2 (\delta t)^2 $$

(14)$$Q_k = E\{ u_k u_k^T \} \approx \exp (0{\cdot}5A\delta t)GQG^T \exp (0{\cdot}5A^T \delta t)\delta t$$

where Q=E{uu^T}; the E operator represents the mathematical expectation.

3. OBSERVATION MODELLING

Since the GPS antenna phase centre is not located at the same location as the IMU centre, the velocity and position GPS observations correspond to not only the translational state, i.e. the velocity and the position, but also the rotational state, i.e. the attitude of the vehicle, or the body frame.

Assume that the north and east velocities are observed (Geng et al., 2011) as

(15)$$\eqalign{ v_{GPS}^n &= v^n + C_b^n \omega _{nb}^b \times l^b + n_{vGPS} \cr & = v^n + ( - \omega _{en}^n - \omega _{ie}^n + C_b^n \omega _{ib}^b ) \times l^b + n_{vGPS}} $$

where v _GPSⁿ and n _vGPS are the GPS measured velocity and the corresponding noise expressed in the n frame, l^b is the 3D constant lever arm vector expressed in the b frame.

Since it is difficult to express the lever arm effect on the positions of the GPS and IMU represented in their latitude, longitude, and height, the position observation model is represented in the e frame (Geng et al., 2011) as

(16)$$p_{GPS}^e = p^e + C_n^e C_b^n l^b + n_{GPS} $$

with

(17)$$C_b^n = (I + [\phi \times ])C_{b,INS}^n $$

(18)$$[\phi \times ] = \left[ {\matrix{ 0 & { - \phi _D} & {\phi _E} \cr {\phi _D} & 0 & { - \phi _N} \cr { - \phi _E} & {\phi _N} & 0 \cr}} \right]$$

(19)$$\omega _{en}^n = \left[ {\matrix{ {\displaystyle{{v_E} \over {R + h}}} & {\displaystyle{{ - v_N} \over {R + h}}} & {\displaystyle{{ - v_E \tan \varphi} \over {R + h}}} \cr}} \right]^T $$

(20)$$C_e^n = \left[ {\matrix{ {\cos \varphi} & { - \sin \varphi \sin \lambda} & {\sin \varphi \cos \lambda} \cr 0 & {\cos \lambda} & {\sin \lambda} \cr { - \sin \varphi} & { - \cos \varphi \sin \lambda} & {\cos \varphi \cos \lambda} \cr}} \right]$$

(21)$$\omega _{ie}^n = C_e^n \omega _{ie}^e = C_e^n \left[ {\matrix{ {\omega _{ie}} \cr 0 \cr 0 \cr}} \right] = \left[ {\matrix{ {\cos \varphi} \cr 0 \cr { - \sin \varphi} \cr}} \right]\omega _{ie} $$

(22)$$p^e = \left( {\matrix{ {x^e} \cr {y^e} \cr {z^e} \cr}} \right) \approx \left( {\matrix{ {(R + h)\cos \varphi \cos \lambda} \cr {(R + h)\cos \varphi \sin \lambda} \cr {(R + h)\sin \varphi} \cr}} \right)$$

The true values in the above equations, such as v _N, v_E, φ, and λ, as the ones to be estimated, are certainly not available and are represented as the sums of the INS-estimated values and the corresponding errors, e.g.

(23)$$v_N = v_{N,INS} + \delta v_N $$

Equations (15) and (16) are combined into a vector function

(24)$$y_k = h(x_k ) + n_k $$

It can be concluded from Equation (15) that the velocity observation is the nonlinear function of attitude errors, gyro biases, and from Equation (16) that the position observation is the nonlinear function of the latitude and longitude errors, attitude errors and gyro biases. So as a whole, in Equation (24), the five-element observation vector is nonlinearly related to the attitude errors, the latitude and longitude errors, and the gyro biases, with the remaining part of the state vector, while the north and east velocity error and the accelerometer biases are linearly mapped to the observation vector.

4. KALMAN FILTER DESIGN

The Kalman filter, providing the optimal estimation in the sense of minimum mean squared errors (Anderson and Moore, 1979), has been recognized as one of the most powerful state estimation techniques, and has been successfully implemented in INS/GPS integrated systems. In this section, the basic KF/EKF and the standard UKF are briefly introduced in the first two subsections, and then, the MUKF is derived and the hybrid implementation of MUKF used in the INS/GPS integrated system is summarised in the last two sections.

4.1. Kalman filter and extended Kalman filter

The KF is a two-stage algorithm, i.e. the time update and the observation update, also termed as the prediction and the correction. Assuming that the state space equation and observation equation are both linear, and with additive noises

(25)$$x_k = F_{k - 1} x_{k - 1} + w_{k - 1} $$

(26)$$y_k = H_k x_k + v_k $$

w _k−1, v _k are process and measurement noises respectively, both of which are zero-mean uncorrelated Gaussian white noise satisfying E[w _kw_j^T]=Q _kδ_kj, E[v _kv_j^T]=R _kδ_kj, E[w _kv_j^T]=0, where Q _k and R _k are corresponding covariances, and δ _kj is the Kronecker delta function whose value is one when k=j, and zero otherwise. The basic discrete KF is summarized as follows:

ALGORITHM 1

A. Initialization

The initial estimate of the state vector and covariance matrix are given or assumed as $\hat x_0^ + $ and P ₀⁺ which are independent with the noises at any instance.

B. Prediction

Getting the a priori mean and covariance, for k=1, 2, …

(27)$$\hat x_k^ - = F_{k - 1} \hat x_{k - 1}^ +, \;P_k^ - = F_{k - 1} P_{k - 1}^ + F_{k - 1}^T + Q_{k - 1} $$

C. Correction

Getting the a posteriori mean and covariance,

(28)$$P_{xy} = P_k^ - H_k^T, \;P_{yy} = H_k P_k^ - H_k^T + R_k $$

(29)$$K_k = P_{xy} (P_{yy} )^{ - 1} $$

(30)$$\hat x_k^ + = \hat x_k^ - + K_k (\tilde y_k - H_k \hat x_k^ - ),\;P_k^ + = P_k^ - - K_k P_{yy} K_k^T $$

D. Recursion

If it does not terminate, k←k+1, and go to B.

If the state space and/or observation equations are nonlinear, EKF can be adopted, in which the state space equation is linearized at $\hat x_{k - 1}^ + $ to get F _k−1 and/or the observation equation is linearized at $\hat x_k^ - $ to get H _k, and then Algorithm 1 can be implemented.

4.2. Standard unscented Kalman filter

As stated in previous sections, the simple first-order linearization in EKF may introduce large errors in the transformation of mean and covariance through nonlinear equations and the SPKFs can address this problem. In this subsection the SPKF, or specifically UKF is introduced in an intuitive manner, i.e. the deterministically sampling manner adopted by Julier et al. (2000). It is noted that UKF, or specifically its core, UT, bears other, more theoretical meanings (see the “introduction” section). The UT is firstly introduced as follows, and then incorporated into the KF scheme as stated in Algorithm 1 to get the UKF.

Without loss of generality, UT addresses the following problem: given an arbitrary nonlinear function with additive zero-mean normally distributed noise y=f(x)+η, which can be the state and/or observation equations in the filtering problem, and the mean $\bar x$ and the covariance P^xx, the mean $\bar y$ and covariance P^yy are to be estimated.

Firstly an ensemble of 2n+1 samples, called sigma points, with corresponding weights, is generated deterministically, where n is the dimension of x.

(31)$$\eqalign{ & \chi _0 = \bar x \cr & \chi _i = \bar x + \left[ {\sqrt {(n + \lambda )P_{xx}}} \right]_i, \quad i = 1,2, \cdots, n \cr & \chi _{i + n} = \bar x - \left[ {\sqrt {(n + \lambda )P_{xx}}} \right]_i, \quad i = 1,2, \cdots, n} $$

where $\left[ {\sqrt {(n + \lambda )P_{xx}}}\hskip2 \right]_i $ is the ith column or row of the matrix $\sqrt {(n + \lambda )P_{xx}} $ which is the square root of the matrix (n+λ)P _xx. The square root of a matrix can be found through matrix decompositions, such as the efficient Cholesky decomposition. The associated weights of the above sigma points are

(32)$$\eqalign{ & W_0^{(m)} = \displaystyle{\lambda \over {n + \lambda}} \cr & W_0^{(c)} = \displaystyle{\lambda \over {n + \lambda}} + 1 - \alpha ^2 + \beta \cr & W_i^{(m)} = W_i^{(c)} = \displaystyle{\lambda \over {2(n + \lambda )}},\;\;i = 1,2, \cdots, 2n} $$

where W ^(m) and W ^(c) are used to weigh the sigma points in the calculation of propagated mean and covariance respectively. The scaling parameter λ is defined as

(33)$$\lambda = \alpha ^2 (n + \kappa ) - n$$

The tuning parameters α, β and κ are used to tune the UT. Different parameters result in a different sigma-points construction strategy that differentiates the UTs and their corresponding accuracies, numerical stabilities, computational cost, etc.

Then, every sigma point is propagated through a nonlinear filter to yield an ensemble of sigma points capturing the statistics of y

(34)$$\gamma _i = f\,(\chi _i ),\;i = 0,1,2, \cdots, 2n$$

The mean and covariance matrix of y are approximated by the weighted mean and covariance of the above transformed sigma points as

(35)$$\bar y \approx \sum\limits_{i = 0}^{2n} {W_i^{(m)} \gamma _i} $$

(36)$$P_{yy} \approx \sum\limits_{i = 0}^{2n} {W_i^{(c)} (\gamma _i - \bar y)(\gamma _i - \bar y)^T} + R$$

(37)$$P_{xy} \approx \sum\limits_{i = 0}^{2n} {W_i^{(c)} (\chi _i - \bar x)(\gamma _i - \bar y)^T} $$

Implementing the above UT approach into the scheme of the KF gives the UKF method which can effectively address the nonlinearities in the state and/or observation equations.

4.3. Marginalised unscented Kalman filter

The aforementioned discussion about the UKF or specifically about the UT is only the general case, however, for some nonlinear problems with special structure, some ingenious versions of the UT with both non-degrading accuracy and less computational burden can be constructed. The marginalized UT (MUT) used in the MUKF is just a refined form of UT derived by exploiting the conditionally linear substructure in the nonlinear equations.

The MUT is proposed to address the transformation of the mean and covariance through the following nonlinear equation

(38)$$y = f\,(x) + n = f\left( {\left[ {\matrix{ a \cr b \cr}} \right]} \right) + n = f_1 (a) + f_2 (a)b + n$$

where f ₁(·) is a nonlinear function while f ₂(·) is an arbitrary function.

Before the derivation of the MUT, a lemma called conditionally Gaussian distribution Lemma, that for two jointly Gaussian random variables a and b the conditional density p(b|a) is also Gaussian, is presented.

Lemma 4.1. Let the pair of vectors a and b be jointly Gaussian, i.e. with

(39)$$x = [\matrix{ a & b \cr} ]^T $$

(40)$$m_x = [\matrix{ {m_a} & {m_b} \cr} ]^T $$

(41)$$P_{xx} = \left[ {\matrix{ {P_{aa}} & {P_{ab}} \cr {P_{ba}} & {P_{bb}} \cr}} \right]$$

respectively, then b is conditionally Gaussian on a with mean and covariance

(42)$$m_{b\left| a \right.} = m_b + P_{ba} (P_{aa} )^{ - 1} (a - m_a )$$

(43)$$P_{b\left| a \right.} = P_{bb} - P_{ba} (P_{aa} )^{ - 1} P_{ab} $$

A heuristic proof of Lemma 4·1 can be found in Anderson and Moore (1979. P.25).

It can be seen from Equation (42) that m _b|a is the function of a, so in the following, m _b|a is replaced with m _b|a(a), the transformed mean m _y is as

(44)$$m_y = E(y) = \int {\,f_{12} (a)p(a)da} $$

with

(45)$$f_{12} (a) = f_1 (a) + f_2 (a)m_{b\left| a \right.} (a)$$

As Equation (44) can be explained as calculating the mean of the transformed variable y from the variable a through function f ₁₂(a) expressed as Equation (45), the traditional UT can be applied, similar to Equation (31)–(32), sigma points α _i are calculated based on P _aa, then

(46)$$\gamma _i = f_{12} (\alpha _i ),\;i = 0,1,2, \cdots, 2N_a $$

(47)$$m_y \approx \sum\limits_{i = 0}^{2N_a} {w_i \gamma _i} $$

The covariance of y is

(48)$$P_{yy} = \int {[(\,f_1 _2 (a) - m_y )(\,f_1 _2 (a) - m_y )^T + f_2 (a)P_{b|a} (\,f_2 (a))^T ]p(a)da} + Q$$

and can be approximated as

(49)$$P_{yy} \approx \sum\limits_{i = 0}^{2N_a} {w_i ((\gamma _i - m_y )(\gamma _i - m_y )^T + f_2 (\alpha _i )P_{b|a} (\,f_2 (\alpha _i ))^T )} + Q$$

The cross covariance of x and y is

(50)$$P_{xy} = \int {\left( {\left[ {\matrix{ a \cr {m_{b|a} (a)} \cr}} \right] - m_x} \right)(\,f_1 _2 (a) - m_y )^T p(a)da} + \int {\left[ {\matrix{ 0 \cr {P_{b|a}} \cr}} \right](\,f_2 (a))^T p(a)da} $$

and can be approximated as

(51)$$P_{xy} \approx \sum\limits_{i = 0}^{N_a} {w_i \left( {\left( {\left[ {\matrix{ {\alpha _i} \cr {m_{b|a} (\alpha _i )} \cr}} \right] - m_x} \right)(\gamma _i - m_y )^T + \left[ {\matrix{ 0 \cr {P_{b|a} (\,f_2 (\alpha _i ))^T} \cr}} \right]} \right)} $$

Equations (46), (47), (49) and (51) make up the main procedure of MUT, with which replacing the UT in UKF gives the MUKF.

In standard UT, the decomposition of an N _x×N _x matrix P _xx should be carried out to generate the 2N _x+1 sigma points. In MUT the decomposition of an N _a×N _a matrix P _aa should be carried out to generate the 2N _a+1 sigma points {α _i}, and also in MUT the inversion of P _aa and some matrix multiplications should be carried out as shown in Equation (42) to generate m _b|a(α _i) for every α _i. The computational complexity of UT and MUT in the sigma-point-generate stage can be approximately equivalent. However in the sigma-point-propagating stage, there are only 2N _a+1 function evaluations in MUT while 2N _x+1 in UT, moreover, in the propagated sigma point referencing stage only 2N _a+1 samples are used to calculate the transformed mean and covariance, so there may be considerable computation savings using MUT compared to UT; the bigger N _b is, the more computation savings can be achieved.

4.4. Implementation of MUKF in INS/GPS integration

In this contribution, as stated previously, the linear state space model can be addressed in the same way as that in the KF, and the nonlinear observation model is addressed by MUKF instead of UKF.

The linear state Equation (12) and nonlinear observation Equation (24) represent the state space dynamic model to be filtered. Note that the state vector is rearranged in order as

(52)$$x = [\matrix{ {\phi _N} & {\phi _E} & {\phi _D} & {\delta \varphi} & {\delta \lambda} & {\varepsilon _x} & {\varepsilon _y} & {\varepsilon _z} & {\delta v_N} & {\delta v_E} & {{\it\Delta} _x} & {{\it\Delta} _y} & {{\it\Delta} _z} \cr} ]^T $$

Accordingly, Equations (12) and (24) should be rearranged appropriately. The state space vector in Equation (52) is partitioned into two parts with the first eight entities as a and the remaining as b. So Equation (24) is adjusted to

(53)$$y_k = h(x_k ) + n_k = h_1 (a) + h_2 (a)b + n_k = h_{12} (a) + n_k $$

The main algorithm is presented in Algorithm 2.

ALGORITHM 2

A. Initialization

Assume $\hat x_0^ + $ and P ₀⁺ which are independent with the process and observation noises at any instance;

B. Prediction

Get the a priori mean and covariance at instance k $\hat x_k^ - $ and P _k⁻ from $\hat x_{k - 1}^ + $ and P _k−1⁺ as in Equation (27);

C. Correction

Calculate the conditional covariance as in Equation (43) to get P _b|a,

Similar to Equations (31)–(32), sigma points α _i are calculated based on P _aa, and are propagated through Equation (53) to get γ _i, calculate m _y, P_yy, and P _xy as in Equations (47), (49) and (51), update the mean and covariance as in Equations (29) and (30);

D. Recursion

If it does not terminate, k←k+1, and go to B.

5. SIMULATION STUDY

In this section, a simulation study was carried out based on the models constructed in Sections 2 and 3, using UKF and MUKF in the correction stage. Parameters of the craft's true dynamics are artificially set, the INS errors are generated according to Equation (12) and then the INS errors are estimated by UKF and MUKF respectively. The parameters of the inertial sensors are set as follows:

$$\varepsilon = 0{\cdot}01{\curr { \char "000B0}} {\rm /h,} \;ARW = 0{\cdot}0001 {\curr { \char "000B0}} {\rm /} \sqrt {\rm h}, \;{\it \Delta} = 0{\cdot}1mg,\,VRW = 0{\cdot}001{\rm m/s/}\sqrt {\rm h} $$

The time interval between INS error propagations is 0·1 second, and that between GPS integrations is ten minutes (Case 1) and one minute (Case 2). The simulation time span is 18 hours, and GPS information is available from the second hour. A motion in the local plan is assumed with the constant forward speed being five m/s.

To improve the observability of the attitude errors and inertial sensor biases, clockwise and counter clockwise rotation manoeuvres about the downward axis are intentionally introduced to the vehicle on which the INS and GPS are mounted, i.e. the true trajectory is in an “8” shape, see Figures 1 and 7 (Wu et al., 2012). Specifically, from the fourth to the sixth hour, the craft experienced a counter clockwise (from above) circular trajectory, with a successive clockwise trajectory from the sixth to the eighth hour.

Figure 1. True, INS, and filtered trajectories in Case 1.

For Case 1, the true, INS and filtered (using UKF or MUKF) trajectories are shown in Figure 1. The INS errors and the INS errors estimated by UKF and MUKF are shown in Figures 2–4, the true inertial sensor biases and the estimated ones are shown in Figures 5 and 6. Similarly the results of Case 2 are shown in Figures 7–12.

Figure 2. INS attitude errors and their corresponding estimates by UKF and MUKF in Case 1.

Figure 3. INS velocity errors and their corresponding estimates by UKF and MUKF in Case 1.

Figure 4. INS position errors and their corresponding estimates by UKF and MUKF in Case 1.

Figure 5. True and estimated biases of gyros in Case 1.

Figure 6. True and estimated biases of accelerometers in Case 1.

Figure 7. True, INS, and filtered trajectories in Case 2.

Figure 8. INS attitude errors and their corresponding estimates by UKF and MUKF in Case 2.

Figure 9. INS velocity errors and their corresponding estimates by UKF and MUKF in Case 2.

Figure 10. INS position errors and their corresponding estimates by UKF and MUKF in Case 2.

Figure 11. True and estimated biases of gyros in Case 2.

Figure 12. True and estimated biases of accelerometers in Case 2.

It is clear in Figures 1 and 7 that the positioning errors of INS grow over a long period of time, and that the time-dependent INS errors are effectively restrained in the INS/GPS integrated systems in both cases. From Figures 2–4 and Figures 8–10, it can be concluded that the INS errors are characterized by three oscillations with distinct frequencies. The first is the Schuler oscillation, with frequency $\omega _S = \sqrt {g/R} $, or period $T_s = 2\pi \sqrt {R/g} \approx 84{\cdot}4\min $, named after Professor Max Schuler. This oscillation can be found in every error in Figures 2–4 and Figures 8–10. The second is the Foucault oscillation, with frequency ω _F=ω _ie sinφ, named after the French physicist Foucault. This oscillation, maintaining itself as a modulation of the Schuler oscillation, can be found in the horizontal angle errors and horizontal velocity errors in Figures 2 and 8 (the top and middle subplots) and in Figures 3 and 9. The third is earth oscillation with its frequency being the frequency of earth rotation ω _ie. This oscillation, with superimposed Schuler oscillation, can be found mainly in latitude and heading errors in Figures 2 and 8 (the bottom subplot), and Figures 4 and 10 (the top subplot). Besides these oscillations, there is also a ramp error with superimposed Schuler oscillation in the longitude error in Figures 4 and 10 (the bottom subplot). The accumulating character in INS errors is mainly due to this ramp error.

From Figures 2–4 it can be concluded that all the INS errors can be effectively estimated in the integrated INS/GPS system by both the hybrid EKF/UKF and EKF/MUKF. Specifically, from Figures 3 and 4, velocity and position errors estimated by both the filters see their convergences immediately after the GPS information is available. However from Figure 2, complete convergences of the attitude error estimations can only be achieved after the whole manoeuvre That is not surprising because the velocity and position are directly observed by GPS, however the attitude cannot be observed so directly.

From Figures 5 and 6, inertial sensor biases can also be effectively estimated except for the third accelerometer bias. Also, similar to attitude errors, the convergences of these bias estimations can only be achieved after the whole manoeuvre. The unobservability of the bias of the third accelerometer is due to the exclusion of the vertical channel of INS.

From all the graphs in Figures 1–6, by comparing the plots of UKF and MUKF, it can be concluded that MUKF can achieve comparable performance to UKF and MUKF can achieve smoother estimates than UKF. Both UKF and MUKF cannot estimate the third bias of the accelerometer.

By comparing the results in Cases 1 and 2, conclusions similar to the above analysis can be made, and all the errors and parameters can be estimated more accurately in Case 2 than in Case 1 mainly due to the more frequent corrections provided by GPS. In Figures 9 and 10, the velocity and position errors estimated by UKF and MUKF in Case 2 are almost superposed with each other. As in the bottom subplot of Figure 6, from the bottom of Figure 12, the bias of the third accelerometer can not be effectively estimated either even given the more frequent GPS information due to its inherent unobservable nature.

6. CONCLUSION

To fuse the two complementary navigation systems, INS and GPS, loosely coupled INS/GPS considering a constant lever arm effect has been studied in this paper. The linear or linearized INS error function is derived together with the sensor bias models to construct the state space equation based on which the prediction stage of the KF is carried out. The nonlinear observation equation of GPS considering constant lever arm effect is established with the velocity equation expressed in the navigation frame and the position equation in the earth-centred and earth-fixed frame respectively and can be addressed by the standard UKF scheme. The special structure of the nonlinear observation equation implies that the five-element observation vector is a nonlinear function of only eight elements of the 13-element state vector and is conditionally a linear function of the remaining five elements. This special structure, also known as the conditionally linear substructure, is exploited in this contribution by a refined form of UKF named as marginalized UKF, i.e. MUKF, where only 17 sigma points are generated and propagated rather than the 27 sigma points used in the standard UKF. Thus MUKF is adopted instead of UKF to address the INS/GPS integration and the computational expense is effectively reduced while retaining high accuracy and applicability. In the simulation experiment, the craft is assumed to experience two consecutive rotation manoeuvres, making the true trajectory in an “8” shape, to improve the observability of the INS errors and inertial sensor biases. Simulation results show that whether the GPS velocities and positions are available every one minute or every ten minutes, the navigation errors and inertial sensor biases can be effectively estimated by both the UKF and MUKF but at different computational cost. Specifically, in both cases with both filter schemes, the velocity and position of INS are reset immediately after GPS is available, while the convergence of the estimation of INS attitude errors and inertial sensor biases can only be achieved after the whole manoeuvre of the craft and the biases of the third accelerometer cannot be estimated due to the exclusion of the vertical channel in the INS error modelling.