Enhanced Autonomous Vehicle Positioning Using a Loosely Coupled INS/GNSS-Based Invariant-EKF Integration

High-precision navigation solutions are a main requirement for autonomous vehicle (AV) applications. Global navigation satellite systems (GNSSs) are the prime source of navigation information for such applications. However, some places such as tunnels, underpasses, inside parking garages, and urban high-rise buildings suffer from GNSS signal degradation or unavailability. Therefore, another system is required to provide a continuous navigation solution, such as the inertial navigation system (INS). The vehicle’s onboard inertial measuring unit (IMU) is the main INS input measurement source. However, the INS solution drifts over time due to IMU-associated errors and the mechanization process itself. Therefore, INS/GNSS integration is the proper solution for both systems’ drawbacks. Traditionally, a linearized Kalman filter (LKF) such as the extended Kalman filter (EKF) is utilized as a navigation filter. The EKF deals only with the linearized errors and suppresses the higher orders using the Taylor expansion up to the first order. This paper introduces a loosely coupled INS/GNSS integration scheme using the invariant extended Kalman filter (IEKF). The IEKF state estimate is independent of the Jacobians that are derived in the EKF; instead, it uses the matrix Lie group. The proposed INS/GNSS integration using IEKF is applied to a real road trajectory for performance validation. The results show a significant enhancement when using the proposed system compared to the traditional INS/GNSS integrated system that uses EKF in both GNSS signal presence and blockage cases. The overall trajectory 2D-position RMS error reduced from 19.4 m to 3.3 m with 82.98% improvement and the 2D-position max error reduced from 73.9 m to 14.2 m with 80.78% improvement.


Introduction
The main challenge in autonomous vehicle (AV) navigation is providing an accurate, reliable, and continuous navigation solution in all environments. Therefore, the most common source of navigation solutions for AVs is the GNSS because of its long-term solution stability [1]. However, there are several situations where the GNSS cannot provide either an accurate solution or any solution at all. Particularly, in urban areas with tunnels, underground parking, and high-rise buildings where a line of sight 'LOS' between at least four GNSS satellites and the receiver's antenna is lost, a degraded or complete solution loss occurs [2]. Therefore, there is an important need for a backup system that is capable of providing an uninterrupted navigation solution, such as the inertial navigation system (INS). The INS solution is immune to signal interference but its quality depends mainly on the inertial measuring unit (IMU) grade [3].
However, IMUs are vulnerable to various error sources which can be categorized into deterministic and stochastic errors. The deterministic errors are mitigated by calibration while the stochastic errors cannot be calibrated but they can be modeled. Despite that, the INS solution drifts over time because its mechanization utilizes Newton's laws of motion converge within an area of attraction. Moreover, it is independent of the system's trajectory and relies on the IMU dynamics. In addition, this paper shows how the specified system can be used as a process model for the IEKF as it satisfies the property of log-linear error dynamics [33,34]. The contributions of this paper are summarized as follows: • The IEKF-based quaternion rotation is applied using real road data from global positioning system (GPS) receiver measurements and low-cost IMU in a loosely coupled integration scheme. • A performance comparison between the traditional EKF and the proposed IEKF is presented including GPS outage. • An analysis of the two filters' performance during various dynamics with several average speeds and traveled distances is illustrated for validation purposes.
The organization of the paper's remaining sections is as follows: Section 2 provides the methodology of using IEKF as a navigation filter of loosely coupled INS/GPS integration. Section 3 provides the experimental setup and the specifications of the sensors utilized in the road trajectory. Section 4 describes the experimental setup and results. Finally, Section 5 concludes the proposed methodology and gives recommendations for future work.

