2.1. Strapdown Inertial Navigation System

The navigation system implemented works in local level mechanization such that it calculates attitude and body velocity with respect to North-East-Down (NED) frame. In this type of mechanization, the velocity is expressed in the local geographic coordinate system as north-east-down in order to maintain the navigation accuracy over large distances on earth. Here, the rate of change of the vehicle speed with respect to earth is expressed as:

(1)$$\displaystyle{d \over {dt}}{\bf v}_e \vert _n = \displaystyle{d \over {dt}}{\bf v}_e \vert _i - ({\bf \omega} _{ie} + {\bf \omega} _{en} )x{\bf v}_e $$

where ω_ie is the rate of earth with respect to inertial frame and ω_en is the rate of navigation frame with respect to earth. Substituting ${\bf \dot v}_e^i = {\bf f} - {\bf \omega} _{ie} x{\bf v}_e + {\bf g}$ into Equation (1), we have

(2)$$\displaystyle{d \over {dt}}{\bf v}_e \vert _n = {\bf f} - (2{\bf \omega} _{ie} + {\bf \omega} _{en} )x{\bf v}_e + {\bf g}$$

Expressing Equation (2) in the navigation frame:

(3)$${\bf \dot v}_e^n = {\bf C}_b^n {\bf f}^b - (2{\bf \omega} _{ie}^n + {\bf \omega} _{en}^n )x{\bf v}_e^n + {\bf g}^n $$

where f^b is the specific force measured by the accelerometers, gⁿ is the gravity vector represented in navigation frame and C_bⁿ  is the body to navigation transformation matrix. The Direction Cosine Matrix (DCM), C_bⁿ, propagates as

(4)$${\bf \dot C}_b^n = {\bf C}_b^n {\bf \Omega} _{nb}^b $$

where ${\bf \Omega} _{nb}^b $ is the skew symmetric matrix of rate of body frame ${\bf \omega} _{nb}^b $, with respect to navigation frame expressed in body frame, and

(5)$${\bf \omega} _{nb}^b = {\bf \omega} _{ib}^b - {\bf C}_n^b \left( {{\bf \omega} _{ie}^n + {\bf \omega} _{en}^n} \right)$$

The position is expressed in terms of latitude (L), longitude (λ) and height (h) and propagates as

(6)$$\dot L = \displaystyle{{v_n} \over {R_N + h}},\quad \dot \lambda = \displaystyle{{v_e} \over {(R_E + h)\cos L}},\quad \dot h = - v_d $$

Using these equations, the INS computes the navigation output at a rate of 100 Hz for the experiments conducted throughout this paper. It is important to note that an inertial navigation system is a dead reckoning system and any lack of precision is passed from one evaluation to the next and the navigation solution drifts with time. In this study, GPS measurements are integrated to the system via an extended Kalman filter in order to bound the navigation errors.

2.2. INS error dynamic model for small attitude error

The propagation step of the Kalman filter depends on the error state dynamics of the INS. The error state vector of the inertial system includes position, velocity, and attitude errors in addition to the accelerometer and gyroscope errors.

Under the small attitude error assumption, true direction cosine matrix, C_bⁿ, is related to the computed direction cosine matrix, ${\bf \tilde C}_b^n $, as follows:

(7)$${\bf \tilde C}_b^n = ({\bf I} - {\bf \varepsilon} \times ){\bf C}_b^n $$

and the attitude error dynamics are obtained as:

(8)$${\bf \dot \varepsilon} = - {\bf \omega} _{in}^n \times {\bf \varepsilon} + \delta {\bf \omega} _{in}^n - {\bf C}_{\bf b}^{\bf n} \delta {\bf \omega} _{ib}^b $$

Velocity and position error dynamics are

(9)$$\delta {\bf \dot v}^n = {\bf C}_b^n {\bf f}^b \times {\bf \varepsilon} + {\bf C}_b^n \delta {\bf f}^b - (2{\bf \omega} _{ie}^n + {\bf \omega} _{en}^n )x\delta {\bf v}^n - (2\delta {\bf \omega} _{ie}^n + \delta {\bf \omega} _{en}^n )x{\bf v}^n + \delta {\bf g}^n $$

and

(10)$$\delta \dot L = \delta v_n, \quad \delta \dot \lambda = \delta v_e, \quad \delta \dot h = - \delta v_d $$

