Improved IMU Preintegration with Gravity Change and Earth Rotation for Optimization-Based GNSS/VINS

: IMU preintegration technology has been widely used in the optimization-based sensor fusion framework, in order to avoid reintegrating the high-frequency IMU measurements at each iteration and maintain the ability of bias correction when bias estimation changes. Since IMU preintegration technology was ﬁrst proposed, several improved versions have been designed by changing the attitude parameterization or the numerical integration method in the most current related research. However, all of these versions have failed to take the change of gravity and the earth rotation into consideration. In this paper, we redesign the IMU preintegration algorithm in which the earth rotation and gravity vector are calculated from the geodetic position. Compared with the covariance matrix form, in this paper, the uncertainty of the preintegrated IMU measurements is propagated in the form of a square root information matrix (SRIM) for better numerical stability and easy use in the optimization-based framework. We evaluate the improved IMU preintegration algorithm by using the dataset collected by our sensor platform equipped with two different-grade IMUs. The test results show that the improved IMU preintegration algorithm can cope well with the gravity change and earth rotation. The earth rotation must be taken into consideration for the high-grade IMU that can effectively sense the earth rotation. If the change of gravity is omitted, the root-mean-square error (RMSE) of the horizontal attitude is about 1.38 times greater than the geodetic displacement. Additionally, the positioning RMSE does not increase obviously within a limited range, which means tens of kilometers and several hundred meters for the low-grade and high-grade IMU used in the experiment, respectively.


Introduction
Automatic robots have become a hot topic over the last few years due to a growing market demand for products such as autonomous cars, rescue robots, and military robots. In order to succeed in their missions, these robots usually require accurate motion information (position, velocity, and attitude) for control and navigation. Many kinds of sensors can provide useful information for motion estimation, locally or globally, such as a camera, the IMU, and the global navigation satellite system (GNSS). Typically, a modern robot is equipped with several kinds of sensors with complementary sensing capacities, in order to make motion information estimation accurate and robust. Specifically, the inertial navigation system (INS) can provide position, velocity, and attitude information at a high frequency by integrating IMU measurements [1][2][3][4]. However, IMU measurements 1.
We have redesigned the IMU preintegration algorithm by taking the earth rotation and the change of gravity into consideration; 2.
The uncertainty of the preintegrated IMU measurements has been propagated in the form of a square-root information matrix; 3.
We have evaluated the influence of the earth rotation and the change of gravity on the preintegrated IMU measurements by using two different-grade IMUs.
Remote Sens. 2020, 12, 3048 4 of 22 The remainder of this paper is organized as follows. In Section 2, the coordinate frames, notations, attitude parameterizations, and geodetic reference model of the earth are introduced. In Section 3, all of the details of the improved IMU preintegration algorithm are given, including the position, velocity, and attitude dynamic equations in the W frame; the IMU error model; the uncertainty propagation of preintegrated IMU measurements; and the IMU preintegration factors. In Section 4, the joint optimization framework of the VINS is briefly described. In Section 5, all of the details of experiment setup are introduced. In Section 6, all experimental results are given. In Section 7, we discuss the influence of the gravity change and the earth rotation on the IMU preintegration, according to the experimental results. In Section 8, we draw the conclusions.

Preliminaries
In this section, some fundamental professional knowledge used in designing the IMU preintegration algorithm is introduced, including the definitions of coordinate frames and notations, the attitude parameterizations, and the earth geodetic reference model. This fundamental professional knowledge has been well-developed in the INS and GNSS research communities for several decades.

Coordinate Frames
A coordinate frame is defined by three unit vectors that are perpendicular to each other in a right-hand form. For making a mathematical operation valid, all related free vectors and points should be projected into the same reference coordinate frame.
The E frame is the earth-centered, earth-fixed coordinate frame, with its Z axis parallel to the earth polar axis, its X axis passing through the Greenwich meridian, and its Y axis lying in the equatorial plane to form a right-handed coordinate frame.
The I frame is the non-rotating and non-accelerating inertial coordinate frame used as the reference coordinate frame for the IMU measurement. Theoretically, its orientation can be arbitrary. In this paper, the E frame at the initial time is selected as a historically fixed inertial coordinate frame.
The L frame is the local level coordinate frame, with its X axis parallel to the east, Y axis parallel to the north, and Z axis vertical to the local earth surface.
The B frame is the IMU coordinate frame with axes parallel to the nominal orthogonal input axes. The C frame is the camera coordinate frame with the origin at the optical center, the X axis horizontal and pointing to the right, and the Y axis vertical and pointing downwards.
The W frame is the reference coordinate frame for the VINS. In this paper, the L frame at the initial position or another fixed position is selected as the W frame.

