Open Access
This article is

- freely available
- re-usable

*Sensors*
**2015**,
*15*(9),
24269-24296;
https://doi.org/10.3390/s150924269

Article

An Enhanced Error Model for EKF-Based Tightly-Coupled Integration of GPS and Land Vehicle’s Motion Sensors

Electrical and Computer Engineering, Royal Military College of Canada, Kingston, ON K7K 7B4, Canada

^{*}

Author to whom correspondence should be addressed.

^{†}

These authors contributed equally to this work.

Academic Editor:
Luis Javier Garcia Villalba

Received: 20 July 2015 / Accepted: 11 September 2015 / Published: 22 September 2015

## Abstract

**:**

Reduced inertial sensor systems (RISS) have been introduced by many researchers as a low-cost, low-complexity sensor assembly that can be integrated with GPS to provide a robust integrated navigation system for land vehicles. In earlier works, the developed error models were simplified based on the assumption that the vehicle is mostly moving on a flat horizontal plane. Another limitation is the simplified estimation of the horizontal tilt angles, which is based on simple averaging of the accelerometers’ measurements without modelling their errors or tilt angle errors. In this paper, a new error model is developed for RISS that accounts for the effect of tilt angle errors and the accelerometer’s errors. Additionally, it also includes important terms in the system dynamic error model, which were ignored during the linearization process in earlier works. An augmented extended Kalman filter (EKF) is designed to incorporate tilt angle errors and transversal accelerometer errors. The new error model and the augmented EKF design are developed in a tightly-coupled RISS/GPS integrated navigation system. The proposed system was tested on real trajectories’ data under degraded GPS environments, and the results were compared to earlier works on RISS/GPS systems. The findings demonstrated that the proposed enhanced system introduced significant improvements in navigational performance.

Keywords:

inertial sensors; wheel rotation sensors; Global Positioning System; gyroscope; accelerometer; extended Kalman filter## 1. Introduction

After its full operational capability was announced in 1995, the Global Positioning System (GPS) [1] became the dominant navigational system for land vehicles. Based on the principle of trilateration [2], a GPS receiver calculates its position by using range measurements from a minimum of four satellites [1,3]. Although GPS provides good long-term accuracy, there are a few drawbacks associated with it. Firstly, a GPS receiver needs radio signals from at least four satellites with a direct line-of-sight (LOS), which may not be possible in all scenarios. Secondly, a GPS signal might suffer from multipath or interference effects.

To mitigate GPS errors, it can be integrated with other aiding sources with complementary characteristics using estimation algorithms, which are often based on a Kalman filter (KF) or a particle filter (PF). This integration results in superior accuracy compared to what any of the other systems can provide on their own [4,5,6,7,8,9]. A typical aiding source may consist of an inertial navigation system (INS) [10,11,12,13], speed sensors [14,15], light detection and ranging (LiDAR) [16,17,18], vision sensors [19,20,21], maps [22,23,24] etc. There are two major GPS/INS integration schemes, which are referred to as loosely-coupled (TC) or tightly-coupled (TC). The TC integration has the major advantage of providing an integrated solution even if satellite availability drops below four. There are two main variants of KF that are usually employed for GPS/INS integration, which are linearized KF (LKF) and extended KF (EKF). In LKF, the linearization is performed around a nominal trajectory (which is the output of a stand-alone INS), whereas in EKF, the linearization is always carried out around the corrected state, which keeps errors within the acceptable linearization range and performs better than LKF. A general block diagram of such an integrated system is shown in Figure 1.

With the advent of micro-electro-mechanical system (MEMS)-based technology, the cost of inertial sensors dropped significantly, which attracted many researchers to capitalize on this newer technology to develop accurate and economical navigation systems [13,25,26]. To reduce the effect of the complex error characteristics of MEMS-based inertial sensors and to avoid the higher cost of using all six inertial sensors of an inertial measurement unit (IMU), efforts were made to utilize fewer inertial sensors to compute the navigation solution [27]. This system is referred to as the reduced inertial sensor system (RISS), and its details can be found in [8,14,28,29].

Earlier works based on loosely-coupled LKF [28,30], tightly-coupled LKF [14] and PF [7,31] involving RISS/GPS integration suffer from limitations that adversely affect the accuracy of the system. For example, the system error models were simplified based on the assumption that the vehicle is mostly moving on a flat horizontal plane, and important terms in the system dynamic error model were ignored during the linearization process. Another limitation is the use of the simplified estimation of horizontal tilt angles. This was based on simple averaging of accelerometers without modelling accelerometer errors or tilt angles errors. The accelerometer biases were also not estimated in earlier approaches, which were based on LKF instead of EKF.

In this work, an improved EKF-based tightly-coupled RISS and GPS integration scheme is proposed, where an enhanced error model is developed, which considers additional important terms during the linearization of system model, leading to better positional accuracy. Furthermore, in the proposed work, the EKF is augmented to handle tilt (pitch and roll) angle errors and transversal accelerometer errors, taking the accelerometer readings as measurement updates. The proposed EKF-based RISS/GPS was tested on road test trajectories and compared to the two earlier systems that are based on a simplified RISS error model. The results show improved positional accuracy during long GPS outages, where the system maintained reliable accuracy by making use of the most recent sensor biases estimated by EKF.

## 2. The 3D Reduced Inertial Sensor System

The 3D RISS is a three-dimensional, low-cost reduced multi-sensor system consisting of two accelerometers mounted to the lateral and longitudinal directions of the vehicle frame and a vertically-aligned gyroscope in addition to the built-in vehicle speed sensor. The arrangement of these sensors with respect to the body frame is depicted in Figure 2.

**Figure 2.**Arrangement of 3D reduced inertial sensor system (RISS) sensors with respect to the body frame.

The 3D RISS is very attractive in low-cost applications, because of its simplicity and elimination of two gyroscopes, which further reduces the overall cost and simplifies the modelling of complex error characteristics inherent in low-cost sensors. Furthermore, roll and pitch angles are obtained from accelerometers, which avoids mathematical integration, and position calculation involves only a single integration, which keeps the errors bounded.

**RISS Motion Equations**For the ensuing discussion and derivations, the local level reference frame is based on east, north and up (ENU) directions, and the $x,y,z$ axes of the body are aligned with transversal, forward and vertically upward directions, respectively.

The rate of change of heading (azimuth) angle $\dot{A}$ is given as:
where ${\omega}_{z}$ is the vertical gyroscope’s measurement, ${b}_{z}$ is the estimated gyroscope bias, ${\omega}^{e}$ is the Earth rotation rate, φ is the latitude, ${v}_{e}$ is the east velocity, h is the altitude of the vehicle and ${R}_{N}$ is the normal radius of the curvature of the Earth’s ellipsoid. It may be noted here that Equation (1) is different from the previous works on the subject [14,28,30,32], as the gyroscope’s measurement ${\omega}_{z}$ is being compensated for the bias ${b}_{z}$. This will also affect its linearization, as shown later in Section 3.1.4.

$$\dot{A}=-\left[({\omega}_{z}-{b}_{z})-{\omega}^{e}sin\text{\phi}-\left(\frac{{v}_{e}tan\text{\phi}}{{R}_{N}+h}\right)\right]$$

The computation of pitch and roll angles is based on the idea presented in [33,34]. Mathematically, the computation of the pitch angle is expressed as:
where ${f}_{y}$ is the forward accelerometer measurement, ${a}_{o}$ is the vehicle acceleration (${a}_{o}$) derived from the vehicle speed measurements and g is the Earth’s gravity.

$$p={sin}^{-1}\left(\frac{{f}_{y}-{a}_{o}}{g}\right)$$

