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

Abstract

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.

1. 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 and low cost. VIO mainly consists of filter-based methods, including the Multi-Sensor Fusion Approach (MSF) [8], the Robust Visual-Inertial Odometry (ROVIO) [9], the Multi-State Constraint Kalman Filter (MSCKF) [10], Stereo-MSCKF [11], S-MSCKF [12], the Robocentric Visual-Inertial Odometry (R-VIO) [13], Schmidt-MSCKF [14], the Lightweight Hybrid Visual-Inertial Odometry (LARVIO) [15,16] and optimization-based methods including the Open Keyframe-based Visual-Inertial SLAM (OKVIS) [17], ORB-SLAM-VI [18], VINS-Mono [19], PL-VIO [20], ICE-BA [21], VI-DSO [22], VINS-Fusion [23,24], Basalt [25] and ORB-SLAM3 [26]. A detailed review of the VIO methods can be found in the literature [27,28].

However, the standard VIO is subjected to additional unobservable directions under constant acceleration and straight-line driving states or approximate combinations of the above two driving states of ground vehicles, resulting in larger pose-estimation errors [29,30,31]. To address this problem, there are some studies of integrating VIO algorithms with the information from the vehicle proprioceptive sensors, including the drive motor encoder sensor and steering wheel angle sensor. Wu et al. [30] proposed fusing wheel speed encoder measurements into VIO based on the square-root inverse sliding window filter (SR-ISWF) [32], which significantly improved positioning accuracy under special motions of ground differential steering robots. Li et al. [33] proposed a factor-graph-based, gyro-aided localization system by exploiting the wheel odometry and gyro measurements, which achieved better accuracy than ORB-SLAM [34,35]. KO-Fusion [36] was proposed to fuse the Mecanum wheel motion constraint into RGB-D SLAM for ground robots, which improved the robustness of SLAM. Zheng and Liu [37] proposed an SE(2)-constrained pose parameterization optimization-based VIO for ground vehicles, which obtained better accuracy under sharp-turn motion. Dang et al. [38] proposed a tightly-coupled VIO to fuse wheel encoder measurements by considering wheel slippage, which achieved great improvements in positioning accuracy. Quan et al. [39] proposed a tightly coupled monocular simultaneous localization and mapping (SLAM) system, VOSLAM, by integrating visual, odometer and gyroscope measurements, which ensured the system’s accuracy. The VOSLAM improved the robustness of initialization and faulty information by a map initialization method and reliable odometer measurements. Liu et al. [40] proposed a tightly coupled optimization-based VIO to combine a wheel encoder, IMU and visual sensor, which improved the robustness of initialization and positioning accuracy. To improve the positioning accuracy before the first turning in [40], Liu et al. [41] proposed a bidirectional trajectory computation method, which achieved a more accurate real-time trajectory. Zhang et al. [42] proposed a wheel odometry with motion manifold representation for ground robot localization, which achieved accurate 6D pose estimation. Ye et al. [43] proposed a robust pose estimation method with multi-camera, odometer and gyroscope measurements in a tightly coupled optimization framework, which had obvious advantages in loop-closure detection. Gang et al. [44] proposed a tightly coupled SLAM method using wheel speed, IMU and monocular vision, which solved the problem of an unobservable scale and improved the positioning robustness. Zuo et al. [45] proposed a kinematics-constrained VIO and provided detailed observability analysis for a skid-steering mobile robot, and the experimental results showed that the kinematic parameters were observable under general motion and online kinematic parameter estimation can significantly improve positioning accuracy. VINS-Vehicle [46] was proposed to fuse two DOF vehicle dynamics models into VIO based on the sliding window optimization method, which significantly enhanced the robustness and accuracy compared with existing VIO methods. Lee et al. [47] proposed a visual-inertial-wheel odometry system, VIWO, by integrating the measurements of IMU, camera and wheel odometry preintegration, which provided theoretical guidance for the localization of ground differential steering vehicle.

By integrating the information from the vehicle proprioceptive sensors, the above tightly coupled VIO methods can improve the observability of the VIO scale direction and further enhance the vehicle positioning accuracy. However, apart from in [46], the above methods have not taken enough advantage of the measurements from the steering wheel angle sensor, which is a low-cost built-in sensor in ICVs. Moreover, the above methods except [30,47] adopt an optimization-based backend, which requires higher computational complexity. In view of the efficient performance, the Ackermann Multi-State Constraint Kalman Filter (ACK-MSCKF) [48] was proposed for integrating Ackermann error state measurements and S-MSCKF in a tightly coupled filter-based mechanism in our previous work, which improved the positioning accuracy under degenerate motions of ground vehicles. However, it adopts a stereo configuration, which is more costly and less computationally efficient than monocular solutions. Moreover, it assumes that the vehicle velocity is constant between two consecutive camera states, which degrades the positioning accuracy. Lastly and most importantly, the observability, consistency and positioning accuracy of different parameter configurations of ACK-MSCKF with vehicle relative kinematic error measurements need to be further explored.

Therefore, in this paper, a tightly coupled filter-based consistent monocular Ackermann VIO, termed MAVIO, is proposed to combine the vehicle velocity and yaw angular rate error measurements, considering the lever arm effect between the vehicle and IMU coordinates. Similar to in the work [49], to obtain a global position for the vehicle, the raw GNSS error measurement model, termed MAVIO-GNSS, is introduced to further improve the performance of MAVIO. The famous open-source VIO, i.e., S-MSCKF [12], is adopted as the base of this work. The main contributions of this paper are highlighted as follows:

(1)

Additional analyses of different parameter configurations of ACK-MSCKF are performed with more real-world experiments.

(2)

Conducting the formulation and implementation of a consistent monocular Ackermann VIO, MAVIO, which not only improves the observability of the VIO scale direction but also resolves the inconsistency problem of ACK-MSCKF for further improving the positioning accuracy.

(3)

Introducing the raw GNSS error measurement model, MAVIO-GNSS, which further improves the vehicle positioning accuracy under the long-distance driving state.

(4)

The performance of MAVIO and MAVIO-GNSS are comprehensively compared with S-MSCKF and ACK-MSCKF on real-world datasets with twenty rounds, on average, of real-world experiments.

(5)

The source code [50] of MAVIO is publicly available to facilitate the reproducibility of related research.

The remainder of this paper is structured as follows: The proposed approach is introduced in detail in Section 2. Then, Section 3 describes how the real-world experiments were carried out. Besides, the experimental results are discussed in Section 4. Finally, Section 5 presents the conclusions drawn.

2. The Proposed Approach

2.1. Coordinate Systems and Notations

The Vehicle Coordinate System {B}, IMU Coordinate System {I} and Camera Coordinate System {C} had the same definitions as in [48]. The additional four coordinate systems were defined as follows:

(1)

Inertial Coordinate System of IMU {G_I}. The origin of {G_I} is the same as that of {I} at the time of VIO initialization. The axes of {G_I} are obtained by calculation at the time of VIO initialization, and its z-axis is aligned with Earth’s gravity.

(2)

Inertial Coordinate System of Vehicle {G_B}. The origin of {G_B} is the same as that of {B} at the time of VIO initialization. The axes of {G_B} are obtained by calculation at the time of VIO initialization, and its z-axis is aligned with Earth’s gravity.

(3)

GNSS Coordinate System {S}. The origin of {S} lies in the center of the GNSS equipment. The x-axis and y-axis point forward and to the right, respectively, following the right-hand rule.

(4)

Universal Transverse Mercator Coordinate System {U_S}. The {U_S} is a universal global coordinate system. Please refer to [51] for more details.

In this paper, we adopted the Jet Propulsion Laboratory (JPL) Proposed Standard Conventions [52,53] to derive the corresponding formulas.

2.2. 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 ${}^{B}v{}_{B_k}^{}$ at time $t_k$ is derived as

$${}^{B}v{}_{B_k}=C\left({}_{G_I}{}^{B_k}q\right){}^{G_I}v{}_{I_k}^{}+{}^{B}v{}_{l_k}$$

(1)

where ${}^{B}v{}_{l_k}$ denotes the lever arm velocity at time $t_k$ between {B} and {I}, ${}^{B}v{}_{l_k}$ and $C\left({}_{G_I}{}^{B_k}q\right)$ are defined as

$$\{\begin{array}{l}{}^{B}v{}_{l_k}=C\left({}_I{}^{B}q{}_k\right)\left({}^I\dot{p}{}_{B,k}^{}+\lfloor {}^I\omega {}_k\times \rfloor {}^{I}p{}_{B,k}^{}\right)\\ C\left({}_{G_I}{}^{B_k}q\right)=C\left({}_I{}^{B}q{}_k\right)\cdot C\left({}_{G_I}{}^{I_k}q\right)\end{array}$$

(2)

and the vehicle angular ${}^B\omega {}_k$ rate at time $t_k$ is derived as

$${}^B\omega {}_k=C\left({}_I{}^{B}q{}_k\right)\cdot {}^I\omega {}_k+{}^I\omega {}_{ib,k}$$

(3)

where ${}^I\omega {}_{ib,k}$ denotes the relative angular rate between {B} and {I}, which can be negligible with a fixed installed location for the IMU.

2.3. 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]

$$X_{B_k}={\left[\begin{array}{ccccccc}{}_{G_B}{}^{B_k}q{}^{\mathrm{T}}& {}^{G_B}p{}_{B_k}^{\mathrm{T}}& {}^{G_B}v{}_{I_k}^{\mathrm{T}}& {}_I{}^{B}q{}_{{}_k}^{\mathrm{T}}& {}^{I}p{}_{B,k}^{\mathrm{T}}& b_{g,k}^{\mathrm{T}}& b_{a,k}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$$

(4)

where ${}_{G_B}{}^{B_k}q$ denotes the rotation quaternion from {G_B} to {B} at time $t_k$. ${}^{G_B}p{}_{B_k}^{}$ denotes the position of {B} in {G_B}. ${}^{G_B}v{}_{I_k}^{}$ denotes the velocity of {I} in {G_B}. ${}_I{}^{B}q{}_k$ and ${}^{I}p{}_{B,k}^{}$ denote the rotation and translation 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 ${\tilde{X}}_{B_k}\in {ℝ}^{21}$ at time $t_k$ is defined as

$${\tilde{X}}_{B_k}={\left[\begin{array}{ccccccc}{}_{G_B}{}^{B_k}\tilde{\theta }{}^{\mathrm{T}}& {}^{G_B}\tilde{p}{}_{B_k}^{\mathrm{T}}& {}^{G_B}\tilde{v}{}_{I_k}^{\mathrm{T}}& {}_I{}^B\tilde{\theta }{}_{{}_k}^{\mathrm{T}}& {}^I\tilde{p}{}_{B,k}^{\mathrm{T}}& {\tilde{b}}_{g,k}^{\mathrm{T}}& {\tilde{b}}_{a,k}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$$

(5)

The continuous-time kinematic differential equations of the true state are

$$\{\begin{array}{l}{}_{G_B}{}^{B_k}\dot{q}=\frac{1}{2}\mathsf{\Omega }\left(C\left({}_I{}^{B}q{}_k\right){}^I\omega {}_k\right){}_{G_B}{}^{B_k}q\\ {}^{G_B}\dot{p}{}_{B_k}={}^{G_B}v{}_{I_k}+C{\left({}_{G_B}{}^{B_k}q\right)}^{\mathrm{T}}C\left({}_I{}^{B}q{}_k\right)\left(\lfloor {}^I\omega {}_k\times \rfloor {}^{I}p{}_{B,k}^{}\right)\\ {}^{G_B}\dot{v}{}_{I_k}=C{\left({}_{G_B}{}^{B_k}q\right)}^{\mathrm{T}}C\left({}_I{}^{B}q{}_k\right){}^{I}a{}_k+{}^{G}g\\ {}_I{}^B\dot{q}{}_{{}_k}^{}=0_{3\times 1}, {}^I\dot{p}{}_{B,k}^{}=0_{3\times 1}\\ {\dot{b}}_{g,k}^{}=n_{\omega g,k}^{} \\ {\dot{b}}_{a,k}^{}=n_{\omega a,k}^{}\end{array}$$

(6)

where $n_{\omega g,k}$ and $n_{\omega a,k}^{}$ denote the random-walk noises of the gyroscope and accelerometer biases at time $t_k$, respectively. The true IMU angular rate ${}^I\omega {}_k$ and acceleration ${}^{I}a{}_k$ at time $t_k$ are represented as

$$\{\begin{array}{l}{}^I\omega {}_k={}^I\omega {}_{m,k}-b_{g,k}-n_{g,k}\\ {}^{I}a{}_k={}^{I}a{}_{m,k}-b_{a,k}-n_{a,k}\end{array}$$

(7)

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

$${\dot{\tilde{X}}}_{B_k}=F^{B_k}{\tilde{X}}_{B_k}+G^{B_k}{n}^{B_k}$$

(8)

where $n_{B_k}={\left[\begin{array}{llll}{n}_{g,k}^{\mathrm{T}}\hfill & n_{\omega g,k}^{\mathrm{T}}\hfill & n_{a,k}^{\mathrm{T}}\hfill & n_{\omega a,k}^{\mathrm{T}}\hfill \end{array}\right]}^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

$$F^{B_k}=\left[\begin{array}{ccccccc}{F}_{\left(1,1\right)}^{B_k}& 0_{3\times 3}& 0_{3\times 3}& F_{\left(1,4\right)}^{B_k}& 0_{3\times 3}& F_{\left(1,6\right)}^{B_k}& 0_{3\times 3}\\ F_{\left(2,1\right)}^{B_k}& 0_{3\times 3}& F_{\left(2,3\right)}^{B_k}& F_{\left(2,4\right)}^{B_k}& F_{\left(2,5\right)}^{B_k}& F_{\left(2,6\right)}^{B_k}& 0_{3\times 3}\\ F_{\left(3,1\right)}^{B_k}& 0_{3\times 3}& 0_{3\times 3}& F_{\left(3,4\right)}^{B_k}& 0_{3\times 3}& 0_{3\times 3}& F_{\left(3,7\right)}^{B_k}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]$$

(9)

$$G^{B_k}=\left[\begin{array}{cccc}{G}_{(1,1)}^{B_k}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ G_{(2,1)}^{B_k}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& G_{(3,3)}^{B_k}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& G_{(6,2)}^{B_k}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& G_{(7,4)}^{B_k}\end{array}\right]$$

(10)

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

$$X_{C_j}={\left[\begin{array}{cc}{}_{G_B}{}^{C_j}q{}^{\mathrm{T}}& {}^{G_B}p{}_{C_j}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$$

(11)

where ${}_{G_B}{}^{C_j}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 {G_B}. At $t_k=t_j$, ${}_{G_B}{}^{C_j}q$ and ${}^{G_B}p{}_{C_j}$ are given by

$$\{\begin{array}{l}{}_{G_B}{}^{C_j}q={}_I{}^{C}q\otimes {}_B{}^{I}q{}_k\otimes {}_{G_B}{}^{B_k}q\\ {}^{G_B}p{}_{C_j}^{}={}^{G_B}p{}_{B_k}^{}+C{\left({}_{G_B}{}^{B_k}q\right)}^{\mathrm{T}}C\left({}_I{}^{B}q{}_k\right)\left({}^{I}p{}_C-{}^{I}p{}_{B,k}\right)\end{array}$$

(12)

According to Equation (4) and Equation (11), the full state vector $X_k$ with $N$ camera states at time $t_k\in \left[\begin{array}{cc}{t}_j& t_{j+1}\end{array}\right)$ is given by

$$X_k={\left[\begin{array}{cccccccccc}{X}_{B_k}^{\mathrm{T}}& {}_{G_B}{}^{C_{j-N+1}}q{}^{\mathrm{T}}& {}^{G_B}p{}_{C_{j-N+1}}^{\mathrm{T}}& {}_{G_B}{}^{C_{j-N+2}}q{}^{\mathrm{T}}& {}^{G_B}p{}_{C_{j-N+2}}^{\mathrm{T}}& \cdots & {}_{G_B}{}^{C_{j-1}}q{}^{\mathrm{T}}& {}^{G_B}p{}_{C_{j-1}}^{\mathrm{T}}& {}_{G_B}{}^{C_j}q{}^{\mathrm{T}}& {}^{G_B}p{}_{C_j}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$$

(13)

Following Equation (13), the full error state vector ${\tilde{X}}_k\in {ℝ}^{21+6N}$ at time $t_k$ is defined as

$${\tilde{X}}_k={\left[\begin{array}{cccccccccc}{\tilde{X}}_{B_k}^{\mathrm{T}}& {}_{G_B}{}^{C_{j-N+1}}\tilde{\theta }{}^{\mathrm{T}}& {}^{G_B}\tilde{p}{}_{C_{j-N+1}}^{\mathrm{T}}& {}_{G_B}{}^{C_{j-N+2}}\tilde{\theta }{}^{\mathrm{T}}& {}^{G_B}\tilde{p}{}_{C_{j-N+2}}^{\mathrm{T}}& \cdots & {}_{G_B}{}^{C_{j-1}}\tilde{\theta }{}^{\mathrm{T}}& {}^{G_B}\tilde{p}{}_{C_j-1}^{\mathrm{T}}& {}_{G_B}{}^{C_j}\tilde{\theta }{}^{\mathrm{T}}& {}^{G_B}\tilde{p}{}_{C_j}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$$

(14)

The augmented state covariance matrix and monocular visual feature measurement model can be obtained as detailed in [10,12].

2.4. Kinematic Error Measurement Model for Vehicle

2.4.1. 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

$$H_{}^{B_j}=\left[\begin{array}{cccccccc}{0}_{3\times 15}& H_{\left(1,2\right)}^{B_{j,\theta }}& 0_{3\times 3}& 0_{3\times \left(6N-12\right)}& H_{\left(1,5\right)}^{B_{j,\theta }}& 0_{3\times 3}& H_{\left(1,7\right)}^{B_{j,\theta }}& 0_{3\times 3}\\ 0_{3\times 15}& H_{\left(2,2\right)}^{B_{j,v}}& H_{\left(2,3\right)}^{B_{j,v}}& 0_{3\times \left(6N-12\right)}& H_{\left(2,5\right)}^{B_{j,v}}& H_{\left(2,6\right)}^{B_{j,v}}& H_{\left(2,7\right)}^{B_{j,v}}& H_{\left(2,8\right)}^{B_{j,v}}\end{array}\right]$$

(15)

where $H_{\left(1,2\right)}^{B_{j,\theta }}, H_{\left(1,5\right)}^{B_{j,\theta }} \mathrm{and} H_{\left(1,7\right)}^{B_{j,\theta }}$ denote the measurement Jacobian block matrixes of the vehicle relative rotation errors; $H_{\left(2,2\right)}^{B_{j,v}}, H_{\left(2,3\right)}^{B_{j,v}}, H_{\left(2,5\right)}^{B_{j,v}}, H_{\left(2,6\right)}^{B_{j,v}}, H_{\left(2,7\right)}^{B_{j,v}} \mathrm{and} H_{\left(2,8\right)}^{B_{j,v}}$ 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 tunable parameter configurations of ACK-MSCKF.

ACK-MSCKF	Tunable Parameter Configurations
ACK-MSCKF(1)	For Equations (A24) and (A31), $H_{\left(1,5\right)}^{B_{j,\theta }}=H_{\left(2,5\right)}^{B_{j,v}}=H_{\left(2,6\right)}^{B_{j,v}}=0_{3\times 3}$
ACK-MSCKF(2)	For Equations (A24) and (A31), $H_{\left(1,2\right)}^{B_{j,\theta }}=H_{\left(1,5\right)}^{B_{j,\theta }}=H_{\left(1,7\right)}^{B_{j,\theta }}=H_{\left(2,5\right)}^{B_{j,v}}=H_{\left(2,6\right)}^{B_{j,v}}=0_{3\times 3}$
ACK-MSCKF(3)	Same as Equations (A24) and (A31)
ACK-MSCKF(4)	For Equation (A24), $H_{\left(1,5\right)}^{B_{j,\theta }}=0_{3\times 3}$

.

2.4.2. Measurements of Vehicle Velocity and Angular Rate Error

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

$$r^{B_k}=\left(\begin{array}{c}{}^{B}r{}_{{\omega }_k}^{}\\ {}^{B}r{}_{v_k}^{}\end{array}\right)=\left(\begin{array}{c}{}^B\omega {}_k-{}^B\widehat{\omega }{}_k\\ {}^{B}v{}_{B_k}-{}^B\widehat{v}{}_{B_k}\end{array}\right)$$

(16)

where ${}^{B}r{}_{v_k}^{}$ and ${}^{B}r{}_{{\omega }_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\omega {}_k$ denote the vehicle velocity and angular rate measurements at time $t_k$ in {B}, respectively. ${}^B\widehat{v}{}_{B_k}$ and ${}^B\widehat{\omega }{}_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\omega {}_k$ are represented as

$$\{\begin{array}{l}{}^{B}v{}_{B_k}={\left[\begin{array}{ccc}{}^{B}v{}_{B_k,x}& 0& 0\end{array}\right]}^{\mathrm{T}}\\ {}^B\omega {}_k={\left[\begin{array}{ccc}0& 0& {}^B\omega {}_{k,z}\end{array}\right]}^{\mathrm{T}}\end{array}$$

(17)

where ${}^{B}v{}_{B_k,x}$ and ${}^B\omega {}_{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\omega {}_{k,z}$ is represented as

$${}^B\omega {}_{k,z}={}^{B}v{}_{B_k,x}/R_k$$

(18)

where $R_k$ denotes the steering radius at time $t_k$; it can be obtained according to the Ackermann steering geometry [55]:

$$R_k=\frac{2L-B\tan {\alpha }_{o,k}}{2\tan {\alpha }_{o,k}}$$

(19)

Following Equation (19), ${}^B\omega {}_{k,z}$ is represented as

$${}^B\omega {}_{k,z}={}^{B}v{}_{B_k,x}/R_k=\frac{2{}^{B}v{}_{B_k,x}\tan {\alpha }_{o,k}}{2L-B\tan {\alpha }_{o,k}}$$

(20)

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. ${\alpha }_{o,k}$ denotes the outer front wheel steering angle at time $t_k$, which is represented as

$${\alpha }_{o,k}={\delta }_{o,k}/\lambda$$

(21)

where $\lambda$ denotes the steering angular transmission ratio obtained from the offline calibration. ${\delta }_{o,k}$ denotes the steering wheel angle at time $t_k$, which can also be obtained from the vehicle CAN-bus.

Based on the lever arm effect between {B} and {I}, ${}^B\widehat{\omega }{}_k$ and ${}^B\widehat{v}{}_{B_k}$ are represented as

$$\{\begin{array}{l}{}^B\widehat{\omega }{}_k=C\left({}_I{}^B\widehat{q}{}_k\right)\cdot {}^I\widehat{\omega }{}_k\\ {}^B\widehat{v}{}_{B_k}=C\left({}_{G_B}{}^{B_k}\widehat{q}\right){}^{G_B}\widehat{v}{}_{I_k}+C\left({}_I{}^B\widehat{q}{}_k\right)\left(\lfloor {}^I\widehat{\omega }{}_k\times \rfloor {}^I\widehat{p}{}_{B,k}^{}\right)\end{array}$$

(22)

By linearizing Equation (16) at current error state ${\tilde{X}}_k$, $r_{B_k}$ is approximated as

$$r^{B_k}\simeq H^{B_k}{\tilde{X}}_k+n^{B_k}$$

(23)

where $n^{B_k}$ denotes the measurement noise vector at time $t_k$:

$$n^{B_k}={\left[\begin{array}{cccccc}{n}_{\omega x}^B& n_{\omega y}^B& n_{\omega z}^{B_k}& n_{vx}^B& n_{vy}^B& n_{vz}^B\end{array}\right]}^{\mathrm{T}}$$

(24)

where $n_{\omega x}^B, n_{\omega y}^B \mathrm{and} n_{\omega z}^{B_k}$ denote the measurement noises of ${}^B\omega {}_k$, and $n_{vx}^B, n_{vy}^B \mathrm{and} n_{vz}^B$ denote the measurement noises of ${}^{B}v{}_{B_k}$. The measurement covariance matrix of $n^{B_k}$ is given by

$$U_k=diag\left\{\begin{array}{cccccc}{\sigma }_{B,\omega x}^2& {\sigma }_{B,\omega y}^2& U_{\omega z}^{B_k}& {\sigma }_{B,vx}^2& {\sigma }_{B,vy}^2& {\sigma }_{B,vz}^2\end{array}\right\}$$

(25)

where ${\sigma }_{B,vx}^{}, {\sigma }_{B,vy}^{}, {\sigma }_{B,vz}^{}, {\sigma }_{B,\omega x}^{} \mathrm{and} {\sigma }_{B,\omega y}^{}$ denote the standard deviations of the measurement noise. $U_{\omega z}^{B_k}$ denotes the variance of $n_{\omega z}^{B_k}$ at time $t_k$; it is obtained by

$$U_{\omega z}^{B_k}=Z_{k}V{Z}_k^{\mathrm{T}}+Q_{\omega z}^{B_k}$$

(26)

where $Q_{\omega z}^{B_k}$ denotes the variance of additional uncertain noise, and $V$ denotes the covariance matrix of the input noise:

$$V=\left[\begin{array}{cc}{\sigma }_{\alpha }^2& 0\\ 0& {\sigma }_{B,vx}^2\end{array}\right]$$

(27)

where ${\sigma }_{\alpha }$ 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

$$Z_k=\left[\begin{array}{cc}{Z}_{k,\alpha }& Z_{k,vx}\end{array}\right]$$

(28)

where

$$\{\begin{array}{l}{Z}_{k,\alpha }=\frac{\partial {}^B\omega {}_{k,z}}{\partial {\alpha }_{o,k}}=\frac{L{}^{B}v{}_{B_k,x}\left({\tan }^2{\alpha }_{o,k}+1\right)}{{\left(R_k\tan {\alpha }_{o,k}\right)}^2}\\ Z_{k,vx}=\frac{\partial {}^B\omega {}_{k,z}}{\partial {}^{B}v{}_{B_k,x}}=\frac{1}{R_k}\end{array}$$

(29)

$H^{B_k}$ in Equation (23) represents the measurement Jacobian matrix of vehicle velocity and angular rate errors; it is derived as

$$H^{B_k}=\left[\begin{array}{cccccccc}{0}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& H_{(1,4)}^{B_k,\omega }& 0_{3\times 3}& H_{(1,6)}^{B_k,\omega }& 0_{3\times 3}& 0_{3\times 6N}\\ H_{(2,1)}^{B_k,v}& 0_{3\times 3}& H_{(2,3)}^{B_k,v}& H_{(2,4)}^{B_k,v}& H_{(2,5)}^{B_k,v}& H_{(2,6)}^{B_k,v}& 0_{3\times 3}& 0_{3\times 6N}\end{array}\right]$$

(30)

where $H_{(1,4)}^{B_k,\omega } \mathrm{and} H_{(1,6)}^{B_k,\omega }$ denote the measurement Jacobian block matrixes of the vehicle angular rate errors; $H_{(2,1)}^{B_k,v}, H_{(2,3)}^{B_k,v}, H_{(2,4)}^{B_k,v}, H_{(2,5)}^{B_k,v} \mathrm{and} H_{(2,6)}^{B_k,v}$ 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. The tunable parameter configurations of the monocular Ackermann visual–inertial odometry (VIO) (MAVIO).

MAVIO	Tunable Parameter Configurations
MAVIO(1)	Same as Equations (A35) and (A40)
MAVIO(2)	For Equation (A35), $H_{(1,4)}^{B_k,\omega }=H_{(1,6)}^{B_k,\omega }=0_{3\times 3}$

.

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).

2.5. 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

$${}^{U_S}r{}_{p_k}^{}={}^{U_S}p{}_{S_k}-{}^{U_S}\widehat{p}{}_{S_k}$$

(31)

where ${}^{U_S}p{}_{S_k}$ and ${}^{U_S}\widehat{p}{}_{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_S}\widehat{p}{}_{S_k}$ is derived as

$$\begin{array}{l}{}^{U_S}\widehat{p}{}_{S_k}={}^{U_S}p{}_{G_B}+C{\left({}_{U_S}{}^{G_B}q\right)}^{\mathrm{T}}{}^{G_B}\widehat{p}{}_{S_k}\\ ={}^{U_S}p{}_{G_B}+C{\left({}_{U_S}{}^{G_B}q\right)}^{\mathrm{T}}\left({}^{G_B}\widehat{p}{}_{B_k}+C{\left({}_{G_B}{}^{B_k}\widehat{q}\right)}^{\mathrm{T}}{}^B\widehat{p}{}_{S,k}^{}\right)\\ ={}^{U_S}p{}_{G_B}+C{\left({}_{U_S}{}^{G_B}q\right)}^{\mathrm{T}}{}^{G_B}\widehat{p}{}_{B_k}+C{\left({}_{U_S}{}^{G_B}q\right)}^{\mathrm{T}}C{\left({}_{G_B}{}^{B_k}\widehat{q}\right)}^{\mathrm{T}}C\left({}_I{}^B\widehat{q}{}_k\right)\left({}^{I}p{}_S^{}-{}^I\widehat{p}{}_{B,k}^{}\right)\end{array}$$

(32)

where ${}_{U_S}{}^{G_B}q$ denotes the rotation quaternion from {U_S} to {G_B}. ${}^{U_S}p{}_{G_B}$ denotes the position of {G_B} in {U_S}. ${}^{I}p{}_S^{}$ denotes the translation extrinsic parameter between {S} and {I}.

By linearizing Equation (32) at current error state ${\tilde{X}}_k$, ${}^{U_S}r{}_{p_k}^{}$ is approximated as

$${}^{U_S}r{}_{p_k}^{}\simeq H^{S_k}{\tilde{X}}_k+n^{S_k}$$

(33)

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

$$H^{S_k}=\left[\begin{array}{cccccccc}{H}_{(1,1)}^{S_k,p}& H_{(1,2)}^{S_k,p}& 0_{3\times 3}& H_{(1,4)}^{S_k,p}& H_{(1,5)}^{S_k,p}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 6N}\end{array}\right]$$

(34)

where $H_{(1,1)}^{S_k,p}, H_{(1,2)}^{S_k,p}, H_{(1,4)}^{S_k,p} \mathrm{and} H_{(1,5)}^{S_k,p}$ 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).

3. Experiments and Results

3.1. 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 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. List of the real-world experimental datasets adapted from [54].

Dataset	Vehicle Driving State	Travel Duration (s)	Travel Distance (m)	Data Bulk
				Vehicle CAN-Bus	Stereo Images	IMU	Ground Truth
VD01	Straight	54	109	8657	1615	10,808	10,818
VD02	S-shaped	60	122	9644	1820	12,146	12,186
VD03	Circular	99	162	15,631	2959	19,716	19,789
VD04	Straight and Turning	151	371	24,171	4532	30,164	30,244
VD05	S-shaped and Straight and Turning	170	400	27,135	5102	33,933	34,025
VD06	Straight and Turning	367	1085	58,463	11,014	73,169	73,386

.

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.

3.2. 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 $\pm 3\sigma$ (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

$$S_{rmssr,k}^{}=\sqrt{\frac{1}{k-1}{\displaystyle \sum _{i=2}^k{S}_i^2}}$$

(35)

where $k$ denotes the number of output pose estimation results. $S_i$ denotes the scale ratio [30] at time $t_i$

$$S_i=\{\begin{array}{ll}\frac{\Delta {\widehat{d}}_i}{\Delta d_i}-1 & \mathrm{for} \Delta {\widehat{d}}_i>\Delta d_i\\ -\left(\frac{\Delta d_i}{\Delta {\widehat{d}}_i}-1\right)& \mathrm{for}\Delta d_i>\Delta {\widehat{d}}_i\end{array}$$

(36)

where,

$$\begin{array}{l}\Delta {\widehat{d}}_i={}^{B_{i-1}}\widehat{p}{}_{B_i}^{\mathrm{T}}\cdot {}^{B_{i-1}}\widehat{p}{}_{B_i}^{}={\left({}^{G_B}\widehat{p}{}_{B_i}^{}-{}^{G_B}\widehat{p}{}_{B_{i-1}}^{}\right)}^{\mathrm{T}}\cdot \left({}^{G_B}\widehat{p}{}_{B_i}^{}-{}^{G_B}\widehat{p}{}_{B_{i-1}}^{}\right)\\ \Delta d_i={}^{B_{i-1}}p{}_{B_i}^{\mathrm{T}}\cdot {}^{B_{i-1}}p{}_{B_i}^{}={\left({}^{G_B}p{}_{B_i}^{}-{}^{G_B}p{}_{B_{i-1}}^{}\right)}^{\mathrm{T}}\cdot \left({}^{G_B}p{}_{B_i}^{}-{}^{G_B}p{}_{B_{i-1}}^{}\right)\end{array}$$

(37)

where ${}^{G_B}\widehat{p}{}_{B_i}^{}$ and ${}^{G_B}\widehat{p}{}_{B_{i-1}}^{}$ denote the estimated positions of {B} in {G_B} at time $t_i$ and $t_{i-1}$, respectively. ${}^{G_B}p{}_{B_i}^{}$ and ${}^{G_B}p{}_{B_{i-1}}^{}$ denote the ground-truth positions of {B} in {G_B} at time $t_i$ and $t_{i-1}$, respectively.

${}_{U_S}{}^{G_B}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 ${}_I{}^{B}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.

3.2.1. Observability and Consistency Comparison

Figure 4 and Figure 5 show the ground-truth pose errors and estimated $\pm 3\sigma$ bounds of ACK-MSCKF(1) and ACK-MSCKF(2) with time, respectively.

Figure 4 and Figure 5 show that the estimated three-axis position standard deviation of ACK-MSCKF(1) and ACK-MSCKF(2) with time for the VD06 dataset is close to zero, and the estimated three-axis attitude angle standard deviation tends to be constant with time. These results indicate that the three-axis vehicle position error state ${}^{G_B}\tilde{p}{}_{B_{}}^{}$ and attitude angle error state ${}_{G_B}{}^{B_{}}\tilde{\theta }$ are both observable. Actually, the vehicle relative kinematic error measurements do not provide information on the vehicle yaw angle and three-axis position in {G_B}. Therefore, ACK-MSCKF(1) and ACK-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\sigma$ with time.

Figure 6 and Figure 7 show the ground-truth pose errors and estimated $\pm 3\sigma$ bounds of ACK-MSCKF(3) and ACK-MSCKF(4) with time, respectively.

Similarly, it is seen from Figure 6 and Figure 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.

Figure 8 and Figure 9 show the ground-truth pose errors and estimated $\pm 3\sigma$ bounds of MAVIO(1) and MAVIO(2) with time, respectively.

It is concluded from Figure 8 and Figure 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 $\pm 3\sigma$ 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 $\pm 3\sigma$ is attributed to the robust implementation of S-MSCKF [12].

Figure 10 shows the ground-truth pose errors and estimated $\pm 3\sigma$ bounds of MAVIO-GNSS with time.

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. The average root mean square scale ratio (RMSSR) at the last time step on each experimental dataset.

Methods	VD01	VD02	VD03	VD04	VD05	VD06
MAVIO	1.29 (a)	1.29 (a,b)	1.14 (b)	1.42 (a)	1.42 (b)	1.63 (a)
ACK-MSCKF	1.21 (f)	1.11 (f)	0.98 (d,e,f)	1.31 (f)	1.21 (f)	1.46 (f)
S-MSCKF	1.68	1.38	1.22	1.99	1.57	2.08

^(a) From MAVIO(1); ^(b) From MAVIO(2); ^(c) From ACK-MSCKF(1); ^(d) From ACK-MSCKF(2); ^(e) From ACK-MSCKF(3); ^(f) From ACK-MSCKF(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.

3.2.2. Positioning Accuracy Comparison

Figure 11 shows the vehicle trajectory estimation results with a top view.

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.

Figure 12 demonstrates that the RTEPs of ACK-MSCKF and the proposed MAVIO are significantly reduced in comparison to the RTEP of S-MSCKF within the sub-trajectory length of 5 m for all of the datasets. Based on the average results for twenty rounds of experiments, the better performance of ATE among the different configurations along the whole trajectory for each experimental dataset is shown in

Table 5. The average absolute trajectory error (ATE) along the whole trajectory for each experimental dataset.

Methods	VD01 (m)	VD02 (m)	VD03 (m)	VD04 (m)	VD05 (m)	VD06 (m)
MAVIO	1.31 (b)	2.45 (b)	0.78 (a)	2.01 (b)	3.16 (b)	4.73 (b)
MAVIO-GNSS	1.22	2.27	0.79	1.67	3.59	3.28
ACK-MSCKF	1.60 (d)	2.57 (c)	0.87 (d)	2.46 (c,d)	3.67 (d)	3.80 (d)
S-MSCKF	9.78	2.31	2.44	14.83	4.25	21.76

^(a) From MAVIO(1); ^(b) From MAVIO(2); ^(c) From ACK-MSCKF(1); ^(d) From ACK-MSCKF(2); ^(e) From ACK-MSCKF(3); ^(f) From ACK-MSCKF(4).

, and

Table 6. The overall relative translation error (RTE) for all of the experimental datasets.

Sub-Trajectory Length (m)		Relative Translation Error (m)
	MAVIO	MAVIO-GNSS	ACK-MSCKF	S-MSCKF
1	0.08 (a, b)	0.08	0.08 (c, d)	0.22
5	0.23 (b)	0.21	0.26 (c, d)	0.93
10	0.40 (b)	0.35	0.44 (c, d)	1.71
20	0.70 (b)	0.60	0.76 (d)	3.00
50	1.51 (b)	1.29	1.51 (d)	5.57
100	2.79 (b)	2.29	2.57 (d)	8.33

^(a) From MAVIO(1); ^(b) From MAVIO(2); ^(c) From ACK-MSCKF(1); ^(d) From ACK-MSCKF(2); ^(e) From ACK-MSCKF(3); ^(f) From ACK-MSCKF(4).

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.

4. 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.

5. 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.