Inertial Navigation Systems (INS)
The strap-down inertial navigation system (INS) depends on the IMU measurements, the navigation mechanization, and the initial states from the GPS or manually entered by the user. Basically, the IMU utilizes a coincident 3 accelerometers, to provide the measured specific forces, and 3 gyroscopes to provide the measured angular rates [35][36][37][38].
The INS as a self-contained navigation system uses the knowledge of its carrying platform's initial states (position, velocity, and attitude (P, V, A)) and accordingly updates its current states.
Starting with the angular rates (ω x , ω y , ω z ), the attitude angles pitch, roll, and yaw (p, r, y) can be obtained after calculating the transformation matrix. Simultaneously, the rotation matrix is applied to the accelerations ( f x , f y , f z ) to convert them into forces in the navigation/ local-level frame (LLF). Afterward, the velocity is provided by integrating the transformed forces and the position is derived by integrating the calculated velocity [1,39]. Finally, the produced PVA becomes the initial state of the next epoch. The attitude in the quaternion is represented in Equation (1).
where A is the attitude and (q 0 , q 1 , q 2 , q 3 ) are the rotation matrix's quaternion parameters.
The attitude ratesȦ are calculated as Equation (2).
Also, the quaternion attitude can be transferred to Euler's angles roll, pitch, and yaw, respectively, as in Equation (3).
where φ is the roll angle in radians, θ is the pitch angle in radians, and ψ is the yaw angle in radians. The velocity (v) components in the LLF are shown in Equation (4). They are derived from the transformed forces as in Equations (7) and (8).
where V N is the north velocity, V E is the east velocity, and V D is the down velocity. The position (p) components are shown in Equation (5) where ϕ is the latitude, λ is the longitude, and h is the altitude. Equation (6) shows the transformation matrix from the body frame to LLF using quaternion states [40].
The specific forces can be transformed into LLF using the transformation matrix C n b and are obtained as in Equation (7).
The velocity rates can be obtained as in Equation (8).
whereλ is the longitude rate,φ is the latitude rate, w e = 7.2921158 × 10 −5 rad/s is the Earth's rotation rate, and g is the modeled gravity that is calculated as shown in Equation (9).
where g WGS0 = 9.78032677 m/s 2 is the nominal gravity measured at the equator, g WGS1 = 0.00193185138639 m/s 2 is the gravity formula constant, and E 2 = 0.0818191908426 is the 2nd eccentricity that identifies Earth's flattening.
where R M and R N are semi-major and semi-minor axes radii of Earth's ellipsoid model, respectively.
Nevertheless, the INS provided navigation solution suffers from error growth over time as a result of the target's acceleration two-times integration process. The errors in the INS can be classified as deterministic or stochastic/random. Deterministic errors include bias offset, scale factor, and axis misalignment. In contrast, random errors include bias drift, bias stability, scale factor stability, and noise. Deterministic errors can be reduced or compensated if the sensors, particularly high-end sensors, are properly calibrated, whereas stochastic errors are modeled randomly to reduce their effect [1,42].
Moreover, the categorization of the INS systems relies mainly on both the accuracy and error reduction capabilities of the utilized IMU. Therefore, the need to compensate for the low-cost commercial IMUs' errors has been elevated by assisting the INS system with other systems that have long-term stability, such as the GNSS systems. The integration between the two systems produces a more accurate and reliable navigation system. However, the error growth rate between each consecutive update is limited by the quality of the INS system only. Traditionally, EKF is utilized in open-or closed-loop schemes to bind the error growth. Unfortunately, the EKF is bounded by the first-order part of the system's errors derived by using Taylor expansion as an initial step.
A detailed block diagram of the utilized INS system mechanization using the quaternion angles is illustrated in Figure 1.

Position
Velocity rate Position rate initial position in the n-frame initial velocity n the n-frame initial attitude provided as quaterions Initial condition  

Extended Kalman Filter
The EKF is applied to be a closed-loop configuration, in which the state's error can be estimated and fed back again to the INS sensor to obtain more accurate INS solutions and maintain the linearity of the system model [6]. Moreover, the EKF uses the Taylor expansion as a linearizing technique for nonlinear systems and the EKF deals only with the linearized errors and suppresses the higher orders using the Taylor expansion up to the first order. Thus, using EKF provides unbounded errors in the updated error covariance which leads to solution divergence [1,43].

State Representation
To estimate (p, v, A) of the vehicle in the navigation frame, the continuous system model states are represented by Equation (11).
The state vector including error components is given by Equation (12).
where δp e = [δϕ, δλ, δh] T are the position error states, δv e = [δv N , δv E , δv D ] T are the velocity error states, and δA e = [δp, δr, δy] T are the attitude error states. Furthermore, F is the dynamic coefficient matrix and can described as in Equation (13).
where F p denotes the position's coefficient matrix, F v represents the velocity's coefficient matrix, and F A denotes the attitude's coefficient matrix, and each matrix dimension is 3 × 3 as detailed in [1]. Finally, G denotes the noise distribution, represented by Equation (14), and w is the unit-variance white Gaussian noise.
The system model for a loosely coupled integration is given by Equation (15).
This system model equation can be discretized as in Equation (16).

