Adaptive Filtering on GPS-Aided MEMS-IMU for Optimal Estimation of Ground Vehicle Trajectory

Abstract

Fusion of the Global Positioning System (GPS) and Inertial Navigation System (INS) for navigation of ground vehicles is an extensively researched topic for military and civilian applications. Micro-electro-mechanical-systems-based inertial measurement units (MEMS-IMU) are being widely used in numerous commercial applications due to their low cost; however, they are characterized by relatively poor accuracy when compared with more expensive counterparts. With a sudden boom in research and development of autonomous navigation technology for consumer vehicles, the need to enhance estimation accuracy and reliability has become critical, while aiming to deliver a cost-effective solution. Optimal fusion of commercially available, low-cost MEMS-IMU and the GPS may provide one such solution. Different variants of the Kalman filter have been proposed and implemented for integration of the GPS and the INS. This paper proposes a framework for the fusion of adaptive Kalman filters, based on Sage-Husa and strong tracking filtering algorithms, implemented on MEMS-IMU and the GPS for the case of a ground vehicle. The error models of the inertial sensors have also been implemented to achieve reliable and accurate estimations. Simulations have been carried out on actual navigation data from a test vehicle. Measurements were obtained using commercially available GPS receiver and MEMS-IMU. The solution was shown to enhance navigation accuracy when compared to conventional Kalman filter.

1. Introduction

The GPS (Global Positioning System) and the INS (Inertial Navigation System) can provide stand-alone navigation solutions; however, both are prone to intrinsic errors/biases which may limit the accuracy and productivity of each navigation technique. The GPS provides fairly precise positioning information but at a low frequency of 1 Hz. It may also suffer from phenomena such as unavailability of a clear line-of-sight to the satellites, multipath propagation/interference in dense urban environments, signal jamming, and degradation in accuracy or complete denial to service by the US government [1,2]. INS, on the other hand, finds preference being a dead reckoning system completely independent of external disturbances, which can provide continuous estimates of not only position and velocity, but also of orientation, at a high measurement output rate. However, it offers only short-term accuracy due to an unbounded growth in error inherent to the inertial sensors. Low cost micro-electro-mechanical-systems-based inertial measurement units (MEMS-IMUs) have extremely poor accuracy and suffer adversely from intrinsic errors such as scale factor, drift, bias, and non-orthogonality. Hence, their outputs contain high noise and large uncertainties. As a result, errors inherent to a low-cost IMU in terms of attitude, velocity, and position, grow swiftly in stand-alone mode [3]. Moreover, being a dead reckoning system, the INS requires accurate initialization and alignment [4,5], which in itself, presents a challenge without an external aid, if low-cost MEMS-based IMUs are used [6,7].

Precise estimation remains an essential requirement for any navigation system, especially applications related to autonomous navigation [8,9,10] and the smart city concept [11,12]. Calibrating an inertial sensor can considerably improve its accuracy as the majority of the uncertainties are associated with the error behavior of a sensor. However, one of the most commonly used methodologies to improve accuracy is augmenting the INS with some aiding source; for example, the GPS [13,14,15] Dead-reckoning using data-fusion on GPS-aided IMU can be used for greater accuracy, reliability, and robustness against jamming [16]. For development of smaller, reliable, and low-cost navigation systems, while also targeting global competitiveness in consumer market, it is inevitable to utilize inexpensive GPS receivers and inertial sensors, especially low grade MEMS-based IMUs, even though they provide less accurate motion information.

This paper proposes a novel framework based on adaptive Kalman filtering, implemented using the GPS-INS loose-coupling integration strategy due to its simplicity, robustness, and comparatively lesser computational load. System modeling has been carried out by implementation of position, velocity, and attitude error models of the inertial sensors to enhance estimation accuracy. The proposed scheme is based on a hybrid of the Sage-Husa filter and the strong tracking filter. The proposed algorithm offers a balance between accuracy and strong convergence to optimum estimation for highly dynamic systems. It is a practical algorithm and provides a reliable, accurate, and cost-effective solution, as shown by simulations on real vehicle navigation data. The strategy was verified practically by aiding a low-cost MEMS-IMU (YH-7000 commercial grade MEMS IMU) with Novatel’s OEMV Series GPS receiver.

The paper is organized as follows: Section 2 provides a brief account of GPS-INS loose coupling scheme, conventional linear Kalman filtering, and adaptive Kalman filtering. Algorithms for Sage-Husa and strong tracking filters are also explained. Afterwards, GPS and INS models are discussed. The proposed scheme is explained in Section 3, along with system model by implementing IMU position; velocity and attitude errors; an earth model; and gravity correction. Simulations and results are presented in Section 4, while the discussion is concluded in Section 5.

2. Inertial Navigation Mechanism