where ε is the tilt angle vector, δ vⁿ the velocity error in navigation frame, δL, δλ and δh the north, east and height position errors, respectively. In addition to these nine error states standing for inertial equation errors, another six states are used in order to model the accelerometer and gyro bias errors, as first-order Markov processes:

(11)$$\delta \dot f^b = - \displaystyle{1 \over {\tau _a}} \delta f^b + w,\,\,\,\delta \dot \omega ^b = - \displaystyle{1 \over {\tau _g}} \delta \omega ^b + w$$

where w is white noise and τ _a and τ _g are time constants. Thus, the GPS/INS filter has 15 states.

2.3. Integration filter

The GPS/INS system benefits from the bounded error characteristics of the GPS and the high frequency characteristics of the INS at the same time by integrating them via a Kalman filter. Under the assumption of a small attitude error, a 15-state Kalman filter is used in order to integrate the INS and the GPS systems. The integration filter uses a dynamic error model (Section 2.2) to create the continuous state transition matrix, F, for the error states, which is then discretized using the first order Taylor series approximation to yield Φ=I+FΔt where Δt is chosen as one second and Kalman propagation of the covariance

(12)$${\bf P}^ - = {\bf \Phi P}^ + {\bf \Phi} + {\bf Q}$$

is done at 1 Hz for our application.

The Kalman update is triggered at every GPS measurement (1 Hz) using:

(13)$$\eqalign{ & {\bf K} = {\bf P}^ - {\bf H}^T ({\bf HP}^ - {\bf H}^T + {\bf R})^{ - 1} \cr & \delta {\bf x} = {\bf Ky} \cr & {\bf P}^ + = ({\bf I} - {\bf KH}){\bf P}^ -} $$

where δ x is the estimated error states, y the difference between GPS measurements (position and velocity) and INS measurements, K the Kalman gain matrix, P the state covariance matrix, R the measurement covariance matrix, Q the input covariance matrix and H the measurement matrix where the position and velocity errors are observed.

In a conventional GPS/INS system, R=diag ([2 2 3 0·1 0·1 0·1]²) is chosen based on the GPS output uncertainties. At initial P, the diagonal matrix has diagonal entities of state variances, where standard deviations of position, velocity and attitude are 5 m, 0·1 m/s and 1 mrad, respectively. The initial bias values of the accelerometers and gyroscopes are taken as 10 mg and 10°/hr. The Q matrix is created using the inertial sensors’ noise statistics.

2.4. Conventional GPS/INS performance

2.4.2. Navigation results

During the road test the GPS position, velocity, and GPS/INS position, velocity and attitude data were recorded for approximately 180 seconds. Figure 1 presents the horizontal and vertical solutions provided by the conventional GPS/INS system described in Section 2.

Figure 1. (a) Horizontal navigation using conventional GPS/INS. (b) Vertical profile of the test route output by conventional GPS/INS.

3.1. INS error dynamic model for large heading error

A Direction Cosine Matrix (DCM), C_bⁿ, holds the attitude information in between the body and navigation frames. C_bⁿ is represented by roll (ϕ) , pitch (θ), and yaw-heading (ψ) angles and can be divided into C_b^h and C_hⁿ matrices.

(14)$${\bf C}_b^n = \underbrace {\left[ {\matrix{ {\cos \psi} & { - \sin \psi} & 0 \cr {\sin \psi} & {\cos \psi} & 0 \cr 0 & 0 & 1 \cr}} \right]}_{{\bf C}_h^n} \underbrace {\left[ {\matrix{ {\cos \theta} & {\sin \phi \sin \theta} & {\cos \phi \sin \theta} \cr 0 & {\cos \phi} & { - \sin \phi} \cr { - \sin \theta} & {\sin \phi \cos \theta} & {\cos \phi \cos \theta} \cr}} \right]}_{{\bf C}_b^h} $$

where h stands for horizontal frame. Assuming small roll and pitch misalignments (ε _x, ε_y), true C_b^h, is related to the computed ${\bf \tilde C}_b^h $ as follows:

(15)$${\bf \tilde C}_b^h = (I_{3x3} - {\bf \varepsilon} x){\bf C}_b^h, \quad {\bf \varepsilon} x = \,\left[ {\matrix{ 0 & 0 & {\varepsilon _y} \cr 0 & 0 & { - \varepsilon _x} \cr { - \varepsilon _y} & {\varepsilon _x} & 0 \cr}} \right]$$

As a result of large heading error, the computed ${\bf \tilde C}_h^n $ is related to true C_hⁿ as:

(16)$${\bf \tilde C}_h^n = {\bf C}_h^n + \delta {\bf C}_h^n = \,{\bf C}_h^n + \,\,\left[ {\matrix{ {\delta \cos \psi} & { - \delta \sin \psi} & 0 \cr {\delta \sin \psi} & {\delta \cos \psi} & 0 \cr 0 & 0 & 0 \cr}} \right]$$

From Equations (15) and (16), the body to navigation attitude error is calculated as:

(17)$$ \eqalign{\delta {\bi C}_b^n = & {\bi \tilde C}_h^n {\bi \tilde C}_b^h - {\bi C}_b^n \cr = & \left( {\delta {\bi C}_h^n - {\bi C}_h^n {\bi \varepsilon }x} \right){\bi C}_b^h } $$

The DCM error is rewritten as:

(18)$$\delta {\bf C}_b^n = {\bf DC}_b^h = \left[ {\matrix{ {\delta \cos \psi} & { - \delta \sin \psi} & { - \varepsilon _x \sin \psi - \varepsilon _y \cos \psi} \cr {\delta \sin \psi} & {\delta \cos \psi} & {\varepsilon _x \cos \psi - \varepsilon _x \sin \psi} \cr {\varepsilon _y} & { - \varepsilon _x} & 0 \cr}} \right]{\bf C}_b^h $$

The sine and cosine of yaw error is chosen as the heading error states since the error Equation (18) is nonlinear in yaw error, while it is linear in γ ₁=δ sin ψ and γ ₂=δ cos ψ. Thus, INS attitude error is represented by four states of γ ₁, γ ₂, ε _x and ε _y and

(19)$${\bf D} = \left[ {\matrix{ {\gamma _2} & { - \gamma _1} & { - \varepsilon _x \sin \psi - \varepsilon _y \cos \psi} \cr {\gamma _1} & {\gamma _2} & {\varepsilon _x \cos \psi - \varepsilon _x \sin \psi} \cr {\varepsilon _y} & { - \varepsilon _x} & 0 \cr}} \right]$$

Differentiating Equation (19), one obtains the way the attitude errors propagate in time. After some algebraic manipulations, the derivative becomes

(20)$${\bf \dot D} = ({\bf D\Omega} _{nh}^h + \delta {\bf \Omega} _{nb}^n {\bf C}_h^n )$$

Ω_nh^h and δΩ_nbⁿ are the skew-symmetric matrices of ${\bf \omega} _{nh}^h $ and $\delta {\bf \omega} _{nb}^n $.

(21)$$\eqalign{ & {\bf \omega} _{nh}^h = [\matrix{ 0 & 0 & { - \dot \psi} \cr} ]^T \cr & \dot \psi = \omega _y \sin \phi \sec \theta + \omega _z \cos \phi \sec \theta \cr & \delta {\bf \omega} _{nb}^n = [\matrix{ {\delta \omega _n} & {\delta \omega _e} & {\delta \omega _d} \cr} ]^T = {\bf DC}_b^h {\bf \omega} _{ib}^b + {\bf C}_b^n \delta {\bf \omega} _{ib}^b - \left( {\delta {\bf \omega} _{ie}^n + \delta {\bf \omega} _{en}^n} \right)} $$

where ω_y and ω_z are the body rates. Using Equation (21) in Equation (20), the attitude error equations are obtained as follows:

(22)$$\eqalign{\dot \varepsilon _x & = - \dot \psi \varepsilon _y - \cos \psi \kern1pt \delta \omega _n - \sin \psi \kern1pt \delta \omega _e \cr \dot \varepsilon _y & = - \dot \psi \varepsilon _x + \sin \psi \kern1pt\delta \omega _n - \cos \psi \kern1pt \delta \omega _e \cr \dot \gamma _1 & = \dot \psi \gamma _2 + \cos \psi \kern1pt \delta \omega _d \cr \dot \gamma _2 & = - \dot \psi \gamma _1 - \sin \psi \kern1pt \delta \omega _d} $$

Velocity error is the difference between true velocity and computed velocity.

(23)$$\delta {\bf v}_{}^n = {\bf v}_{}^n - {\bf \tilde v}_{}^n $$

We obtain the following differential equations by differentiating Equation (23)

(24)$$\delta {\bf \dot v}_e^n = {\bf DC}_b^h {\bf f}^b + {\bf C}_b^n \delta {\bf f}^b - (2{\bf \omega} _{ie}^n + {\bf \omega} _{en}^n )x\delta {\bf v}_e^n - (2\delta {\bf \omega} _{ie}^n + \delta {\bf \omega} _{en}^n )x{\bf v}_e^n + \delta {\bf g}^n $$

Although, the δ C_bⁿ term of the velocity error dynamics given in Equations (9) and (24) differs under large heading error, the differential equations of position error and sensor biases remain the same as the error equations in Section 2.2.

3.2. GPS measurement model

As stated in Section 2.3, GPS/INS filters introduced in this study use GPS position and velocity measurements as external observations. However, the measurement equations developed for large heading error is different from the ones used in the conventional approach. This difference is due to lever arm compensation that requires body to navigation frame transformation. The position of INS with respect to GPS antenna centre is given by

(25)$${\bf p}_{INS} = {\bf p}_{ANT} - {\bf TC}_b^n {\bf LA}^b $$

where LA^b is the three-dimensional lever arm vector in body frame and

(26)$${\bf T} = \left[ {\matrix{ {{\raise0.7ex\hbox{$1$} \!\mathord{\left/ {\vphantom {1 {\left( {R_n + h} \right)}}}\right.\kern-\nulldelimiterspace} \!\lower0.7ex\hbox{${\left( {R_n + h} \right)}$}}} & 0 & 0 \cr 0 & {{\raise0.7ex\hbox{$1$} \!\mathord{\left/ {\vphantom {1 {\left( {R_e + h} \right)\cos L}}}\right.\kern-\nulldelimiterspace} \!\lower0.7ex\hbox{${\left( {R_e + h} \right)\cos L}$}}} & 0 \cr 0 & 0 & { - 1} \cr}} \right]$$

is used to convert north, east and down components of the lever arm into latitude, longitude and height. Using Equation (25), position measurement of the system is

(27)$$\delta {\bf p}_{meas} = {\bf \tilde p}_{INS} + {\bf T\tilde C}_b^n {\bf L\tilde A}^b - {\bf p}_{ANT} $$

and inserting Equation (18) into Equation (27) it becomes

(28)$$\delta {\bf p}_{meas} = \delta {\bf p} + {\bf TDC}_b^h {\bf LA}^b + {\bf TC}_b^n \delta {\bf LA}^b $$

Investigating the position measurement equation, it is seen that the heading error is directly observable under the non-zero lever arm condition and the quality of the heading depends on the length of the lever arm. However, using a false lever arm term or totally excluding the lever arm may cause filter divergence.

Comparing Equation (28) to the Super Short Baseline (SSBL) measurement given in Hong et al. (Reference Hong, Lee and Park2004), it is obvious that the GPS position measurement is much more effective than the SSBL measurement in heading estimation. This is due to the fact that the SSBL provides the position solution directly while the GPS gives a position solution with a lever arm offset that increases the heading observability.

The second measurement provided by GPS is velocity. The velocity of INS with respect to the antenna velocity is as follows:

(29)$${\bf v}_{INS} = {\bf v}_{ANT} - {\bf \dot C}_b^n {\bf LA}^b $$

Using the above relation, velocity measurement is

(30)$$\eqalign{\delta {\bf v}_{meas} = \ & {\bf \tilde v}_{INS} - {\bf \dot {\tilde C}}_b^n {\bf L\tilde A}^b - {\bf v}_{ANT} \cr = \ & \delta {\bf v} + {\bf \dot C}_b^n \delta {\bf LA}^b + \delta {\bf \dot C}_b^n {\bf LA}^b} $$

Using Equation (4) and the derivative of Equation (18) in Equation (31), the measurement is rearranged as

(31)$$\delta {\bf v}_{meas} = \delta {\bf v} + ({\bf D\Omega} _{nb}^h + \delta {\bf \Omega} _{nb}^n {\bf C}_h^n ){\bf C}_b^h {\bf LA}^b + {\bf C}_b^n {\bf \Omega} _{nb}^b \delta {\bf LA}^b $$

where δ Ω_nbⁿ is the skew symmetric matrix of δ ω_nbⁿ given in Equation (21).

The velocity measurement given in Equation (31) differs from the position measurement in terms of heading observability. The heading error in Equation (31) is coupled with the rotation rate as well as the lever arm. Thus, the quality of the heading depends not only on the length of the lever arm but also the instantaneous rotation of the vehicle. High rotation rates yield faster heading convergence.