Measurement Model
The measurement model in the discrete-time domain is given by Equation (17).
where δx n is the state vector, η n is a vector of measurement noise, and H n is the measurement design matrix at time t n . H n in a simple form is given in Equation (19).
The full measurement model is given as in Equation (20).
The expanded form of the measurement model is given as in Equation (21).
The covariance matrix of the measurement model error R n which contains the variances of the measured states is given as in Equation (22).
The covariance matrix of the states prediction P n which contains the variances of predicted states is given as in Equation (23). Moreover, it is a 9 × 9 element square matrix.

Invariant Extended Kalman Filter (IEKF)
The states of IEKF are not the typical Jacobian linearization along a trajectory as in the traditional EKF. On the contrary, the invariant error on the Lie group can be exactly recovered from its solution using the Riccati equation [44][45][46][47][48]. In this section, the IEKF utilizes position, velocity, and attitude (PVA) from the INS solution and the position from the GPS receiver as its control input parameters in a loosely coupled scheme.
Let a matrix Lie group denoted by G be the group of 3 × 3 rotation matrices that preserve orientation and its Lie algebra is the space of skew-symmetry matrices denoted by g that takes an element in the tangent space of G at the identity to its corresponding matrix representation as follows [46]: then the exponential map is shown as in the equation that relates a matrix Lie group to its associated Lie algebra as follows: and where exp m (·) is the standard matrix exponential.
A process dynamics evolving on the Lie group is shown as follows: where the state X t belongs to the Lie group G and u t is an input variable as shown in Equation (29), the true state trajectory is X t , and the estimated trajectory of it is X t ; then, the state estimation error is defined using left multiplication of X −1 t as shown in Equation (28).
where η l t is the left-invariant errors between two trajectories X t and X t and Γ ∈ G is an arbitrary element of the group, and A system is an affine group if the system dynamics f u t (·) satisfies that: for all t > 0 and X 1 , X 2 ∈ G Where I d represents the identity matrix. Moreover, if this condition is satisfied then the left-invariant error dynamics are trajectory independent and satisfy Equation (31).