The computation of the roll angle is given by:
where ${f}_{x}$ is the transversal accelerometer measurement,${v}_{o}$ is the vehicle speed obtained from wheel rotation sensor measurements and ${\omega}_{z}$ is the angular rate measured by the vertically-aligned gyroscope.

$$r=-{sin}^{-1}\left[\frac{{f}_{x}+{v}_{o}({\omega}_{z}-{b}_{z})}{gcosp}\right]$$

Once the azimuth and pitch angles are known, we can transform the vehicle’s speed along the forward direction ${v}_{o}$ to east, north and up velocities (${v}_{e},{v}_{n}$ and ${v}_{u}$, respectively) according to the following expressions:

$${v}_{e}={v}_{o}sinAcosp$$

$${v}_{n}={v}_{o}cosAcosp$$

$${v}_{u}={v}_{o}sinp$$

The east and north velocities are transformed into geodetic coordinates and then integrated over the sample interval to obtain positions in latitude φ and longitude λ. The vertical component of velocity is integrated to obtain altitude h. These quantities are calculated using the following equations:
where ${R}_{M}$ is the meridian radius of curvature and ${R}_{N}$ is the normal radius of the curvature of the Earth’s ellipsoid.

$$\dot{\text{\phi}}=\frac{{v}_{n}}{({R}_{M}+h)}$$

$$\dot{\text{\lambda}}=\frac{{v}_{e}}{({R}_{N}+h)cos\text{\phi}}$$

$$\dot{h}={v}_{u}$$

A block diagram that illustrates the RISS mechanization system is shown in Figure 3.

A close look at Equations (1)–(6) reveals that the major source of errors in the RISS system is the error in the gyroscope measurements (gyroscope bias ${b}_{z}$), because it propagates to the azimuth, which propagates into an error in the horizontal channel velocity and position. Since no integration is involved in roll and pitch calculation in Equations (2) and (3), errors in the accelerometers’ measurements have a small effect.

## 3. Proposed EKF-Based 3D RISS/GPS Integration Algorithm

In the proposed algorithm, new system and measurement models were developed where the estimation of pitch and roll angles was included in EKF for better estimation of the attitude. Similarly, accelerometer bias estimation was performed through a filtering process. Furthermore, a new RISS error model was derived, which included additional important terms to improve the accuracy of the algorithm. The following sections will give the details of the system and measurement models for the proposed EKF-based integration of RISS and GPS.

#### 3.1. System Model

The RISS error model is derived by linearization of the RISS mechanization Equations (1) and (4)–(9) use Taylor series expansion and retain only the first order terms. The error state vector of the 3D RISS is defined as follows:
where $\delta \text{\phi}$ is latitude error, $\delta \text{\lambda}$ is longitude error, $\delta h$ is altitude error, $\delta {v}_{e}$ is east velocity error, $\delta {v}_{n}$ is north velocity error, $\delta {v}_{u}$ is upward velocity error, $\delta A$ is azimuth error, $\delta {a}_{o}$ is error in acceleration derived from wheel rotation sensor measurements, $\delta {b}_{z}$ is gyroscope bias error, $\delta r$ is error in roll, $\delta p$ is error in pitch, $\delta {b}_{x}$ is bias error in lateral accelerometer, $\delta {b}_{y}$ is bias error in the forward accelerometer and $\delta {v}_{o}$ is error in forward velocity.

$$\begin{array}{cc}\hfill \delta {\mathbf{x}}_{R}=& {[\delta \text{\phi},\delta \text{\lambda},\delta h,\delta {v}_{e},\delta {v}_{n},\delta {v}_{u},\delta A,\delta {a}_{o},\delta {b}_{z},\delta r,\delta p,\delta {b}_{x},\delta {b}_{y},\delta {v}_{o}]}^{T}\hfill \end{array}$$

To get the rate of change of error states for the RISS system model, the motion equations of Section 2 have to be linearized through Taylor’s series expansion, and only the first order terms are retained. The error models for wheel rotation the sensor, gyroscope, accelerometers, pitch and roll angles are based on the first order Gauss–Markov processes. It is worth emphasizing here that the major contribution of this work lies in the treatment of velocities, azimuth and horizontal channel errors by augmenting EKF with additional error states. Therefore, their derivations will include some details where necessary. In order to facilitate the formulation of the final state transition matrix F, the position of the terms in the F matrix will be indicated by placing ${F}_{ab}$ under them. This indicates their position in the final F matrix, where subscript a denotes the row and b denotes the column.

#### 3.1.1. Latitude

The time rate of change of latitude φ is given in Equation (7), which is linearized using Taylor’s series as:
since ${({R}_{M}+h)}^{2}$ is a very large term, therefore:

$$\begin{array}{cc}\hfill \delta \dot{\text{\phi}}& =\frac{\partial [.]}{\partial {v}_{n}}\delta {v}_{n}+\frac{\partial [.]}{\partial h}\delta h\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta \dot{\text{\phi}}& =\frac{\delta {v}_{n}}{({R}_{M}+h)}-\frac{{v}_{n}\delta h}{{({R}_{M}+h)}^{2}}\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta \dot{\text{\phi}}& \approx \underset{{\text{F}}_{15}}{\underbrace{\frac{1}{({R}_{M}+h)}}}\delta {v}_{n}\hfill \end{array}$$

#### 3.1.2. Longitude

The time rate of change of longitude λ of Equation (8) is linearized through Taylor’s series as:
after ignoring the right most term due to ${({R}_{M}+h)}^{2}$ being very large, we get:

$$\begin{array}{cc}\hfill \delta \dot{\text{\lambda}}& =\frac{\partial [.]}{\partial {v}_{e}}\delta {v}_{e}+\frac{\partial [.]}{\partial \text{\phi}}\delta \text{\phi}+\frac{\partial [.]}{\partial h}\delta h\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta \dot{\text{\lambda}}& =\frac{\delta {v}_{e}}{({R}_{N}+h)cos\text{\phi}}+\frac{{v}_{e}sin\text{\phi}\delta \text{\phi}}{({R}_{N}+h){cos}^{2}\text{\phi}}-\frac{{v}_{e}\delta h}{{({R}_{N}+h)}^{2}cos\text{\phi}}\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta \dot{\text{\lambda}}& \approx \underset{{\text{F}}_{24}}{\underbrace{\frac{1}{({R}_{N}+h)cos\text{\phi}}}}\delta {v}_{e}+\underset{{\text{F}}_{21}}{\underbrace{\frac{{v}_{e}tan\text{\phi}}{({R}_{N}+h)cos\text{\phi}}}}\delta \text{\phi}\hfill \end{array}$$

#### 3.1.3. Altitude

The rate of change of altitude is directly related to the vertical velocity (Equation (9)), which can be linearized as:

$$\begin{array}{cc}\hfill \delta \dot{h}& =\underset{{\text{F}}_{36}}{\underbrace{1}}\times \delta {v}_{u}\hfill \end{array}$$

#### 3.1.4. Attitude

The differential equation of azimuth angle (refer Equation (1)) can also be linearized using Taylor’s series as follows:
we get:
taking care of the minus sign and ignoring the right most term, which is negligible:

$$\begin{array}{cc}\hfill \delta \dot{A}& =\frac{\partial [.]}{\partial {b}_{z}}\delta {b}_{z}+\frac{\partial [.]}{\partial \text{\phi}}\delta \phi +\frac{\partial [.]}{\partial {v}_{e}}\delta {v}_{e}+\frac{\partial [.]}{\partial h}\delta h\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta \dot{A}& =-\left[-\delta {b}_{z}-{\omega}^{e}cos\text{\phi}\delta \text{\phi}-\frac{{v}_{e}{sec}^{2}\text{\phi}\delta \text{\phi}}{{R}_{N}+h}-\frac{tan\text{\phi}\delta {v}_{e}}{{R}_{N}+h}+\frac{{v}_{e}tan\text{\phi}\delta h}{{({R}_{N}+h)}^{2}}\right]\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta \dot{A}& \approx \underset{{\text{F}}_{79}}{\underbrace{1}}\times \delta {b}_{z}+\underset{{\text{F}}_{71}}{\underbrace{\left({\omega}^{e}cos\text{\phi}+\frac{{v}_{e}{sec}^{2}\text{\phi}}{{R}_{N}+h}\right)}}\delta \text{\phi}+\underset{{\text{F}}_{74}}{\underbrace{\frac{tan\text{\phi}}{{R}_{N}+h}}}\delta {v}_{e}\hfill \end{array}$$

#### 3.1.5. East Velocity

East velocity ${v}_{e}$ is computed from the vehicle’s forward velocity ${v}_{o}$ (obtained through the wheel rotation sensor) as given in Equation (4). We first take its derivative with respect to time as:
substituting ${a}_{o}$ and ${v}_{n}$:

$$\begin{array}{cc}\hfill {\dot{v}}_{e}& ={\dot{v}}_{o}sinAcosp+{v}_{o}cosAcosp\dot{A}\hfill \end{array}$$

$$\begin{array}{cc}\hfill {\dot{v}}_{e}& ={a}_{o}sinAcosp+{v}_{n}\dot{A}\hfill \end{array}$$

Now, linearizing the above equation using Taylor’s series:
and after some mathematical manipulation, we finally obtain:

$$\begin{array}{cc}\hfill \delta {\dot{v}}_{e}& =\frac{\partial [.]}{\partial {a}_{o}}\delta {a}_{o}+\frac{\partial [.]}{\partial A}\delta A+\frac{\partial [.]}{\partial {v}_{n}}\delta {v}_{n}+\frac{\partial [.]}{\partial \dot{A}}\delta \dot{A}\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta {\dot{v}}_{e}& =\underset{{\text{F}}_{48}}{\underbrace{sinAcosp}}\delta {a}_{o}+\underset{{\text{F}}_{47}}{\underbrace{{a}_{o}cosAcosp}}\delta A-\underset{{\text{F}}_{45}}{\underbrace{\left({\omega}_{z}-{b}_{z}-{\omega}^{e}sin\text{\phi}-\frac{{v}_{e}tan\text{\phi}}{{R}_{N}+h}\right)}}\delta {v}_{n}+\underset{{\text{F}}_{49}}{\underbrace{{v}_{n}}}\delta {b}_{z}\hfill \\ & +\underset{{\text{F}}_{41}}{\underbrace{{v}_{n}\left({\omega}^{e}cos\text{\phi}+\frac{{v}_{e}{sec}^{2}\text{\phi}}{{R}_{N}+h}\right)}}\delta \text{\phi}+\underset{{\text{F}}_{44}}{\underbrace{\frac{{v}_{n}tan\text{\phi}}{{R}_{N}+h}}}\delta {v}_{e}\hfill \end{array}$$

#### 3.1.6. North Velocity

The vehicle’s north velocity ${v}_{n}$ is obtained from its forward velocity ${v}_{o}$, as depicted by Equation (5). Its time derivative is given as:

$$\begin{array}{cc}\hfill {\dot{v}}_{n}& ={\dot{v}}_{o}cosAcosp-{v}_{o}sinAcosp\dot{A}\hfill \end{array}$$

Substituting ${a}_{o}$ and ${v}_{e}$:

$$\begin{array}{cc}\hfill {\dot{v}}_{n}& ={a}_{o}cosAcosp-{v}_{e}\dot{A}\hfill \end{array}$$

Now, linearizing the above equation using Taylor’s series:
and after some mathematically manipulation, we get the following form:

$$\begin{array}{cc}\hfill \delta {\dot{v}}_{e}& =\frac{\partial [.]}{\partial {a}_{o}}\delta {a}_{o}+\frac{\partial [.]}{\partial A}\delta A+\frac{\partial [.]}{\partial {v}_{e}}\delta {v}_{e}+\frac{\partial [.]}{\partial \dot{A}}\delta \dot{A}\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta {\dot{v}}_{n}& =\underset{{\text{F}}_{58}}{\underbrace{cosAcosp}}\delta {a}_{o}-\underset{{\text{F}}_{57}}{\underbrace{{a}_{o}sinAcosp}}\delta A+\underset{{\text{F}}_{54}}{\underbrace{\left({\omega}_{z}-{b}_{z}-{\omega}^{e}sin\text{\phi}-\frac{2{v}_{e}tan\text{\phi}}{{R}_{N}+h}\right)}}\delta {v}_{e}\underset{{\text{F}}_{59}}{\underbrace{-{v}_{e}}}\delta {b}_{z}\hfill \\ & -\underset{{\text{F}}_{51}}{\underbrace{{v}_{e}\left({\omega}^{e}cos\text{\phi}+\frac{{v}_{e}{sec}^{2}\text{\phi}}{{R}_{N}+h}\right)}}\delta \text{\phi}\hfill \end{array}$$

#### 3.1.7. Up Velocity

Vertical velocity is related to the forward velocity and pitch angle, as shown in Equation (6). First, we take the derivative with respect to time:

$$\begin{array}{cc}\hfill {\dot{v}}_{u}& ={\dot{v}}_{o}sinp\hfill \end{array}$$

$$\begin{array}{cc}\hfill {\dot{v}}_{u}& ={a}_{o}sinp\hfill \end{array}$$

Now, linearize the above equation using Taylor’s series:

$$\begin{array}{cc}\hfill \delta {\dot{v}}_{u}& =\underset{{\text{F}}_{68}}{\underbrace{sinp}}\delta {a}_{o}\hfill \end{array}$$

#### 3.1.8. Forward Velocity

The vehicle’s forward velocity is obtained from wheel rotation sensor measurements (odometer measurements), and the rate of change of its error is equal to the error in acceleration. This can be modelled as:

$$\delta {\dot{v}}_{o}=\underset{{F}_{14,8}}{\underbrace{1}}\times \delta {a}_{o}$$

#### 3.1.9. Modelling of Horizontal Channel Errors

For land vehicles, roll and pitch angles have a limited range of values. Furthermore, in RISS, pitch and roll angles are computed using accelerometers instead of gyroscopes. Therefore, the errors in the horizontal channel are estimated through the use of first order Gauss–Markov processes as follows [35,36]:
where β and σ represent the reciprocal of the correlation times and standard deviations of the respective errors.

$$\begin{array}{cc}\hfill \delta \dot{r}& =-{\beta}_{r}\delta r+\sqrt{2{\beta}_{r}{\sigma}_{r}^{2}}w\left(t\right)\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta \dot{p}& =-{\beta}_{p}\delta p+\sqrt{2{\beta}_{p}{\sigma}_{p}^{2}}w\left(t\right)\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta {\dot{b}}_{x}& =-{\beta}_{x}\delta {b}_{x}+\sqrt{2{\beta}_{x}{\sigma}_{x}^{2}}w\left(t\right)\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta {\dot{b}}_{y}& =-{\beta}_{z}\delta {b}_{y}+\sqrt{2{\beta}_{y}{\sigma}_{y}^{2}}w\left(t\right)\hfill \end{array}$$

#### 3.1.10. Modelling of the Wheel Rotation Sensor and Gyroscope

The stochastic errors of the wheel rotation sensor and gyroscope are modelled by well-known first order Gauss–Markov processes.

$$\begin{array}{cc}\hfill \delta {\dot{a}}_{o}& =-{\beta}_{o}\delta {a}_{o}+\sqrt{2{\beta}_{o}{\sigma}_{o}^{2}}w\left(t\right)\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta {\dot{b}}_{z}& =-{\beta}_{z}\delta {b}_{z}+\sqrt{2{\beta}_{z}{\sigma}_{z}^{2}}w\left(t\right)\hfill \end{array}$$

#### 3.2. The GPS System Model for Tightly-Coupled RISS/GPS Integration

For tightly-coupled RISS/GPS integration, we have two additional states for the system model, which are GPS receiver clock bias $\delta {b}_{r}$ and its drift $\delta {d}_{r}$. The state vector for GPS error states is written as [14]:

$$\delta {\mathbf{x}}_{G}={[\delta {b}_{r},\delta {d}_{r}]}^{T}$$

The GPS receiver clock drift is modelled by a random walk process, and the receiver bias is the integral of the drift.

$$\delta {\dot{b}}_{r}=\delta {d}_{r}$$

$$\delta {\dot{d}}_{r}={\sigma}_{d}{w}_{G}$$

Based on the equations of various errors states derived in Section 3.1, the complete dynamic matrix can be constructed as shown in Equation (42):

$$F=\left[\begin{array}{cccccccccccccccc}0& 0& 0& 0& {F}_{15}& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ {F}_{21}& 0& 0& {F}_{24}& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ {F}_{41}& 0& 0& {F}_{44}& {F}_{45}& 0& {F}_{47}& {F}_{48}& {F}_{49}& 0& 0& 0& 0& 0& 0& 0\\ {F}_{51}& 0& 0& {F}_{54}& 0& 0& {F}_{57}& {F}_{58}& {F}_{59}& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& {F}_{68}& 0& 0& 0& 0& 0& 0& 0& 0\\ {F}_{71}& 0& 0& {F}_{74}& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& {F}_{88}& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& {F}_{99}& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& -{\beta}_{r}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& -{\beta}_{p}& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& -{\beta}_{x}& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& -{\beta}_{y}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$$

#### 3.3. Measurements Model for Tightly-Coupled RISS/GPS Integration

The general nonlinear measurement model for KF is expressed as:
where $\mathbf{z}$ is the measurement vector and $h\left(x\right)$ is the measurement design matrix, which relates the measurement vector with state vector $\mathbf{x}$ in a nonlinear fashion. The quantity
where $\delta \mathbf{z}$ is the error in the measurement vector, His the measurement design matrix and $\delta \mathbf{x}$ is the error state vector.

$$\mathbf{z}=h\left(\mathbf{x}\right)+\mathbf{\eta}$$

**η**represents the measurements noise vector. Since KF works on linearized models, the linearized measurement model is given as:
$$\delta \mathbf{z}=H\delta \mathbf{x}+\mathbf{\eta}$$

This work contains two sets of measurements that come from the GPS and two lateral accelerometers. Both of these are non-linearly related to the state vector; therefore, the next two sub-sections will address these models. The GPS measurement model has been detailed in earlier literature; therefore, only relevant details will be given, and the appropriate literature will be referenced. The accelerometer measurement model is a new contribution, which shall be derived in detail.

#### 3.3.1. GPS Measurement Updates

For the tightly-coupled integration, the measurement model is more involved, because the measurements are the difference in pseudoranges and pseudorange rates (instead of position and velocity), whereas the state vector consists of position, velocity, attitudes, etc. The measurement vector for M number of satellites is expressed as [14]:
where ${\text{\rho}}_{RSS}$, ${\dot{\text{\rho}}}_{RSS}$ are RISS estimated range and range rates and ${\text{\rho}}_{GPS}$, ${\dot{\text{\rho}}}_{GPS}$ are GPS measured range and range rates between the satellite and the receiver.

$$\delta {\mathbf{z}}_{GPS}=\left[\begin{array}{c}{\text{\rho}}_{RSS}^{1}-{\text{\rho}}_{GPS}^{1}\\ {\text{\rho}}_{RSS}^{2}-{\text{\rho}}_{GPS}^{2}\\ \vdots \\ {\text{\rho}}_{RSS}^{M}-{\text{\rho}}_{GPS}^{M}\\ {\dot{\text{\rho}}}_{RSS}^{1}-{\dot{\text{\rho}}}_{GPS}^{1}\\ {\dot{\text{\rho}}}_{RSS}^{2}-{\dot{\text{\rho}}}_{GPS}^{2}\\ \vdots \\ {\dot{\text{\rho}}}_{RSS}^{M}-{\dot{\text{\rho}}}_{GPS}^{M}\end{array}\right]$$

The H matrix of the overall GPS measurement model for the tightly-coupled integration with pseudorange and Doppler measurements from a total of M satellites will be of the following form [14]:
where ${H}_{\text{\rho}}=G\times L$ and ${H}_{\dot{\rho}}=G\times {R}_{l}^{e}$. Here, G is the geometry matrix, L is a matrix, which represents the linearized form of the relationship that converts the geodetic position coordinates to ECEFCartesian coordinates, and ${R}_{l}^{e}$ is the transformation matrix from the local level frame to the ECEF frame. These matrices are defined as:
where ${\mathbf{1}}_{x,RSS}^{m}$ is the line of sight unit vector from the m-th satellite to the receiver position based on the output of RISS mechanization.

$$\begin{array}{cc}& {H}_{GPS}=\left[\begin{array}{cccccc}{H}_{\text{\rho},M\times 3}& {0}_{M\times 3}& {0}_{M\times 3}& -{1}_{M\times 1}& {0}_{M\times 1}& {0}_{M\times 5}\\ {0}_{M\times 3}& {H}_{\dot{\rho},M\times 3}& {0}_{M\times 3}& {0}_{M\times 1}& -{1}_{M\times 1}& {0}_{M\times 5}\end{array}\right]\hfill \end{array}$$

$$G=\left[\begin{array}{ccc}{\mathbf{1}}_{x,RSS}^{1}& {\mathbf{1}}_{y,RSS}^{1}& {\mathbf{1}}_{z,RSS}^{1}\\ {\mathbf{1}}_{x,RSS}^{2}& {\mathbf{1}}_{y,RSS}^{2}& {\mathbf{1}}_{z,RSS}^{2}\\ \vdots \\ {\mathbf{1}}_{x,RSS}^{M}& {\mathbf{1}}_{y,RSS}^{M}& {\mathbf{1}}_{z,RSS}^{M}\end{array}\right]$$

$$L=\left[\begin{array}{ccc}-\left({R}_{N}+h\right)sin\text{\phi}cos\text{\lambda}& -\left({R}_{N}+h\right)cos\text{\phi}sin\text{\lambda}& cos\text{\phi}cos\text{\lambda}\\ -\left({R}_{N}+h\right)sin\text{\phi}sin\text{\lambda}& +\left({R}_{N}+h\right)cos\text{\phi}cos\text{\lambda}& cos\text{\phi}sin\text{\lambda}\\ \left[{R}_{N}\left(1-{e}^{2}\right)+h\right]cos\text{\phi}& 0& sin\text{\phi}\end{array}\right]$$

$${R}_{l}^{e}=\left[\begin{array}{ccc}-sin\text{\lambda}& -sin\text{\phi}cos\text{\lambda}& cos\text{\phi}cos\text{\lambda}\\ cos\text{\lambda}& -sin\text{\phi}sin\text{\lambda}& cos\text{\phi}sin\text{\lambda}\\ 0& cos\text{\phi}& sin\text{\phi}\end{array}\right]$$

#### 3.3.2. Horizontal Channel Measurement Updates

Apart from GPS, we also have measurements from forward accelerometer $fy$ and lateral accelerometer $fx$, which constitute the horizontal channel. The measurement vector for the two accelerometers is expressed as:
where $EST$ denotes the estimated quantity and $OBS$ denotes the observed or measured value.

$$\delta {\mathbf{z}}_{RSS}=\left[\begin{array}{c}{f}_{x,EST}-{f}_{x,OBS}\\ {f}_{y,EST}-{f}_{y,OBS}\end{array}\right]$$

Since these measurements are not linearly related to the states, we need to linearize them, as well. This linearization is carried out next, where the terms below underbraces represent their place in design matrix ${H}_{RSS}$.

**Lateral Accelerometer**Assuming that the lateral accelerometer measurement has a bias ${b}_{x}$, it can be related to various states as follows:

$$\begin{array}{cc}\hfill {f}_{x}& =-gsinrcosp-{v}_{o}({\omega}_{z}-{b}_{z})+{b}_{x}\hfill \end{array}$$

Linearizing the above equation using Taylor’s series:
we obtain the following:

$$\begin{array}{cc}\hfill \delta {f}_{x}& =\frac{\partial [.]}{\partial r}\delta r+\frac{\partial [.]}{\partial p}\delta p+\frac{\partial [.]}{\partial {v}_{o}}\delta {v}_{o}+\frac{\partial [.]}{\partial {b}_{z}}\delta {b}_{z}+\frac{\partial [.]}{\partial {b}_{x}}\delta {b}_{x}\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta {f}_{x}& =\underset{{H}_{1,12}}{\underbrace{-gcosrcosp}}\delta r+\underset{{H}_{1,13}}{\underbrace{gsinrsinp}}\delta p\underset{{H}_{1,16}}{\underbrace{-({\omega}_{z}-{b}_{z})}}\delta {v}_{o}+\underset{{H}_{1,9}}{\underbrace{{v}_{o}}}\delta {b}_{z}+\underset{{H}_{1,14}}{\underbrace{\delta {b}_{x}}}\hfill \end{array}$$

**Forward Accelerometer**Similarly, assuming that the forward accelerometer measurement has a bias ${b}_{y}$, it can be related to various states as follows:

$$\begin{array}{cc}\hfill {f}_{y}& =gsinp+{a}_{o}+{b}_{y}\hfill \end{array}$$

Linearizing the above equation using Taylor’s series:
we obtain the following:

$$\begin{array}{cc}\hfill \delta {f}_{y}& =\frac{\partial [.]}{\partial p}\delta p+\frac{\partial [.]}{\partial {a}_{o}}\delta {a}_{o}+\frac{\partial [.]}{\partial {b}_{y}}\delta {b}_{y}\hfill \end{array}$$

$$\begin{array}{cc}\hfill \delta {f}_{y}& =\underset{{H}_{2,13}}{\underbrace{gcosp}}\delta p+\underset{{H}_{1,8}}{\underbrace{\delta {a}_{o}}}+\underset{{H}_{2,15}}{\underbrace{\delta {b}_{y}}}\hfill \end{array}$$

In light of the above, the ${H}_{RSS}$ matrix of the measurement model for the two accelerometers can be written as:

$${H}_{RSS}=\left[\begin{array}{ccccccccc}{\mathbf{0}}_{1\times 7}& 0& {H}_{1,9}& {\mathbf{0}}_{2\times 2}& {H}_{1,12}& {H}_{1,13}& {H}_{1,14}& 0& {H}_{1,16}\\ {\mathbf{0}}_{1\times 7}& {H}_{2,8}& 0& {\mathbf{0}}_{2\times 2}& 0& {H}_{2,13}& 0& {H}_{2,15}& 0\end{array}\right]$$

The overall measurement model can be expressed as:

$$\left[\begin{array}{c}\delta {\mathbf{z}}_{GPS}\\ \delta {\mathbf{z}}_{RSS}\end{array}\right]={\left[\begin{array}{c}{H}_{GPS}\\ {H}_{RSS}\end{array}\right]}_{(2M+2)\times 16}\delta \mathbf{x}+\mathbf{\eta}$$

The Kalman filter and its variants are well known, and there is a great deal of literature that explains all of their details and nuances. Therefore, the details will not be repeated in this text. However, the reader is referred to some excellent material in [35,36,37,38]. The block diagrams of tightly-coupled integration with a closed-loop scheme are shown in Figure 4.

## 4. Experimental Work and Results

#### 4.1. Equipment Setup

The performance of the proposed improved tightly-coupled 3D RISS/GPS algorithm was examined on real road data using a low-cost MEMS-based Crossbow IMU300CC along with odometry obtained from the vehicle’s OBDIIinterface using the Carchip data logger. Figure 5 shows the equipment inside a van that was used to conduct the road experiments to collect the data.

The reference systems are based on the Honeywell HG1700 AG17 high-end tactical-grade IMU, which is tightly integrated with the NovAtel OEM4 GPS receiver using the OEM4-G2 ProPak-G2plus SPAN unit developed by NovAtel. Table 1 lists the major specification of the IMU300CC and HG1700 IMUs.

The results of two test trajectories are reported for this paper to show the performance of the proposed algorithm and for a comparison with earlier works. The selection of these trajectories was based on the criteria that they include driving conditions that a driver may encounter in a typical road trip through downtown, urban and suburban areas. The purpose of downtown driving, especially for the Toronto downtown trajectory, was to see the performance of the algorithm in severe multipath and poor GPS satellite visibility.

**Figure 5.**Navigation equipment and supporting hardware placed in a van for trajectory data collection.

Crossbow IMU300CC | HG1700 | |
---|---|---|

Size | 7.62 × 9.53 × 8.13 cm | 16 × 16 × 10 cm |

Weight | 0.59 kg | 3.4 kg |

Max data rate | 200 Hz | 100 Hz |

Start-up time | <1 s | <0.8 s |

Accelerometer | ||

Range | ±2 g | ±50 g |

Bias | $<\pm $ 30 mg | 1 mg |

Scale factor | <1 % | 300 ppm |

Random walk | <0.15 m/s/$\sqrt{\text{h}}$ | <0.198 m/s/$\sqrt{\text{h}}$ |

Gyroscope | ||

Range | $\pm {100}^{\circ}$/s | $\pm {1000}^{\circ}$/s |

Bias | $<\pm 2.{0}^{\circ}$/s | 1${}^{\circ}$/h |

Scale factor | <1% | 150 ppm |

Random walk | $<2.{25}^{\circ}/\sqrt{\text{h}}$ | $<0.{125}^{\circ}/\sqrt{\text{h}}$ |

#### 4.2. Evaluation and Comparison Criteria

The criteria for evaluating the algorithm were based on the maximum 2D position errors during the satellite blockages. The 2D positional accuracy of the proposed system (TC-EKF) is compared to earlier RISS/GPS integrated systems by quantifying the overall percentage improvement. These include loosely-coupled LKF (LC-LKF) [28] and loosely-coupled PF (LC-PF) [31], as well as tightly-coupled LKF (TC-LKF) [14]. When comparing the performance of the proposed system with other systems during GPS outages, we take the zero satellite case. In addition, comparative error plots shall be given when comparing the TC-LKF with the proposed TC-EKF for a more thorough examination of positional errors.

#### 4.3. Kingston Downtown Trajectory

This trajectory was conducted in the heart of Kingston city, and ten GPS signal outages were introduced at locations, such that they encompass all challenging driving scenarios, including low speeds, normal turns, sharp turns, high speeds and slopes, etc. The outage duration was fixed to 60 s, and every outage was repeated four times, where visible satellites were decreased from three to zero, one by one, while the response and accuracy of the algorithm was analysed. Figure 6 depicts this trajectory where outage locations are shown with black circles.

#### 4.3.1. Positional Errors

The resulting maximum horizontal position errors during the ten outages, each introduced by limiting the number of satellite to 3, 2, 1 and zero, are plotted as a bar graph in Figure 7.

Here, it is important to note that, although, in general, the errors increase when the number of satellites decreases, however, in some outages, the addition of satellites actually increases the error. This happens when a satellite is at a low elevation; its signal is severely affected by atmospheric errors, and it is more prone to produce multipath effects [39]. Therefore, when this satellite is added to available satellites, the accuracy will decrease instead of improving. We also notice that in some cases, zero and one available satellite show better performance than two or three. This can happen if the external sensor aiding (inertial and wheel rotation sensors) is better due to reduced sensors and a superior algorithm, which accurately estimates sensor errors. This shows that that the algorithm can handle the partial, as well as total GPS signal blockage very well. Taking the zero satellite case as a reference, there is an improvement of 57% over LC-LKF [28], 16% over TC-LKF [14] and 28% over PF [31] of earlier works. A comparison of proposed TC-EKF, TC-LKF of [14] and PF of [31] is given in the bar plots of Figure 8 for the overall maximum horizontal positional errors. It may be noted that for the case of PF, only the zero satellite case can be compared, as it is based on a loosely-coupled architecture. It is evident that TC-EKF performs better then TC-LKF for all cases (3, 2, 1 and 0 satellites), and on average, the maximum position error is always less then 12 m, irrespective of the number of visible satellites. Figure 9 shows a similar comparison for the overall RMS horizontal positional errors. PF could not be included in this comparison, as it was not reported in the PF paper [31].

**Figure 8.**A comparison of tightly-coupled (TC-EKF), TC-linearized KF (LKF) and particle filter (PF) for the average maximum 2D position error for the Kingston downtown trajectory.

**Figure 9.**A comparison of TC-EKF with TC-LKF for the average RMS 2D position error for the Kingston downtown trajectory.

Table 2 summarizes the RMS errors for 3D position, attitude, as well as velocities during all of the full outages (zero available satellite) for the Kingston downtown trajectory.

Outage No. | Position (m) | Attitude (${}^{\circ}$) | Velocity (m/s) | ||||||||
---|---|---|---|---|---|---|---|---|---|---|---|

Lat | Long | Alt | Pitch | Roll | Azi | Ve | Vn | Vu | |||

1 | 4.46 | 2.42 | 3.71 | 1.73 | 0.70 | 1.17 | 0.55 | 0.48 | 1.12 | ||

2 | 12.54 | 3.99 | 4.76 | 1.73 | 0.55 | 1.48 | 0.59 | 0.75 | 1.20 | ||

3 | 2.86 | 8.12 | 4.50 | 1.74 | 0.48 | 0.37 | 0.74 | 0.33 | 0.62 | ||

4 | 2.35 | 7.26 | 5.12 | 1.88 | 0.31 | 0.33 | 0.67 | 0.23 | 0.92 | ||

5 | 7.55 | 6.67 | 1.78 | 2.84 | 0.20 | 1.15 | 0.77 | 0.20 | 0.44 | ||

6 | 4.70 | 1.60 | 1.36 | 2.51 | 0.31 | 0.28 | 0.36 | 0.57 | 0.87 | ||

7 | 2.27 | 4.17 | 4.65 | 1.31 | 0.58 | 1.80 | 0.67 | 0.56 | 1.00 | ||

8 | 7.70 | 8.25 | 4.83 | 1.65 | 0.35 | 0.90 | 0.49 | 0.26 | 0.57 | ||

9 | 6.14 | 1.32 | 1.70 | 1.37 | 0.53 | 1.41 | 0.73 | 0.57 | 0.70 | ||

10 | 4.24 | 2.70 | 0.76 | 1.68 | 0.59 | 0.47 | 0.19 | 0.41 | 0.39 | ||

Average | 5.48 | 4.65 | 3.32 | 1.84 | 0.46 | 0.94 | 0.58 | 0.44 | 0.78 |

To show the robustness of the RISS system and the ability to reject or mitigate positional errors due to multipath or degraded GPS signals, a portion of the trajectory is shown in Figure 10 where GPS updates (in red) are totally unreliable (most likely due to multipath effects). The figure clearly shows the efficacy of the proposed TC-EKF system (in green) in mitigating the positional inaccuracy during the wrong GPS updates by keeping the trajectory on the road instead of following the erratic GPS measurements.

#### 4.3.2. Tilt Angle Errors

As indicated earlier, the proposed algorithm updates the pitch and roll angles by estimating accelerometer biases through EKF. Although the pitch and roll angles are very small for a land vehicle application, they are reported here to have a feel of their trend and to show the performance of the algorithm and its limitations. The plots of pitch and roll angles for the whole trajectory are shown in Figure 11 and Figure 12, respectively. These figures show the performance of the proposed algorithm (labelled as “Kalman”) with respect to the reference (labelled as “NovAtel”). They also show the plot of the difference (error) between the two systems. For this trajectory, the RMS for the pitch angle was 1.82${}^{\circ}$, and for roll angle, it was 0.52${}^{\circ}$.

**Figure 11.**(Top) Performance of estimated pitch angle versus the reference (NovAtel) for the Kingston trajectory. The bottom plot shows the difference between the two systems.

**Figure 12.**(Top) Performance of estimated roll angle versus the reference (NovAtel) for the Kingston trajectory. The bottom plot shows the difference between the two systems.

It can be observed from Figure 11 that the pitch angle estimation is relatively noisy. This is due to the fact that its computation involves the differentiation of wheel rotation sensor measurements, which resulted in an increased noise level. This is one of the limitations of our method, which calls for future research.

#### 4.3.3. Gyroscope Bias

As described earlier, the proposed system developed a new approach to estimate the gyroscope bias, which helps to improve the navigational accuracy during GPS outages. The convergence of this bias to its actual value is an important test for the algorithm and crucial to positional accuracy. Figure 13 depicts the the behaviour of the gyroscope bias convergence where its estimate converged to its true bias value of ≈−0.24${}^{\circ}$/s. The convergence starts with motion and takes almost 300 s to converge. A similar behaviour and the convergence value is shown later for the other three trajectories, as well, proving the repeatability of the algorithm.

#### 4.4. Toronto Downtown Trajectory

To show the performance of the proposed algorithm in severe natural GPS outages, the Toronto downtown trajectory was selected. The trajectory lasted for about 90 min, and the main portion of the trajectory is depicted in Figure 14, where natural outages are marked with black circles/ellipses and their duration is written beside each outage. This trajectory is particularly challenging, as the test vehicle was driven through the heart of this metropolitan city, which has very tall buildings, tight turns, bridges and long overpasses, which severely restrict the availability of the visible satellite and also generate strong multipath signals.

Figure 15 gives an idea about the limited number of GPS satellites that were visible during the trajectory. Except for the initial portion and a couple of other places, most of the trajectory sees less than four satellites, and in many cases, the visible satellites even drop to zero. In this condition, we cannot take the earlier approach, where we reduced the number of available satellites from four gradually to zero in post-processing; rather, we pick the places of the natural outages and compare these to the results of earlier works.

#### 4.4.1. Positional Errors

The 2D maximum positional errors of the proposed work during nine natural GPS outages of varying duration are compared to three of the earlier works. Figure 16 shows the comparison of the results with the LC-LKF of [28], the TC-LKF of [14] and the PF of [31] for all nine natural GPS outages of different durations. It can be seen that except for a couple of outages, the proposed TC-EKF performed better then both of the competitors based on KF. On average, TC-EKF outperformed LC-LKF by 50% and TC-LKF by 63%. As compared to PF [31], TC-EKF performed 35% poorer; however, it may be noted that KF still has the advantage of being computationally less expensive.

Table 3 lists the RMS errors for 3D position, attitude, as well as velocities during all of the natural outages for the Toronto downtown trajectory.

Outage No. | Outage Duration | Position (m) | Attitude (deg) | Velocity (m/s) | ||||||||
---|---|---|---|---|---|---|---|---|---|---|---|---|

Lat | Long | Alt | Pitch | Roll | Azi | Ve | Vn | Vu | ||||

1 | 350 | 9.05 | 14.24 | 12.18 | 3.17 | 0.61 | 2.31 | 1.28 | 1.92 | 1.94 | ||

2 | 95 | 8.34 | 5.18 | 3.16 | 3.05 | 0.35 | 1.99 | 0.62 | 0.58 | 0.98 | ||

3 | 172 | 15.77 | 6.10 | 10.60 | 2.94 | 0.32 | 3.80 | 0.64 | 0.41 | 0.55 | ||

4 | 65 | 7.77 | 11.78 | 42.20 | 1.63 | 0.14 | 2.08 | 0.34 | 0.10 | 0.35 | ||

5 | 44 | 7.82 | 2.53 | 5.35 | 4.34 | 0.23 | 1.28 | 0.64 | 0.24 | 1.06 | ||

6 | 36 | 2.44 | 1.64 | 4.73 | 3.59 | 0.86 | 4.18 | 1.97 | 1.76 | 2.04 | ||

7 | 425 | 9.42 | 14.37 | 9.70 | 3.43 | 0.41 | 3.56 | 0.76 | 0.75 | 0.94 | ||

8 | 100 | 5.36 | 2.04 | 3.54 | 4.28 | 0.96 | 48.79 | 1.24 | 1.40 | 1.58 | ||

9 | 38 | 1.83 | 6.36 | 4.25 | 3.26 | 1.41 | 7.59 | 1.49 | 2.13 | 0.32 | ||

Average | 147 | 7.53 | 7.14 | 10.63 | 3.30 | 0.59 | 8.40 | 1.00 | 1.03 | 1.08 |

The Toronto downtown trajectory contains many signal multipath phenomena, which typically result in huge positional error. To show the performance of the proposed scheme in these environments, two sections of the trajectory are chosen; the first one encompasses outage Numbers 2, 3 and 4 and is shown in Figure 17; the second section contains outage Number 7, which is shown in Figure 18. In these examples, the GPS signal (in green) is going haywire, which is totally untrustworthy. However, the proposed algorithm (in red) is not affected by these jumps in GPS signal and stays very close to the road even in these severe environments.

#### 4.4.2. Tilt Angle Errors

The plots of pitch and roll angles for the Toronto trajectory are shown in Figure 19 and Figure 20, respectively. These figures show the performance of the proposed algorithm (labelled as “Kalman”) with respect to the reference (labelled as “NovAtel”) and the difference between them. For this trajectory, the RMS for the pitch angles was 3.11${}^{\circ}$, and for roll angle, it was 0.64${}^{\circ}$.

Here, also, it can be noticed from Figure 19 that the pitch angle estimation is relatively noisy for the same reason as highlighted earlier for the Kingston trajectory.

After incorporating the correction of pitch and roll angles (through the estimation of corresponding accelerometers biases), we noticed no appreciable improvement in pitch and roll angles. Although there is no tangible difference in pitch/roll estimation accuracy using the KF-based approach, the KF-based design enables the system to accept external roll/pitch updates (if/when available). This has the advantage of having the roll/pitch angle estimation within the filter design and processed in the main-stream of the KF algorithm, which makes the filter expandable for future design.

**Figure 19.**(Top) Performance of estimated pitch angle versus the reference for the Toronto trajectory. The bottom plot shows the difference between the two systems.

**Figure 20.**(Top) Performance of estimated roll angle versus the reference for the Toronto trajectory. The bottom plot shows the difference between the two systems.

#### 4.4.3. Gyroscope Bias

It is interesting to see the convergence of the gyroscope bias value during this challenging trajectory, because the GPS updates are really noisy, and at times, there is no GPS signal at all, as already seen in Figure 15. Figure 21 shows the convergence of gyroscope bias during this trajectory, which successfully converges close to its actual values and stays smooth and steady. This trend helps the algorithm to stay close to the reference during GPS outages and noisy GPS updates.

## 5. Conclusions

This paper introduced an EKF-based 3D RISS/GPS integrated system where a new RISS dynamic error model was introduced, which models the effect of tilt angel errors and accelerometer errors. Furthermore, it included important terms in the system dynamic error model previously ignored during the linearization process in earlier works on RISS. As opposed to the earlier LKF-based algorithms, this proposed scheme uses EKF for RISS/GPS integration, where the RISS navigation output is corrected by EKF-estimated errors, which continuously keeps the linearization within an acceptable, accurate range, keeping the errors bounded. Road test results showed that the proposed EKF-based RISS/GPS system with the new error model outperforms the loosely-coupled LKF-based and tightly-coupled LKF-based RISS/GPS systems (with the over-approximated error model), providing more sustainable performance during long GPS outages. The proposed system demonstrated more accurate estimation of navigation state errors and sensor biases. In general, the proposed integration scheme showed higher accuracy, robustness and repeatability, outperforming the loosely-coupled LKF-based RISS/GPS integration by 53% and the tightly-coupled LKF-based RISS/GPS integration by 40% in overall maximum 2D positional accuracy. It showed slightly (4%) poorer performance than the loosely-coupled PF-based RISS/GPS. However, it may be noticed that KF has the advantage of lower computational burden over PF, which is crucial for real-time applications. Based on the superior results and use of low-cost sensors, the proposed navigation system is very promising for land vehicle navigation, as well as robotic applications.

## Acknowledgments

The authors are thankful to Natural Sciences and Engineering Research Council of Canada (NSERC) and Queens University for their scholarship awards.

## Author Contributions

Authors contributed equally to this paper.

## Conflicts of Interest

There is no conflict of interest.

## References

- Misra, P.; Enge, P. Global Positioning System: Signals, Measurements, and Performance; Ganga-Jamuna Press: Lincoln, MA, USA, 2001. [Google Scholar]
- Hui, L.; Darabi, H.; Banerjee, P.; Jing, L. Survey of wireless indoor positioning techniques and systems. IEEE Trans. Syst. Man Cybern. Part C (Appl. Rev.)
**2007**, 37, 1067–1080. [Google Scholar] - El-Rabbany, A. Introduction to GPS: The Global Positioning System, 2nd ed.; Artech House: Norwood, MA, USA, 2006. [Google Scholar]
- Farrell, J.A. Aided Navigation: GPS with High Rate Sensors; McGraw-Hill: New York, NY, USA, 2008. [Google Scholar]
- Groves, P.D. Principles of GNSS, Inertial, and Multi-sensor Integrated Navigation Systems; Artech House: Boston, MA, USA, 2008. [Google Scholar]
- Dissanayake, G.; Sukkarieh, S.; Nebot, E.; Durrant-Whyte, H. The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications. IEEE Trans. Robot. Autom.
**2001**, 17, 731–747. [Google Scholar] [CrossRef] - Atia, M.M.; Georgy, J.; Korenberg, M.J.; Noureldin, A. Real-time implementation of mixture particle filter for 3D RISS/GPS integrated navigation solution. Electron. Lett.
**2010**, 46, 1083–1084. [Google Scholar] [CrossRef] - Iqbal, U.; Okou, F.; Noureldin, A. An Integrated Reduced Inertial Sensor System-RISS/GPS for Land Vehicles. In Proceeding of the IEEE/ION Position, Location and Navigation Symposium, Monterey, CA, USA, 5–8 May 2008; The Institute of Navigation: Monterey, CA, USA; pp. 1014–1021.
- Wang, J.H.; Gao, Y. Land Vehicle Dynamics-Aided Inertial Navigation. IEEE Trans. Aerosp. Electron. Syst.
**2010**, 46, 1638–1653. [Google Scholar] [CrossRef] - Noureldin, A.; Karamat, T.B.; Georgy, J. Fundamentals of Inertial Navigation, Satellite-Based Positioning and Their Integration; Springer: Heidelberg, Germany, 2013. [Google Scholar]
- Titterton, D.H.; Weston, J. Strapdown Inertial Navigation Technology, 2nd ed.; IEE Radar, Sonar, Navigation and Avionics Series; American Institute of Aeronautics and Astronautics: New York, NY, USA, 2005. [Google Scholar]
- Lawrence, A. Modern Inertial Technology: Navigation, Guidance, and Control, 2nd ed.; Springer: New York, NY, USA, 1998. [Google Scholar]
- Aggarwal, P.; Syed, Z.; Aboelmagd, N.; Naser, E.S. MEMS-Based Integrated Navigation; Technology and Applications, Artech House: Boston, MA, USA; London, UK, 2010. [Google Scholar]
- Karamat, T.B.; Georgy, J.; Iqbal, U.; Noureldin, A. A Tightly-Coupled Reduced Multi-Sensor System for Urban Navigation. In Proceedings of the 22nd International Technical Meeting of the Satellite Division of the Institute of Navigation-ION GNSS 2009, Savannah, GA, USA, 22–25 September 2009; pp. 582–592.
- Rezaei, S.; Sengupta, R. Kalman Filter-Based Integration of DGPS and Vehicle Sensors for Localization. IEEE Trans. Control Syst.Technol.
**2007**, 15, 1080–1088. [Google Scholar] [CrossRef] - Adams, M.D. Lidar design, use, and calibration concepts for correct environmental detection. IEEE Trans. Robot. Autom.
**2000**, 16, 753–761. [Google Scholar] [CrossRef] - Soloviev, A.; Bates, D.; van Graas, F. Tight coupling of laser scanner and inertial measurements for a fully autonomous relative navigation solution. Navig. J. Inst. Navig.
**2007**, 54, 189–205. [Google Scholar] [CrossRef] - Degen, C.; El Mokni, H.; Govaers, F. Evaluation of a coupled laser inertial navigation system for pedestrian tracking. In Proceedings of the 15th International Conference on Information Fusion, FUSION 2012, Singapore, 9–12 July 2012; pp. 1292–1299.
- Soloviev, A.; Venable, D. Integration of GPS and vision measurements for navigation in GPS challenged environments. In Proceedings of the Position Location and Navigation Symposium (PLANS), 2010 IEEE/ION, Indian Wells, CA, USA, 4–6 May 2010; pp. 826–833.
- Gamallo, C.; Quintia, P.; Iglesias-Rodriguez, R.; Lorenzo, J.; Regueiro, C. Combination of a low cost GPS with visual localization based on a previous map for outdoor navigation. In Proceedings of the 2011 11th International Conference on Intelligent Systems Design and Applications (ISDA), Cordoba, Spain, 22–24 November 2011; pp. 1146–1151.
- Lin, F.; Chen, B.; Lee, T. Vision aided motion estimation for unmanned helicopters in GPS denied environments. In Proceedings of the 2010 IEEE Conference on Cybernetics and Intelligent Systems (CIS), Singapore, 28–30 June 2010; pp. 64–69.
- Mannings, R. Ubiquitous Positioning; Artech House: Boston, MA, USA, 2008. [Google Scholar]
- Miller, I.; Campbell, M. Particle filtering for map-aided localization in sparse GPS environments. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 2008), Pasadena, CA, USA, 19–23 May 2008; pp. 1834–1841.
- Jianjun, D.; Xiaohong, C. An improved map-matching model for GPS data with low polling rates to track vehicles. In Proceedings of the 2011 International Conference on Transportation, Mechanical, and Electrical Engineering (TMEE), Changchun, China, 16–18 December 2011; pp. 299–302.
- Abdulrahim, K.; Hide, C.; Moore, T.; Hill, C. Aiding MEMS IMU with building heading for indoor pedestrian navigation. UPINLBS
**2010**, 2010, 1–6. [Google Scholar] - Lou, L.; Xu, X.; Cao, J.; Chen, Z.; Xu, Y. Sensor fusion-based attitude estimation using low-cost MEMS-IMU for mobile robot navigation. In Proceedings of the 2011 6th IEEE Joint International Information Technology and Artificial Intelligence Conference (ITAIC), Chongqing, China, 20–22 August 2011; Volume 2, pp. 465–468.
- Brandt, A.; Gardner, J. Constrained Navigation Algorithms for Strapdown Inertial Navigation Systems with Reduced Set of Sensors. In Proceedings of the 1998 American Control Conference (ACC), Philadelphia, PA, USA, 24–26 June 1998; American Autom. Control Council: Philadelphia, PA, USA; Volume 3, pp. 1848–1852.
- Iqbal, U.; Karamat, T.B.; Okou, A.F.; Noureldin, A. Experimental Results on an Integrated GPS and Multisensor System for Land Vehicle Positioning. Int. J. Navig. Observ. Hindawi Publ. Corp.
**2009**, 2009, 18. [Google Scholar] [CrossRef] - Georgy, J.; Karamat, T.B.; Iqbal, U.; Noureldin, A. Enhanced MEMS-IMU/odometer/GPS integration using mixture particle filter. GPS Solut.
**2010**, 15, 239–252. [Google Scholar] [CrossRef] - Cossaboom, M.; Georgy, J.; Karamat, T.B.; Noureldin, A. Augmented Kalman Filter and Map Matching for 3D RISS/GPS Integration for Land Vehicles. Int. J. Navig. Observ.
**2012**, 2012, 16. [Google Scholar] [CrossRef] - Georgy, J.; Noureldin, A.; Korenberg, M.J.; Bayoumi, M.M. Low-cost three-dimensional navigation solution for RISS/GPS integration using mixture particle filter. IEEE Trans. Vehicul. Technol.
**2010**, 59, 599–615. [Google Scholar] [CrossRef] - Iqbal, U. An Integrated Low Cost Reduced Inertial Sensor System/GPS for Land Vehicle Applications. Ph.D. Thesis, Royal Military College of Canada, Kingston, ON, Canada, 2008. [Google Scholar]
- Noureldin, A.; Irvine-Halliday, D.; Mintchev, M.P. Measurement-while-drilling surveying of highly inclined and horizontal well sections utilizing single-axis gyro sensing system. Meas. Sci. Technol.
**2004**, 15, 2426–2434. [Google Scholar] [CrossRef] - Noureldin, A.; Irvine-Halliday, D.; Mintcheve, M. Accuracy Limitations of FOG-Based Continuous Measurement-While-Drilling Surveying Instruments for Horizontal Wells. IEEE Trans. Instrum. Meas.
**2002**, 51, 1177–1190. [Google Scholar] [CrossRef] - Gelb, A. (Ed.) Applied Optimal Estimation; M.I.T. Press: Cambridge, MA, USA, 1974.
- Brown, R.G.; Hwang, P.Y.C. Introduction to Random Signals and Applied Kalman Filtering: With MATLAB Exercises and Solutions, 3rd ed.; Wiley: New York, NY, USA, 1997. [Google Scholar]
- Grewal, M.S.; Andrews, A.P. Kalman Filtering: Theory and Practice Using MATLAB; Wiley: Oxford, UK, 2008. [Google Scholar]
- Simon, D. Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches; John Wiley: Hoboken, NJ, USA; Chichester, UK, 2006. [Google Scholar]
- Kaplan, E.D.; Hegarty, C.J. Understanding GPS Principles and Applications, 2nd ed.; Artech House: Boston, MA, USA, 2006. [Google Scholar]

© 2015 by the authors; licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/4.0/).