1. INTRODUCTION
1.1. Inertial Navigation Systems (INS)
Navigation systems (Farrell, Reference Farrell2008) are a core component in many application areas such as intelligent transportation systems (Ming and Yuming, Reference Ming and Yuming2006), robotics and vehicular networks (Abrougui et al., Reference Abrougui, Boukerche and Pazzi2010). Commonly, the navigation component is developed as an embedded system where various sensors are processed by sophisticated signal processing techniques to provide accurate real-time navigation information (Farrell, Reference Farrell2008; Titterton and Weston, Reference Titterton and Weston2004). Before the development of Global Navigation Satellite Systems (GNSS) (Misra and Enge, Reference Misra and Enge2011; El-Rabbany, Reference El-Rabbany2006), most navigation systems utilized inertial sensors (accelerometers and gyroscopes) (Titterton and Weston, Reference Titterton and Weston2004; El-Sheimy, et al., Reference El-Sheimy, Hou and Niu2007) that provided linear acceleration and angular rate measurements to provide self-contained accurate high-rate inertial navigation systems (INS) (Titterton and Weston, Reference Titterton and Weston2004) (Britting, Reference Britting2010). Recently, most INS are based on the strapdown scheme (Titterton and Weston, Reference Titterton and Weston2004) where gyroscope measurements are used to calculate the platform orientation and then accelerometer measurements are transformed from body-frame to a local navigation frame to be integrated twice to calculate platform position. Although INS is accurate in the short-term, errors accumulate due to measurement integration and can grow without bounds. In fact, the high cost and drifts are two major limitations of INS. Fortunately, the recent advances, cost-reduction and wide availability of Micro-Electro-Mechanical Systems (MEMS)-based inertial motion sensors have attracted many researchers to utilise this technology to develop low-cost accurate navigation systems (Faulkner et al., Reference Faulkner, Cooper and Jeary2002; Titterton and Weston, Reference Titterton and Weston2004; El-Sheimy, et al., Reference El-Sheimy, Hou and Niu2007). However, the accuracy of MEMS sensors is still a challenge. To overcome MEMS-INS limitations, integration with other sensors/systems is commonly used (Farrell, Reference Farrell2008; Iqbal et al., Reference Iqbal, Okou and Noureldin2008; Atia et al., Reference Atia, Georgy, Korenberg and Noureldin2010).
1.2. Global Positioning System (GPS)
When the US Global Positioning System (GPS) became publicly available, it quickly became the dominant navigation component in almost all modern navigation systems. The most important advantage of a GNSS is consistent long-term accuracy. However, due to dependency on radio-frequency (RF) ranging, GNSS can suffer from multipath and signal availability problems. Additionally, to provide accurate navigation information, a GPS receiver must be able to acquire and track signals from at least four satellites in open-sky with direct Line-Of-Sight (LOS) signals (El-Rabbany, Reference El-Rabbany2006; Misra and Enge, Reference Misra and Enge2011). Even with four satellites visible in the sky, GNSS accuracy may deteriorate due to multipath. In multipath conditions, the GPS receiver receives multiple copies of satellite signals reflected/refracted through obstacles such as high buildings, mountains or trees. In addition to multipath problems, a complete GPS signal blockage may happen in tunnels, narrow alleys, and under bridges.
1.3. Integrated Navigation Systems
Due to the complementary error characteristics of GNSS and INS, integration between both systems maximizes the benefits and minimizes the drawbacks of both GNSS and INS (Farrell, Reference Farrell2008). In navigation systems where a basic motion dynamic model is used to predict the system navigation state, systems integration is implemented by fusing multi-sensor measurements using advanced estimation algorithms known as filtering algorithms (Hwang, Reference Hwang1997; Farrell, Reference Farrell2008; Faulkner et al., Reference Faulkner, Cooper and Jeary2002). The most common filtering algorithms are Kalman Filtering (KF) (Farrell, Reference Farrell2008) and Particle Filtering (PF) (Arulampalam et al., Reference Arulampalam, Maskell, Gordon and Clapp2002). Regardless of the filtering technique used, the navigation state x is predicted by a basic dynamic model given by
$$\dot x(t) = f\,(x(t),u(t)) + w(t)$$where f(x(t), u(t)) is the transition model and w(t) represent system noise. The integrated navigation system receives external measurements (observations) in the following general form:
$$z(t) = h(x(t)) + v(t)$$where h(x(t)) is the measurement model and v(t) represents measurements noise. The objective of filtering is to provide best estimate of system state x(t) given partial state observations z(t). Typically, in INS/GNSS systems, an INS dynamic model is used to predict a navigation state x(t) and GNSS position/velocity updates are used as observations z(t). For application-specific systems such as land-vehicle navigation, some constraints and extra sources of information can be used to enhance the integrated INS/GNSS navigation performance (Mannings, Reference Mannings2008; Iqbal et al., Reference Iqbal, Karamat and Okou2009).
2. PREVIOUS WORK
Commonly, speed information is used to enhance INS/GNSS integrated systems for land-vehicles. In this context, a reduced inertial sensor system (RISS) integrated with GNSS was introduced as a low-cost integrated navigation system for wheeled vehicles (Iqbal et al., Reference Iqbal, Okou and Noureldin2008; Georgy et al., Reference Georgy, Noureldin, Korenberg and Bayoumi2010; Atia et al., Reference Atia, Georgy, Korenberg and Noureldin2010). Instead of utilizing a full 3D inertial measurements unit (IMU) to provide 3D navigation, the RISS/GNSS consists of only a vertically aligned gyroscope, two level accelerometers, wheel sensors (odometer) and GNSS receiver. This configuration can provide 3D navigation without a full IMU and reduces both system complexity and cost. In earlier works, RISS/GNSS integration was either based on an approximated linearized RISS error dynamic model and an open-loop KF algorithm (Iqbal et al., Reference Iqbal, Okou and Noureldin2008; Karamat et al., Reference Karamat, Georgy and Iqbal2009; Cossaboom et al., Reference Cossaboom, Georgy and Karamat2012) or based on a dynamic motion model (instead of error dynamic model) and an open-loop Particle Filter (PF) algorithm (Georgy et al., Reference Georgy, Noureldin, Korenberg and Bayoumi2010). Both integration schemes are described in the following two subsections.
2.1. Integration of RISS/GNSS using open-loop KF
The RISS dynamic motion model is a dynamic model that predicts the vehicle state based on measurements from RISS sensors (single gyroscope, two accelerometers and the odometer). This step is called mechanisation. As it will be shown in subsequent sections, the RISS dynamic motion model is not linear. In this scheme, in order to use KF, the system dynamic model described by Equation (1) has to be linear with the assumption that system noise is zero-mean Gaussian noise (Farrell, Reference Farrell2008). Thus, the RISS dynamic motion model is linearized around a nominal navigation state 
$\bar x(t)$ using a Taylor expansion and considering only the first order term as follows:
$$\delta \dot x(t) = \left. {\displaystyle{{\partial f\,(x(t),u(t))} \over {\partial x}}} \right|_{\bar x(t)} \delta x(t) + Higher.Order.Terms$$In this case, the KF system state is the error in navigation state 
$\delta x = (x(t) - \bar x(t))$ instead of the navigation state itself x(t). In the earlier open-loop KF scheme (Iqbal et al., Reference Iqbal, Okou and Noureldin2008; Karamat et al., Reference Karamat, Georgy and Iqbal2009; Iqbal et al., Reference Iqbal, Karamat and Okou2009; Cossaboom et al., Reference Cossaboom, Georgy and Karamat2012) the nominal navigation state where the linearization is performed around (
$\bar x_k $) is the pure un-aided RISS navigation state calculated by the RISS dynamic motion model. There is no feedback from the KF to the RISS dynamic motion model. This RISS/GPS open-loop KF integration scheme is shown in Figure 1, which is taken from Cossaboom et al. (Reference Cossaboom, Georgy and Karamat2012). The dotted line represents the feedback from the filter to the RISS which is not implemented in an open-loop KF.
RISS/GPS Open-loop KF.
2.2. Integration of RISS/GNSS using open-loop Particle Filter (PF)
In this scheme, and due to the ability of PF to handle nonlinear models (Atia et al., Reference Atia, Georgy, Korenberg and Noureldin2010; Georgy et al., Reference Georgy, Noureldin, Korenberg and Bayoumi2010), the nonlinear RISS dynamic motion model is directly used in prediction of the navigation state x k. This scheme is also an open-loop scheme since no feedback from the PF to the RISS mechanisation exists. The RISS/GPS PF system block diagram is shown in Figure 2. PF is a Monte-Carlo based solution for the Bayesian Filtering problem that can handle nonlinear/non-Gaussian dynamic systems. States are modelled as a conditional probability density function (conditioned on all external measurements). Probability density functions are approximated by a set of weighted samples called particles. Each iteration of PF has three important steps; prediction, update, and resampling. In prediction, the RISS dynamic motion model probability density function is applied to each particle to obtain a new particle set that constitutes the predictive state probability density function. In the update phase, when measurements are available (z k), each particle is weighted using an observation likelihood function. Then all weights are normalized. In the resampling step, the new particles set is obtained by randomly selecting from the weighted particles such that each particle is selected a number of times proportional to its weight.
RISS/GPS Open-loop PF.
3. PROBLEM DEFINITION AND OBJECTIVES
3.1. Limitations of Open-loop KF RISS/GPS and PF RISS/GPS
In the earlier open-loop KF scheme (Iqbal et al., Reference Iqbal, Okou and Noureldin2008; Karamat et al., Reference Karamat, Georgy and Iqbal2009; Iqbal et al., Reference Iqbal, Karamat and Okou2009; Cossaboom et al., Reference Cossaboom, Georgy and Karamat2012) the nominal navigation state where the linearization is performed around (
$\bar x_k\!\!\!$) is the un-aided RISS navigation output which drifts continuously without bounds pushing the error in navigation state (δx(t)) out of acceptable linearization range. Consequently, the prediction step becomes significantly inaccurate during GNSS outages. In addition, the linearized RISS error dynamic model derived is a simplified model with some important terms ignored in the derivation as will be described in the subsequent sections. Although open-loop PF outperforms open-loop KF as reported by Georgy et al. (Reference Georgy, Noureldin, Korenberg and Bayoumi2010), it is computationally expensive due to the processing of huge numbers of particles. In addition, due to the open-loop design of the scheme, no feedback is taken from the corrected state to the RISS mechanization and no sensor errors are estimated. Consequently, sensor measurements (mainly gyroscope in RISS) are not corrected.
3.2. Objectives
The objectives of this work are to target the limitations in the existing open-loop KF and PF developed for the RISS/GPS configuration. The methodology to target these limitations is to derive a new RISS dynamic error model and develop a closed loop KF (known also as extended KF or EKF). In the proposed EKF scheme, the nominal navigation state is continuously compensated by the feedback coming from EKF-estimated correction. In addition, the RISS sensors measurements are continuously compensated for biases estimated by EKF. The expected advantages from this work can be summarised as follows:
• An EKF integration scheme is introduced where linearization is performed around the corrected RISS output that keeps errors within acceptable accurate linearization range. This should lead to a more sustainable performance during longer GPS outages.
• A more sustainable performance during long GNSS outages is obtained by compensating gyroscope measurements using the recent gyroscope bias estimated by EKF.
• A new more accurate error model is given.
• An accuracy comparable to PF using only KF is obtained while the expensive computation load of PF is avoided.
In the following sections, detailed model derivations for the proposed EKF-based RISS/GNSS integrated navigation system are given in addition to experimental setup, results and analysis.
4. METHODOLOGY
4.1. The 3D RISS/GNSS System
3D RISS is a 3D low-cost reduced inertial sensors system consisting of two accelerometers mounted to the lateral (x) and longitudinal (y) directions of the vehicle fame and a vertically aligned gyroscope (z) in addition to the vehicle speed sensor as shown in Figure 3. There are some basic benefits from using this reduced configuration for land-vehicles:
• Using RISS instead of full 3D inertial measurement unit (IMU) eliminates two gyroscopes, which reduces the overall cost of the system.
• Computational complexity of RISS motion equations is less than full IMU mechanization equations (Titterton and Weston, Reference Titterton and Weston2004).
• Roll and Pitch calculations do not use mathematical integration and hence contain no drifts or error growth without bounds.
3D RISS Configuration.
The 3D RISS navigation state vector is defined as x=[φ, λ, h, ve, vn, vu, r, p, A]T where φ is latitude, λ is longitude, h is altitude, ve is east velocity, vn is north velocity, vu is upward velocity, r is roll, p is pitch and A is azimuth. The RISS dynamic motion model equations are given by:
$$p = \sin^{ - 1} \left( {\displaystyle{{\,f_y - a_{od}} \over g}} \right)$$
$$r = - \sin^{ - 1} \left( {\displaystyle{{f_x + v_f (w_z - b_z )} \over {g\cos p}}} \right)$$
$$\dot A = - \left( {[w_z - b_z ] - w^e \sin (\varphi ) - \displaystyle{{v^e {\rm tan}(\varphi )} \over {R_n + h}}} \right)$$
$$\dot \varphi = \displaystyle{{v^n} \over {R_m + h}}$$
$$\dot \lambda = \displaystyle{{v^e} \over {(R_n + h) \cos\, (\varphi )}}$$
$$\dot h = v^u $$
$$v^e = v_f \sin (A)\cos (\,p)$$
$$v^n = v_f\cos (A)\cos (p)$$
$$v^u = v_f \sin (\,p)$$f x and f y are the accelerometers measurements and v f is the vehicle forward speed and ω z is the gyroscope measurements, b z is the estimated gyroscope bias, we is the earth rotation rate and R n is the earth normal radius of curvature. R m is the earth meridian radius of curvature. A block diagram that shows RISS mechanization system is shown in Figure 4.
3D RISS Mechanization Block Diagram.
Analysis of RISS motion Equations (4) to (12) shows that the major source of errors in the RISS system is the error in gyroscope measurements (gyroscope bias). Actually any error in the gyroscope measurements will propagate directly to azimuth which then propagates indirectly into an error in the horizontal channel velocity and consequently in position. Regarding pitch and roll calculations, because no integration is involved in the pitch and roll calculations as shown in Equations (4) and (5), error in the level accelerometer measurements have a very minor effect and can be reduced by common sensor calibration techniques (Farrell, Reference Farrell2008; Titterton and Weston, Reference Titterton and Weston2004; El-Sheimy et al., Reference El-Sheimy, Hou and Niu2007). Regarding the odometry, since the speed of the vehicle was not too high during tests (52 km/hr in downtown and 91 km/hr in suburbs and highway), the scale factor effect is minor with respect to the gyroscope bias error. Thus, special attention will be given to the vertically aligned gyroscope in the results and discussions section.
4.2. The RISS Error Dynamic System Model
The RISS error model used in EKF is derived by linearization of the RISS mechanisation Equations (4) to (12) using Taylor expansion (Britting, Reference Britting2010; Farrell, Reference Farrell2008) and then considering only the first order terms and ignoring the higher order terms to have the system model in the following linear form:
$$\delta \dot x(t) = F(t)\delta x(t) + G(t)w(t)$$where w(t) is zero mean Gaussian noise vector. The covariance matrix of w(t) is defined as Q and called the system noise matrix and is given by:
$$Q = \; \langle w(t)w(t)^T \rangle $$F(t) is the transition matrix and G(t) is noise shaping parameter matrix.
The error state vector of 3D RISS is defined as:
$$\delta x{\rm \;} = [\delta \varphi, \delta \lambda, \delta h,\delta v^e, \delta v^n, \delta v^u, \delta A,\delta b_z, \delta a_{od} ]^T $$where δφ is latitude error, δλ is longitude error, δh is altitude error, δve is east velocity error, δvn is north velocity error, δvu is upward velocity error, δA is azimuth error, δb z is the error in gyroscope bias and δa od is error in acceleration (a od) derived from odometer measurements. The roll error and pitch angle errors (δr, δp) were not considered in the model since they are calculated independently using gravity measurements without mathematical integration. By linearizing the RISS motion Equations (4) to (12) around an operating point, an error model is obtained. In this work, we consider a more accurate and detailed linearized error model. The new RISS error model has a list of advantages that contribute significantly in the enhancement of the performance and the applicability of EKF closed loop filtering scheme. Those advantages differ from the previously introduced simplified model in the following points:
• In azimuth (δA) error equation derivation, the derivatives with respect to φ, ve and h were performed and included.
• In longitude (δλ) error model, the derivative with respect to φ, and h was performed and included.
• In the time derivative of ve and vn, the term of time derivative of azimuth was considered.
• In addition, the model for azimuth error was modified to be a function of error in gyroscope bias (δb z) instead of gyroscope bias itself.
These terms were ignored in the previously introduced error model in earlier works. Actually, it turned out that those terms have an effect on the stability of the error model and the overall EKF-based dynamic system. This was verified by applying the proposed closed-loop EKF using the old simplified error model derived in earlier works. In this case, the EKF filter diverges quickly no matter what the system noise, measurements noise, and other EKF parameters are.
The overall performance of the derived error model will be examined in the experimental work section. To derive the azimuth error equation, a Taylor expansion is applied to azimuth Equation (6) as follows:
$$\dot \delta A = \displaystyle{{\partial [.]} \over {\partial b_z}} \delta b_z + \displaystyle{{\partial [.]} \over {\partial \varphi}} \delta \varphi + \displaystyle{{\partial [.]} \over {\partial v^e}} \delta v^e + \displaystyle{{\partial [.]} \over {\partial h}}\delta h$$which gives
$$\dot \delta A = \delta b_z + \omega ^e \cos (\varphi )\delta \varphi + \displaystyle{{v^e \sec^2 (\varphi )\delta \varphi} \over {R_n + h}} + \displaystyle{{\tan(\varphi ) \; \delta v^e} \over {R_n + h}} - \displaystyle{{v^e \tan \varphi \delta h} \over {(R_n + h)^2}} $$Because the term (R n+h)2 is fairly very large with respect to the term vetanφδh, it can be safely cancelled and the following Azimuth error model equation is obtained:
$$\dot \delta A = \delta b_z + \omega ^e \cos (\varphi )\delta \varphi + \displaystyle{{v^e \sec^2 (\varphi )\delta \varphi} \over {R_n + h}} + \displaystyle{{\tan(\varphi )\delta v^e} \over {R_n + h}}$$To derive the latitude error equation, a Taylor expansion is applied to latitude Equation (7) as follows:
$$\dot \delta \varphi = \displaystyle{{\partial [.]} \over {\partial v^n}} \delta v^n + \displaystyle{{\partial [.]} \over {\partial h}}\delta h$$which gives
$$\dot \delta \varphi = \displaystyle{{\delta v^n} \over {R_m + h}} - \displaystyle{{v^n \delta h} \over {(R_m + h)^2}} $$Because the term (R m+h)2 is fairly very large (earth radius squared in metres) with respect to term vnδh, it can be safely cancelled and the following latitude error model equation is obtained:
$$\dot \delta \varphi = \displaystyle{{\delta v^n} \over {R_m + h}}$$To derive the longitude error equation, a Taylor expansion is applied to longitude Equation (8) as follows:
$$\dot \delta \lambda = \displaystyle{{\partial [.]} \over {\partial v^e}} \delta v^e + \displaystyle{{\partial [.]} \over {\partial h}}\delta h + \displaystyle{{\partial [.]} \over {\partial \varphi}} \delta \varphi $$which gives
$$\dot \delta \lambda = \displaystyle{{\delta v^e} \over {(R_n + h) \cos (\varphi )}} + \displaystyle{{v^e \tan(\varphi )\delta \varphi} \over {(R_n + h)\cos (\varphi )}} + \displaystyle{{v^e \delta h} \over {(R_n + h)^2 \cos (\varphi )}}$$Because the term (R n+h)2 cos(φ) is fairly large (earth radius squared in metres) with respect to term vnδh, it can be safely cancelled and the following longitude error model equation is obtained:
$$\dot \delta \lambda = \displaystyle{{\delta v^e} \over {(R_n + h) \cos (\varphi )}} + \displaystyle{{v^e \tan (\varphi )\delta \varphi} \over {(R_n + h) \cos (\varphi )}}$$Regarding the altitude error model, it is directly related to error in up velocity as follows:
$$\dot \delta h = \delta v^u $$To derive the east velocity error equation, we first take the derivative with respect to time assuming constant pitch angle (since pitch is fairly small in this land-vehicle application) which gives
$$\dot v^e = a_{od} \sin (A) \cos (\,p) + v_f \cos (A) \cos (\,p)\dot A = a_{od} \sin (A) \cos (\,p) + v^{n} \dot{\skew3 A}$$Applying a Taylor expansion as follows:
$$\dot \delta v^e = \displaystyle{{\partial [.]} \over {\partial a_{od}}} \delta a_{od} + \displaystyle{{\partial [.]} \over {\partial A}}\delta A + \displaystyle{{\partial [.]} \over {\partial v^n}} \delta v^n + \displaystyle{{\partial [.]} \over {\partial \dot A}}\delta\dot{\skew3 A}$$which gives
$$\eqalign{\dot \delta v^e = & \sin (A) \cos (\,p)\delta a_{od} + a_{od}\cos (A) \cos (\,p)\delta A \cr & - \left[ {\omega _z - b_z - \omega \sin (\varphi ) - \displaystyle{{v^e \tan (\varphi )} \over {R_n + h}}} \right]\delta v^n + v^n \delta b_z \cr & + \left[ {v^n \omega ^e \cos (\varphi ) + \displaystyle{{v^e v^n \sec^2 (\varphi )} \over {R_n + h}}} \right]\delta \varphi + \displaystyle{{v^n \tan (\varphi )} \over {R_n + h}}\delta v^e} $$The north velocity error equation can be derived similarly using the same steps applied in case of east velocity error equation derivation. This will lead to the following north velocity error equation:
$$\eqalign{\dot \delta v^n =\tab \cos (A) \cos (\,p)\delta a_{od} - a_{od} \sin (A) \cos (\,p)\delta A \cr & + \left[ {\omega _z - b_z - \omega ^e \sin (\varphi ) - \displaystyle{{2v^e \tan (\varphi )} \over {R_n + h}}} \right]\delta v^e - v^e \delta b_z \cr &- \left[ {v^e \omega ^e \cos (\varphi ) + \displaystyle{{v^{e2} \sec^2 (\varphi )} \over {R_n + h}}} \right]\delta \varphi} $$Regarding the up velocity error model equation, taking the time derivative of Equation (12) assuming small constant pitch angle, it can be easily shown that the error model equation for velocity up is given by:
$$\dot \delta v^u = {\rm sin}(\,p)\delta a_{od} $$Regarding the errors in gyroscope bias (δb z) and odometer deriving error (δa od), they are modelled as a first-order Gauss-Markov random process (Farrell, Reference Farrell2008) as follows:
$$\dot \delta a_{od} = - \beta _{od} \delta a_{od} + \sqrt {2\beta _{od} \sigma _{od}^2} w$$
$$\dot \delta b_z = - \beta _z \delta b_z + \sqrt {2\beta _z \sigma _z^2} w$$where (β z, σz), (β od, σod) are Gauss-Markov (Farrell, Reference Farrell2008; Britting, Reference Britting2010; Iqbal et al., Reference Iqbal, Karamat and Okou2009) process parameters for δb z and δa od error states respectively. To estimate those parameters, autocorrelation calculations of stationary measurements of the sensors are collected and error analysis similar to the analysis in Britting (Reference Britting2010) and Iqbal et al. (Reference Iqbal, Karamat and Okou2009) is conducted. For details about how to estimate those Gauss-Markov parameters, the reader is referred to Farrell (Reference Farrell2008).
4.3. The RISS/GNSS Measurements Model
The second major part of the proposed integrated navigation system is the measurement model. The measurement model relates the observations which are the difference between GPS position/velocity and RISS predicted position/velocity. The measurement δz model is given by:
$$\delta z = H\delta x + v$$Where δz is the measurements vector defined by:
$$\delta z = \left( {\matrix{ {\varphi _{GNSS} - \varphi _{RISS}} \cr {\lambda _{GNSS} - \lambda _{RISS}} \cr {h_{GNSS} - h_{RISS}} \cr {v^e _{GNSS} - v^e _{RISS}} \cr {v^n _{GNSS} - v^n _{RISS}} \cr {v^u _{GNSS} - v^u _{RISS}} \cr}} \right)$$and H (the design matrix of the filter (Farrell, Reference Farrell2008; Iqbal et al., Reference Iqbal, Okou and Noureldin2008; Iqbal et al., Reference Iqbal, Karamat and Okou2009)) is given by:
$$H = \left( {\matrix{ 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \cr 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \cr}} \right)$$and v is zero-mean Gaussian noise vector defined as measurement noise whose covariance matrix R is given by:
$$R = \; \langle vv^T \rangle $$4.4. The Extended Kalman Filter for the 3D RISS/GNSS System
Having the system dynamic model defined by Equation (13) and measurement model defined by Equation (32) and having H and F matrices clearly defined, the EKF-based RISS/GNSS system prediction and update are performed in the discrete domain as follows:
• Prediction Step:
(36)
$$\emptyset _{k,k + 1} = I + F_k T$$(37)$$ P_{k + 1}^ - = \emptyset _{k,k + 1} P_k^ + \emptyset _{k,k + 1} ^T + Q_d $$
where 
$\emptyset _{k,k + 1} $ is the discrete transition matrix, T is the sampling period, I is the identity matrix, 
$\; P_{k + 1}^ - $ is the a priori estimate of error state covariance matrix, Q d is the system noise matrix defined by
$$Q_d = \; GQG^T T$$• Measurements (Update) Step:
(39)$$K_{k + 1} = P_{k + 1}^ - H^T [HP_{k + 1}^ - H^T + R_k ]^{ - 1} $$(40)$$\delta x_{k + 1}^ + = K_{k + 1} \delta z_{k + 1} $$(41)where$$P_{k + 1}^ + = (I - K_{k + 1} )\; P_{k + 1}^ - $$$\delta x_{k + 1}^ + $ is the error state estimation and $P_{k + 1}^ + $ is the posterior error covariance matrix estimation.
4.5. The closed loop scheme
A block diagram of the proposed system is given in Figure 5. Two important modifications are introduced:
• The estimated error in position, velocity and attitude is used to correct RISS navigation state x k so in the next epoch mechanization, x k+1 is predicted taking x k as the corrected RISS navigation output instead of the un-aided RISS navigation output in earlier work.
• The estimated gyroscope bias error is added to the previous bias value to obtain a corrected gyroscope bias.
In addition, the sub-sequent gyroscope measurements are continuously compensated for this new bias.
EKF-Based RISS/GNSS Integrated Navigation System Block Diagram. Note the feedback to RISS mechanization and gyroscope measurements.
5. EXPERIMENTAL WORK
To test the proposed closed loop EKF-based RISS/GNSS system, real road experiments were performed in the city of Kingston and Napanee, Ontario, Canada. The testing strategy was to build a platform that has a MEMS-based RISS/GNSS system running side by side with a high-end tactical grade INS/GNSS integrated navigation system used as a ground-truth. After data collection, the proposed EKF-based RISS/GNSS system was used in post-processing mode to compute a navigation solution. The calculated solution was compared to the ground-truth calculated by the high-end navigation grade reference system. The testing strategy was to impose GPS outages during the post-processing mode and calculate the positional errors during those outages. Then, comparisons were made with earlier works that either used open-loop KF with the simplified RSS dynamic error model or open-loop PF. To test the performance during noisy multipath GPS conditions, some portions of the trajectories at which GPS multipath exists will be displayed on a map to show the performance of the proposed system during these challenging portions. Furthermore, some analysis on the accuracy of the estimated sensors biases will be given.
5.1. Accuracy of the Ground Truth System
The ground-truth system used in the experiments consisted of a tactical-grade ring laser gyro (RLG)-based IMU called HG1700 AG11 from Honeywell. This tactical grade IMU is integrated with NovAtel G2 Pro-Pack GPS receiver using a system called SPAN developed by Novatel (2010). According to Novatel (2013), this ground-truth system is capable of keeping position error below 2.5 metres under 60 seconds of complete GPS outage. In partial GPS outages and multipath conditions, it can keep the errors under 3 metres for longer GPS outages due to the utilisation of the phase measurements even from two visible satellites. When a limited number of satellites are available, phase updates can be used to constrain the growth of errors in the solution. As the number of satellites available increases, with two available satellites corresponding to one phase update, the growth in the error is constrained further. In addition to the high accuracy of the real-time solution calculated by this ground-truth system, the ground-truth solution used in comparisons is further improved offline using a smoothing tool called “Inertial Explorer” from Novatel Inc. The offline processed solution is even better than the real-time solution because of the backward smoothing mechanism (Wang, Reference Wang2012) it applies to the solution in offline processing. A detailed quantitative analysis of the accuracy of this ground-truth system can be found in Novatel (2013).
5.1. Experimental Setup
The experimental setup used in this work is described as follows:
5.1.1. RISS System Configuration
Crossbow IMU300CC MEMS-based inertial sensors were used for the experiments (IMU300, 2011). This IMU was utilized in the RISS architecture, and the performance was examined on real road data collected over various trajectories. The error parameters of the Crossbow IMU300CC MEMS-based inertial sensors are shown in Table 1. The forward speed (odometer data) was gathered from the vehicle's built in sensors and collected by the On-Board Diagnostics version II (OBD II) interface using CarChip (CarChip, 2011).
Specifications of Crossbow IMU300CC MEMS-based IMU.
5.1.2. Reference Navigation Solution
As mentioned earlier, the reference solution used to evaluate the proposed method is based on the Honeywell HG1700 AG11 high-end tactical-grade IMU. This IMU was integrated with the NovAtel GPS receiver using an off-the-shelf assembly, the G2 Pro-Pack SPAN unit developed by NovAtel (Novatel, 2010). The error parameters of the HG1700 AG11 high-end tactical-grade IMU are shown in Table 1.
5.2. Testing Trajectories
Three trajectories were used for testing (see Figures 6 to 8). These trajectories have been used in earlier work using either open-loop KF or open-loop PF and results of earlier works are already published. This makes the comparison with the new proposed EKF scheme easier. The Kingston Downtown trajectory is the same trajectory used in earlier works in Cossaboom et al. (Reference Cossaboom, Georgy and Karamat2012) using open-loop KF and in Georgy et al. (Reference Georgy, Noureldin, Korenberg and Bayoumi2010) using open-loop PF. The Kingston suburbs trajectory is the same trajectory used in earlier works in Cossaboom et al. (Reference Cossaboom, Georgy and Karamat2012) using open-loop KF. The Napanee trajectory is the same trajectory used in earlier works by Iqbal et al. (Reference Iqbal, Karamat and Okou2009) using open-loop KF. In the Kingston Downtown and Kingston suburb trajectories, ten GPS outages of 60 seconds long were introduced to check the performance of the integrated navigation system when GPS is completely lost. To further check the proposed closed loop EKF scheme, the Napanee trajectory was tested with ten GPS outages of 120 seconds instead of 60 seconds. The GPS outages were selected such that they contained turns and different dynamics.
Testing Trajectory in Kingston downtown, ON, Canada.
Testing Trajectory in Kingston sub-urban, ON, Canada.
Testing Trajectory in Napanee, ON, Canada.
5.3. Results and Discussions
5.3.1. Performance during GPS Outages
5.3.1.1. Comparison with Simplified Open-loop KF
In Figures 9 to 11, the maximum position error calculated during the GPS outages are displayed for both the new EKF with the new error model against the old error model in the open-loop KF scheme. In Figures 9 and 10, it is noticed that although the position error in the proposed EKF-based scheme with the new error model is better, the differences are not significant enough to build a strong conclusion. However, Figures 9 and 10 show clearly that while the error in the old open-loop KF scheme can go to the edge of a hundred metres, the error in the proposed EKF-based went to a maximum of 25 metres. This result agrees with the analysis mentioned earlier in this paper that says the new EKF-scheme is expected to perform better in the situations where the old open-loop KF scheme significantly fails. To increase the confidence in this conclusion, an outage of 120 seconds was simulated in the Napanee trajectory and results are shown in Figure 11. Results show clearly that while the open-loop KF scheme significantly fails (error of an average of 75 metres) in longer GPS outages, the proposed new EKF-based scheme performed almost the same as in the 60 seconds outages (error of an average of 18 metres). To further show conclusive results, we displayed the average maximum position error over all GPS outages in all the three trajectories and results are displayed in Figure 12.
Maximum Position Error Analysis in Kingston Downtown Trajectory. EKF+new Error Model Vs. Old Error Model + Open-loop KF.
Maximum Position Error Analysis in Kingston Suburbs Trajectory. EKF+new Error Model Vs. Old Error Model + Open-loop KF.
Maximum Position Error Analysis in Napanee Trajectory. EKF+new Error Model Vs. Old Error Model + Open-loop KF.
Average Maximum Position Error Analysis over all outages in all trajectories. EKF+new Error Model Vs. Old Error Model + Open-loop KF.
5.3.1.2. Comparison with Open-loop PF
In Figures 13 and 14, the maximum position error calculated during the GPS outages are displayed for both the proposed new EKF scheme with the new error model against an open-loop PF scheme. In Figures 13 and 14, it can be seen that the performance of the two schemes is very close while open-loop PF performs slightly better due to the probabilistic nature and the number of particles processed (120 particles as reported in (Georgy et al. Reference Georgy, Noureldin, Korenberg and Bayoumi2010)). If the computational load is taken into consideration in the comparison, the proposed EKF scheme performs faster than PF. As reported in Georgy et al. (Reference Georgy, Noureldin, Korenberg and Bayoumi2010), KF performs 20% faster than PF. It has to be taken into consideration that the PF presented in Georgy et al. (Reference Georgy, Noureldin, Korenberg and Bayoumi2010) is already optimized. If a standard PF is to be used, this percentage will significantly increase. Average maximum position over all outages in the two trajectories (Kingston Downtown and Napanee) is displayed in Figure 15. This confirms the conclusion that the proposed EKF-based scheme performs similarly to the open-loop PF presented in Georgy et al. (Reference Georgy, Noureldin, Korenberg and Bayoumi2010).
Maximum Position Error Analysis in Kingston Downtown Trajectory. EKF+new Error Model Vs. Open-loop PF.
Maximum Position Error Analysis in Napanee Trajectory. EKF+new Error Model Vs. Open-loop PF.
Average Maximum Position Error Analysis over all outages in all trajectories. EKF+new Error Model Vs. Open-loop PF.
5.3.2. Gyroscope Bias Estimation
Regarding the estimation of gyroscope bias, Figures 16 to 18 show the behaviour of the gyroscope bias convergence in the filter. It can be seen that all estimations converged around the same bias value (approximately −0.24 deg/s). The convergence starts with motion and takes almost 300 seconds in Kingston Downtown Trajectory and 250 seconds in Kingston suburb trajectory and around 300 seconds in the Napanee trajectory. The spikes in the beginning are mainly due to the initialization of the biases to zero. The convergence starts with motion because when the vehicle starts to move it receives velocity updates from GPS, which indirectly affect the observability of the azimuth and consequently affects the gyroscope bias estimation. After taking sufficient GPS measurement updates, the EKF converges and the gyroscope biases are accurately estimated. This estimation of the gyroscope bias enables the system to sustain a good performance during GPS outages because all subsequent gyroscope measurements are compensated for this bias.
Gyroscope Bias Estimation Convergence in Kingston Downtown Trajectory.
Gyroscope Bias Estimation Convergence in Kingston Suburban Trajectory.
Gyroscope Bias Estimation Convergence in Napanee Trajectory.
5.3.3. Effect of Gyroscope Bias Estimation on Roll Calculations
The estimated gyroscope bias is used to compensate the incoming gyroscope measurements. This has a direct effect on the roll angle estimation as shown in Equation (5). To check this effect, the roll angle was calculated with and without this compensation and results are shown in Figure 19. The root mean square error of the roll angle in degrees shows a slight improvement with gyroscope bias compensation. The effect is clearer in the Napanee trajectory.
Effect of Gyroscope Bias Compensation on Roll Accuracy.
5.3.4. Performance during GPS multipath
The proposed EKF-based RISS/GPS system proved that it not only provides better accuracy in GPS complete outage, but it also enhances GPS multipath mitigation in noisy GPS conditions. This is mainly performed by balancing the measurements covariance matrix R and the system noise Q. Due to the efficient gyroscope bias estimation of the proposed EKF-based scheme and due to the closed-loop feedback configuration, the prediction performed by the RISS dynamic motion model is trusted, which enables the system to adaptively ignore noisy GPS updates in multipath conditions. To show this effect, portions of the trajectory are shown in Google maps using the GPS Visualizer tool. These portions contain noisy GPS positioning and they are shown in Figures 20 to 22. These figures show two important notes. Firstly, the proposed EKF-based RISS/GPS system handles the GPS noisy updates well. Secondly, the ground-truth system accuracy is further verified because figures show clearly that the ground-truth system output is perfectly coincident with streets/roads on the map.
Noisy GPS updates in Kingston Downtown Trajectory.
Noisy GPS updates in Kingston Suburban Trajectory.
Noisy GPS updates in Napanee Trajectory.
6. CONCLUSION
In this paper, an enhanced closed loop EKF-based multi-sensor 3D integrated navigation system for land-vehicles was introduced. The system utilises a reduced inertial sensor set (RISS) integrated with GPS. This configuration was introduced previously in earlier works. However, the integration scheme was either built on open-loop Kalman Filter or a nonlinear computationally expensive open-loop Particle Filtering. Furthermore, the integration used a simplified RISS dynamic error model. The main contribution of the work presented in this paper is the development of a closed loop EKF-based integration scheme with a new RISS dynamic error model. The experiments on three different trajectories showed that the proposed closed loop EKF integration scheme outperforms the open-loop KF-based scheme and performs comparably with open-loop PF. In GPS outages, improvements ranging from 20% to 80% were obtained. Improvements were stronger with longer GPS outages. In addition, performance of the proposed EKF-based RISS/GPS system is comparable to open-loop PF that has been introduced in earlier works with the advantage of having similar accuracy with less computations and faster processing. This may be preferable for real-time embedded realization of the proposed navigation system.