State Representation
To estimate (p t , v t , A t ) of the vehicle in the navigation frame, the continuous system model states are represented by Equation (24).
Where the system model is denoted by X t , the position p t states (latitude, longitude, and altitude(ϕ, λ, h)), the velocity v t states (V N , V E , V D ), the transformation from the body frame to the navigational frame C t states, and the index t refers to the time invariant of IEKF at time (t). Moreover, the adjoint operator Ad X t plays a key role in the theory of Lie groups, it is as described in the adjoint map, and it represents the linear map of the Lie group as follows [46]: where ξ is the invariant error, L g (ξ) is the log of the invariant error, and X is the state vector.
where ξ p t 3×1 is the position invariant error, ξ v t 3×1 is the velocity invariant error, and ξ C t 3×3 the transformation matrix.
The exponential mapping is given as in Equation (36).
The adjoint operator matrix is given as shown in Equation (37).
The dynamic system model can be expressed as follows: the noise due to the IMUs' gyroscopes, and w f t is the noise due to the IMUs' accelerometers. Moreover, depending on Equation (38), then the system dynamics [ f u t (·)] satisfy the affine group property as mentioned in Equation (30). Therefore, the left-invariant error dynamics will evolve independently of the system's state and are represented as in Equation (39).
Moreover, the invariant error satisfies a log-linear property if A t is defined by Equation (40).
As the dynamic system model X t satisfies the affine group property, the left-invariant error dynamics are independent of the system's state. Moreover, the left-invariant error satisfies the log-linear theorem and its log [ ξ ∈ R dimg ] satisfies the linear system as shown in Equation (30).
where A t is the linear system error coefficient. Moreover, A t can be represented by the Lie group as a representation of the state transition matrix of the discrete-time linear dynamic system Φ. As the IEKF adopts the Kalman filter properties, there are prediction and update stages. In the prediction stage, the state transition matrix is used in calculating the covariance matrix P using the Riccati equation as follows: where P n is the estimation covariance of state estimation uncertainty in matrix form, and Φ is the state transition matrix that is represented by the exponential map of the adjoint operator A t as described in Equation (43).
moreover, A t is obtained from the linearization of the left-invariant error dynamics ( g l u t ) by using first-order approximation as follows: where η l t is derived from the log-linear property theorem as a result of the autonomous error dynamics theorem that defines the system as an affine group if its dynamics satisfy for all time epochs and t > 0 [23,46].
where g l u t η l t is the Lie group of the left-invariant error dynamics ξ t and calculated as follows: where I is the identity matrix and its dimension is 3 × 3, ξ t is the vector map of the navigation element (position, velocity, or rotation) of the Lie group, (·) ∧ denotes a 3 × 3 skew-symmetric matrix, and g is the the modeled gravity as in Equation (9).
To simplify the previous equations, then g l u t (η l t ) can be represented as shown in Equation (48).
Accordingly, the covariance matrix, P t , is computed using the Riccati equation as follows: where where (·) ∧ denotes a 3 × 3 skew-symmetric matrix and Finally, the previous state system covariance is calculated as shown in Equation (53): From the previous derivation, it is clear that the linear system error coefficient, A t , depends on the IMU measurements that are taken in the body frame and the covariance matrix P calculation depends on it.
This system model equation can be discretized and linearized as in Equation (54): where t n is the discrete nature of the position, velocity, and attitude vectors, and the upper hat is used for the state's estimations. The measurement model is represented by Equation (55).
where ε t n is the measurement error, and b is the measurement model's design matrix and is represented as in Equation (56).
The continuous dynamics can be discretized by assuming a zero-order hold on the inputs and performing quaternion integration from within a ∆t time frame starting at t n−1 and ending at t n .
The discrete dynamics of each state element (position, velocity, and orientation) are represented by Equations (57)-(59), respectively.
The position states are derived as in Equation (57) using the previously known position and velocity in addition to the current state acceleration.
wherep t n−1 is the old position state,v t n−1 is the old velocity,Ĉ t n−1 is the quaternion transformation matrix from the body frame to the navigation frame using previous orientation angles, and f t is the current acceleration state.
v t n =v t n−1 + Ĉ t n−1 f t + g ∆t (58) where ∆t is the time update and exp(·) is the exponential map.
The linearized invariant error first-order approximation η l t n is represented by Equation (60) as follows [25,46,49]: where η l t n is the old linearized invariant error first-order approximation, L t n is the filter gain matrix, exp(·) is the exponential map, b is the measurement model's design matrix, X −1 t n is the estimate of the system model states, and ε t n is the measurement error. The updated estimate of the system model states is shown in Equation (61).
The covariance update equation of the IEKF is shown in Equation (62).
where H is the measurement model linearized design matrix reduced form given as shown in Equation (63) [46]: and A detailed derivation of the relation between H and b is explained using both autonomous error dynamics and log-linear property of the error theorems in [46].

Measurement Model
The measurement model for the GPS readings Y t n is represented as in Equation (55) [50]. Where The expanded form of the measurement model is given as in Equation (66).
where the measurements covariance matrix R n is given as in Equation (22).
A simplified block diagram of the IEKF algorithm shown in Figure 2 indicates the process of the prediction and the obtained corresponding covariance matrix using the Riccati equation. Moreover, the update of the states and the updated covariance matrix is also mentioned.

INS-GPS Integration Using IEKF
Despite the GPS's long-term stability and the INS's short-term stability, the fusion/ integration between the two systems provides a better solution than each of them separately. The integration scheme shown in Figure 3 is a loosely coupled framework. However, in this type of integration, a GPS solution is a main requirement for the filter to work properly.
The INS system provides a continuous PVA solution with unbounded overtime drift [4]. Therefore, aiding information is demanded for error control and solution enhancement. The GPS provides position readings to update the INS. Traditionally, the extended Kalman filter (EKF) is the utilized navigation filter [51]. However, the EKF in general is not an optimal estimator, used for nonlinear systems [8], as it will fail if the functions are highly nonlinear (1st-order approximation) because its state estimates depend on Jacobians, but invariant extended Kalman filter states are not the typical Jacobian linearization along a trajectory because the invariant error on the Lie group can be exactly recovered from its solution.
The main advantage of IEKF over EKF is that the state linearization and measurement models are independent of the current estimation of the state leading to state-independent Jacobians at any linearization point [27,28]. In this work, an IEKF works as a navigation filter. The algorithm provides accurate positioning information.