GPS position and velocity measurements inherently involve attitude errors and provide direct observability of the large heading error. This enhances the system performance and enables fast heading convergence even under low dynamics. Moreover, it is possible to increase the convergence speed by increasing the length of the lever arm.

3.3. Kalman filter corrections

A Kalman filter based on the error dynamics given in Section 3.1 and the measurements described in Section 3.2 is used to generate corrections for diminishing the position, velocity, attitude and inertial sensor errors. As a result, the overall GPS/INS output becomes driftless with bounded uncertainty. The Kalman estimated error states are as follows:

(32)$$\delta {\bf x} = \left[ {\matrix{ {\delta {\bf p}} & {\delta {\bf v}} & {\delta \sin \psi} & {\delta \cos \psi} & {\varepsilon _x} & {\varepsilon _y} & {\delta {\bf f}^b} & {\delta {\bf \omega} ^b} \cr}} \right]^T $$

The estimated errors are fed back to the inertial sensors and inertial navigation system after every update step of Kalman filter. Corrections to the navigation system are realized as:

(33)$$\eqalign{& {\bf p} = {\bf \tilde p} - \delta {\bf p} \cr & {\bf v} = {\bf \tilde v} - \delta {\bf v} \cr & {\bf C}_b^n = {\bf \tilde C}_b^n - {\bf DC}_b^h \cr & {\bf f}^b = {\bf \tilde f}^b - \delta {\bf f}^b \cr & {\bf \omega} ^b = {\bf \tilde \omega} ^b - \delta {\bf \omega} ^b} $$

The position, velocity and sensor bias corrections are straightforward. The attitude correction can be realized by using the estimated γ ₁, γ ₂, ε _x and ε _y states and current values of roll (ϕ) and pitch (θ) angles as follows:

(34)$$\delta {\bf C}_b^n = \left[ {\matrix{ {\gamma _2} & { - \gamma _1} & { - \varepsilon _x \sin \psi - \varepsilon _y \cos \psi} \cr {\gamma _1} & {\gamma _2} & {\varepsilon _x \cos \psi - \varepsilon _x \sin \psi} \cr {\varepsilon _y} & { - \varepsilon _x} & 0 \cr}} \right]\left[ {\matrix{ {\cos \theta} & {\sin \phi \sin \theta} & {\cos \phi \sin \theta} \cr 0 & {\cos \phi} & { - \sin \phi} \cr { - \sin \theta} & {\sin \phi \cos \theta} & {\cos \phi \cos \theta} \cr}} \right]$$

However, heading corrections that can be tens of degrees can cause numerical instability. In order to prevent loss of orthonormality in C_bⁿ, an orthonormalization process is run after every correction step.

4.1. Test setup

The land vehicle set-up employed in the proposed GPS/INS system is exactly the same as the one given in Section 2.4.1. The test was conducted for 180 seconds and the raw IMU data, GPS data and GPS/INS data were collected throughout the test. The proposed system's navigation data was obtained by post-processing of the raw IMU data and the GPS data using models given in Section 3.

4.2. Test results

In this section the GPS/INS computed navigation solution obtained during the road test (Figure 1) is employed as reference. The conventional system given in Section 2 and the proposed system given in Section 3 are compared to this reference.

Collected GPS and IMU data are used to simulate the conventional and the proposed systems with different initial heading errors. The heading solutions under an initial error of ±90° are given in Figure 2.

Figure 2. Heading error comparison of proposed and conventional systems with a) 90° and b) −90° initial heading error.

It is seen in Figure 2 that the proposed system shows higher performance with 90° and −90° initial heading error and converges faster compared to the system using the small angle error assumption. In the proposed system, the heading error decreases below 5° in 40 seconds. However, the heading error of the conventional system does not reach 5° during the entire test.

The behaviour of the compared systems with 180° initial heading error is more striking. As seen in Figure 3, the proposed system converges while the conventional one does not in this case. The proposed system attains 4° heading error at the end of the test.

Figure 3. Heading error comparison of proposed and conventional systems with 180° initial heading error.

It is also important to see that the position solution of the proposed system with 180° initial heading error is similar to the position of the reference system (Figures 4 and 5). Thus, the proposed system's navigation solutions can be used from the beginning of the mission.

Figure 4. Horizontal position errors of the proposed system with 180° initial heading error.

Figure 5. Vertical position error of the proposed system with 180° initial heading error.