GO-INO: Graph Optimization MEMS-IMU/NHC/Odometer Integration for Ground Vehicle Positioning

Global navigation satellite system (GNSS) and inertial navigation system (INS) are indispensable for ground vehicle position and navigation. The Kalman filter (KF) is the first choice to integrate them and output more reliable navigation solutions. However, the GNSS signal is denied in urban areas, i.e., tunnels, and the INS position errors diverge quickly over time. Under normal conditions, the ground vehicle will not slide or jump off the ground; nonholonomic constraints (NHC) and odometers are available to aid the INS and reduce its position errors. Factor graph optimization (FGO) recently attracted attention as an advanced sensor fusion algorithm. This paper implemented the FGO method based on GNSS/INS/NHC/Odometer integration. In the FGO, state transformation, measurement model, the NHC, and the odometer were all regarded as constraints employed to construct a graph; an iterative process was utilized to find the optimal estimation results. Two experiments were carried out: firstly, the FGO-GNSS/INS performance was assessed and compared with the KF-GNSS/INS; secondly, we compared the FGO-GNSS/INS/NHC/Odometer and KF-GNSS/INS/NHC/Odometer under GNSS denied environments. Experimental results supported that the FGO improved the performance.


Introduction
With the booming of the location-based service (LBS), i.e., autonomous driving, unmanned aerial vehicles (UAV), reliable positioning results are critical for these applications [1][2][3][4][5]. GNSS (global navigation satellite system) is the dominant tool for providing precise and dependable three-dimensional positioning information. In the open-sky environments [6], there are usually enough in-view satellites, and the GNSS receivers can generate exact three-dimensional positions. However, GNSS signals might be obstructed in urban areas with tall buildings or reflected by the surrounded buildings. The signal blockage will degrade the geometry distribution of the in-view satellites, and the positioning accuracy will decrease [7,8]. Therefore, GNSS is usually integrated with the INS to generate more reliable navigation solutions [9,10].
INS is a self-contained navigation system that generates position, velocity, and attitude (PVA) information through processing the gyroscope and raw accelerometer measurements from the inertial measurement unit (IMU) [11][12][13]. Due to the complex noises contained in the basic measurements from the IMU, INS positioning errors diverge quickly over time. In the GNSS/INS-integrated navigation system, the integration filter, usually the Kalman filter (KF), estimates the IMU error states and compensates for its errors; in this manner, the INS can still output precise navigation solutions in a short time during GNSS signal outage [14,15]. In urban areas, the GNSS signal is usually blocked by surrounding buildings. Improving the INS accuracy during GNSS signal outages is significant [16][17][18].

KF-GNSS/INS/NHC Integration
While the ground vehicles usually drive on the road, we can find two motion characteristics in the vehicle body coordinates (1) up-direction velocity is approximately zero; (2) velocity of the lateral is approximately zero. The equations are written as [44]: where v v x denotes the lateral velocity in the vehicle body coordinates, v v z indicates the up-direction velocity. The ground vehicle coordinate is presented in Figure 1. X axis of the vehicle body coordinate is defined as pointing to the right side of the vehicle body, the Y axis of the vehicle body coordinate is defined as pointing to the direction of the vehicle heading, the Z axis of the vehicle body coordinate is defined as pointing to the direction of the roof.

KF-GNSS/INS/NHC Integration
While the ground vehicles usually drive on the road, we can find two motion characteristics in the vehicle body coordinates (1) up-direction velocity is approximately zero; (2) velocity of the lateral is approximately zero. The equations are written as [44]: where v x v denotes the lateral velocity in the vehicle body coordinates, v z v indicates the up-direction velocity. The ground vehicle coordinate is presented in Figure 1. X axis of the vehicle body coordinate is defined as pointing to the right side of the vehicle body, the Y axis of the vehicle body coordinate is defined as pointing to the direction of the vehicle heading, the Z axis of the vehicle body coordinate is defined as pointing to the direction of the roof. While the ground vehicle drives smoothly on the road, these velocity constraints can be employed to suppress the diverging positioning errors of the INS. The motion constraints can be used as measurements in the GNSS/INS integration filter. While the IMU is installed in the vehicle for sensing the motions, there is a misalignment angle between the IMU body coordinate and the vehicle body coordinate. While applying the movement constraints in the integration filter, the measurement vector should be projected to the IMU body coordinate through the conversion matrix calculated through the misalignment angle. Here, assuming the misalignment matrix is b v C , then the conversion can be mod- sin cos cos cos