Experimental Setup
A real road trajectory was conducted to study the proposed method's performance in the urban area of the city of Kingston, Ontario, Canada in 2020. The vehicle used in the conducted trajectory is shown in Figure 4 and three major units are used as shown in Figure 5. The reference is provided by a tightly coupled INS/GPS integration between the Novatel Propack6 GPS receiver and the KVH-1750 IMU. Those units' specifications are described as follows [52]:  • NOVATEL ProPak6 is a GNSS receiver with a 2D-position accuracy of less than 1.5 m, a velocity accuracy of less than 0.03 m/s, and a time accuracy of 20 ns. For the purpose of this experiment, its data rate that can reach 100 Hz is adjusted to 1 Hz. • KVH-1750 IMU's gyroscopes have a bias instability of ≤ 0.05 • / hr , 1σ, a maximum of ≤ 0.1 • /hr, 1σ, with bias offset ±2 • /hr, and angle random walk of ≤ 0.012 • / √ hr ≤ 0.7 • /hr/ √ Hz . The IMU's accelerometers have a bias instability of 0.01mg −1σ , a scale factor linearity of 0.008, and a velocity random walk of 0.024 mg/ √ Hz. For the purpose of this experiment, its data rate that can reach 1000 Hz is adjusted to 100 Hz • VTI-IMU (model number: SCC1300-D04) is a low-cost MEMS-grade IMU. It has a gyro bias drift of 1.5 deg/s, a scale factor linearity ≤ 2%, a velocity random walk of 0.86 deg/ √ Hz, and an update rate of 20 Hz.