Nomenclature
A 0 , A 1 , A 2 arbitrary orthogonal right-handed coordinate frames; f specific force measured by the accelerometer, which is produced by a non-gravitational force; g local plumb-bob gravity vector that is a function of the geodetic position; position vector of frame A 2 relative to frame A 1 , with projection in frame A 0 ; velocity vector of frame A 2 relative to frame A 1 , with projection in frame A 0 ; angular velocity vector of frame A 2 relative to frame A 1 , with projection in frame A 0 ; direction cosine matrix that transforms a vector from frame A 2 to frame A 1 ; rotation/angle-axis vector defined by the axis of rotation and the magnitude of rotation around the axis; I n n-dimension identity matrix; 0 m×n zero matrix with m rows and n columns.
V q the quaternion form of the vector, as follows: V q = 0 V ; V the bar above the vector means it is a constant vector; V the hat above the vector means it is an estimation; V the tilde above the vector means it is a measurement; . V the point above the vector means it is the time derivative of the vector.

Attitude Parameterization
The direction cosine matrix, the rotation vector, and the attitude quaternion can be used to parameterize the attitude (rotation) between two coordinate frames with no singularity. Moreover, the three parameterization methods are mathematically equivalent.
The direction cosine matrix C A 1 A 2 is a unit and orthogonal matrix, and the columns are equal to the projections of the unit vectors along the axes of frame A 2 into frame A 1 . For an arbitrary vector V ∈ R 3 with projections V A 1 , V A 2 in the coordinate frame A 1 , A 2 , respectively, the following relationship holds [1]: According to Equation (1), it is easy to derive the chain rule of the direction cosine matrix: The rotation vector θ A 2 and the magnitude φ around the unit vector. According to the Rodrigues formula [1,35], the counterpart direction cosine matrix C A 1 A 2 is a function of the rotation vector, as follows: The attitude quaternion is composed of four elements, as follows: where We refer readers to reference [35] for more details about the quaternion. The attitude quaternion is also a function of the rotation vector, as follows: The chain rule of the attitude quaternion is defined on the basis of quaternion multiplication, as follows: q where ⊗ is the quaternion multiplication operator and q A 0 . The left and right quaternion multiplication matrices are as follows: Remote Sens. 2020, 12, 3048 The conversion relationships between the direction cosine matrix and the attitude quaternion are as follows: where C , the sum of the diagonal elements of the direction cosine matrix.

Time Derivative of the Attitude Parameter
The time derivative of the direction cosine matrix C A 1 A 2 is as follows: The time derivative of the attitude quaternion q A 1 A 2 is as follows:

The Earth Model
In this paper, the WGS-84 geodetic reference model is adopted for modeling the earth as shown in Figure 1. Set the geodetic position (latitude, longitude, and altitude) in the WGS-84 reference frame as ϕ, λ, and h. Then, the direction cosine matrix between the L frame and E frame is as follows: The XYZ position in the E frame can be recovered from the geodetic position, as follows: where e= 0.08181919104282 is the eccentricity of the earth and R N is the radius of curvature of the prime vertical plane. It is a function of the latitude, i.e., R N (ϕ) = a/ 1 − e 2 sin 2 ϕ . a = 6378137 m is the equatorial earth radius. The geodetic position can also be recovered from the XYZ position in the E frame. The longitude can be recovered directly as follows: Remote Sens. 2020, 12, 3048 7 of 22 The latitude and altitude need to be recovered by the following iterative algorithm. The initial values are set as follows: Then, the latitude and altitude are iteratively calculated until convergence, as follows: For notation convenience, the computation process of Equations (15), (16), and (17) is donated as follows: The normal gravity in the L frame is as follows: where g γ (ϕ, h) is calculated by the WGS-84 gravity model parameters, as follows: The gravity anomaly is omitted in our algorithm. Therefore, the normal gravity is approximately considered as the local gravity in this paper.
The earth rotation vector in the E frame is as follows: where Ω IE = 7.292115 × 10 −5 rad/s. The measurement of the accelerometer is a specific force. The influence of gravity needs to be removed from the preintegrated IMU measurements, in order to recover the metric motion. Therefore, we need to know the gravity vector projected in the W frame. The measurement of a gyroscope (rate sensor) is the angular velocity relative to the I frame. The influence of the earth rotation needs to be removed from the gyroscope measurement, in order to recover the real angular The measurement of the accelerometer is a specific force. The influence of gravity needs to be removed from the preintegrated IMU measurements, in order to recover the metric motion. Therefore, we need to know the gravity vector projected in the W frame. The measurement of a gyroscope (rate sensor) is the angular velocity relative to the I frame. The influence of the earth rotation needs to be removed from the gyroscope measurement, in order to recover the real angular velocity relative to the earth.
Set the geodetic position of the W frame as ϕ 0 , λ 0 , and h 0 . The XYZ position p E EW in the E frame can be calculated by Equation (14). Then, the direction cosine matrix C E L 0 = C E W between the W frame and E frame can be calculated by Equation (13).
The earth rotation vector in the W frame is calculated as follows: Set the position of the IMU at time t in the W frame as p W WB t . Then, its position in the E frame can be calculated as follows: Then, the geodetic position of IMU at time t can be calculated by Equation (18), i.e., [ϕ t , λ t , h t ] = LLA(p E EB t ). The local normal gravity vector g L t γ (ϕ t , h t ) in the L frame can be calculated by Equation (19). The direction cosine matrix C E L t (ϕ t , λ t ) can be calculated by Equation (13). Therefore, the direction cosine matrix between the L frame and W frame is Finally, the gravity vector in the W frame is calculated as follows:

The Improved IMU Preintegration Technology
The position, velocity, and attitude are obtained by integrating the IMU measurements. For performing such integrations, the dynamic equations for the position, velocity, and attitude should be derived first. Generally, the position, velocity, and attitude of the VINS are parametrized in the W frame. Hence, the dynamic equations are also derived in the W frame.
First, the relationship of the positions parameterized in the W frame and I frame is as follows: where p E IW = p E EW because the origins of the E frame and I frame are coincident, i.e., p E IE = 0 3×1 , and the W frame is fixed with respect to the earth.
According to Equation (11), the time derivative of Equation (25) is as follows: where . ω E IE is the earth rotation in the E frame. For obtaining the acceleration information, we need to further derive the time derivative of Equation (26) as follows: where a I IB = . (27), we have Remote Sens. 2020, 12, 3048 9 of 22 Let us left-multiply both sides of Equation (28) by C B I . After re-organization, we have According to the theory of the force composition and decomposition, the acceleration a B IB relative to the inertial space is the resultant force of the specific force f B and the gravitational force G B , i.e., where is the well-known Coriolis acceleration. Equation (30) is the velocity differential equation. The position differential equation can be easily expressed as follows: The attitude is parameterized by the attitude quaternion. Furthermore, its time derivative can be derived according to Equation (12), as follows: Then, the position, velocity, and attitude can be reckoned by numerically integrating the three dynamic equations, as follows: where ∆t k.k+1 = t k+1 − t k . t k is written as k in many subscripts and superscripts for convenience. N is the sampling number during [t k , t k+1 ] and ∆t i is the IMU sampling interval. The change of gravity is negligible during [t k , t k+1 ]. Therefore, g W k is used to represent the gravity during the time interval. Moreover, α k,k+1 is the so-called IMU position preintegration, as follows: The velocity is reckoned as follows: where β k,k+1 is the so-called IMU velocity preintegration, as follows: Finally, the attitude increment is the so-called IMU attitude preintegration. It can be reckoned by integrating Equation (32), as follows:

IMU Error Model and Preintegrated IMU Measurements
Typically, the IMU measurements are corrupted by several kinds of errors, such as the scale factor error, misalignment, sensor-induced random output noise, and slowly time-varying biases. In this paper, we assume that the IMU has been well-calibrated offline. Therefore, the scale factor error and misalignment are negligible. The sensor-induced random output noise is modeled as a white noise process. The slowly time-varying biases are molded as one-order Markov processes. Then, the IMU measurements are modeled as follows: where η Acc,t and η Gyro,t are Gaussian white noise processes, and η Acc, Gyro ). N ( * , * ) denotes a normal distribution. b Acc,t and b Gyro,t are IMU time-varying biases and are modeled as follows: where τ Acc and τ Gyro are the correlation times of Markov processes. n Acc,t and n Gyro,t are driving Gaussian white noise processes, and n Acc, (36), and (37) can be solved by numerical integration. The initial body frame B k is selected as the reference frame for integration. Additionally, the initial values are known and independent of the position, velocity, and attitude in the W coordinate frame, i.e., α k, This is the core idea of the IMU preintegration algorithm. Set the IMU preintegrations at time t i asα k,i ,β k,i , andγ , and the IMU bias estimations asb Gyro,k andb Acc,k . When new IMU measurements f B j and ω , and the IMU preintegrations are updated recursively, as follows: where ∆t ij = t j − t i .

Uncertainty Propagation and Jacobian Matrix of Bias Correction
The uncertainty propagation of IMU preintegration is similar to the time update of the Bayesian filter used in the INS/GNSS integrated navigation system. Due to the IMU preintegration errors being time-varying, we also need linear dynamic equations for tracking the errors, just like Psi-and Phiangle error equations in the INS/GNSS integrated navigation system [36]. The Jacobian matrices for belated bias correction are contained in the transition matrix of the IMU preintegration errors. For iteratively updating the transition matrix, we also need the linear dynamic equations of the IMU preintegration errors. Hence, in this section, we will derive the linear dynamic equations first.
(1). Attitude preintegration error dynamic equation: The attitude preintegration error is defined in a right perturbation form. Set the attitude preintegration error at time t as δθ Remote Sens. 2020, 12, 3048

of 22
The gyroscope bias error is defined as follows: Substituting the Equations (41) and (42) into Equation (2), we havê where t k < t i < t, and ∆t = t − t i , ∆t is sufficiently small. The expanded form of Equation (43) is as follows: All second-order small terms are omitted.
Then, the attitude preintegration error dynamic equation can be derived as follows: (2). Velocity preintegration error dynamic equation: The velocity preintegration error is defined as follows: where δβ k,t is the velocity preintegration error at time t.
The accelerometer bias error is defined as follows: b Acc,k =b Acc,k + δb Acc,k .
According to the Equations (40), (41), (46), and (47), we havê The expanded form of Equation (48) is as follows: Then, the velocity preintegration error dynamic equation can be derived as follows: (3). Position preintegration error dynamic equation: The position preintegration error is defined as follows: where δα k,t is the position preintegration error at time t.

of 22
The position preintegration is the integration of velocity preintegration, so Rewriting the Equations (39), (45), (50), and (52) in a matrix form, we have Rewriting Equation (53) in compact form, where According to the linear system theorem [37], the system can be discretized as follows: where u equ ij is the equivalent noise during [t i , t j ], as follows: The mean of u equ ij is zero. The covariance is calculated as follows: where . Additionally, the equivalent noise u equ ij is a Gaussian white noise sequence.
Hence, the square root information matrix of the equivalent noise is as follows: The transition matrix from t k to t k+1 is propagated as follows: where t i and t j are adjacent IMU sampling epochs, and t i , t j ∈ [t k , t k+1 ]. Set the square root information matrix of the IMU preintegration errors at time t i as Λ i . Then, the square root information matrix Λ j can be calculated by performing QR decomposition on the following data matrix [33], as follows: where U ij is a unitary matrix. The right side of Equation (60) is an upper triangular matrix. The initial square-root information matrix at time t k should be large enough, because the initial values of IMU preintegration are deterministic. In our implementation, the initial square-root information matrix is set as Λ k = 10 8 × I 15 .