State Propagation and Measurement Model
In the KF-GNSS/INS integration, attitude errors, velocity errors, position errors, gyroscope three-axis bias, and accelerometer three-axis bias are included in the state vector. While the ground vehicle drives smoothly on the road, these velocity constraints can be employed to suppress the diverging positioning errors of the INS. The motion constraints can be used as measurements in the GNSS/INS integration filter. While the IMU is installed in the vehicle for sensing the motions, there is a misalignment angle between the IMU body coordinate and the vehicle body coordinate. While applying the movement constraints in the integration filter, the measurement vector should be projected to the IMU body coordinate through the conversion matrix calculated through the misalignment angle. Here, assuming the misalignment matrix is C b v , then the conversion can be modeled as: where α ψ denotes the misalignment heading angle, α θ denotes the pitch misalignment angle, V b denotes the velocity vector in the IMU body coordinate, v b x , v b y and v b z denote the three-axis velocity in the IMU body coordinate, V v denotes the velocity vector in the vehicle body frame, and v v x , v v y and v v z denote the three-axis velocity in the vehicle body coordinate.

State Propagation and Measurement Model
In the KF-GNSS/INS integration, attitude errors, velocity errors, position errors, gyroscope three-axis bias, and accelerometer three-axis bias are included in the state vector. The integration filter estimates these state variables, which is employed for compensating the INS errors. Assuming the state vector is termed as X I NS . represents the state transformation matrix, and w indicates the state model noise matrix [35].
In the GNSS/INS integration, the position and velocity from GNSS and INS are subtracted and then employed to compose the measurement vector of the integration filter. The measurement vector is written as: 1 1 where the measurement noise, the superscript "INS" and "GNSS" denote the information from the INS and GNSS, respectively, indicates the measurement vector, which is composed of the position and velocity difference between the GNSS and INS (Equation (7)), μ denotes measurement noise, indicates the observation matrix and it is written as

NHC Measurement Model
As aforementioned, while the vehicle drives smoothly on the road, the up and lateral velocity in the vehicle body coordinates are both almost zero and are termed as b x v and b z v . Converting the speed from the vehicle body frame to the IMU body frame: where I b C denotes the velocity conversion matrix, ( ) denotes the converted velocity in the IMU body coordinates; and then, converting the velocity from the IMU frame to the navigation frame and the procedure is expressed as = ∇ x ∇ y ∇ z refers to the three-axis gyroscope bias errors, respectively. The state transformation model of the GNSS/INS integration model is built based on the INS error propagation; the state equation is written as where X I k denotes the state vector, F k,k+1 represents the state transformation matrix, and w indicates the state model noise matrix [35].
In the GNSS/INS integration, the position and velocity from GNSS and INS are subtracted and then employed to compose the measurement vector of the integration filter. The measurement vector is written as: where the measurement noise, the superscript "INS" and "GNSS" denote the information from the INS and GNSS, respectively, Z k+1 indicates the measurement vector, which is composed of the position and velocity difference between the GNSS and INS (Equation (7)), µ denotes measurement noise, H k+1 indicates the observation matrix and it is written as

NHC Measurement Model
As aforementioned, while the vehicle drives smoothly on the road, the up and lateral velocity in the vehicle body coordinates are both almost zero and are termed as v b x and v b z . Converting the speed from the vehicle body frame to the IMU body frame: where C I b denotes the velocity conversion matrix, (V NHC ) I b denotes the converted velocity in the IMU body coordinates; and then, converting the velocity from the IMU frame to the navigation frame and the procedure is expressed as where C n I denotes the velocity conversion matrix, and the (V NHC ) I b and (V NHC ) n I denote the velocity in the IMU body frame and the navigation frames. The NHC measurement vector is written as V I NS n denotes the velocity obtained from processing the IMU raw measurements.

Odometer Measurement Model
An odometer can provide odometry measurements of the ground vehicle, and the ground vehicle velocity can be estimated with the odometry difference. The odometer output is written as The odometer velocity is measured in IMU body coordinates; we assumed the misalignment angle between the odometer and the IMU is zero. Then, converting the velocity from the IMU body frame to the East-North-Up navigation coordinates, the measurement vector is written as where V I NS n denotes the IMU velocity, (V Odo ) b denotes the velocity from the odometer, H Odo denotes the observation matrix, and V Odo denotes the measurement noise vector.

Kalman Filter
State and measurement models of the KF-GNSS/INS integration methods are listed in Equations (4)-(11); the KF is usually employed to estimate the state vector recursively. In the typical KF, five equations are generally essential for the two steps in KF: predicting and updating procedures.
In the prediction step, KF predicts the state vector through the state propagation model, and the state estimation errors covariance matrix is also predicted through the state propagation matrix. The equations of the prediction are written as The state vector, state estimation errors covariance matrix, and the Kalman gain matrix are updated in the updating step. The equations are written as where F k|k−1 denotes the state transformation matrix, Q denotes the covariance matrix of the state process noise, and R denotes the covariance matrix of the measurement noise, P denotes the state estimation errors covariance matrix, H denotes the observation matrix, "−"denotes the prediction, "ˆ"denotes the estimation, and K denotes the Kalman gain matrix. While the GNSS is expected, the system works with GNSS/INS integration mode, and reliable navigation solutions are expected. At the same time, GNSS is denied; according to the vehicle state detection results, NHC and odometer measurements are added to construct the integration to suppress the INS position errors.

FGO-GNSS/INS/NHC/Odometer
Recently, factor graph optimization (FGO) has attracted much attention in multisensor fusion [42,43]; the FGO utilizes the graphic model to represent the states and the measurements [43]. Unlike the KF, the FGO encodes the historical state transformation and measurements as the factors. It estimates the set of states at each epoch through a Gauss-Newton method or Levenberg-Marquardt (LM) method. In the FGO, the edge connects the factor node and the variable node. The factorized function is written as [13,14]: where χ i denotes nodes of the state variables. In the FGO, the optimal estimatesχ are obtained by minimizing the errors of the entire graph, and the procedure is summarized aŝ Here, there are four types of measurements: GNSS measurements, INS measurements, NHC measurements, and the odometer measurements. Suppose the process noise and the measurement noise are both subject to zero-mean Gaussian distributions with covariance matrix Σ k and Λ k . The optimal estimation of the state variables can be expressed aŝ where Z i refers to the measurement vector, u i refers to the control input, andX refers to the estimates of the state vector. Then, the optimal state estimates of the state can be converted to a nonlinear square problem, and the optimal state can be obtained by minimizing the cost function, which is written as: where · means the Mahalanobis norm. Here, there are two different conditions, and Figure 2 presents the factor graph structure for the GNSS/INS integration ( Figure 2a) and INS/NHC/Odometer integration (Figure 2b). The following subsections present the IMU, NHC, and odometer factors.

IMU Preintegration Factor
A standard IMU contains a three-axis accelerometer and gyroscope, which measures the acceleration and the rotation. The IMU measurement model is written as: where ω I (t) denotes the angular velocity in the IMU coordinates, ω W I (t) denotes the angular velocity of the IMU frame relative to the world frame, b g (t) denotes the angular rate time-varying bias, η g (t) denotes the Gaussian white noise,ã I (t) denotes the accelerometer measurements in the IMU frame, R T W I (t) denotes the rotation matrix from the world frame to the IMU frame, a W (t) and g W denote the acceleration measurements and the gravity vector in the world frame, and b a (t) and η a (t) denote the angular rate time-varying bias and the Gaussian white noise.

IMU Preintegration Factor
A standard IMU contains a three-axis accelerometer and gyroscope, which measures the acceleration and the rotation. The IMU measurement model is written as: The IMU kinetic model is written as: where ∧ ω is written as The IMU kinetic model is written as: .
where ω ∧ is written as Suppose that the angular rate and acceleration remain invariant during the IMU measurement updating period (t, t + ∆t). The discrete form of the motion model is written as: The IMU factor can be written as Micromachines 2022, 13, 1400 8 of 16 where h I NS (·) denotes the measurement function, A k denotes the IMU measurements, and Σ I NS k denotes the covariance matrix.

GNSS Factor
Similarly, the cost function for GNSS measurement is written as where z GNSS k denotes the GNSS measurements, h GNSS (·) denotes the measurement function, and Σ GNSS k denotes the covariance matrix of the measurement noise.

NHC Factor
As the measurement model listed in Equations (9)-(11), the cost function of the NHC measurement is written as where z NHC k denotes the NHC measurements, h NHC (·) denotes the measurement function, and Σ NHC k denotes the covariance matrix of the measurement noise.

Odometer Factor
The cost function of the odometer measurement is written as where z Odo k denotes the odometer measurements, h Odo (·) denotes the measurement function, and Σ Odo k denotes the covariance matrix of the measurement noise.

FGO
Optimal estimates of the states are obtained through minimizing the cost functions, which is formulated as Under GNSS-denied conditions, the optimal states in the FGO-IMU/NHC/Odometer are solved through the formulations listed as

Experiments and Results
To assess the performance of the proposed method, we carried out two different experiments. Raw GNSS, IMU, and odometer measurements were collected through our data-collecting platform installed in a ground vehicle. Our previous paper revealed the mode details of the MEMS IMU and data collecting [37], Table 1 lists the specifications of the employed IMU in this experiment. Here, we first assess the FGO-based GNSS/INS performance for ground vehicle position applications; then, FGO-INS/NHC/Odometer integration is evaluated with the collected raw data. We utilize a postprocessing GNSS/INS reference system POS320 as the trajectory reference for the experiments to calculate the position errors.

FGO-GNSS/INS Performance Assessment
We carried out the field test near the Nanjing University of Science and Technology. Figure 3 presents the trajectory plotted in the Google Map. Figure 4 shows the available satellites for the experiment. It can be seen that the amount of the in-view satellites is satisfactory. Position errors from the GNSS/INS integration methods KF-GNSS/INS and FGO-GNSS/INS are presented in Figure 5 together for comparison. Specifically, Figure 5a shows the east-direction position errors comparison, Figure 5b           Here, we assumed the position errors are subject to Gaussian distribution, and we analyzed the position errors distribution. Figure 7 presents the distributions of these position errors. In this experiment, the GNSS receiver was developed by us with DSP + FPGA and C++ programming language. It seems that the position errors are not extremely subject to Gaussian distribution. In Figure 7, the upper part of each figure shows the FGO-  Figure 6 shows the distribution of the horizontal position errors, and it also supports that the FGO-GNSS/INS has superior performance.   Here, we assumed the position errors are subject to Gaussian distribution, and we analyzed the position errors distribution. Figure 7 presents the distributions of these position errors. In this experiment, the GNSS receiver was developed by us with DSP + FPGA and C++ programming language. It seems that the position errors are not extremely subject to Gaussian distribution. In Figure 7  Here, we assumed the position errors are subject to Gaussian distribution, and we analyzed the position errors distribution. Figure 7 presents the distributions of these position errors. In this experiment, the GNSS receiver was developed by us with DSP + FPGA and C++ programming language. It seems that the position errors are not extremely subject to Gaussian distribution. In Figure 7

FGO-INS/NHC/Odometer under GNSS-Denied Environments
We collected the raw measurements using the exact same ground vehicle in the second field test. The testing trajectory is presented in Figure 8, and the in-view satellite's amount during the testing is shown in Figure 9. In Figure 9, the red line represents the inview satellite's amount of the reference, and the blue line represents the in-view satellite's

FGO-INS/NHC/Odometer under GNSS-Denied Environments
We collected the raw measurements using the exact same ground vehicle in the second field test. The testing trajectory is presented in Figure 8, and the in-view satellite's amount during the testing is shown in Figure 9. In Figure 9, the red line represents the in-view satellite's amount of the reference, and the blue line represents the in-view satellite's amount of our system. We simulated the GNSS-denied environment by removing the antenna from the GNSS receiver. Here, at approximately 75 s, we remove the GNSS antenna, and the available satellite's amount immediately decreases to zero. Here the dataset is the same as our previously published paper [37]. (c)

FGO-INS/NHC/Odometer under GNSS-Denied Environments
We collected the raw measurements using the exact same ground vehicle in the second field test. The testing trajectory is presented in Figure 8, and the in-view satellite's amount during the testing is shown in Figure 9. In Figure 9, the red line represents the inview satellite's amount of the reference, and the blue line represents the in-view satellite's amount of our system. We simulated the GNSS-denied environment by removing the antenna from the GNSS receiver. Here, at approximately 75 s, we remove the GNSS antenna, and the available satellite's amount immediately decreases to zero. Here the dataset is the same as our previously published paper [37].  Latitude and longitude position errors are presented in Figure 10. The red lines represent the position errors from the KF method, and the blue lines represent the position errors from the FGO method. During 0~75 s, the available satellites are regular, the system works under GNSS/INS integration system, and the latitude and longitude errors are smaller than five meters. Under this model, the FGO method slightly decreases the positioning errors. Tables 3 and 4 Figure 11; it is evident that the FGO method contributes to a better distribution of the position errors, including more minor mean and maximum position errors.

Limitations and Discussion
The experimental results reveal that the FGO-INS/NHC/Odometer integration c contribute to better outcomes for the KF-INS/NHC/Odometer during signal outage. Ho ever, we think there are still the following limitations: (1) in the paper, the optimization is conducted using all the past information, and t computation load increases exponentially. Now, it is still hard to implement the FG in a real-time manner; it is helpful to reduce the computation load with a fix smoothing window. (2) the measurement noises are subject to Gaussian distribution, as the position erro distribution plotted in Figure 7 is not strictly subject to standard Gaussi distribution. In addition, due to the environmental influence, the GNSS or IN measurement errors covariance matrix might be changed; it was more feasible adaptively tuning the errors matrix in the FGO method; in fact, some adaptive K have been proposed and demonstrated in dealing with this problem, and som strategies could be adopted and utilized in FGO. (3) in urban areas, GNSS measurements might face abnormal measures induced by t multipath or non-line-of-sight (NLOS) signals; it is adequate to add some kern functions to the FGO to mitigate the adverse effects and improve the robustness the navigation solutions.

Conclusions
Improving the position accuracy for ground vehicles under GNSS-signal-challengi environments is a hot research topic in the community, this paper investigated FG GNSS/INS and FGO-INS/NHC/Odometer integration, especially for GNSS signal outag Two different field experiments were carried out. With analyzing the experimental sults, the following findings are concluded: (1) the FGO method is effective for improvi the accuracy of the INS navigation solutions compared with that from KF; (2) the FG method can improve the position accuracy without GNSS compared with KF. This wo preliminarily demonstrates that FGO is a better senor fusion method for GNSS/INS in gration applications compared with KF. FGO is a feasible method to improve positi

Limitations and Discussion
The experimental results reveal that the FGO-INS/NHC/Odometer integration can contribute to better outcomes for the KF-INS/NHC/Odometer during signal outage. However, we think there are still the following limitations: (1) in the paper, the optimization is conducted using all the past information, and the computation load increases exponentially. Now, it is still hard to implement the FGO in a real-time manner; it is helpful to reduce the computation load with a fixed smoothing window. (2) the measurement noises are subject to Gaussian distribution, as the position errors' distribution plotted in Figure 7 is not strictly subject to standard Gaussian distribution. In addition, due to the environmental influence, the GNSS or INS measurement errors covariance matrix might be changed; it was more feasible for adaptively tuning the errors matrix in the FGO method; in fact, some adaptive KFs have been proposed and demonstrated in dealing with this problem, and some strategies could be adopted and utilized in FGO. (3) in urban areas, GNSS measurements might face abnormal measures induced by the multipath or non-line-of-sight (NLOS) signals; it is adequate to add some kernel functions to the FGO to mitigate the adverse effects and improve the robustness of the navigation solutions.

Conclusions
Improving the position accuracy for ground vehicles under GNSS-signal-challenging environments is a hot research topic in the community, this paper investigated FGO-GNSS/INS and FGO-INS/NHC/Odometer integration, especially for GNSS signal outages. Two different field experiments were carried out. With analyzing the experimental results, the following findings are concluded: (1) the FGO method is effective for improving the accuracy of the INS navigation solutions compared with that from KF; (2) the FGO method can improve the position accuracy without GNSS compared with KF. This work preliminarily demonstrates that FGO is a better senor fusion method for GNSS/INS integration applications compared with KF. FGO is a feasible method to improve position accuracy under GNSS signal challenging environments. Actually, with the FGO framework, more sensors can be added to the fusion framework to obtain a better position result. For instance, a LiDAR or visual camera can be easily integrated with GNSS and INS, which could adaptively select proper measurements from sensors to compose a feasible integration system and obtain the most optimal navigation solutions.

Conflicts of Interest:
The authors declare no conflict of interest.