Inertial navigation requires determining a vehicle’s attitude and position in a three-dimensional frame of reference. This is accomplished by using three perpendicularly mounted gyroscopes to provide rate-of-turn in three different axes, while three orthogonally mounted accelerometers provide accelerations in these three axes as experienced by the vehicle. Measurements provided by the gyroscopes are with respect to the inertial frame of reference. However, accelerometers cannot provide the acceleration with respect to the inertial frame only. Their measurements also include a component of acceleration caused by the gravitational force. Measurement provided by an accelerometer can be written as difference between the true acceleration of a body in space and the acceleration due to gravitational pull; that is,

$$f=a-g.$$

(1)

This quantity is known as the specific force, and can be described as the non-gravitational force exerted on per unit mass of the body. The equation can be rearranged to calculate the true acceleration. This equation is termed the navigation equation in literature (and can take different forms depending upon the choice of reference frame). If r is the unit vector representing the vehicle’s position from origin, then acceleration of the vehicle in an inertial frame is given by:

$$\frac{d^{2}r}{d{t}^2}=f_i+g_i.$$

(2)

However, when the navigation process is being carried out in a non-fixed rotating frame of reference, such as the Earth frame, additional forces act on the vehicle as a function of the rotation of the reference frame. This effect is known as the Coriolis effect, and the velocity of vehicle can then be calculated using the inertial velocity as follows:

$$v_e=\frac{dr}{dt}=v_i-{\omega }_{ie}\times r,$$

(3)

where ${\omega }_{ie}={\left[00{\Omega }_e\right]}^T$ represents the Earth’s rate of turn with respect to the inertial frame, and the symbol × indicates vector product. The symbol ${\Omega }_e=15.041067$°$/hr$ is the Earth’s angular speed. The specific force measurements provided by accelerometers are expressed in the vehicle’s body frame as:

$$f_b={\left[f_x{f}_y{f}_z\right]}^T.$$

(4)

Specific force measurements from body frame represented by the subscript, b, are then transformed to the local geodetic or the navigation frame using the transformation matrix, $C_b^n$:

$$f_n=\left[\begin{array}{c}{f}_N\\ f_E\\ f_D\end{array}\right]=C_b^n{f}_b,$$

(5)

where the subscript b and superscript n of the transformation matrix, $C_b^n$, denote body and frame references, respectively, and the transformation matrix is given by,

$$C_n^b=C_b^{nT}=\begin{array}{cc}\hfill & \left[\begin{array}{cc}cos\psi cos\phi -sin\psi sin\theta sin\phi & -sin\psi cos\theta \\ sin\psi cos\phi +cos\psi sin\theta sin\phi & cos\psi cos\theta \\ -cos\theta sin\phi & sin\theta \end{array}\right.\hfill \\ \hfill & \left.\begin{array}{c}cos\psi sin\phi +sin\psi sin\theta cos\phi \\ sin\psi sin\phi -cos\psi sin\theta cos\phi \\ cos\theta cos\phi \end{array}\right],\hfill \end{array}$$

(6)

where $\phi$, $\theta$, and $\psi$ are vehicle’s roll, pitch, and yaw angles, respectively. However, the three acceleration components expressed in the navigation frame cannot directly provide velocity components in the navigation frame. This is due to three reasons: (1) The Earth’s rotation rate, which is expressed as angular velocity vector ${\omega }_{ie}^n$ in the local navigation frame. (2) There is a change in orientation of the local navigation frame with respect to the Earth’s frame, which is because of the definition of the local level north and vertical directions. The local north is at a tangent to the Earth’s meridian at all times, whereas the vertical direction is perpendicular to the Earth’s surface. This effect can be expressed as the angular velocity vector ${\omega }_{en}^n$, which is the turn-rate of the navigation frame with respect to the Earth-fixed frame, and is known as the transport rate. (3) the third reason is gravity of the Earth, expressed in terms of the local level gravity vector, $g_l^n$. Mathematically, these three factors can be expressed as

$${\omega }_{ie}^n=\left[\begin{array}{c}{\Omega }_{e}cosL\\ 0\\ {\Omega }_{e}sinL\end{array}\right],$$

(7)

where L represents vehicle’s latitude.

$${\omega }_{en}^n=\left[\begin{array}{c}\frac{v_E}{R+h}\\ -\frac{v_N}{R+h}\\ -\frac{v_{E}tanL}{R+h}\end{array}\right],$$

(8)

where $v_N$ and $v_E$ represent vehicle’s north and east velocity components, respectively; $R=6,378,137$ m is the equatorial radius of Earth; and h is the height of vehicle above the Earth’s surface.

$$g_l^n=\left[\begin{array}{c}0\\ 0\\ g\end{array}\right],$$

(9)

where g is obtained from the normal gravity model, which is a well-known expression [16], given as:

$$g=a_1(1+a_2{sin}^2\phi +a_3{sin}^4\phi )+(a_4+a_5{sin}^2\phi )h+a_6{h}^2,$$

(10)

where the coefficients $a_1$ through $a_6$ are constants given as; $a_1$ = 9.7803267714 m/s${}^2$, $a_2=0.0052790414$, $a_3$ = 0.0000232718, $a_4$ = $-0.0000030876910891$/s${}^2$, $a_5=0.000000004397731$/s${}^2$, and $a_6=0.000000000000721$/ms${}^2$.

Taking into account all the factors discussed above, the rate of change of vehicle’s north, east and down (NED) velocities in a navigation frame can be described as:

$${\dot{v}}_N=f_N-2{\Omega }_e{v}_{E}sinL+\frac{v_N{v}_D-v_E^{2}tanL}{R+h}+g$$

(11)

$${\dot{v}}_E=f_E+2{\Omega }_e(v_{N}sinL+v_{D}cosL)+\frac{v_E(v_D+v_{N}tanL)}{R+h}-g$$

(12)

$${\dot{v}}_D=f_D-2v_{E}cosL+\frac{v_E^2+v_N^2}{R+h}+g.$$

(13)

The position of the navigating vehicle can be expressed in terms of latitude $\left(\dot{L}\right)$, longitude $\left(\dot{l}\right)$, and height $\left(\dot{h}\right)$ above the Earth’s surface as:

$$\dot{L}=\frac{v_N}{R+h}$$

(14)

$$\dot{l}=\frac{v_{E}secL}{R+h}$$

(15)

$$\dot{h}=-v_D.$$

(16)

The whole principle of strap-down inertial navigation in the local navigation frame is demonstrated by the schematic diagram in Figure 1. The GPS model considered in this research can be found in [17].

3. Filtering

Development of a reliable aided inertial navigation system depends on selecting an appropriate estimation method. The quality of parameter estimates depends chiefly on the type and accuracy of inertial sensors. Thus, the information from sensors should adequately be formulated with careful consideration of error characteristics in each sensor. Based upon the type of coupling, there are two basic GPS/INS integration modes; namely, loose and tight coupling modes [18]. The loosely coupled integration is the simplest and most robust technique of coupling. Navigation solutions are generated by both the INS and the GPS receiver, independently. A third navigation solution is then derived by using an estimator to merge the information provided by the two independent solutions. The Kalman filter including its derivatives is the most widely used filter to realize this fusion [19,20,21]. The fusion Kalman filter calculates error estimates between the two solutions, which are fed back to provide corrected attitude, velocity and position. The robustness of the loosely coupled scheme is due to the fact that if one system fails, the other sensor can still provide navigation, although with reduced accuracy. This research work utilizes the loose coupling strategy for INS-GPS fusion due to its simplicity, robustness, and lesser computational load.

3.1. Conventional Kalman Filter

The Kalman filter is a mathematical estimator used for predicting a system’s behavior from noisy observations. It is a recursive algorithm which uses a series of steps, involving prediction and updating, to obtain an optimal minimum variance estimate of a system or state vector. It is commonly used for estimation in the field of navigation, particularly for autonomous applications [22,23]. It combines noisy sensor outputs (such as those from GPS, accelerometers, and gyroscopes measurements) and estimates the dynamic state of a system (which may include a vehicle’s attitude, acceleration, velocity, position, etc.). For a discrete-time system, the Kalman filter algorithm is given by the following expressions:

Prediction step:

$${\widehat{x}}_{k+1|k}={\Phi }_{k+1|k}{\widehat{x}}_{k|k}$$

(17)

$$P_{k+1|k}={\Phi }_{k+1|k}{P}_{k|k}{\Phi }_{k+1|k}^T+Q_k$$

(18)

Measurement update step:

$$K_{k+1}=P_{k+1|k}{H}_{k+1}^T{[H_{k+1}{P}_{k+1|k}{H}_{k+1}^T+R_{k+1}]}^{-1}$$

(19)

$$P_{k+1|k+1}=[I-K_{k+1}{H}_{k+1}]P_{k+1|k}$$

(20)

$${\widehat{x}}_{k+1|k+1}={\widehat{x}}_{k+1|k}+K_{k+1}[z_{k+1}-H_{k+1}{\widehat{x}}_{k+1|k}],$$

(21)