IMU Preintegration Factor
Given the preintegrated IMU measurements, all residual functions are listed as follows: All of the above residual functions should be weighted by the square-root information matrix Λ k+1 before adding them to optimization.

Nonlinear Joint Optimization
We use the hybrid sliding window nonlinear optimization for state estimation (more details can be found in our previous work [34]).
The full state vector is defined as follows: where where ψ m is the inverse depth parameter of the m-th mappoint from its first observing keyframe, N is the number of keyframes in the sliding window, M is the number of visible mappoints, and t BC is the camera-IMU calibration parameter. The joint optimization problem can be formulated as a nonlinear least-squares problem. The cost function is the sum of the prior and the Mahalanobis norm of all measurement residuals within the sliding window, as follows: where r p and H p represent the prior information; B, C, and G are the measurement set of the IMU, camera, and GNSS, respectively; r I MU k+1,k (χ) is the IMU preintegration residual function, as shown in Equation (61); r C j l (χ) is the re-projection residual function of the l-th mappoint in the j-th image (more details can be found in [34]); ρ( * ) is the Huber loss function for coping with potential outliers [38]; Λ C j l is the square root information matrix of the visual measurement; and r G k (χ) is the GNSS residual at time t k , as follows: where C W E and p E EW are functions of the geodetic position of the selected world frame; p B Bgnss is the lever arm vector between the GNSS antenna and IMU, measured manually; and p E Egnss is the XYZ position of the GNSS antenna in the E frame, and provided by the GNSS receiver.

Experiments
In order to evaluate the performance of the proposed IMU preintegration algorithm, we conducted experiments by using the dataset collected by our sensor platform. Our sensor platform consists of a left-right stereo camera and two different grades of IMU, as shown in Figure 2. The stereo camera consists of two Prosilica GT 1910 monocular cameras by AVT. Both of them are global shutter monochrome CCD cameras which deliver 1920 × 1080 images. The cameras are synchronously triggered by an Arduino Uno R3 microcontroller at 10 Hz. The microcontroller parses GPS time from the NMEA messages provided by Ublox NEO-M8N. The two cameras are equipped with LM8HC lenses by Kowa. The horizontal field of view of the lens is about 60 degrees. The two IMUs are MTi-G-710 and POS620, respectively. The MTi-G-710 is an industrial-grade MEMS-based IMU by Xsens. The POS620 is a quasi-navigation-grade IMU by the MPSTNAV company of China, and contains three-axis high-precision fiber optic gyroscopes (FOGs). The technical parameters of the two different-grade IMUs are listed in Table 1. During data collection, the two IMUs were sampled at 200 Hz.  The technical parameters of the two different-grade IMUs are listed in Table 1. During data collection, the two IMUs were sampled at 200 Hz. Our dataset contains two sequences collected at two different places in Wuhan city of China, namely the Wuhan Research and Innovation Center (WRIC) and the Guanggu financial harbor (GFH). There are many low-slung buildings and trees in the two places. These static objects provide a high-quality texture for visual measurements, as shown in Figure 3. Additionally, navigation satellite signals are not easily shielded or reflected in the two places. The GNSS raw data was recorded by the Trimble receiver. Furthermore, we used the GINS software developed by the MPSTNAV company of China to post-process the recorded GNSS raw data, in order to obtain high-precision real-time kinematic (RTK) positioning results. The technical parameters of the two different-grade IMUs are listed in Table 1. During data collection, the two IMUs were sampled at 200 Hz. Our dataset contains two sequences collected at two different places in Wuhan city of China, namely the Wuhan Research and Innovation Center (WRIC) and the Guanggu financial harbor (GFH). There are many low-slung buildings and trees in the two places. These static objects provide a high-quality texture for visual measurements, as shown in Figure 3. Additionally, navigation satellite signals are not easily shielded or reflected in the two places. The GNSS raw data was recorded by the Trimble receiver. Furthermore, we used the GINS software developed by the MPSTNAV company of China to post-process the recorded GNSS raw data, in order to obtain highprecision real-time kinematic (RTK) positioning results.  Generally, the RTK positioning error was less than 0.10 m. The integrated navigation results of fusing the RTK positions and the POS620 measurements by Rauch-Tung-Striebel (RTS) smoothing served as the ground truth data. The dataset was used to evaluate the proposed IMU preintegration algorithm. It did not contain GNSS deny environment and visual challenges, such as overexposure, illumination changes, and poorly textured scenes. The motions performed by the vehicle during data collection were variable, including linear acceleration and deceleration, zero-speed motion, turning motion, and backward motion, i.e., almost all motions that can be performed by a ground vehicle. Additionally, changing slopes also exist in the two places. Therefore, the motions in the collected dataset were complicated enough to evaluate the proposed IMU preintegration algorithm.

Gyroscope Accelerometer
Because it is quite difficult to obtain sufficient disparities from images recorded by a monocular camera when a vehicle runs along a straight road, we could not initialize the monocular visual-inertial system. For coping with this issue, stereo images were used to initialize the VINS. After initialization, only the images of the left side camera were used in optimization.
It is well-known that the earth is an approximate spheroid, and the direction of gravity approximately points to the center of the earth. Therefore, the direction and amplitude of gravity change with the position on the earth. For two positions that are far apart, the gravity vectors will be quite different. Therefore, it is imprecise for the traditional IMU preintegration algorithms to assume that the gravity vector is constant in the W frame, especially after the vehicle has traveled a long distance. An example is where the VINS system (maybe another system where the IMU preintegration technology is used) was initialized at city A and traveled to city B, where the W coordinate frame was located at city A. To simulate this situation, i.e., to amplify the influence of the change of gravity, we deliberately moved the W coordinate frame a certain distance from the starting position of the testing dataset. Specifically, if the geodetic position of the starting position was ϕ, λ, and h, the geodetic displacement was δϕ, δλ, and δh, respectively. Then, the new position of the W coordinate frame was ϕ + δϕ, λ + δλ, and h + δh, respectively. The new position of the W frame may not be located in a living area, and may be a mountain or a lake. However, this does not affect the evaluation of the proposed algorithm mathematically. In order to systematically evaluate the influence of the change of gravity, the geodetic displacement of the world frame was incrementally set as 0, 0. motion, and backward motion, i.e., almost all motions that can be performed by a ground vehicle. Additionally, changing slopes also exist in the two places. Therefore, the motions in the collected dataset were complicated enough to evaluate the proposed IMU preintegration algorithm.
Because it is quite difficult to obtain sufficient disparities from images recorded by a monocular camera when a vehicle runs along a straight road, we could not initialize the monocular visualinertial system. For coping with this issue, stereo images were used to initialize the VINS. After initialization, only the images of the left side camera were used in optimization.
It is well-known that the earth is an approximate spheroid, and the direction of gravity approximately points to the center of the earth. Therefore, the direction and amplitude of gravity change with the position on the earth. For two positions that are far apart, the gravity vectors will be quite different. Therefore, it is imprecise for the traditional IMU preintegration algorithms to assume that the gravity vector is constant in the W frame, especially after the vehicle has traveled a long distance. An example is where the VINS system (maybe another system where the IMU preintegration technology is used) was initialized at city A and traveled to city B, where the W coordinate frame was located at city A. To simulate this situation, i.e., to amplify the influence of the change of gravity, we deliberately moved the W coordinate frame a certain distance from the starting position of the testing dataset. Specifically, if the geodetic position of the starting position was ϕ, λ , and h , the geodetic displacement was δϕ , δλ , and h δ , respectively. Then, the new position of the W coordinate frame was ϕ δϕ + , λ δλ + , and h+ h δ , respectively. The new position of the W frame may not be located in a living area, and may be a mountain or a lake. However, this does not affect the evaluation of the proposed algorithm mathematically. In order to systematically evaluate the influence of the change of gravity, the geodetic displacement of the world frame was  In order to achieve good alignment with the geodetic frame, the RTK positions were added into optimization during the initial period, i.e., we directly used the RTK positioning results. Then, the RTK positions were removed after initialization, for better reflecting the impact of the IMU preintegration. At this time, the VINS system will have accurate prior information. For analyzing the influence of the earth rotation, the performance of the proposed IMU preintegration algorithm was also evaluated with or without a consideration of the earth rotation. Finally, the performance of the proposed IMU preintegration algorithm was evaluated under one of the four settings, as follows: Setting A: Both the change of gravity and the earth rotation are considered, i.e., the proposed algorithm is used in a normal way; Setting B: The change of gravity is considered, but the earth rotation is omitted; Setting C: The change of gravity is omitted, but the earth rotation is considered; In order to achieve good alignment with the geodetic frame, the RTK positions were added into optimization during the initial period, i.e., we directly used the RTK positioning results. Then, the RTK positions were removed after initialization, for better reflecting the impact of the IMU preintegration. At this time, the VINS system will have accurate prior information. For analyzing the influence of the earth rotation, the performance of the proposed IMU preintegration algorithm was also evaluated with or without a consideration of the earth rotation. Finally, the performance of the proposed IMU preintegration algorithm was evaluated under one of the four settings, as follows: Setting A: Both the change of gravity and the earth rotation are considered, i.e., the proposed algorithm is used in a normal way; Setting B: The change of gravity is considered, but the earth rotation is omitted; Setting C: The change of gravity is omitted, but the earth rotation is considered; Setting D: Both the change of gravity and the earth rotation are omitted, and the proposed IMU preintegration algorithm degenerates to a traditional IMU preintegration algorithm.
During the initial period, the proposed IMU preintegration algorithm was used under Setting A.

Results
The root-mean-square errors (RMSEs) of the estimated position, horizontal attitude, and yaw were calculated by comparing the estimated states with the ground truth data. All results are listed in Tables 2-4. Because all states were estimated from noisy sensor measurements by joint optimization, the final results were random due to the keyframe selection method and the change of the initial condition. However, the variation tendency of RMSEs was still obvious under different settings, especially for the RMSEs of the POS620 due to its high-precision measurements. The estimation results were diverged under Setting B and Setting D if the POS620 measurements were used in our implementation. Therefore, the RMSEs are not listed in Table 4. Additionally, the diverged estimation errors of POS620 are plotted in Figure 5.
The trajectories of the two IMUs for the sequences WRIC and GFH under different settings and two-degree geodetic displacements are plotted in Figure 6.
Due to the limited space of the paper, only some of the RMSEs obtained under different settings and different geodetic displacements are plotted in Figures 5 and 7-9.     The trajectories of the two IMUs for the sequences WRIC and GFH under different settings and two-degree geodetic displacements are plotted in Figure 6.  The trajectories of the two IMUs for the sequences WRIC and GFH under different settings and two-degree geodetic displacements are plotted in Figure 6.

Discussion
In this section, we will discuss the influence of the gravity change and the earth rotation on the IMU preintegration, according to the results presented in the previous section.
According to the RMSEs of the horizontal attitude of the MTi-G-710 and POS620 under Setting A and Setting C presented in Tables 2-4, we can observe that ignoring the change of gravity makes the horizontal attitude errors increase as the geodetic displacement increases linearly. The RMSEs of the horizontal attitude error are about 1.38 times greater than the geodetic displacement. This tendency is also obvious in Figures 8b and 9b, where the pitch and roll errors fluctuate sharply. If the vehicle moves in a small area, i.e., the geodetic displacement is almost 0 degrees, ignoring the change of gravity does not have much of an influence on the horizontal attitude errors, as shown in Tables 2-4, under Setting C. This phenomenon can also be directly observed in Figure 7.
According to the position RMSEs of MTi-G-710 under Setting A and Setting C presented in Table 2, we can observe that the RMSEs of the position are lower than 5.6 m under Setting A. The position RMSEs increase and become greater than 8.73 m under Setting C in Table 2, if the geodetic displacement is greater than 0.5 degrees. We can also observe a similar tendency under Setting A and Setting C in Table 3. The increase of the position RMSE becomes obvious when the geodetic displacement is greater than 1 degree, due to the short length of the sequence GFH.
According to the position RMSEs of POS620 under Setting A and Setting C presented in Table 4, we can observe that the position RMSEs are lower than 1.9 m in sequence WRIC and lower than 0.85 m in sequence GFH under Setting A. Additionally, the position RMSEs become greater than 2.36 m in sequence WRIC and 1.83 m in sequence GFH, if the geodetic displacement is greater than 0.1 degrees. The motion estimation results diverge under Setting C, if the geodetic displacement is greater than 0.25 degrees in the sequence WRIC and 0.1 degrees in the sequence GFH. Compared with the MTi-G-710, the POS620 is more sensitive to the change of gravity, due to its high-precision measurements, which means that it has a greater weight during information fusion. If the vehicle moves in a small area, i.e., the geodetic displacement is almost 0 degrees, ignoring the change of gravity has no obvious influence on the position errors for both low-grade and high-grade IMUs, as shown in Tables 2-4 under Setting C.
According to the RMSEs of the MTi-G-710 under Setting A and Setting B presented in Tables 2 and 3, we can observe that ignoring the earth rotation has no obvious influence on the accuracy of the position and the horizontal attitude. From another perspective, the RMSEs of MTi-G-710 under Setting B and Setting D presented in Tables 2 and 3 show that the positioning accuracy is obviously increased under large geodetic displacement by taking the change of gravity into consideration, but ignoring the earth rotation. Therefore, the earth rotation can be safely omitted for the industrial-grade MEMS-based IMU. In contrast, the motion estimation results diverge if the earth rotation is omitted from the measurements of the navigation-grade IMU (POS620), as shown in Figure 5. This is because the earth rotation is about 15deg/h, and the gyro bias instability and noise density of the angler random walk of the MTi-G-710 are 100deg/h and 0.6deg/ √ h, respectively, making it impossible to sense the earth rotation. In contrast, the gyro bias instability and noise density of the angler random walk of the POS620 are about 0.02deg/h and 0.005deg/ √ h, respectively. Therefore, the POS620 can sense the earth rotation effectively. At the same time, it should be noted that the higher noise density of MTi-G-710 makes the weight of its preintegrated IMU measurements lower, which makes the visual constrains more significant. However, the high grade of POS620 results in a greater weight of the preintegrated IMU measurements and reduces the effect of the visual constrains, making the system solution more sensitive to the modeling error of the earth rotation.
Since the yaw state is unobservable for the VINS and therefore cannot be estimated effectively [14], the drift of the yaw angle is completely random, making the yaw RMSEs stochastic under different settings and geodetic displacements, as shown in Tables 2-4.