Results and Discussion
The IEKF filter is applied to a loosely coupled INS/GPS integration for AV navigation. The results show a significant improvement in the positioning information provided to the vehicle compared to the traditional EKF.
The proposed INS/GPS-IEKF integrated navigation system performed better during several dynamics, such as straight driving, turns, and consecutive turns at various speeds. Figure 6 shows the overall trajectory overlaid onto a digital map based on the reference from a tightly coupled INS/GPS integration between the Novatel Propack6 GPS receiver and the KVH-1750 IMU compared to the INS/GPS-based EKF and IEKF. Moreover, the performance of the two applied filters is tested using two different trajectory parts. The first part is shown in Figure 7 and contains several dynamics such as a right turn, straight driving, and left-right turns. In this part, the INS/GPS-based EKF diverges, unlike the INS/GPS-based IEKF, which converges with the reference. Furthermore, Figure 9 shows two dynamics, straight driving between two right turns. The proposed INS/GPS-based EKF still diverges unlike the IEKF that keeps on its convergence with the reference.  Finally, the estimated 2D-position RMS error of the proposed INS/GPS-based IEKF compared to its correspondence from the INS/GPS-based EKF over the whole trajectory is shown in Figure 10.
Moreover, a comparison between the position component errors in the north, east, and down frames between the traditional INS/GPS-based EKF and the proposed INS/GPSbased IEKF is shown in Figure 11. The results show that the output north and east position component errors when applying the proposed method are less than the traditional INS/GPS-based EKF over the whole trajectory. This leads to a better 2D-position estimation when using the proposed method. Furthermore, the altitude error is significantly decreased when using the proposed method which results in a better 3D-position estimation. However, the EKF altitude results in a significant error during the first stationary 3 min due to the start location. This location suffered from high vertical dilution of precision (VDOP) during the experiment. Therefore, a divergence occurred until the vehicle started to move as the results imply. Moreover, the EKF state correction depends on a time-based GPS update and uses the last reading in the measurement model which leads to a solution drift if the updates are incorrect or blocked. On contrary, during the same period of time the IEKF altitude results in a better estimation due to the right estimation of the INS errors. Generally, the proposed method provides a better position estimation than the traditional one. For further analysis, Table 1 shows both mean and standard deviation (STD) of each system's position component error. The results confirm the superiority of the proposed system as both mean and STD of the north, east and down (NED) position component error are minimum when applying the proposed INS/GPS-based IEKF system. Furthermore, the whole trajectory minute by minute RMS error comparison between the two systems is shown in Figure 12. In detail, the overall trajectory takes about 41 min involving a stationary three minutes in the beginning. During the first three minutes there were INS alignment processes and the GPS readings had a high DOP error due to the location surroundings. This stationary period is used for checking the performance of the two systems in a no-motion state. Afterward, when the vehicle started to move a significant change in performance appears. Clearly, at minute number 5 the proposed INS/GPS-based EKF diverges due to high speed unlike the performance of the proposed INS/GPS-based IEKF that converges with the reference. Moreover, the max RMS error occurred at the 17th minute for both systems. However, during this exact minute, the average speed was almost 16.5 m/s and the travel distance was almost 1 km. In particular, the proposed INS/GPS-based IEKF system with a 2D-position RMS error reached 5.5 m, while the INS/GPS-based EKF reached 39 m within this particular period.
Generally, the performance of the proposed system during the whole trajectory produced a significant reduction in the 2D-position RMS error which provides a great enhancement of the position information provided to the vehicle. The average 2D-position RMS error was reduced from 19.3 m to 3.3 m when applying the proposed INS/GPS-based IEKF algorithm with an enhancement percentage of 82.9%. More details about the results are illustrated in Tables 2 and 3. Table 2 shows the position max errors. The tabulated results show a significant enhancement in the 3D position when using the proposed INS/GPS-based IEKF system due to the superiority in the altitude estimation unlike the traditional system INS/GPS-based EKF system. Moreover, from Table 2 the 2D-position max error reduced from 73.9 m to 14.2 m with 80.78% enhancement and the 3D-position max error reduced from 94.7 m to 30.9 m with 67.37% enhancement. The results presented in Table 3 provide information on the position RMS errors of each position component (N, E, and D) as well as the 2D and 3D position RMSE for both systems. The proposed INS/GPS-based IEKF system exhibited a significant improvement for all position components, with a particularly noticeable enhancement in the 3D position due to its superior altitude estimation when compared to the traditional INS/GPS-based EKF system. Specifically, the 3D-position RMS error was reduced from 25.7 m to 3.5 m, representing an 86.38% improvement. Similarly, the 2D-position RMS error was reduced from 19.4 m to 3.3 m, indicating an 82.98% improvement. To compare the performance of the proposed INS/GPS-based IEKF system and the traditional INS/GPS-based EKF system in a more intuitive way, two radar maps depicting the maximum error and RMSE of position parameters are presented in Figures 13 and 14. These figures reveal that the proposed system exhibits a smaller error area as compared to the traditional system. In conclusion, the results demonstrate that the proposed INS/GPS integrated navigation system utilizing IEKF offers superior error estimation and provides more accurate position information than the EKF-based system.

Conclusions
In this paper, we proposed an IEKF filter for a loosely coupled INS/GPS integration scheme to improve the navigation of autonomous vehicles. The proposed method resulted in a significant improvement in the accuracy of the positioning information provided to the vehicle. We presented a detailed explanation of the proposed integrated navigation system and tested it over a real road trajectory. Our results demonstrated that the proposed INS/GPS-based IEKF system outperformed the traditional INS/GPS-based EKF system during various vehicle dynamics such as straight driving, turns, and consecutive turns at different speeds. Specifically, the 2D-position RMSE was reduced from 19.4 m to 3.3 m with an 82.98% improvement, and the 3D-position RMSE was reduced from 25.7 m to 3.5 m with an 86.38% improvement. Furthermore, the 2D-position max error was reduced from 73.9 m to 14.2 m with an 80.78% improvement, and the 3D-position max error was reduced from 94.7 m to 30.9 m with a 67.37% improvement compared to the traditional system.
For future studies, we plan to assess the proposed approach under various GPS outage scenarios and expand the updates to include both velocity and IMU sensor measurements. Additionally, we will conduct a performance comparison study of the four systems: