Consistent Monocular Ackermann Visual–Inertial Odometry for Intelligent and Connected Vehicle Localization

The observability of the scale direction in visual–inertial odometry (VIO) under degenerate motions of intelligent and connected vehicles can be improved by fusing Ackermann error state measurements. However, the relative kinematic error measurement model assumes that the vehicle velocity is constant between two consecutive camera states, which degrades the positioning accuracy. To address this problem, a consistent monocular Ackermann VIO, termed MAVIO, is proposed to combine the vehicle velocity and yaw angular rate error measurements, taking into account the lever arm effect between the vehicle and inertial measurement unit (IMU) coordinates with a tightly coupled filter-based mechanism. The lever arm effect is firstly introduced to improve the reliability for information exchange between the vehicle and IMU coordinates. Then, the process model and monocular visual measurement model are presented. Subsequently, the vehicle velocity and yaw angular rate error measurements are directly used to refine the estimator after visual observation. To obtain a global position for the vehicle, the raw Global Navigation Satellite System (GNSS) error measurement model, termed MAVIO-GNSS, is introduced to further improve the performance of MAVIO. The observability, consistency and positioning accuracy were comprehensively compared using real-world datasets. The experimental results demonstrated that MAVIO not only improved the observability of the VIO scale direction under the degenerate motions of ground vehicles, but also resolved the inconsistency problem of the relative kinematic error measurement model of the vehicle to further improve the positioning accuracy. Moreover, MAVIO-GNSS further improved the vehicle positioning accuracy under a long-distance driving state. The source code is publicly available for the benefit of the robotics community.


Introduction
Intelligent and connected vehicles (ICVs) are a popular research topic in intelligent transportation systems [1,2]. The Global Navigation Satellite System (GNSS) is one of the most mature global positioning technologies for vehicle navigation [3]. However, the GNSS signal is vulnerable under tunnels, trees and other obstacles [4]. Providing real-time and accurate vehicle pose information in GNSS-denied environments is a necessary prerequisite for ICV navigation [5][6][7]. Visual-inertial odometry (VIO), which integrates a visual sensor and inertial measurement unit (IMU), can provide pose information with six degrees of freedom (DOF). It is widely used in the pose estimation of mobile robots owing to the advantages of favorable robustness, small size

The Lever Arm Effect between the Vehicle and IMU Coordinates
In the practical application of VIO systems for vehicles, the IMU and camera are usually installed on the front of the vehicle. Due to the existence of translation extrinsic parameters between {B} and {I}, the lever arm effect between {B} and {I} cannot be negligible. The lever arm effect between {B} and {I} is shown in Figure 1.

The Lever Arm Effect between the Vehicle and IMU Coordinates
In the practical application of VIO systems for vehicles, the IMU and camera are usually installed on the front of the vehicle. Due to the existence of translation extrinsic parameters between {B} and {I}, the lever arm effect between {B} and {I} cannot be negligible. The lever arm effect between {B} and {I} is shown in Figure 1. According to the presentation of the lever arm effect in [54], the vehicle velocity and the vehicle angular B k ω rate at time k t is derived as where , I ib k ω denotes the relative angular rate between {B} and {I}, which can be negligible with a fixed installed location for the IMU.

Process Model and Monocular Visual Measurement Model
The vehicle state vector k B X at the sampling time for the IMU k t is defined as [54] T T  T  T  T  T  T  T  , , ,  According to the presentation of the lever arm effect in [54], the vehicle velocity B v B k at time t k is derived as where B v l k denotes the lever arm velocity at time t k between {B} and {I}, B v l k and C B k G I q are defined as p B,k + I ω k × I p B,k C B k G I q = C B I q k · C I k G I q (2) and the vehicle angular B ω k rate at time t k is derived as where I ω ib,k denotes the relative angular rate between {B} and {I}, which can be negligible with a fixed installed location for the IMU.

Process Model and Monocular Visual Measurement Model
The vehicle state vector X B k at the sampling time for the IMU t k is defined as [54]  extrinsic parameters between {B} and {I} at time t k , respectively. The symbols b g,k and b a,k denote the gyroscope and accelerometer biases of the IMU at time t k , respectively. Following Equation (4), the error state vector X B k ∈ R 21 at time t k is defined as The continuous-time kinematic differential equations of the true state are where n ωg,k and n ωa,k denote the random-walk noises of the gyroscope and accelerometer biases at time t k , respectively. The true IMU angular rate I ω k and acceleration I a k at time t k are represented as where n g,k and n a,k denote the Gaussian white noises of the gyroscope and accelerometer measurements at time t k , respectively. According to Equation (6), the linearized continuous-time kinematic differential equation of the error state at time t k follows where n B k = n T g,k n T ωg,k n T a,k n T ωa,k T denotes the continuous-time input noise vector. F B k and G B k are the continuous-time error-state transition matrix and input noise Jacobian matrix at time t k , respectively. F B k and G B k are represented as Sensors 2020, 20, 5757 6 of 32 where the analytical expressions of each block matrix in F B k and G B k are derived as described in Appendix A.
The camera state at the sampling time of camera t j is defined as where C j G B q denotes the rotation quaternion from {G B } to {C} at time t j and G B p C j is the position of {C} in q and G B p C j are given by According to Equation (4) and Equation (11), the full state vector X k with N camera states at time t k ∈ t j t j+1 is given by Following Equation (13), the full error state vector X k ∈ R 21+6N at time t k is defined as The augmented state covariance matrix and monocular visual feature measurement model can be obtained as detailed in [10,12].

Measurements of Vehicle Relative Kinematic Error
The refined measurement Jacobian matrix H B j of the vehicle relative kinematic errors in Equation (18) of [48] is represented as (1,5) and H B j,θ (1,7) denote the measurement Jacobian block matrixes of the vehicle relative rotation errors; H (2,7) and H B j,v (2,8) denote the measurement Jacobian block matrixes of the vehicle relative translation errors; and their analytical expressions are derived as described in Appendix B.
Considering the fact that the measurements of vehicle kinematic error originate from the vehicle velocity, angular rate, and the characteristics of the vehicle relative kinematic errors between two consecutive camera states, the method using a kinematic error measurement model for the vehicle has different parameter configurations in practical application. Therefore, following Equation (15), the tunable parameter configurations of ACK-MSCKF are as shown in Table 1. The limitation of the above relative kinematic error measurement model for the vehicle derives from the assumption that the vehicle velocity is constant for low-speed motion between two consecutive camera states. To address this problem, the vehicle velocity and yaw angular rate error measurement model, taking into account the lever arm effect between {B} and {I}, is presented as one of the primary contributions of this paper.
The measurement residual r B k at time t k is defined as where B r v k and B r ω k denote the vehicle velocity measurement residual and angular rate measurement residual at time t k in {B}, respectively. B v B k and B ω k denote the vehicle velocity and angular rate measurements at time t k in {B}, respectively. Bv B k and Bω k denote the vehicle velocity and angular rate estimations at time t k in {B}, respectively.
Considering the nonholonomic constraint of ground vehicles [54], B v B k and B ω k are represented as where B v B k ,x and B ω k,z denote the vehicle's longitudinal speed and the yaw angular rate measurements at time t k in {B}, respectively. B v B k ,x can be obtained from the vehicle controller area network-bus (CAN-bus), and B ω k,z is represented as where R k denotes the steering radius at time t k ; it can be obtained according to the Ackermann steering geometry [55]: Following Equation (19), B ω k,z is represented as where R k denotes the steering radius at time t k . L and B denote the wheel base and the distance between the steering king pins, respectively, which can be acquired from manual measurement. α o,k denotes the outer front wheel steering angle at time t k , which is represented as where λ denotes the steering angular transmission ratio obtained from the offline calibration. δ o,k denotes the steering wheel angle at time t k , which can also be obtained from the vehicle CAN-bus.
By linearizing Equation (16) at current error state X k , r B k is approximated as where n B k denotes the measurement noise vector at time t k : where n B ωx , n B ωy and n B k ωz denote the measurement noises of B ω k , and n B vx , n B vy and n B vz denote the measurement noises of B v B k . The measurement covariance matrix of n B k is given by where σ B,vx , σ B,vy , σ B,vz , σ B,ωx and σ B,ωy denote the standard deviations of the measurement noise.
where Q B k ωz denotes the variance of additional uncertain noise, and V denotes the covariance matrix of the input noise: where σ α denotes the standard deviation of the outer front wheel steering angle.
Z k in Equation (26) denotes the Jacobian matrix of the input noise; it is represented as where H B k in Equation (23) represents the measurement Jacobian matrix of vehicle velocity and angular rate errors; it is derived as where H B k ,ω (1,4) and H B k ,ω (1,6) denote the measurement Jacobian block matrixes of the vehicle angular rate errors; H (2,5) and H B k ,v (2,6) denote the measurement Jacobian block matrixes of the vehicle velocity errors; and their analytical expressions are derived as described in Appendix C.
Following Equation (30), the tunable parameter configurations of MAVIO are shown in Table 2. Following Equations (8) and (23), the updated covariance matrix and state vector of MAVIO at time t k are obtained by using the standard extended Kalman filter (EKF).

Raw GNSS Error Measurement Model
To obtain a global position for the vehicle, the raw GNSS error measurement model is proposed to further refine the estimator after the process of MAVIO(1).
The measurement residual U S r p k at time t k is defined as where U S p S k and U Sp S k denote the measured and estimated values of {S} in {U S } at time t k , respectively. U S p S k can be converted from the raw latitude and longitude of the dual-antenna Spatial NAV982-RG inertial navigation system without the carrier-phase based differential process by using the robot_localization [56] process. U Sp S k is derived as By linearizing Equation (32) at current error state X k , U S r p k is approximated as where n S k denotes the measurement noise vector of the raw GNSS data at time t k . H S k represents the measurement Jacobian matrix of the raw GNSS errors; it is derived as where H (1,5) denote the measurement Jacobian block matrixes of the raw GNSS errors, and their analytical expressions are derived as described in Appendix D.
Following Equations (8) and (33), the updated covariance matrix and state vector of MAVIO-GNSS at time t k are obtained by using the standard extended Kalman filter (EKF).

Experimental Vehicle Platform and Real-World Datasets
The real-world experiments were performed with a medium-size Ackermann steering vehicle, i.e., Vehicle_a27, which is shown in Figure 2. For further details of the experimental vehicle setup, please refer to [48].

Experimental Vehicle Platform and Real-World Datasets
The real-world experiments were performed with a medium-size Ackermann steering vehicle, i.e., Vehicle_a27, which is shown in Figure 2. For further details of the experimental vehicle setup, please refer to [48]. Taking into account the different vehicle driving states and travel distances, six different realworld experimental datasets were acquired by using Vehicle_a27, i.e., the VD01 dataset, VD02 dataset, VD03 dataset, VD04 dataset, VD05 dataset and VD06 dataset. The VD01 dataset and VD02 dataset were, respectively, acquired under a straight-line driving state and an S-shaped-curve driving state. The VD03 dataset was acquired under a circular-curve driving state for five circles. The VD04 dataset was acquired under straight-line and turning driving states by traveling around the building in a test field for one circle. The VD05 dataset was acquired under S-shaped-curve, straight-line and turning driving states by traveling around the building in a test field for one circle. The VD04 and VD05 datasets correspond to the AM_01 and AM_02 datasets in [48], respectively. The VD06 dataset was acquired under straight-line and turning driving states by traveling around the building in a test field for three circles. The details of the real-world experimental datasets are shown in Table 3. Taking into account the different vehicle driving states and travel distances, six different real-world experimental datasets were acquired by using Vehicle_a27, i.e., the VD01 dataset, VD02 dataset, VD03 dataset, VD04 dataset, VD05 dataset and VD06 dataset. The VD01 dataset and VD02 dataset were, respectively, acquired under a straight-line driving state and an S-shaped-curve driving state. The VD03 dataset was acquired under a circular-curve driving state for five circles. The VD04 dataset was acquired under straight-line and turning driving states by traveling around the building in a test field for one circle. The VD05 dataset was acquired under S-shaped-curve, straight-line and turning driving states by traveling around the building in a test field for one circle. The VD04 and VD05 datasets correspond to the AM_01 and AM_02 datasets in [48], respectively. The VD06 dataset was acquired under straight-line and turning driving states by traveling around the building in a test field for three circles. The details of the real-world experimental datasets are shown in Table 3. Table 3. List of the real-world experimental datasets adapted from [54].  Figure 3 shows the ground-truth vehicle longitudinal speeds for the real-world datasets.  Figure 3 shows the ground-truth vehicle longitudinal speeds for the real-world datasets.   Figure 3 indicates that the ground-truth vehicle longitudinal acceleration was close to a constant value in short periods of time for all of the real-world datasets, which provides potential support for the data with degenerate motions of ground vehicles.

Experimental Results
Considering the differences between each round of real-world experimental results caused by the non-real-time computer operating system and other uncertain factors, the performance of MAVIO and MAVIO-GNSS were compared with S-MSCKF and ACK-MSCKF using real-world datasets by averaging all twenty rounds of experimental results. The arithmetic mean was adopted for the averaging of many rounds of vehicle position and root mean square scale ratio data. For the average of the vehicle attitude data, the weighted average quaternion [57] was used. For qualitative and quantitative positioning accuracy evaluation, the estimated trajectory, absolute trajectory error (ATE), relative translation error (RTE) and relative translation error percentage (RTEP) were calculated based on the average results by using the rpg_trajectory_evaluation [58] package. Owing to this operation, the results in this paper are more convincing. The source code for the data average processing can be publicly accessed from [59].
For consistency of comparison, we adopted the ground-truth pose errors and estimated ±3σ (triple standard deviation) bounds [60] along with time in one round of experiments. For observability comparisons for scale direction, the root mean square scale ratio (RMSSR) S rmssr,k at time t k was used as an evaluation index. The smaller the root mean square scale ratio S rmssr,k , the more consistent the estimated scale of the positioning trajectory with the real scale [54]. S rmssr,k is defined as where k denotes the number of output pose estimation results. S i denotes the scale ratio [30] at time t i where, q and U S p G B are initialized by the trajectory alignment method [61]; the open source code [62] is referenced in the implementation of the trajectory alignment. For simplicity, the origin of {B} coincides with that of {S}. The initial translation extrinsic parameter vector I p B between {B} and {I} is obtained by manual measurement. The initial rotation extrinsic parameter vector B I q between {B} and {I} was calibrated by the method proposed in Section 2.4 of [48]. Taking into account the effects of vehicle vibration and temperature changes, the IMU noise parameters calibrated with the kalibr_allan [63] package were expanded by 10 times in all the real-world experiments.        MSCKF(2) obtain spurious information along the direction of the vehicle yaw angle and three-axis position, which leads to an inconsistency problem in that the ground-truth errors of the vehicle yaw angle and three-axis position are far greater than their estimated 3 with time. Figures 6 and 7 show the ground-truth pose errors and estimated 3  bounds of ACK-MSCKF(3) and ACK-MSCKF(4) with time, respectively.   MSCKF(2) obtain spurious information along the direction of the vehicle yaw angle and three-axis position, which leads to an inconsistency problem in that the ground-truth errors of the vehicle yaw angle and three-axis position are far greater than their estimated 3 with time. Figures 6 and 7 show the ground-truth pose errors and estimated 3  bounds of ACK-MSCKF(3) and ACK-MSCKF(4) with time, respectively.    Similarly, it is seen from Figures 6 and 7 that ACK-MSCKF(3) and ACK-MSCKF(4) are subjected to the same problem as above, especially along the direction of the vehicle yaw angle. From the above analysis, there appears the problem of estimator inconsistency in all the tunable parameter configurations of ACK-MSCKF.     Similarly, it is seen from Figures 6 and 7 that ACK-MSCKF(3) and ACK-MSCKF(4) are subjected to the same problem as above, especially along the direction of the vehicle yaw angle. From the above analysis, there appears the problem of estimator inconsistency in all the tunable parameter configurations of ACK-MSCKF.    It is concluded from Figures 8 and 9 that the estimator of MAVIO(1) and MAVIO(2) is consistent according to the fact that the ground-truth errors of the vehicle yaw angle and three-axis position are enclosed in their estimated ±3σ with time. Moreover, the three-axis vehicle position error state and yaw angle error state of the different parameter configurations of MAVIO are unobservable, which agrees with the result of the existing VIO observability analysis [64]. Therefore, MAVIO can overcome the inconsistency problem of ACK-MSCKF by introducing the vehicle velocity and angular rate error measurements, which could have a positive effect on improving positioning accuracy. Note that the reset phenomenon of the estimated ±3σ is attributed to the robust implementation of S-MSCKF [12]. Figure 10 shows the ground-truth pose errors and estimated ±3σ bounds of MAVIO-GNSS with time.

Observability and Consistency Comparison
Sensors 2020, 20, x FOR PEER REVIEW 15 of 30 It is concluded from Figures 8 and 9 that the estimator of MAVIO(1) and MAVIO (2) is consistent according to the fact that the ground-truth errors of the vehicle yaw angle and three-axis position are enclosed in their estimated 3  with time. Moreover, the three-axis vehicle position error state and yaw angle error state of the different parameter configurations of MAVIO are unobservable, which agrees with the result of the existing VIO observability analysis [64]. Therefore, MAVIO can overcome the inconsistency problem of ACK-MSCKF by introducing the vehicle velocity and angular rate error measurements, which could have a positive effect on improving positioning accuracy. Note that the reset phenomenon of the estimated 3  is attributed to the robust implementation of S-MSCKF [12].  It can be seen from Figure 10 that the three-axis vehicle position error state and yaw angle error state of MAVIO-GNSS are observable, which benefits from the raw GNSS error measurement model. This implies that MAVIO-GNSS can obtain a global position for a vehicle under a long-distance driving state.
By averaging all rounds of experimental results, the better performance of the RMSSR among the different configurations at the last time step for each experimental dataset is shown in Table 4.   It can be seen from Figure 10 that the three-axis vehicle position error state and yaw angle error state of MAVIO-GNSS are observable, which benefits from the raw GNSS error measurement model. This implies that MAVIO-GNSS can obtain a global position for a vehicle under a long-distance driving state.
By averaging all rounds of experimental results, the better performance of the RMSSR among the different configurations at the last time step for each experimental dataset is shown in Table 4.  It can be seen from the quantitative results in Table 4 that the average RMSSRs of MAVIO and ACK-MSCKF are smaller than the RMSSR for S-MSCKF for all of the datasets. Taken together, the above results indicate that both MAVIO and ACK-MSCKF can improve the observability of the VIO scale direction. It can be seen from the quantitative results in Table 4 that the average RMSSRs of MAVIO and ACK-MSCKF are smaller than the RMSSR for S-MSCKF for all of the datasets. Taken together, the above results indicate that both MAVIO and ACK-MSCKF can improve the observability of the VIO scale direction.   Figure 11 shows that S-MSCKF has larger scale drift, especially for the VD01, VD03, VD04 and VD06 datasets. Meanwhile, by selecting the appropriate parameter configuration in practice, the estimated trajectories of ACK-MSCKF and proposed MAVIO align to the ground truth better than S-MSCKF for all of the datasets. The performance difference among the different parameter configurations of ACK-MSCKF could be attributed to the inconsistency problem mentioned above. By improving the consistency, MAVIO is more robust than ACK-MSCKF. By introducing the raw GNSS error measurements, MAVIO-GNSS performs better than MAVIO, especially for the longdistance VD06 dataset.

Positioning Accuracy Comparison
The boxplots of the RTEPs are shown in Figure 12.  Figure 11 shows that S-MSCKF has larger scale drift, especially for the VD01, VD03, VD04 and VD06 datasets. Meanwhile, by selecting the appropriate parameter configuration in practice, the estimated trajectories of ACK-MSCKF and proposed MAVIO align to the ground truth better than S-MSCKF for all of the datasets. The performance difference among the different parameter configurations of ACK-MSCKF could be attributed to the inconsistency problem mentioned above. By improving the consistency, MAVIO is more robust than ACK-MSCKF. By introducing the raw GNSS error measurements, MAVIO-GNSS performs better than MAVIO, especially for the long-distance VD06 dataset.
The boxplots of the RTEPs are shown in Figure 12.   Table 5, and Table 6 shows the overall RTE for all of the experimental datasets.  The quantitative results in Table 5 show that the average ATEs of MAVIO, MAVIO-GNSS and ACK-MSCKF is smaller than the ATE of S-MSCKF for all of the datasets but VD02. The trajectory error difference of S-MSCKF with that in [48] for the VD05 dataset is attributed to the data average processing which ensures more generalizable result. Compared with S-MSCKF, MAVIO reduces the   Table 5, and Table 6 shows the overall RTE for all of the experimental datasets.  The quantitative results in Table 5 show that the average ATEs of MAVIO, MAVIO-GNSS and ACK-MSCKF is smaller than the ATE of S-MSCKF for all of the datasets but VD02. The trajectory error difference of S-MSCKF with that in [48] for the VD05 dataset is attributed to the data average processing which ensures more generalizable result. Compared with S-MSCKF, MAVIO reduces the average ATE by 86.61%, 68.03%, 86.45%, 25.65% and 78.26% for the VD01, VD03, VD04, VD05 and VD06 datasets, respectively. Moreover, MAVIO performs better than ACK-MSCKF for the VD01, VD02, VD03, VD04 and VD05 datasets, which reduces the average ATEs by 18.13%, 4.67%, 10.34%, 18.29% and 13.90%, respectively. It is also observable that MAVIO performs worse than ACK-MSCKF for the VD06 dataset. A potential reason is that the extrinsic parameters between {B} and {I} are not accurate enough. Furthermore, MAVIO-GNSS performs better than MAVIO for the VD01, VD02, VD04 and VD06 datasets, which reduces the average ATEs by 6.87%, 7.35%, 16.92% and 30.66%, respectively.   Table 6 shows that the overall RTE of MAVIO within the sub-trajectory length of 20 m is smaller than that of ACK-MSCKF. MAVIO-GNSS performs best in terms of the RTE within the sub-trajectory length of 100 m. The lowest RTE from MAVIO-GNSS is 2.29 m in the sub-trajectory length of 100 m, which is reduced, respectively, by 17.92%, 10.89% and 72.51% under the same conditions compared to that with MAVIO, ACK-MSCKF and S-MSCKF.

Discussion
ACK-MSCKF significantly improves the observability of the VIO scale direction and positioning accuracy under degenerate motions of ground vehicles. However, for practical implementation, the appropriate parameter configuration is needed to obtain better performance due to the inconsistency problem.
By improving the observability of the VIO scale direction and overcoming the inconsistency of ACK-MSCKF, MAVIO is more robust and further improves vehicle positioning accuracy using only a monocular configuration, which mainly benefits from the vehicle velocity and yaw angular rate error measurement model instead of the relative kinematic error measurement model for the vehicle. Compared with S-MSCKF, MAVIO reduces the average ATE by 86.61%, 68.03%, 86.45%, 25.65% and 78.26% for the VD01, VD03, VD04, VD05 and VD06 datasets, respectively. Moreover, MAVIO performs better than ACK-MSCKF for the VD01, VD02, VD03, VD04 and VD05 datasets, which reduces the average ATE by 18.13%, 4.67%, 10.34%, 18.29% and 13.90%, respectively. By introducing the raw GNSS error measurement model, MAVIO-GNSS further improves the vehicle positioning accuracy under a long-distance driving state. The lowest RTE from MAVIO-GNSS is 2.29 m in the sub-trajectory length of 100 m, and it is reduced, respectively, by 17.92%, 10.89% and 72.51% under the same conditions compared to the values obtained with MAVIO, ACK-MSCKF and S-MSCKF.

Conclusions
This paper proposed a consistent monocular Ackermann VIO termed MAVIO. The vehicle velocity and yaw angular rate error measurement model was introduced in detail, considering the lever arm effect between {B} and {I}. To obtain a global position for the vehicle, the raw GNSS error measurement model was introduced to further improve the performance of MAVIO. The observability and positioning accuracy were comprehensively compared using real-world datasets, averaging twenty rounds of experimental results. The experimental results demonstrated that the proposed MAVIO not only improved the observability of the VIO scale direction under degenerate motions of ground vehicles, but also resolved the inconsistency problem of ACK-MSCKF to further improve the vehicle positioning accuracy. Moreover, MAVIO-GNSS further improved the vehicle positioning accuracy under a long-distance driving state.
A limitation derives from the assumptions of approximate planar motion for a low-speed driving state and highly accurate extrinsic parameters between {B} and {I}. A future research target is to further investigate the performance impact of the vehicle speed and vehicle-IMU extrinsic parameter calibration to better enhance the positioning accuracy.

Acknowledgments:
We would like to express our gratitude for the support received from Yu Yang, and we acknowledge that some of the content of this paper refers to the dissertation [54] of J.S.

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

Appendix A
According to the derivation of Equation (177) in [53], where B ω k is derived from the following formula: According to the error quaternion representation [53], Equation (A2) is represented as By ignoring the second order terms, Equation (A4) is represented as Thus, Equation (A1) is represented as Sensors 2020, 20, 5757 22 of 32 Therefore, the Jacobian matrix of θ is represented as Following Equation (6), G B . p B k is calculated as Then, Therefore, the Jacobian matrix of G B . p B k is represented as Following Equation (6), G B . v I k is calculated as Sensors 2020, 20, 5757 23 of 32 Equation (A12) is represented as Then, Therefore, the Jacobian matrix of G B . v I k is represented as According to the characteristics of the IMU noise model [53], Therefore, Thereby, the analytical expressions of each block matrix in Equations (9) and (10) are derived.

Appendix B
Following Equation (7) in [48], according to the error quaternion representation [53], the Ackermann steering attitude angle error B j−1 B j θ from time t j−1 to time t j is derived as Equation (A19) can be represented as Then, According to the properties of the cross product skew-symmetric matrix [53], Equation (A21) is represented as Then, Sensors 2020, 20, 5757 25 of 32 Therefore, the measurement Jacobian matrixes of the vehicle relative rotation errors are represented as Similarly, the velocity measurement residual r B v j at time t j is represented as Sensors 2020, 20, 5757 26 of 32 Thus, Equation (A27) is represented as Then, Sensors 2020, 20, 5757 27 of 32 Therefore, the measurement Jacobian matrixes of the vehicle relative translation errors are represented as Thereby, the analytical expressions of all the block matrixes in Equation (15) are derived.

Appendix C
Following Equation (16), the vehicle angular rate measurement residual B r ω k at time t k in {B} is represented as Based on the lever arm effect between {B} and {I}, Equation (A32) is represented as According to the error quaternion representation [53], Equation (A33) is represented as Therefore, the measurement Jacobian matrix of the vehicle angular rate errors is represented as Similarly, following Equation (16), the vehicle velocity measurement residual B r v k at time t k in {B} is represented as Based on the lever arm effect between {B} and {I}, Equation (A36) is represented as Sensors 2020, 20, 5757 28 of 32 According to the error quaternion representation [53], Equation (A37) is represented as By ignoring the second order terms, Equation (A38) is represented as Therefore, the measurement Jacobian matrix of the vehicle velocity errors is represented as

Appendix D
According to the error quaternion representation [53], Equation (31) is represented as Following Equation (32), Equation (A41) is represented as Therefore, the measurement Jacobian matrix of the raw GNSS errors is represented as