where $\Phi$ is the $n\times n$ state transition matrix; ${\widehat{x}}_{k|k}$ and ${\widehat{x}}_{k+1|k}$ represent the corrected(or a posteriori) value and the predicted (or a priori) value of the estimated state vector x, respectively; $P_{k+1|k+1}$ and $P_{k+1|k}$ are the a posteriori and a priori state covariance matrices of order $n\times n$, respectively, at time $t_k$; and the matrix $K_{k+1}$ of order $n\times m$ is the Kalman gain. The term $[z_{k+1}-H_{k+1}{\widehat{x}}_{k+1|k}]$ is called the residual or measurement innovation, and $H_k$ is the measurements matrix of order $m\times n$. The matrices Q (of order $n\times n$) and R (of order $m\times m$) represent the system and measurement noise covariances, respectively. Assuming that system noise ($w_k$) and measurement noise ($v_k$) are uncorrelated and have characteristics of white noise, we can write the mean matrices as:

$$E\left[w_k\right]=0$$

(22)

$$E\left[v_k\right]=0.$$

(23)

And covariance matrices can be written as:

$$Cov[w_k,w_j]=\left\{\begin{array}{c}{Q}_k,j=k\hfill \\ 0,j\ne k\hfill \end{array}\right.$$

(24)

$$Cov[v_k,v_j]=\left\{\begin{array}{c}{R}_k,j=k\hfill \\ 0,j\ne k\hfill \end{array}\right.$$

(25)

$$Cov[w_k,v_j]=0,\forall j,k.$$

(26)

3.2. Adaptive Kalman Filtering

Due to its reliance on a priori information (of dynamic model and noise parameters), the Kalman filter may suffer from degradation or even divergence in situations where this a priori information is time-varying (or unknown) [24]. The matrix R usually contains information provided in a sensor’s data sheet, while the matrix Q is attained analytically. Thus, any errors in Q and/or R may lead to sub-optimal estimation. Although usually taken as constants, Q and R for most practical situations must be adjusted to incorporate time-varying states and sensor inaccuracies which also vary under different dynamic conditions [25,26]. Similarly, in situations where system states are changing abruptly, robustness of the filter also becomes very critical, so that the filter can keep up with sharp changes in observations/measurements. This is achieved by adjusting Kalman gain K, or process covariance matrix P [27]. In order to achieve strong convergence, filter accuracy may have to be suppressed in such cases. This type of filtering is known as adaptive Kalman filtering

3.2.1. Sage-Husa Adaptive Kalman Filter

The Sage-Husa Adaptive Kalman filter [28] is based on a maximum a priori estimation technique. Since the old measurements can influence the estimation process, the filter performance can be optimized by utilizing a finite set of recent measurements. This is achieved by assigning weights to Q and R matrices, which causes the filter to adjust the gain K adaptively. So, in situations where noise statistics vary with time, increasing the weights of latest innovations will lead to enhanced estimation accuracy. For systems with time varying noise characteristics, we can write the mean matrices as

$$E\left[w_k\right]=q_k$$

(27)

$$E\left[v_k\right]=r_k.$$

(28)

Filtering functions for Sage-Husa–Kalman filter are:

Prediction step:

$${\widehat{x}}_{k+1|k}={\Phi }_{k+1|k}{\widehat{x}}_{k|k}+{\widehat{q}}_{k+1}$$

(29)

$$P_{k+1|k}={\Phi }_{k+1|k}{P}_{k|k}{\Phi }_{k+1|k}^T+Q_k.$$

(30)

Measurement update step:

$$K_{k+1}=P_{k+1|k}{H}_{k+1}^T{[H_{k+1}{P}_{k+1|k}{H}_{k+1}^T+R_{k+1}]}^{-1}$$

(31)

$$P_{k+1|k+1}=[I-K_{k+1}{H}_{k+1}]P_{k+1|k}$$

(32)

$${\tilde{z}}_k=z_{k+1}-H_{k+1}{\widehat{x}}_{k+1|k}-{\widehat{r}}_{k+1}$$

(33)

$${\widehat{x}}_{k+1|k+1}={\widehat{x}}_{k+1|k}+K_{k+1}{\tilde{z}}_k,$$

(34)

where

$${\widehat{r}}_{k+1}=(1-d_k){\widehat{r}}_k+d_k(z_{k+1}-H_{k+1}{\widehat{x}}_{k+1|k})$$

(35)

$${\widehat{R}}_{k+1}=(1-d_k){\widehat{R}}_k+d_k({\tilde{z}}_{k+1}{\tilde{z}}_{k+1}^T-H_{k+1}{P}_{k+1|k}{H}_{k+1}^T$$

(36)

$${\widehat{q}}_{k+1}=(1-d_k){\widehat{q}}_k+d_k({\widehat{x}}_{k+1}-{\Phi }_{k+1|k}{\widehat{x}}_k$$

(37)

$$\begin{array}{ccc}{\widehat{Q}}_{k+1}& =& (1-d_k){\widehat{Q}}_k+d_k(K_{k+1}{\tilde{z}}_{k+1}{\tilde{z}}_{k+1}^T{K}_{k+1}^T\\ & & +P_{k+1}-{\Phi }_{k+1|k}{P}_{k|k}{\Phi }_{k+1|k}^T).\end{array}$$

(38)

The parameter d is known as the attenuation factor, given by

$$d_k=\frac{1-b}{1-b^{k+1}},0<b<1,$$

(39)

where b is called the forgetting factor. Generally, b is selected using trial and error method but a value between 0.95 and 0.99 has been found to work well [29] to converge the filter to optimal Q and R.

3.2.2. Strong Tracking Robust Kalman Filter

When dealing with inaccurate linear or non-linear dynamic system models, or in situations where state changes are more sudden, the strong tracking filter may provide another effective method for adaptive state estimation [27]. Although lacking in extensive theoretical analysis, the filter has widely been used in practical applications. It makes use of a time varying fading factor which is computed from the most recent measurement innovation, and dynamically corrects the error covariance for state prediction, providing strong tracking of a maneuvering target with abrupt state changes.

The strong tracking Kalman filter algorithm updates the system covariance matrix P by suppressing the filter accuracy, but making it more robust for real time situations, as generalized below:

$$P_{k+1|k}={\lambda }_{k+1}{\Phi }_{k+1|k}{P}_k{\Phi }_{k+1|k}^T+Q_k,$$

(40)

where the parameter $\lambda$ is called the fading factor and is calculated upon the arrival of new measurements, as proposed in [30].

$${\lambda }_{k+1}=diag[{\lambda }_{1(k+1)},{\lambda }_{2(k+1)},{\lambda }_{3(k+1)},\dots {\lambda }_{n(k+1)}],$$

(41)

where

$${\lambda }_{i(k+1)}=\left\{\begin{array}{c}{C}_{k+1},if{C}_{k+1}>1\hfill \\ 1,if{C}_{k+1}\le 1,\hfill \end{array}\right.$$

(42)

and $C_{k+1}$ is defined as

$$C_{k+1}=\frac{Tr[v_{o(k+1)}-R_{k+1}-H_{k+1}{Q}_k{H}_{k+1}^T]}{Tr\left[{\Phi }_{k+1|k}{P}_k{\Phi }_{k+1|k}^T{H}_{k+1|k}^T{H}_{k+1}\right]},$$

(43)

and $v_{o(k+1)}$ is the measurement innovation given by

$$v_{o(k+1)}=\left\{\begin{array}{c}{\tilde{z}}_1{\tilde{z}}_1^T,(k=0)\hfill \\ \frac{\rho v_{o\left(k\right)}+{\tilde{z}}_{k+1}{\tilde{z}}_{k+1}^T}{1+\rho },(k\ge 1,0\le \rho <1).\hfill \end{array}\right.$$

(44)

The value of parameter $\rho$ is generally taken to be 0.95–0.99. The filter is active only when the system is experiencing abrupt state change ($\lambda >1$); that is, when

$$\begin{array}{c}Tr[v_{o(k+1)}-R_{k+1}-H_{k+1}{Q}_k{H}_{k+1}^T]>\\ Tr\left[{\Phi }_{k+1|k}{P}_k{\Phi }_{k+1|k}^T{H}_{k+1|k}^T{H}_{k+1}\right].\end{array}$$

(45)

4. Proposed Scheme

A hybrid filter based on Sage-Husa and strong tracking Kalman filters is presented which offers balance between accuracy and robustness in practical real-time situations where variation in noise statistics is also accompanied by sudden changes in system states. When state change is not abrupt ($\lambda \le 1$), the filter will prioritize estimation by Sage-Husa algorithm; otherwise, when $\lambda >1$, estimation will be carried out through strong tracking filter. The flow diagram demonstrates the filter operation. The filter has been implemented using a loosely coupled scheme due to its simplicity and robustness. The proposed algorithm is depicted in Figure 2.

Mathematical Model

In state-space representation for a linear dynamic system, a system of n-order differential equations is transformed into a set of n-coupled, first-order differential equations. Similarly, for the case of INS-GPS integration, the differential equations representing INS errors can be expressed in a state-space form, which will then be used as system model for Kalman filter. The state-space model consists of two equations; i.e., a dynamic equation and a measurement equation, which can be written respectively as:

$$\dot{x}=Fx+w$$

(46)

$$z=Hx+v,$$

(47)

where x is state vector of order $n\times 1$, F is called system dynamic matrix with order $n\times n$; w, an $n\times 1$ vector, represents the system noise; z and v are called the observation vector and the measurement noise vector, respectively, both of order $m\times 1$. H is the measurement matrix of order $m\times n$, with m measurements and n states. The above equations can be written in discrete form as:

$$x_{k+1}={\Phi }_{k+1}{x}_k+w_k$$

(48)

$$z_k=H_k{x}_k+v_k,$$

(49)

where $x_k$ is the state vector of order $n\times 1$ at time $t_k$; ${\Phi }_{k+1}$ is the $n\times n$ state transition matrix at time $t_{k+1}$; $w_k$ is the system noise of order $n\times 1$; $z_k$ is the observation vector of order $m\times 1$; $H_k$ is the measurements matrix of order $m\times n$; and $v_k$ is the $m\times 1$ measurement noise vector at time $t_k$. For a discrete-time system, the state transition matrix can be written as:

$$\Phi =e^{F△t}.$$

(50)

Taylor series expansion of this equation over a small time interval $△t$ yields the following expression (ignoring the higher terms):

$$\Phi =I+F△t,$$

(51)

where I is the identity matrix. The system matrix F is a $9\times 9$ matrix formed by incorporating the attitude, position and velocity error models of the inertial sensors, which are explained by Titterton in [31]. The system matrix F is given by

$$F=\left[\begin{array}{ccc}{F}_1& F_2& F_3\\ F_4& F_5& F_6\\ F_7& F_8& F_9\end{array}\right],$$

(52)

where $F_1$ through $F_9$ are all $3\times 3$ matrices given as

$$F_1=\left[\begin{array}{ccc}0& -(\Omega S_L+\frac{v_E{T}_L}{r_E}& \frac{v_N}{r_N}\\ \Omega S_L+\frac{v_E{T}_L}{r_E}& 0& \Omega C_L+\frac{v_E}{r_E}\\ -\frac{v_N}{r_N}& -(\Omega C_L+\frac{v_E}{r_E})& 0\end{array}\right]$$

(53)

$$F_2=\left[\begin{array}{ccc}0& \frac{1}{r_N}& 0\\ -\frac{1}{r_N}& 0& 0\\ 0& -\frac{T_L}{r_E}& 0\end{array}\right]$$

(54)

$$F_3=\left[\begin{array}{ccc}-\Omega S_L& 0& -\frac{v_E}{r_E^2}\\ 0& 0& \frac{v_N}{r_N^2}\\ -(\Omega C_L+\frac{v_E}{r_E{C}_L^2})& 0& \frac{v_E{T}_L}{r_E^2}\end{array}\right]$$

(55)

$$F_4=\left[\begin{array}{ccc}0& -f_D& f_E\\ f_D& 0& -f_N\\ -f_E& f_N& 0\end{array}\right]$$

(56)

$$F_5=\left[\begin{array}{ccc}\frac{v_D}{r_N}& -2(\Omega S_L+\frac{v_E{T}_L}{r_E})& \frac{v_N}{r_N}\\ 2\Omega S_L+\frac{v_E{T}_L}{r_E}& \frac{v_N{T}_L+v_D}{r_N}& 2\Omega C_L+\frac{v_E}{r_E}\\ -\frac{2v_N}{r_N}& -2(\Omega C_L+\frac{v_E}{r_E})& 0\end{array}\right]$$

(57)

$$F_6=\left[\begin{array}{ccc}-v_E(2\Omega C_L+\frac{v_E}{r_E{C}_L^2})& 0& \frac{v_E^2{T}_L}{r_E^2}-\frac{v_N{v}_D}{r_N^2}\\ 2\Omega (v_N{C}_L-v_D{S}_L)+\frac{v_N{v}_E}{r_E{C}_L^2}& 0& -\frac{v_E(v_N{T}_L+v_D)}{r_E^2}\\ 2\Omega v_E{S}_L& 0& \frac{v_N^2+v_E^2}{r_E^2}\end{array}\right]$$

(58)

$$F_7=0_{3x3}$$

(59)

$$F_8=\left[\begin{array}{ccc}\frac{1}{r_N}& 0& 0\\ 0& \frac{1}{r_E{C}_L}& 0\\ 0& 0& -1\end{array}\right]$$

(60)

$$F_9=\left[\begin{array}{ccc}0& 0& -\frac{v_N}{r_N^2}\\ \frac{v_E{T}_L}{r_E{C}_L}& 0& -\frac{v_E}{r_E^2{C}_L}\\ 0& 0& 0\end{array}\right],$$

(61)

where $f_N$, $f_E$, and $f_D$ are the north, east, and down components of specific force, whereas, $v_N$, $v_E$, and $v_D$ are the north, east, and down components of vehicle’s velocity, respectively. $\Omega =15.041067$${}^{\circ }$/hr represents the Earth’s angular speed. $r_N=R_N+h$ and $r_E=R_E+h$, where $R_N$ and $R_E$ are known as Earth’s meridian radius of curvature and transverse radius of curvature, respectively, when an ellipsoidal model is considered for Earth. $S_L$, $C_L$, and $T_L$ represent sin, cos, and tan of the vehicle’s latitude (L), respectively.

$$R_N=\frac{R_e(1-e^2)}{{(1-e^2{sin}^{2}L)}^{(3/2)}}$$

(62)

$$R_E=\frac{R_e}{{(1-e^2{sin}^{2}L)}^{(1/2)}},$$

(63)

where e is the eccentricity of the Earth, with a constant value of $e=0.08181919082423$, and $R_e$ is known as the equatorial radius of the earth, with value $R_e=6378137$ m. The state vector x is given by

$$x={\left[\begin{array}{ccccccccc}\delta \alpha & \delta \beta & \delta \gamma & \delta v_N& \delta v_E& \delta v_D& \delta L& \delta l& \delta h\end{array}\right]}^T,$$

(64)

where $\delta \alpha$, $\delta \beta$, and $\delta \gamma$ are the errors in roll, pitch, and yaw; $\delta v_N$, $\delta v_E$, and $\delta v_D$ are the errors in north, east, and down velocities; and $\delta L$, $\delta l$, and $\delta h$ are the errors in latitude, longitude, and altitude, respectively.

The system noise matrix Q consists of variance errors in its principal diagonal and is formed directly from the IMU sensor data sheet, whereas the uncertainties in the GPS measurements are used to derive the measurement noise covariance matrix R.

$$Q=diag\left[\begin{array}{ccccccccc}{\sigma }_{w_x}^2& {\sigma }_{w_y}^2& {\sigma }_{w_z}^2& {\sigma }_{a_x}^2& {\sigma }_{a_y}^2& {\sigma }_{a_z}^2& {\sigma }_{a_x}^2& {\sigma }_{a_y}^2& {\sigma }_{a_z}^2\end{array}\right]$$

(65)

$$R=diag\left[\begin{array}{cccccc}{\sigma }_{v_N}^2& {\sigma }_{v_E}^2& {\sigma }_{v_D}^2& {\sigma }_L^2& {\sigma }_l^2& {\sigma }_h^2\end{array}\right].$$

(66)

The difference between GPS and IMU velocity and position measurements forms the measurement vector z.

$$z=\left[\begin{array}{c}{v}_{N\left(GPS\right)}-v_{N\left(IMU\right)}\\ v_{E\left(GPS\right)}-v_{E\left(IMU\right)}\\ v_{D\left(GPS\right)}-v_{D\left(IMU\right)}\\ la{t}_{\left(GPS\right)}-la{t}_{\left(IMU\right)}\\ lo{n}_{\left(GPS\right)}-lo{n}_{\left(IMU\right)}\\ al{t}_{\left(GPS\right)}-al{t}_{\left(IMU\right)}\end{array}\right].$$

(67)

The latitude and longitude measurements are in radians with very small values, which can make the measurement matrix H singular. This can cause numerical instabilities in Kalman gain K calculation. So the measurement vector can be modified as:

$$z=\left[\begin{array}{c}{v}_{N\left(GPS\right)}-v_{N\left(IMU\right)}\\ v_{E\left(GPS\right)}-v_{E\left(IMU\right)}\\ v_{D\left(GPS\right)}-v_{D\left(IMU\right)}\\ (la{t}_{\left(GPS\right)}-la{t}_{\left(IMU\right)})(R_E+h)\\ (lo{n}_{\left(GPS\right)}-lo{n}_{\left(IMU\right)})(R_N+h)cos\lambda \\ al{t}_{\left(GPS\right)}-al{t}_{\left(IMU\right)}\end{array}\right].$$

(68)

The measurement matrix (H) is written as:

$$H=\left\{\begin{array}{ccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& r_N& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& r_{E}cosL& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right\}$$

(69)

All navigation calculations are being carried out in NED frame of reference.

5. Simulation, Data Collection and Results

The field test drive for data collection each lasting for fifteen minutes was carried out or Murree road (34.156024${}^{\circ }$ N, 73.222560${}^{\circ }$ E), Abbottabad city, Pakistan in January 2019. A total of twenty GPS and IMU measurement data sets were gathered over a period of three days. Two of the data sets were discarded due to erroneous data as a result of a hardware malfunction. Due to mountainous terrain of the city, satellite coverage was moderate and the average number of satellites available was equal to six. In order to avoid any timing errors between the GPS and IMU data, software written in MATLAB was used to time stamp the MEMS IMU data.

The YH-7000 commercial grade MEMS IMU was used for inertial measurements, which include angular rates and specific force data provided by three dimensional gyros and accelerometers. IMU data was obtained at 200 Hz. Simulations have been carried out on real vehicle test drive data in MATLAB R2014A platform (3.1 GHz, Intel, Core i5 CPU). In order to better visualize the filter’s performance for highly dynamic systems, Gaussian noise was introduced to the measurements during the interval between 300 s and 500 s. The actual GPS data was used to compare the estimation results. A total of 250 Monte Carlo simulations were carried out to omit any bias introduced by adding the noise.

Novatel’s OEMV series GPS receiver was used to obtain raw data which was converted to position and velocity measurements using the on-board software at 1 Hz. The GPS measurements included vehicle’s position in terms of latitude, longitude, and altitude, and vehicle’s velocity in terms of north, east and down velocities. Typical error of commercial GPS data is about 30 m in latitude and longitude, while error in altitude is about $1.5$ times greater than latitude/longitude error. The YH-7000 commercial grade MEMS IMU Sensor specifications are given in

Table 1. IMU Sensor specifications.

Parameter	Gyro	Accelerometer
Bias Repeatability	<0.02${}^{\circ }$/s, 1 $\sigma$	<2 mg, 1 $\sigma$
Random Walk	<6${}^{\circ }$/hr${}^{1/2}$	<0.3 m/s2/hr${}^{1/2}$
Scale Factor Stability	<0.3%, 1 $\sigma$	<0.2%, 1 $\sigma$
Bias Variation	<0.1${}^{\circ }$/s, 1 $\sigma$	<5 mg, 1 $\sigma$
Bandwidth	>100 Hz, Gain @ $-3$ dB	>100 Hz, Gain @ −3 dB

:

The results for the conventional linear Kalman filter and the hybrid adaptive Kalman filter for INS-GPS integrated navigation are now compared, as shown in the following simulation results with actual sensor data. Vehicle position in terms of latitude, longitude, and altitude is shown first in Figure 3, Figure 4 and Figure 5, whereas vehicle’s velocity in terms of north, east, and down components is shown in Figure 6, Figure 7 and Figure 8, respectively.

The proposed hybrid filter produces better estimates for vehicle’s position and velocity. Superior performance of the hybrid filter is more evident during the interval between 300 s and 500 s, when random noise was added to the actual sensor measurements. It converges more rapidly compared to the conventional Kalman filter. Similarly, when there is abrupt change in vehicle’s latitude and longitude measurements (near time = 650 s), the hybrid adaptive filter clearly excels at convergence (Figure 3 and Figure 4). This behavior is more pronounced for vehicle’s velocity estimation, which is understandable, given the fact that measurements have been taken for a ground vehicle, which generally experiences more abrupt changes in its velocity/acceleration rather than its position. Figure 6 and Figure 7 clearly show this trend for vehicle’s north and east velocity components.

The position and velocity estimation curves for both the linear Kalman filter and the hybrid adaptive Kalman filter appear to be close and the two filters generate almost similar results. However, the hybrid filter, in effect, performs better, which can be seen from the moving mean square error (MMSE) plots of vehicle’s position and velocity (Figure 9, Figure 10, Figure 11, Figure 12, Figure 13, Figure 14 and Figure 15). Overall improvements made by the hybrid filter in vehicle’s position and velocity estimates are also shown by the accumulative error plots of vehicle’s position and velocity in Figure 15 and Figure 16.

The moving mean square error (MMSE) plots provided in Figure 9, Figure 10, Figure 11, Figure 12, Figure 13, Figure 14 and Figure 15 offer a better insight into the performance of proposed hybrid adaptive filter compared to conventional Kalman filter. These curves clearly illustrate superior estimation accuracy of the proposed filter scheme. A quantitative measure of the filter’s accuracy has also been provided in terms of accumulative error plots (Figure 15 and Figure 16), which show overall improvements made by the proposed hybrid scheme in vehicle’s position and velocity estimates.

These results prove the efficiency of the proposed scheme for parameter estimation in navigation, especially for the case when measurements contain large uncertainties/errors, or when states are changing more abruptly, such as the vehicle’s velocity.

Switching decision between the Sage-Husa filter and the robust tracking filter is made based on the check provided in Equation (45). When an anomaly in the measurements is detected, like a large error, or abrupt state change, the condition in Equation (45) is satisfied, and the filter switches to robust mode where more weight is assigned to latest measurements, thus, providing strong tracking of the target.

6. Conclusions

A novel algorithm based on a hybrid of the Sage-Husa filter and the strong tracking filter using a loosely coupled scheme is proposed for GPS-INS integration. The proposed algorithm has generated superior navigation for a vehicle’s position and velocity estimates. The algorithm offers a balance between accuracy and strong convergence for optimum estimation for highly dynamic systems. It inherently performs much better when implemented for more dynamic systems, as proven by the results. The same hybrid filter will, thus, produce even better navigation estimates for target tracking applications or in case of aerial vehicles, where a conventional Kalman filter will certainly lag behind due to its reliance on a priori information, and consequently, its inability to keep track of sudden changes in system states. The hybrid filter based on Sage-Husa–Kalman filter and strong tracking Kalman filter can, thus, be acknowledged as an optimal filter when compared to the conventional Kalman filter.