Conclusions
In this paper, we have redesigned the IMU preintegration algorithm used in the optimization-based sensor fusion framework by taking the earth rotation and the change of gravity into consideration. Both of the two terms are functions of the geodetic position. In order to evaluate the proposed algorithm, we collected a dataset by using our sensor platform equipped with two different-grade IMUs. Then, the proposed algorithm was systematically evaluated by using the collected field dataset under different settings and test conditions. The test results led to the following conclusions: (1) The earth rotation can be safely omitted for industrial-grade MEMS-based IMUs that cannot sense the rotation effectively. However, for quasi-navigation-grade IMUs that can effectively sense the earth rotation, the earth rotation must be considered so as to maintain the positioning accuracy of the VINS; (2) if the change of gravity is omitted, the horizontal attitude error increases linearly as the geodetic displacement increases. Moreover, the RMSEs of the horizontal attitude error are about 1.38 times greater than the geodetic displacement. If the accuracy of the horizontal attitude matters, the change of gravity must be carefully considered; (3) if the change of gravity is omitted, the position error does not increase much within the limited working area. Furthermore, the threshold of the size of the limited working area is related to the IMU grade. According to the test results, for industrial-grade IMUs, the positioning error will not obviously increase within 0.25-degree geodetic displacement (36.7 km) if the gravity change is omitted. Additionally, for quasi-navigation-grade IMUs, the positioning error will also not increase if the limited size is about several hundred meters; (4) the performance of the proposed algorithm is consistent under different geodetic displacements and settings. Therefore, the proposed algorithm can cope well with the change of gravity and the earth rotation, forming a more robust GNSS/VINS solution.
Author Contributions: Raw idea, J.J. and X.N.; methodology, J.J. and X.N.; software, J.J.; writing-original draft preparation, J.J.; writing-review and editing, X.N.; resources, X.N. and J.L.; supervision, X.N. and J.L.; funding acquisition, X.N. and J.L.; All authors have read and agreed to the published version of the manuscript.