Hostname: page-component-7b9c58cd5d-9k27k Total loading time: 0 Render date: 2025-03-15T14:32:37.186Z Has data issue: false hasContentIssue false

Graph-optimisation-based self-calibration method for IMU/odometer using preintegration theory

Published online by Cambridge University Press:  13 January 2022

Shiyu Bai
Affiliation:
College of Automation Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing 211100, People's Republic of China
Jizhou Lai*
Affiliation:
College of Automation Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing 211100, People's Republic of China
Pin Lyu
Affiliation:
College of Automation Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing 211100, People's Republic of China
Yiting Cen
Affiliation:
College of Automation Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing 211100, People's Republic of China
Bingqing Wang
Affiliation:
College of Automation Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing 211100, People's Republic of China
Xin Sun
Affiliation:
College of Automation Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing 211100, People's Republic of China
*
*Corresponding author. E-mail: laijz@nuaa.edu.cn
Rights & Permissions [Opens in a new window]

Abstract

Determination of calibration parameters is essential for the fusion performance of an inertial measurement unit (IMU) and odometer integrated navigation system. Traditional calibration methods are commonly based on the filter frame, which limits the improvement of the calibration accuracy. This paper proposes a graph-optimisation-based self-calibration method for the IMU/odometer using preintegration theory. Different from existing preintegrations, the complete IMU/odometer preintegration model is derived, which takes into consideration the effects of the scale factor of the odometer, and misalignments in the attitude and position between the IMU and odometer. Then the calibration is implemented by the graph-optimisation method. The KITTI dataset and field experimental tests are carried out to evaluate the effectiveness of the proposed method. The results illustrate that the proposed method outperforms the filter-based calibration method. Meanwhile, the performance of the proposed IMU/odometer preintegration model is optimal compared with the traditional preintegration models.

Type
Research Article
Copyright
Copyright © The Author(s), 2022. Published by Cambridge University Press on behalf of The Royal Institute of Navigation

1 Introduction

The integration of an odometer and inertial measurement unit (IMU) has been widely studied for several decades (Georgy et al., Reference Georgy, Karamat, Iqbal and Noureldin2011; Li et al., Reference Li, Wang, Li, Gao and Tan2014; Nemec et al., Reference Nemec, Šimák, Janota, Hruboš and Bubeníková2019). The odometer is a cost-effective and easily deployed sensor for land vehicles and mobile robots. As a relative positioning sensor, it can provide speed and incremental distance along the vehicle trajectory (Wu et al., Reference Wu, Goodall and El-Sheimy2010). However, an odometer can be easily affected by road conditions and vehicle manoeuvres. When relative slippage between the wheels and road surface occurs, the accuracy of the odometer will decrease. Although the error of an IMU will accumulate over time, the IMU is completely self-contained. Thus, IMU/odometer fusion can deliver an enhanced performance over the individual system.

The performance of an IMU/odometer integrated navigation system is susceptible to some parameters. First, the scale factor of the odometer is the main source that affects the positioning accuracy. It differs at every manoeuvre and also slowly changes with the temperature, load and tyre pressure (Li et al., Reference Li, Sun, Yang, Ding, Wang, Jiang, Pu, Ren, Hu and Guo2018). Second, the IMU frame misaligns the odometer frame in attitude and position (Seo et al., Reference Seo, Lee, Lee and Park2006; Wang et al., Reference Wang, Fu, Deng and Ma2012; Gao et al., Reference Gao, Ge, Li, Shen, Zhang and Schuh2018), which is a common problem in real multisensory systems. Manual measurement is a typical calibration method, but some parameters such as attitude misalignment and scale factor are not easy to measure. Thus, an online self-calibration method without manual operations needs to be developed for practical purposes (Wang et al., Reference Wang, Chen, Li and Huang2019).

Nowadays, the self-calibration methods are mainly based on the Kalman filter frame. Wu (Reference Wu2014) designed a self-calibration method based on an extended Kalman filter (EKF), which can estimate the misalignment parameters and achieve good positioning performance. Gao et al. (Reference Gao, Li and Chen2017) fused a single-axis rotational IMU with an odometer based on a Kalman filter, which could estimate the scale factor of the odometer and simultaneously improve the navigation performance of a land vehicle. To suppress the influence of nonlinear effects, Liu et al. (Reference Liu, El-Sheimy and Qin2016) developed an unscented Kalman filter (UKF)-based fusion scheme, and the observability of different manoeuvrers was also discussed. Zhao et al. (Reference Zhao, Miao and Shao2017) proposed an adaptive two-stage Kalman filter for an IMU and odometer integrated navigation system, which could estimate the varying parameters and was insusceptible to the varying process noises. Zhang et al. (Reference Zhang, Hancock, Lau, Roberts and de Ligt2019) developed a tightly coupled integration to calibrate the parameters between the IMU and odometer and removed the effects of the odometer measurement outliers. The Kalman filter and its variants estimate the states based on measurements at the current instant and states at the last instant, which cannot take advantage of historical states and observations (Wen et al., Reference Wen, Bai, Kan and Hsu2019b).

In recent years, the graph-optimisation theory has been widely used in the robotics field (Strasdat et al., Reference Strasdat, Montiel and Davison2012; Mascaro et al., Reference Mascaro, Teixeira, Hinzmann, Siegwart and Chli2018). Different sensors are fused based on the optimal estimation of the historical and current states (Indelman et al., Reference Indelman, Williams, Kaess and Dellaert2013). Meanwhile, multiple iterations and re-linearisation can effectively help the graph-optimisation method to approach the optimal state estimation and improve the robustness compared with the filter-based method (Wen et al., Reference Wen, Kan and Hsu2019a). Some graph-optimisation-based methods using an odometer have been proposed (Chiu et al., Reference Chiu, Zhou, Carlone, Dellaert, Samarasekera and Kumar2014; Dang et al., Reference Dang, Wang and Pang2018). Since an odometer provides speed along the vehicle trajectory, the attitude of the vehicle platform needs to be used to adjust the measurement to the reference frame. In graph-optimisation-based methods, states at every optimisation instant are frequently adjusted. This results in the odometer speed needing to be repeatedly integrated, which comes at the cost of increased computational complexity. To solve this problem, preintegration theory (Forster et al., Reference Forster, Carlone, Dellaert and Scaramuzza2017; Yuan et al., Reference Yuan, Lai, Lyu, Shi, Zhao and Huang2018) was introduced into the odometer. He et al. (Reference He, Guo, Ye and Yuan2017) proposed a calibration method for the camera/odometer extrinsic parameters and derived the covariance matrix of the odometer preintegration model, but this method requires the angular velocity from a two-wheel differential odometer. Quan et al. (Reference Quan, Piao, Tan and Huang2019) coupled gyroscope measurements into the preintegration model, thus angular velocity provided by the two-wheel differential odometer was not required. To avoid the drift of horizontal attitudes, roll and pitch are set to zero in this method, but this is not suitable for land vehicles manoeuvring in uneven terrain. Liu et al. (Reference Liu, Gao and Hu2019) proposed an IMU and odometer preintegration model to be applied to outdoor vehicles, and extrinsic parameters between the IMU and odometer could be simultaneously calibrated. However, the scale factor was not included in the odometer preintegration model, which resulted in an additional positioning error. Similarly, misalignments were regarded as known quantities given by a filter-based calibration method in the preintegration model (Chang et al., Reference Chang, Niu and Liu2020) and could not be further optimised in the graph.

In this paper, a graph-optimisation-based self-calibration method for the IMU/odometer using preintegration theory is proposed. Different from the existing preintegrations, the complete IMU/odometer preintegration model is derived in which the influence of the scale factor of the odometer, and misalignments in attitude and position between the IMU and odometer are fully considered. With the aid of global positioning system (GPS) measurements, the system parameters are estimated using a graph-optimisation algorithm. The proposed algorithm is evaluated on KITTI datasets (Geiger et al., Reference Geiger, Lenz, Stiller and Urtasun2013) and real data from field tests. The results indicate that the proposed method can obtain highly accurate parameters estimation compared with the filter-based method and improves the navigation performance of the IMU/odometer fusion.

The rest of the paper is organised as follows: in Section 2, an overview of the system is described. The IMU/odometer preintegration model and graph optimisation fusion are stated in Section 3 and Section 4, respectively. The simulation and experiment results are given in Section 5.

2 Overview

The structure of the proposed method is shown in Figure 1. The system is composed of navigation sensors, model formulation and graph optimisation. The navigation sensors consist of an IMU, odometer and GPS. The IMU provides gyros and accelerometers measurements while the odometer gives velocity measurements, which are used to form the IMU/odometer preintegration. Preintegration is a relative measurement that connects two successive states. The GPS position serves as an absolute measurement. The relative and absolute measurements are then used for building a residual error model. Finally, the navigation states and sensors parameters are estimated by the graph optimisation. The estimated parameters are sent to the navigation sensors to compensate for errors.

Figure 1. The structure of the proposed method

The IMU and odometer are mounted on different parts of a land vehicle, as shown in Figure 2. The odometer is installed on the rear wheel whose heading does not change during a manoeuvre. Above all, some coordinate systems need to be built to describe the spatial relationships among different measurements.

Figure 2. IMU and odometer frame

(1) IMU frame

The gyros and accelerometers are measured in the IMU frame, denoted as the $b$ frame. The three axes of the IMU frame are expressed as ${x_b}$, ${y_b}$ and ${z_b}$, which point to rightward, forward and upward, respectively. The IMU is mounted at one point of the land vehicle, so it misaligns with the vehicle body in practical situations. The IMU frame at instant $t$ is denoted as the ${b_t}$ frame.

(2) Odometer frame

The odometer frame is fixed on the land vehicle, denoted as the $o$ frame. The three axes of the odometer frame are represented by ${x_o}$, ${y_o}$ and ${z_o}$. The velocity measured by the odometer is in the moving direction of the vehicle, along the ${y_o}$ direction. The odometer frame at instant $t$ is denoted as the ${o_t}$ frame.

(3) World frame

The world frame is denoted as the $w$ frame. The origin is located at the IMU at the initial instant and the three axes of the world frame point east, north and up, respectively. Meanwhile, some typical symbols that are used throughout the paper are defined in Table 1.

Table 1. Definitions of typical symbols

3 Derivation of IMU/odometer preintegration

In this paper, the IMU preintegration is extended by incorporating odometer measurements. Different from existing IMU/odometer preintegrations, the complete IMU/odometer preintegration model is derived in consideration of the scale factor of the odometer, and misalignments in attitude and position between the IMU and odometer. The models of raw gyroscopes and accelerometers are given by

(1)\begin{equation} \begin{aligned} {{\hat w}_{{b_t}}} & = {w_{{b_t}}} + {b_{{w_t}}} + {n_w}\\ {{\hat a}_{{b_t}}} & = {l_{{b_t}}} + {b_{{a_t}}} + R_w^{{b_t}}{g^{w}} + {n_a} \end{aligned} \end{equation}

where ${\hat w_{{b_t}}}$ and ${\hat a_{{b_t}}}$ are IMU readings at instant $t$; ${w_{{b_t}}}$ and ${l_{{b_t}}}$ denote the true value of the angular rate and motion acceleration at instant $t$; and ${b_{{w_t}}}$ and ${b_{{a_t}}}$ represent the gyro and accelerometer bias. The rotation matrix from world frame to the ${b_t}$ frame is denoted by $R_w^{{b_t}}$ and ${g^{w}}$ denotes the gravity expressed in world frame. The noise in the gyros ${n_w}$ and accelerometers ${n_a}$ is assumed as Gaussian, ${n_w} \sim {N}( {0,\sigma _{{w}}^{2}} )$, ${n_a} \sim {N}( {0,\sigma _{{a}}^{2}} )$. The gyro bias and accelerometer bias are modelled as random walk, whose derivatives are Gaussian, ${n_{{b_w}}} \sim {N}( {0,\sigma _{{b_w}}^{2}} )$, ${n_{{b_a}}} \sim {N}( {0,\sigma _{{b_a}}^{2}} )$ (Qin et al., Reference Qin, Li and Shen2018):

(2)\begin{equation} \begin{aligned} {{\dot b}_{{w_t}}} & = {n_{{b_w}}}\\ {{\dot b}_{{a_t}}} & = {n_{{b_a}}} \end{aligned} \end{equation}

The model of the odometer is given by

(3)\begin{equation} {s_{{o_t}}}{\hat v_{{o_t}}} = {v_{{o_t}}} + {n_s} \end{equation}

where ${\hat v_{{o_t}}}$ is the odometer readings at instant $t$ and ${v_{{o_t}}}$ denotes the true value of odometer at instant $t$. The raw odometer measurements are affected by the scale factor ${s_{{o_t}}}$ and noise ${n_s}$. The noise is assumed as Gaussian, ${n_s} \sim {N}( {0,\sigma _s^{2}} )$. The scale factor is modelled as random walk, whose derivative is Gaussian, $n_{{s_o}} \sim {N}(0,\sigma _{{s_o}}^{2})$:

(4)\begin{equation} \dot s_{{o_t}} = n_{{s_o}}\ \end{equation}

The position of the odometer expressed in the world frame $p_o^{w}$ is given by

(5)\begin{equation} p_o^{w} = p_b^{w} + R_b^{w}p_o^{b} \end{equation}

where $p_b^{w}$ denotes the position of the IMU expressed in the world frame, $R_b^{w}$ represents the rotation matrix from the IMU frame to world frame and $p_o^{b}$ is the position of the odometer expressed in the IMU frame, which is the misalignment in position.

Given two instants ${t_k}$ and ${t_{k + 1}}$, the position, velocity and orientation states in the world frame can be propagated by the IMU and odometer measurements, which can be formulated as

(6)\begin{equation} \begin{aligned} p_{{b_{k + 1}}}^{w} & = p_{{b_k}}^{w} + v_{{b_k}}^{w}\Delta {t_k} + \int {\int_{t \in [ {{t_k},{t_{k + 1}}} ]} {( {R_{{b_t}}^{w}( {{{\hat a}_{{b_t}}} - {b_{{a_t}}} - {n_a}}) - {g^{w}}} )\,d{t^{2}}} }\\ v_{{b_{k + 1}}}^{w} & = v_{{b_k}}^{w} + \int_{t \in [ {{t_k},{t_{k + 1}}}]} {( {R_{{b_t}}^{w}( {{{\hat a}_{{b_t}}} - {b_{{a_t}}} - {n_a}}) - {g^{w}}})} \,dt\\ q_{{b_{k + 1}}}^{w} & = q_{{b_k}}^{w} \otimes \int_{t \in [ {{t_k},{t_{k + 1}}}]} {\frac{1}{2}} \Omega ( {{{\hat w}_{{b_t}}} - {b_{{w_t}}} - {n_w}})q_{{b_t}}^{{b_k}}\,dt\\ p_{{b_{k + 1}}}^{w} & = p_{{b_k}}^{w} + R_{{b_k}}^{w}p_{{o_k}}^{{b_k}} + \int_{t \in [ {{t_k},{t_{k + 1}}} ]} {R_{{b_t}}^{w}R_{{o_t}}^{{b_t}}( {{s_{{o_t}}}{{\hat v}_{{o_t}}} - {n_s}} )\,dt} - R_{{b_{k + 1}}}^{w}p_{{o_{k + 1}}}^{{b_{k + 1}}} \end{aligned} \end{equation}

where

(7)\begin{equation} \Omega (w) = \left[\begin{array}{@{}cc@{}} - [w]_ \times & w\\ -w^{T} & 0 \end{array}\right],\quad [w]_ \times = \left[\begin{array}{ccc} 0 & { - {w_z}} & {{w_y}}\\ {{w_z}} & 0 & { - {w_x}}\\ { - {w_y}} & {{w_x}} & 0 \end{array} \right] \end{equation}

where $p_{{b_{k + 1}}}^{w}$ and $p_{{b_{k}}}^{w}$ denote the position of IMU expressed in the world frame at ${t_{k + 1}}$ and ${t_{k}}$, respectively; $v_{{b_{k + 1}}}^{w}$ and $v_{{b_{k}}}^{w}$ represent the velocity of the IMU expressed in the world frame at ${t_{k + 1}}$ and ${t_{k}}$, respectively; $q_{{b_{k + 1}}}^{w}$ and $q_{{b_{k}}}^{w}$ separately denote the quaternion from the ${b_{k + 1}}$ and ${b_{k}}$ frame to the world frame; $p_{{o_{k + 1}}}^{{b_{k + 1}}}$ and $p_{{o_k}}^{{b_k}}$ are misalignments in position at ${t_{k + 1}}$ and ${t_{k}}$; $R_{{b_{k + 1}}}^{w}$ and $R_{{b_{k}}}^{w}$ represent the rotation matrices from the ${b_{k + 1}}$ and ${b_{k}}$ frame to the world frame; $\Delta {t_k}$ is the duration between the time interval $[ {{t_k},{t_{k + 1}}} ]$; $R_{{b_t}}^{w}$ expresses the rotation matrix from the ${b_t}$ frame to the world frame; $R_{{o_t}}^{{b_t}}$ denotes the rotation matrix from the ${o_t}$ frame to the ${b_t}$ frame, which is the misalignment in attitude; and $q_{{b_t}}^{{b_k}}$ represents the quaternion from the ${b_t}$ frame to the ${b_k}$ frame.

The last equation in Equation (6) can be obtained from Equation 5) and the propagation of the position of the odometer expressed in the world frame, which can be formulated as

(8)\begin{equation} p_{{o_{k + 1}}}^{w} = p_{{o_k}}^{w} + \int_{t \in [ {{t_k},{t_{k + 1}}} ]} {R_{{b_t}}^{w}R_{{o_t}}^{{b_t}}( {{s_{{o_t}}}{{\hat v}_{{o_t}}} - {n_s}})\,dt} \end{equation}

where $p_{{o_{k + 1}}}^{w}$ and $p_{{o_{k}}}^{w}$ are the positions of the odometer expressed in the world frame at instants ${t_{k + 1}}$ and ${t_{k}}$, respectively.

It can be seen from Equation (6) that the propagation of the position from the odometer measurements requires the rotation and position at instant ${t_k}$. In the graph-optimisation-based algorithm, states at optimisation instants are frequently adjusted, which results in the odometer measurements needing to be re-propagated. To solve this problem, the preintegration theory is introduced into the odometer measurements.

The reference frame is changed from the world frame to the body frame at instant ${t_k}$. Equation (6) can be rewritten as

(9)\begin{equation} \begin{aligned} R_w^{{b_k}}p_{{b_{k + 1}}}^{w} & = R_w^{{b_k}}( {p_{{b_k}}^{w} + v_{{b_k}}^{w}\Delta {t_k} - \tfrac{1}{2}{g^{w}}\Delta t_k^{2}} ) + \alpha_{{b_{k + 1}}}^{{b_k}}\\ R_w^{{b_k}}v_{{b_{k + 1}}}^{w} & = R_w^{{b_k}}( {v_{{b_k}}^{w} - {g^{w}}\Delta {t_k}}) + \beta_{{b_{k + 1}}}^{{b_k}}\\ q_w^{{b_k}} \otimes q_{{b_{k + 1}}}^{w} & = \gamma_{{b_{k + 1}}}^{{b_k}}\\ R_w^{{b_k}}p_{{b_{k + 1}}}^{w} & = R_w^{{b_k}}p_{{b_k}}^{w} - R_w^{{b_k}}R_{{b_{k + 1}}}^{w}p_{{o_{k + 1}}}^{{b_{k + 1}}} + p_{{o_k}}^{{b_k}} + \eta_{{b_{k + 1}}}^{{b_k}} \end{aligned} \end{equation}

where

(10)\begin{equation} \begin{aligned} \alpha_{{b_{k + 1}}}^{{b_k}} & = \int {\int_{t \in [ {{t_k},{t_{k + 1}}} ]} {( {R_{{b_t}}^{{b_k}}( {{{\hat a}_{{b_t}}} - {b_{{a_t}}} - {n_a}} )} )} } \,d{t^{2}}\\ \beta_{{b_{k + 1}}}^{{b_k}} & = \int_{t \in [ {{t_k},{t_{k + 1}}} ]} {( {R_{{b_t}}^{{b_k}}( {{{\hat a}_{{b_t}}} - {b_{{a_t}}} - {n_a}} )} )} \,dt\\ \gamma_{{b_{k + 1}}}^{{b_k}} & = \int_{t \in [ {{t_k},{t_{k + 1}}} ]} {\frac{1}{2}} \Omega ( {{{\hat w}_{{b_t}}} - {b_{{w_t}}} - {n_w}} )\gamma_{{b_t}}^{{b_k}}\,dt\\ \eta_{{b_{k + 1}}}^{{b_k}} & = \int_{t \in [ {{t_k},{t_{k + 1}}} ]} {R_{{b_t}}^{{b_k}}R_{{o_t}}^{{b_t}}( {{s_{{o_t}}}{{\hat v}_{{o_t}}} - {n_s}} )\,dt} \end{aligned} \end{equation}

where $\alpha _{{b_{k + 1}}}^{{b_k}}$, $\beta _{{b_{k + 1}}}^{{b_k}}$ and $\gamma _{{b_{k + 1}}}^{{b_k}}$ denote the position, velocity and attitude preintegrations from the IMU measurements at instant ${t_{k + 1}}$ expressed in the ${b_k}$ frame; and $\eta _{{b_{k + 1}}}^{{b_k}}$ represents the position preintegration from the odometer measurements at instant ${t_{k + 1}}$ expressed in the ${b_k}$ frame. In Equation (10), the reference frame is ${b_k}$. Even though the states at instant ${t_k}$ are adjusted during the optimisation, the integrations in Equation (10) are not affected.

The discrete form of Equation (10) can be expressed as

(11)\begin{equation} \begin{aligned} \hat \alpha_{{b_{t + 1}}}^{{b_k}} & = \hat \alpha_{{b_t}}^{{b_k}} + \hat \beta_{{b_t}}^{{b_k}}\delta t + \tfrac{1}{2}\hat R_{{b_t}}^{{b_k}}( {{{\hat a}_{{b_t}}} - {{\hat b}_{{a_t}}}} )\delta {t^{2}}\\ \hat \beta_{{b_{t + 1}}}^{{b_k}} & = \hat \beta_{{b_t}}^{{b_k}} + \hat R_{{b_t}}^{{b_k}}( {{{\hat a}_{{b_t}}} - {{\hat b}_{{a_t}}}} )\delta t\\ \hat \gamma_{{b_{t + 1}}}^{{b_k}} & = \hat \gamma_{{b_t}}^{{b_k}} \otimes \left[\begin{array}{c} 1\\ \dfrac{1}{2}( {{{\hat w}_{{b_t}}} - {{\hat b}_{{w_t}}}} )\delta t \end{array} \right]\\ \hat \eta_{{b_t}}^{{b_k}} & = \hat \eta_{{b_t}}^{{b_k}} + \hat R_{{b_t}}^{{b_k}}\hat R_{{o_t}}^{{b_t}}{{\hat s}_{{o_t}}}{{\hat v}_{{o_t}}}\delta t \end{aligned} \end{equation}

where $\hat \alpha _{{b_{t + 1}}}^{{b_k}}$, $\hat \beta _{{b_{t + 1}}}^{{b_k}}$ and $\hat \gamma _{{b_{t + 1}}}^{{b_k}}$ are the preintegration from the IMU measurements at instant ${t_{t + 1}}$ expressed in the ${b_k}$ frame; $\hat \eta _{{b_{t + 1}}}^{{b_k}}$ denotes the preintegration from the odometer measurements at instant ${t_{t + 1}}$ expressed in the ${b_k}$ frame; $\hat \alpha _{{b_{t}}}^{{b_k}}$, $\hat \beta _{{b_{t}}}^{{b_k}}$ and $\hat \gamma _{{b_{t}}}^{{b_k}}$ are the preintegration from the IMU measurements at instant ${t_{t}}$ expressed in the ${b_k}$ frame; $\hat \eta _{{b_{t}}}^{{b_k}}$ denotes the preintegration from the odometer measurements at instant ${t_{t}}$ expressed in the ${b_k}$ frame; ${t_t},{t_{t + 1}} \in [ {{t_k},{t_{k + 1}}} ]$; $\delta t$ is the sampling interval of the IMU measurements; $\hat R_{{b_t}}^{{b_k}}$ represents the estimated rotation matrix from the ${b_t}$ frame to the ${b_k}$ frame; $\hat R_{{o_t}}^{{b_t}}$ denotes the estimated rotation matrix from the ${o_t}$ frame to the ${b_t}$ frame; and ${\hat b_{{a_t}}}$, ${\hat b_{{w_t}}}$ and ${\hat s_{{o_t}}}$ separately express the estimated accelerometers bias, gyros bias and scale factor of the odometer.

Then the continuous-time linearised dynamics of error term of Equation (10) can be written as

(12)\begin{align} \left[\begin{array}{@{}c@{}} {\delta \dot \alpha_{{b_t}}^{{b_k}}}\\ {\delta \dot \beta_{{b_t}}^{{b_k}}}\\ {\delta \dot \theta_{{b_t}}^{{b_k}}}\\ {\delta {{\dot b}_{{a_t}}}}\\ {\delta {{\dot b}_{{w_t}}}}\\ {\delta \dot \eta_{{b_t}}^{{b_k}}}\\ {\delta {{\dot s}_{{o_t}}}}\\ {\delta \dot p_{{o_t}}^{{b_t}}}\\ {\delta \dot \theta_{{o_t}}^{{b_t}}} \end{array} \right] & = \left[\begin{array}{ccccccccc} 0 & I & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & { - \hat R_{{b_t}}^{{b_k}}{{[ {{{\hat a}_{{b_t}}} - {{\hat b}_{{a_t}}}}]}_ \times }} & { -\hat R_{{b_t}}^{{b_k}}} & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & { - {{[ {{{\hat w}_{{b_t}}} - {{\hat b}_{{w_t}}}} ]}_ \times }} & 0 & { - I} & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & { - \hat R_{{b_t}}^{{b_k}}{{[ {\hat R_{{o_t}}^{{b_t}}{{\hat s}_{{o_t}}}{{\hat v}_{{o_t}}}} ]}_ \times }} & 0 & 0 & 0 & {\hat R_{{b_t}}^{{b_k}}\hat R_{{o_t}}^{{b_t}}{{\hat v}_{{o_t}}}} & 0 & { - \hat R_{{b_t}}^{{b_k}}\hat R_{{o_t}}^{{b_t}}{{[ {{{\hat s}_{{o_t}}}{{\hat v}_{{o_t}}}} ]}_ \times }}\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{array} \right]\left[\begin{array}{c} {\delta \alpha_{{b_t}}^{{b_k}}}\\ {\delta \beta_{{b_t}}^{{b_k}}}\\ {\delta \theta_{{b_t}}^{{b_k}}}\\ {\delta {b_{{a_t}}}}\\ {\delta {b_{{w_t}}}}\\ {\delta \eta_{{b_t}}^{{b_k}}}\\ {\delta {s_{{o_t}}}}\\ {\delta p_{{o_t}}^{{b_t}}}\\ {\delta \theta_{{o_t}}^{{b_t}}} \end{array} \right]\nonumber\\ & \quad + \left[\begin{array}{cccccccc} 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ { - \hat R_{{b_t}}^{{b_k}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & { - I} & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & I & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & I & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & { - \hat R_{{b_t}}^{{b_k}}\hat R_{{o_t}}^{{b_t}}} & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & I & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & I \end{array} \right]\left[ \begin{array}{c} {{n_a}}\\ {{n_w}}\\ {{n_{{b_a}}}}\\ {{n_{{b_w}}}}\\ {{n_s}}\\ {{n_{{s_o}}}}\\ {{n_{{p_o}}}}\\ {{n_{{\theta_o}}}} \end{array} \right]\nonumber\\ & = {F_t}\delta z_{{b_t}}^{{b_k}} + {G_t}{n_t} \end{align}

where ${F_t}$ and ${G_t}$ represent the state transition matrix and noise matrix; $\delta z_{{b_t}}^{{b_k}}$ denotes the preintegration error vector in which the preintegration errors and sensor parameter errors are included; and $\delta \theta _m^{n}$ is the rotation error, which can be obtained by the following:

(13)\begin{equation} R_m^{n} = \hat R_m^{n}( {I + {{[ {\delta \theta_m^{n}}]}_ \times }}) \end{equation}

In Equation (12), the error term of misalignments in position and attitude are modelled as random walk, whose derivatives are Gaussian, ${n_{{p_o}}} \sim {N}( {0,\sigma _{{p_o}}^{2}} )$, ${n_{{\theta _o}}} \sim {N}( {0,\sigma _{{\theta _o}}^{2}} )$. According to Equation (12), the discrete form of error term can be formulated as

(14)\begin{equation} \delta z_{{b_{i + 1}}}^{{b_k}} = {F_{{d_{i + 1}}}}\delta z_{{b_i}}^{{b_k}} + {G_{{d_{i + 1}}}}n \end{equation}

where ${F_{{d_{i + 1}}}}$ and ${G_{{d_{i + 1}}}}$ represent the discrete state transition matrix and noise matrix, respectively. With the assumption that ${t_k}$ and ${t_{k + 1}}$ are two instants, ${t_i},{t_{i + 1}}, \ldots \in [ {{t_k},{t_{k + 1}}} ]$. Then, the transfer equation from instant ${t_k}$ to instant ${t_{k + 1}}$ can be written as

(15)\begin{align} \delta z_{{b_{k + 1}}}^{{b_k}} & = {F_{{d_{k + 1}}}} \cdots {F_{{d_{i + 1}}}}{F_{{d_i}}}\delta z_{{b_k}}^{{b_k}} + \xi\nonumber\\ & = {J_{{b_{k + 1}}}}\delta z_{{b_k}}^{{b_k}} + \xi \end{align}

where $\xi$ represents the other items for simplicity. Thus, the recursive equation of the state transition matrix ${J_{{b_i}}}$ and covariance matrix ${P_{{d_i}}}$ can be expressed as

(16)\begin{equation} \begin{aligned} {J_{{b_{i + 1}}}} & = {F_{{d_{i + 1}}}}{J_{{b_i}}}\\ {P_{{b_{i + 1}}}} & = {F_{{d_{i + 1}}}}{P_{{b_i}}}F_{{d_{i + 1}}}^{T} + {G_{{d_{i + 1}}}}QG_{{d_{i + 1}}}^{T} \end{aligned} \end{equation}

where $Q$ denotes the covariance matrix of noise, which is given by

(17)\begin{equation} Q = \left[\begin{array}{cccccccc} {\sigma_a^{2}} & 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & {\sigma_w^{2}} & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & {\sigma_{{b_a}}^{2}} & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & {\sigma_{{b_w}}^{2}} & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & {\sigma_s^{2}} & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & {\sigma_{{s_o}}^{2}} & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & {\sigma_{{p_o}}^{2}} & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & {\sigma_{{\theta_o}}^{2}} \end{array} \right] \end{equation}

According to Equation (15), the following can be obtained:

(18)\begin{align} {J_{{b_{k + 1}}}} & = {F_{{d_{k + 1}}}} \cdots {F_{{d_{i + 1}}}}{F_{{d_i}}}\nonumber\\ & = {F_{{d_{k + 1}}}} \cdots {F_{{d_{i + 1}}}}{F_{{d_i}}}I\nonumber\\ & = {F_{{d_{k + 1}}}} \cdots {F_{{d_{i + 1}}}}{F_{{d_i}}}{J_{{b_k}}} \end{align}

In the preintegration method, the reference frame is the IMU frame at instant ${t_k}$, which is ${b_k}$. The error term of preintegration at instant ${t_k}$ is $\delta z_{{b_k}}^{{b_k}}$, which means the error relative to itself. Thus, the initial values of Equation (16) can be set as (Qin et al., Reference Qin, Li and Shen2018)

(19)\begin{equation} \begin{aligned} {J_{{b_k}}} & = I\\ {P_{{b_k}}} & = 0 \end{aligned} \end{equation}

Thus, the Jacobian matrix ${J_{{b_{k + 1}}}}$ and covariance matrix ${P_{{b_{k + 1}}}}$ at instant ${t_{k + 1}}$ can be calculated by combining Equations (16) and (19). The correction equation for preintegration measurements can be expressed as the following form:

(20)\begin{equation} \begin{aligned} \tilde \alpha_{{b_{k + 1}}}^{{b_k}} & = \hat \alpha_{{b_{k + 1}}}^{{b_k}} + J_{\delta {b_{{a_k}}}}^{\delta \alpha_{{b_{k + 1}}}^{{b_k}}}\delta {b_{{a_k}}} + J_{\delta {b_{{w_k}}}}^{\delta \alpha_{{b_{k + 1}}}^{{b_k}}}\delta {b_{{w_k}}}\\ \tilde \beta_{{b_{k + 1}}}^{{b_k}} & = \hat \beta_{{b_{k + 1}}}^{{b_k}} + J_{\delta {b_{{a_k}}}}^{\delta \beta_{{b_{k + 1}}}^{{b_k}}}\delta {b_{{a_k}}} + J_{\delta {b_{{w_k}}}}^{\delta \beta_{{b_{k + 1}}}^{{b_k}}}\delta {b_{{w_k}}}\\ \tilde \gamma_{{b_{k + 1}}}^{{b_k}} & = \hat \gamma_{{b_{k + 1}}}^{{b_k}} \otimes \left[ \begin{array}{c} 1\\ {\dfrac{1}{2}J_{\delta {b_{{w_k}}}}^{\delta \theta_{{b_{k + 1}}}^{{b_k}}}\delta {b_{{w_k}}}} \end{array} \right]\\ \tilde \eta_{{b_{k + 1}}}^{{b_k}} & = \hat \eta_{{b_{k + 1}}}^{{b_k}} + J_{\delta {b_{{w_k}}}}^{\delta \eta_{{b_{k + 1}}}^{{b_k}}}\delta {b_{{w_k}}} + J_{\delta {s_{{o_k}}}}^{\delta \eta_{{b_{k + 1}}}^{{b_k}}}\delta {s_{{o_k}}} + J_{\delta \theta_{{o_k}}^{b}}^{\delta \eta_{{b_{k + 1}}}^{{b_k}}}\delta \theta_{{o_k}}^{{b_k}} \end{aligned} \end{equation}

where $\hat \alpha _{{b_{k + 1}}}^{{b_k}}$, $\hat \beta _{{b_{k + 1}}}^{{b_k}}$, $\hat \gamma _{{b_{k + 1}}}^{{b_k}}$ and $\hat \eta _{{b_{k + 1}}}^{{b_k}}$ are the calculated preintegration, while $\tilde \alpha _{{b_{k + 1}}}^{{b_k}}$, $\tilde \beta _{{b_{k + 1}}}^{{b_k}}$, $\tilde \gamma _{{b_{k + 1}}}^{{b_k}}$ and $\tilde \eta _{{b_{k + 1}}}^{{b_k}}$ are the modified preintegration. Here, $J_{\delta {b_{{a_k}}}}^{\delta \alpha _{{b_{k + 1}}}^{{b_k}}}$ and $J_{\delta {b_{{w_k}}}}^{\delta \alpha _{{b_{k + 1}}}^{{b_k}}}$ are the Jacobian matrices of $\delta {b_{{a_k}}}$ and $\delta {b_{{w_k}}}$ corresponding to $\delta \alpha _{{b_{k + 1}}}^{{b_k}}$, respectively; $J_{\delta {b_{{a_k}}}}^{\delta \beta _{{b_{k + 1}}}^{{b_k}}}$ and $J_{\delta {b_{{w_k}}}}^{\delta \beta _{{b_{k + 1}}}^{{b_k}}}$ denote the Jacobian matrices of $\delta {b_{{a_k}}}$ and $\delta {b_{{w_k}}}$ corresponding to $\delta \beta _{{b_{k + 1}}}^{{b_k}}$; $J_{\delta {b_{{w_k}}}}^{\delta \theta _{{b_{k + 1}}}^{{b_k}}}$ represents the Jacobian matrix of $\delta {b_{{w_k}}}$ corresponding to $\delta \theta _{{b_{k + 1}}}^{{b_k}}$; and $J_{\delta {b_{{w_k}}}}^{\delta \eta _{{b_{k + 1}}}^{{b_k}}}$, $J_{\delta {s_{{o_k}}}}^{\delta \eta _{{b_{k + 1}}}^{{b_k}}}$ and $J_{\delta \theta _{{o_k}}^{b}}^{\delta \eta _{{b_{k + 1}}}^{{b_k}}}$ are the Jacobian matrices of $\delta {b_{{w_k}}}$, $\delta {s_{{o_k}}}$ and $\delta \theta _{{o_k}}^{b}$ corresponding to $\delta \eta _{{b_{k + 1}}}^{{b_k}}$. The above matrices are a sub-block matrix of ${J_{{b_{k + 1}}}}$, which can be obtained from ${J_{{b_{k + 1}}}}$.

The IMU/odometer measurement model in preintegration form can be expressed as

(21)\begin{equation} \left[\begin{array}{c} {\hat \alpha_{{b_{k + 1}}}^{{b_k}}}\\ {\hat \gamma_{{b_{k + 1}}}^{{b_k}}}\\ {\hat \beta_{{b_{k + 1}}}^{{b_k}}}\\ 0\\ 0\\ {\hat \eta_{{b_{k + 1}}}^{{b_k}}}\\ 0\\ 0\\ 0 \end{array}\right] = \left[\begin{array}{c} {R_w^{{b_k}}( {p_{{b_{k + 1}}}^{w} - p_{{b_k}}^{w} - v_{{b_k}}^{w}\Delta {t_k} + \dfrac{1}{2}{g^{w}}\Delta t_k^{2}})}\\ {{{( {q_{{b_k}}^{w}} )}^{ - 1}} \otimes q_{{b_{k + 1}}}^{w}}\\ {R_w^{{b_k}}( {v_{{b_{k + 1}}}^{w} - v_{{b_k}}^{w} + {g^{w}}\Delta {t_k}})}\\ {{b_{{a_{k + 1}}}} - {b_{{a_k}}}}\\ {{b_{{w_{k + 1}}}} - {b_{{w_k}}}}\\ {R_w^{{b_k}}( {p_{{b_{k + 1}}}^{w} - p_{{b_k}}^{w} + R_{{b_{k + 1}}}^{w}p_{{o_{k + 1}}}^{{b_{k + 1}}}}) - p_{{o_k}}^{{b_k}}}\\ {{s_{{o_{k + 1}}}} - {s_{{o_k}}}}\\ {p_{{o_{k + 1}}}^{{b_{k + 1}}} - p_{{o_k}}^{{b_k}}}\\ {2{{[ {{{( {q_{{o_k}}^{{b_k}}} )}^{ - 1}} \otimes q_{{o_{k + 1}}}^{{b_{k + 1}}}} ]}_{xyz}}} \end{array}\right] \end{equation}

The accelerometer bias, gyroscope bias, scale factor, and misalignments in position and attitude between the IMU and odometer are assumed to be constant in a short period, thus the differences between consecutive instants for these terms are zeros.

4 Graph optimisation based on IMU/odometer preintegration

The sensor fusion problem can be represented as a graph model to perform the nonlinear optimisation. There are two types of nodes, factor nodes and variable nodes. The edge connects the factor nodes and variable nodes when the factor involves a corresponding variable. A factor describes an error between the predicted and actual measurements. With an assumption of a Gaussian noise model, a factor can be written as

(22)\begin{equation} f( X ) = \| {z - h( X )} \|_P^{2} \end{equation}

where $X$ is the state; $z$ and $h( X )$ separately denote the actual measurements and predicted measurements; and $P$ represents the covariance matrix. The Mahalanobis norm is defined as $\| r \|_P^{2} = {r^{T}}{P^{ - 1}}r$ (Dellaert and Kaess, Reference Dellaert and Kaess2017).

To solve the nonlinear least-squares optimisation, the entire graph encoded by all the factors should be minimised by adjusting the estimates of the state variables. Graph optimisation is equal to finding a set of states that satisfies all the constraints. This is equivalent to solving the following equation:

(23)\begin{equation} \hat X = \underset{X}{\arg \min} \left( {\prod_i {{f_i}( X )} } \right) \end{equation}

where $\hat X$ is the optimal solution sets and ${f_i}( X )$ denotes the $i$th factor.

The nonlinear optimisation problem in Equation (23) is solved by repeated linearisation within a nonlinear least-squares solver. Staring from the initial value ${x_0}$, the solver finds an update $\Delta$, which can be formulated as

(24)\begin{equation} \arg \min ( {J( {{x_0}} )\Delta - b( {{x_0}} )}) \end{equation}

where $J( {{x_0}} )$ denotes the Jacobian matrix at the current linearisation point ${x_0}$, while $b( {{x_0}} ) = z - h( {{x_0}} )$ is the residual when the initial value is substituted into the predicted model. After solving Equation (24), the state variable is updated by ${x_0} + \Delta$. Then, the iterative process continues until the termination condition is met.

The graph structure of the proposed method is shown in Figure 3. The circle denotes the variable node, which is generated every time a new GPS measurement is available. The rectangle represents the factor node, which is derived from the measurement. The edge can exist between the variable node and factor node when the factor involves the variable. The IMU/odometer factor constrains two consecutive states by preintegration. For a GPS measurement, its factor constrains one state since it is observed at one instant.

Figure 3. Graph structure of the proposed method

The state vector in the proposed method is designed as

(25)\begin{equation} \begin{aligned} X & = [ {{x_0},{x_1}, \ldots ,{x_n}} ]\\ {x_k} & = [ {p_{{b_k}}^{w},v_{{b_k}}^{w},q_{{b_k}}^{w},{b_{{a_k}}},{b_{{w_k}}},{s_{{o_k}}},p_{{o_k}}^{{b_k}},q_{{o_k}}^{{b_k}}} ],\quad k \in[ {0,n}] \end{aligned} \end{equation}

where $X$ stands for the states set and ${x_k}$ represents the state at instant ${t_k}$, which includes the position, velocity, rotation, accelerometer bias, gyroscope bias, scale factor, and misalignments in position and attitude. Here, $n$ denotes the number of states in the sliding window.

According to Equation (21), the residual of the IMU/odometer preintegration is given by

(26)\begin{equation} r_{\textrm{I/O}}( {\hat z_{{b_{k + 1}}}^{{b_k}},X} ) = \left[ \begin{array}{c} {R_w^{{b_k}}( {p_{{b_{k + 1}}}^{w} - p_{{b_k}}^{w} - v_{{b_k}}^{w}\Delta {t_k} + \dfrac{1}{2}{g^{w}}\Delta t_k^{2}} ) - \hat \alpha_{{b_{k + 1}}}^{{b_k}}}\\ {2{{[ {{{( {\hat \gamma_{{b_{k + 1}}}^{{b_k}}})}^{ - 1}} \otimes {{( {q_{{b_k}}^{w}} )}^{ - 1}} \otimes q_{{b_{k + 1}}}^{w}} ]}_{xyz}}}\\ {R_w^{{b_k}}( {v_{{b_{k + 1}}}^{w} - v_{{b_k}}^{w} + {g^{w}}\Delta {t_k}}) - \hat \beta_{{b_{k + 1}}}^{{b_k}}}\\ {{b_{{a_{k + 1}}}} - {b_{{a_k}}}}\\ {{b_{{w_{k + 1}}}} - {b_{{w_k}}}}\\ {( {R_w^{{b_k}}p_{{b_{k + 1}}}^{w} - R_w^{{b_k}}p_{{b_k}}^{w} + R_w^{{b_k}}R_{{b_{k + 1}}}^{w}p_{{o_{k + 1}}}^{{b_{k + 1}}} - p_{{o_k}}^{{b_k}}} ) - \hat \eta_{{b_{k + 1}}}^{{b_k}}}\\ {{s_{{o_{k + 1}}}} - {s_{{o_k}}}}\\ {p_{{o_{k + 1}}}^{{b_{k + 1}}} - p_{{o_k}}^{{b_k}}}\\ {2{{[ {{{( {q_{{o_k}}^{{b_k}}} )}^{ - 1}} \otimes q_{{o_{k + 1}}}^{{b_{k + 1}}}}]}_{xyz}}} \end{array} \right] \end{equation}

The residual of the GPS measurements is formulated as

(27)\begin{align} {r_{\textrm{GPS}}}( {{{\hat z}_{\textrm{GPS}}},X} ) & = z_k^{\textrm{GPS}} - h_k^{\textrm{GPS}}( X )\nonumber\\ & = p_{{b_k}}^{\textrm{GPS}} - p_{{b_k}}^{w} \end{align}

where $z_k^{\textrm {GPS}}$ stands for the observation information at instant ${t_k}$ from the GPS receiver; $h_k^{\textrm {GPS}}( X )$ represents the observation model; and $p_{{b_k}}^{\textrm {GPS}}$ and $p_{{b_k}}^{w}$ represent the position of the GPS receiver and state, respectively.

Based on the residual of the IMU/odometer preintegration and GPS, the error function of the proposed method can be formulated as

(28)\begin{equation} \min_X \left\{ {{{\| {{r_P} - {H_P}X} \|}^{2}} + \sum_{k - n}^{k} {\| {{r_{\textrm{I/O}}}( {\hat z_{{b_{k + 1}}}^{{b_k}},X} )} \|_{P_{{b_{k + 1}}}^{{b_k}}}^{2}} + \sum_{k - n}^{k} {\| {{r_{\textrm{GPS}}}( {{{\hat z}_{\textrm{GPS}}},X} )} \|_{{P_G}}^{2}} } \right\} \end{equation}

where ${\| {{r_P} - {H_P}X} \|^{2}}$ is the prior information from marginalisation. The lower and upper limit of the summation denote the instant ${t_{k - n}}$ and instant ${t_k}$, respectively. Ceres Solver is used for solving Equation (28).

5 Experimental results

Dataset and experimental tests are executed to evaluate the performance of the proposed method compared with the Kalman filter based calibration method of Li et al. (Reference Li, Sun, Yang, Ding, Wang, Jiang, Pu, Ren, Hu and Guo2018). The graph optimisation formulated in this paper is a sliding window graph optimisation in which only past and current measurements are used, thus it can be compared with the Kalman filter under such a condition. Meanwhile, the positioning accuracy of the IMU and odometer fusion using parameters obtained from the proposed method is compared with those which employ parameters given by the preintegration models mentioned by Liu et al. (Reference Liu, Gao and Hu2019) and Chang et al. (Reference Chang, Niu and Liu2020).

5.1 Dataset results

Raw data from the KITTI dataset is first implemented to validate the effectiveness of the proposed calibration method. The KITTI datasets provide the angular rate and acceleration from IMU measurements, and the position and velocity from GPS measurements. The scale factor and misalignments in attitude and position are manually applied into the raw data to simulate the odometer and misaligned IMU measurements.

The manoeuvres of the vehicle in the KITTI dataset involves turning, accelerating and decelerating. According to the observability analysis of Liu et al. (Reference Liu, El-Sheimy and Qin2016), odometer scale factor, $x$-axis and $z$-axis misalignments in attitude, and $x$-axis and $y$-axis misalignments in position are observable. Thus, the estimation results of the above parameters are compared. Monte Carlo simulations are performed 50 times for the simulation, and the mean value and root-mean-square error (RMSE) of 50 simulations are listed in Table 2. The specific setup of the simulation parameters in one simulation is shown in Table 3Figures 46 describe the comparisons of estimation results of parameters in one simulation, the blue dotted line represents the real value which is added into the raw data, the red dot–dash line denotes the estimation result of filter-based calibration method and the green solid line stands for the estimation results of the proposed calibration method.

Table 2. Statistical results in the simulation

Table 3. Setup of simulation parameters in KITTI datasets

Figure 4. Comparison of scale factor of odometer

Figure 5. Comparison of misalignments in attitude: (a) $X$-axis misalignment in attitude; (b) $Z$-axis misalignment in attitude

Figure 6. Comparison of misalignments in position: (a) $X$-axis misalignment in position; (b) $Y$-axis misalignment in position

From Table 2 and Figures 46, it could be noticed that the estimation results of the proposed method present better convergence, which are closer to the true values than the filter-based method.

To further validate the performance of the proposed method, the mean values of the estimation results after convergence are calculated and applied into the IMU and odometer fusion. Smaller accumulative errors of the IMU and odometer fusion meant more accurate calibration results. The trajectory comparison is shown in Figure 7. It shows that the trajectory of the IMU and odometer fusion using calibration parameters from the proposed method is closer to the reference compared with the filter-based method. Figure 8 shows a comparison of the horizontal position error. The blue dot–dash line denotes the horizontal position error of the IMU and odometer fusion using calibration results of the filter-based method, and the red solid line represents the horizontal position error of the IMU and odometer fusion using calibration results of the proposed method. The latter obtained a higher positioning accuracy, which also illustrates the proposed method can achieve better calibration performance.

Figure 7. The trajectory comparison of IMU/odometer fusion using parameters given by the filter-based method and proposed method

Figure 8. The horizontal position error comparison of IMU/odometer fusion using parameters given by the filter-based method and proposed method

The positioning accuracy of the IMU and odometer fusion using parameters given by the preintegration models of Liu et al. (Reference Liu, Gao and Hu2019) and Chang et al. (Reference Chang, Niu and Liu2020) are also compared with the proposed method. A comparison of the trajectory is given in Figure 9 and the position error is shown in Figure 10. The blue dotted line denotes the horizontal position error of the IMU and odometer fusion using parameters from the IMU/odometer preintegration 1 mentioned by Liu et al. (Reference Liu, Gao and Hu2019) and the red dot–dash line stands for the horizontal position error using parameters from the IMU/odometer preintegration 2 mentioned by Chang et al. (Reference Chang, Niu and Liu2020). In Figure 10, the green solid line signifies the horizontal position error using parameters from the proposed method. In preintegration 1, the scale factor of the odometer is not involved in the optimisation process. The scale factor is the main source that affects the positioning accuracy of the odometer. If the scale factor is not estimated and compensated, the distance calculated by the odometer is inaccurate. In preintegration 2, misalignments in the attitude and position between the IMU and odometer are calculated by a Kalman filter and directly substituted into the preintegration. The calibration accuracy from the filter is lower than that from the graph-optimisation method. Therefore, we can see that the proposed method achieves better positioning performance.

Figure 9. The trajectory comparison of IMU/odometer fusion using parameters given by traditional preintegration methods and the proposed method

Figure 10. The horizontal position error comparison of IMU/odometer fusion using parameters given by traditional preintegration methods and the proposed method

Figure 11. Baidu autonomous driving development kit

5.2 Field test results

Field tests are designed to implement further evaluation of the proposed method. The test system consists of a GPS/micro-electromechanical systems (MEMS) IMU and an odometer, which are carried on a Baidu autonomous driving development kit  (Figure 11). The lever arms between the IMU and GPS have been compensated in advance. The specifications of the sensors are shown in Tables 4 and 5. In the tests, the base station service Qianxun Spatial Intelligence is used to provide a continuous RTK solution with centimetre accuracy. The GPS positioning information is collected at a rate of 1 Hz while the IMU and odometer raw measurements are logged at a sampling rate of 50 Hz. The trajectory is shown in Figure 12. In the field experimental tests, the manoeuvres of the vehicle also include turning, accelerating and decelerating, which guarantees the required parameters are observable.

Figure 12. The trajectory of the experimental tests

Table 4. IMU sensor specifications

Table 5. GPS sensor specifications

Figures 1315 describe the comparisons of the estimation results of parameters, where the blue dot–dash line denotes the estimation results of the filter-based calibration method and the red solid line represents the estimation results of the proposed method. It could be noticed that there are larger fluctuations from 220 s to 240 s in the filter-based calibration method. This is due to the outliers of the GPS measurements. The Kalman filter estimates the states based on the measurements at the current instant and states at the last instant, which is sensitive to unexpected outliers. The proposed graph-optimisation-based calibration method takes advantage of the previous information, thus the robustness of the system is increased.

Figure 13. Comparison of scale factor of odometer

Figure 14. Comparison of misalignments in attitude: (a) $X$-axis misalignment in attitude; (b) $Z$-axis misalignment in attitude

Figure 15. Comparison of misalignments in position: (a) $X$-axis misalignment in position; (b) $Y$-axis misalignment in position

Due to the lack of reference of parameters in the experimental tests, the positioning performance of the IMU and odometer fusion is analysed. The estimation results from different methods are applied into the integrated system. Smaller accumulative errors of the IMU and odometer fusion will result in more accurate calibration results. The RTK solution serves as a reference trajectory to evaluate the position error. It can be seen from the above estimation results that the sensor parameters converge to stable ones at the end and there are little variations. Thus, the mean values of the estimation results after convergence are calculated and applied into the IMU and odometer fusion.

The comparison of the horizontal position error is given in Figure 16. The blue dot–dash line denotes the horizontal position error of the IMU and odometer fusion using calibration results of the filter-based method, and the red solid line represents the horizontal position error of the IMU and odometer fusion using calibration results of the proposed method. This indicates that the proposed method can achieve a better calibration performance.

Figure 16. The horizontal position error comparison of IMU/odometer fusion using parameters given by the filter-based method and proposed method

The positioning accuracy of the IMU and odometer fusion using parameters given by preintegration1 and preintegration 2 are also compared with the proposed method, which is shown in Figure 17. The blue dotted line denotes the horizontal position error of the IMU and odometer fusion using parameters from IMU/odometer preintegration 1, the red dot–dash line stands for the horizontal position error using parameters from IMU/odometer preintegration 2 and the green solid line signifies the horizontal position error using parameters from the proposed method. The scale factor of the odometer is not considered in preintegration 1, which results in a large positioning error. In addition, the misalignments in attitude given by the Kalman filter are directly substituted into the model in preintegration 2, thereby the positioning performance is also affected. We can see that the performance of the proposed method is superior to the existing preintegrations.

Figure 17. The horizontal position error comparison of IMU/odometer fusion using parameters given by the filter-based method and proposed method

6 Conclusion

A graph-optimisation-based self-calibration method for the IMU and odometer system using preintegration theory is proposed in this paper. The complete IMU/odometer preintegration model is derived, which considers the influence of the scale factor of the odometer, and misalignments in attitude and position between the IMU and odometer. With the aid of GPS measurements, the system parameters are estimated using a graph-optimisation algorithm. The KITTI dataset and field experimental tests are carried out to evaluate the proposed method. The results illustrate that the estimation accuracy for calibration is improved compared with the filter-based method. The performance of the IMU and odometer fusion is also evaluated by taking parameters obtained from different IMU/odometer preintegration models. This indicates that the proposed model can fully calibrate sensor parameters, thereby improves the positioning accuracy of the IMU and odometer integrated system.

Acknowledgments

This work is partially supported by National Natural Science Foundation of China (61973160, 61703207), Aeronautical Science Foundation of China (2018ZC52037, 2017ZC52017), Civil Aircraft Project of Ministry of Industry and Information (2018-S-36), and Natural Science Foundation of Jiangsu Province (BK20170801).

References

Chang, L., Niu, X. J. and Liu, T. Y. (2020). GNSS/IMU/ODO/LiDAR-SLAM integrated navigation system using IMU/ODO pre-integration. Sensors, 20(17), 4702.CrossRefGoogle ScholarPubMed
Chiu, H. P., Zhou, X. S., Carlone, L., Dellaert, F., Samarasekera, S. and Kumar, R. (2014). Constrained Optimal Selection for Multi-sensor Robot Navigation using Plug-and-Play Factor Graphs. 2014 IEEE International Conference on Robotics and Automation (ICRA). Hong Kong, China, 663–670.CrossRefGoogle Scholar
Dang, Z. Q., Wang, T. M. and Pang, F. M. (2018). Tightly-coupled Data Fusion of VINS and Odometer based on Wheel Slip Estimation. 2018 IEEE International Conference on Robotics and Biomimetics (ROBIO). Kuala Lumpur, Malaysia, 1613–1619.CrossRefGoogle Scholar
Dellaert, F. and Kaess, M. (2017). Factor graphs for robot perception. Foundations and Trends ® in Robotics, 6(1–2), 1139.CrossRefGoogle Scholar
Forster, C., Carlone, L., Dellaert, F. and Scaramuzza, D. (2017). On-Manifold Preintegration for Real-Time Visual--Inertial Odometry. IEEE Transactions on Robotics, 33(1), 121.CrossRefGoogle Scholar
Gao, J. X., Li, K. and Chen, Y. P. (2017). Study on integration of FOG single-axis rotational INS and odometer for land vehicle. IEEE Sensors Journal, 18(2), 752763.CrossRefGoogle Scholar
Gao, Z. Z., Ge, M. R., Li, Y., Shen, W. B., Zhang, H. P. and Schuh, H. (2018). Railway irregularity measuring using Rauch-Tung-Striebel smoothed multi-sensors fusion system: Quad-GNSS PPP, IMU, odometer, and track gauge. GPS Solutions, 22(2), 36.CrossRefGoogle Scholar
Geiger, A., Lenz, P., Stiller, C. and Urtasun, R. (2013). Vision meets robotics: The kitti dataset. The International Journal of Robotics Research, 32(11), 12311237.CrossRefGoogle Scholar
Georgy, J., Karamat, T., Iqbal, U. and Noureldin, A. (2011). Enhanced MEMS-IMU/odometer/GPS integration using mixture particle filter. GPS solutions, 15(3), 239252.CrossRefGoogle Scholar
He, Y. J., Guo, Y., Ye, A. X. and Yuan, K. (2017). Camera-Odometer Calibration and Fusion using Graph based Optimization. 2017 IEEE International Conference on Robotics and Biomimetics (ROBIO). Macau, China, 1624–1629.CrossRefGoogle Scholar
Indelman, V., Williams, S., Kaess, M. and Dellaert, F. (2013). Information fusion in navigation systems via factor graph based incremental smoothing. Robotics and Autonomous Systems, 61(8), 721738.CrossRefGoogle Scholar
Li, Z. K., Wang, J., Li, B. H., Gao, J. X. and Tan, X. L. (2014). GPS/INS/Odometer integrated system using fuzzy neural network for land vehicle navigation applications. The Journal of Navigation, 67(6), 967.CrossRefGoogle Scholar
Li, L. L., Sun, H. X., Yang, S., Ding, X. W., Wang, J., Jiang, J. L., Pu, X. H., Ren, C. H., Hu, N. and Guo, Y. C. (2018). Online calibration and compensation of total odometer error in an integrated system. Measurement, 123, 6979.CrossRefGoogle Scholar
Liu, Z. B., El-Sheimy, N. and Qin, Y. Y. (2016). Low-Cost INS/Odometer Integration and Sensor-to-Sensor Calibration for Land Vehicle Applications. Proceedings of the IAG/CPGPS International Conference on GNSS+(ICG+ 2016). Shanghai, China.Google Scholar
Liu, J. X., Gao, W. and Hu, Z. Y. (2019). Visual-inertial Odometry Tightly Coupled with Wheel Encoder Adopting Robust Initialization and Online Extrinsic Calibration. 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Macau, China, 5391–5397.CrossRefGoogle Scholar
Mascaro, R., Teixeira, L., Hinzmann, T., Siegwart, R. and Chli, M. (2018). GOMSF: Graph-Optimization based Multi-Sensor Fusion for Robust UAV Pose Estimation. 2018 IEEE International Conference on Robotics and Automation (ICRA). Brisbane, Australia, 1421–1428.Google Scholar
Nemec, D., Šimák, V., Janota, A., Hruboš, M. and Bubeníková, E. (2019). Precise localization of the mobile wheeled robot using sensor fusion of odometry, visual artificial landmarks and inertial sensors. Robotics and Autonomous Systems, 112, 168177.CrossRefGoogle Scholar
Qin, T., Li, P. L. and Shen, S. J. (2018). Vins-mono: A robust and versatile monocular visual-inertial state estimator. IEEE Transactions on Robotics, 34(4), 10041020.CrossRefGoogle Scholar
Quan, M. X., Piao, S. H., Tan, M. L. and Huang, S. S. (2019). Tightly-coupled monocular visual-odometric SLAM using wheels and a MEMS gyroscope. IEEE Access, 7, 9737497389.CrossRefGoogle Scholar
Seo, J. W., Lee, H. K., Lee, J. G. and Park, C. G. (2006). Lever arm compensation for GPS/INS/odometer integrated system. International Journal of Control, Automation, and Systems, 4(2), 247254.Google Scholar
Strasdat, H., Montiel, J. M. M. and Davison, A. J. (2012). Visual SLAM: Why filter? Image and Vision Computing, 30(2), 6577.CrossRefGoogle Scholar
Wang, Q. Z., Fu, M. Y., Deng, Z. H. and Ma, H. B. (2012). A Comparison of Loosely-Coupled Mode and Tightly-Coupled Mode for INS/VMS. American Control Conference (ACC). Montréal, Canada, 6346–6351.Google Scholar
Wang, X. F., Chen, H. Y., Li, Y. J. and Huang, H. L. (2019). Online extrinsic parameter calibration for robotic camera–encoder system. IEEE Transactions on Industrial Informatics, 15(8), 46464655.CrossRefGoogle Scholar
Wen, W. S, Kan, Y. C. and Hsu, L. T. (2019a). Performance Comparison of GNSS/INS Integrations Based on EKF and Factor Graph Optimization. Proceedings of the 32nd International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS+ 2019). Miami, USA, 3019-3032.Google Scholar
Wen, W. S., Bai, X. W., Kan, Y. C. and Hsu, L. T. (2019b). Tightly coupled GNSS/INS integration via factor graph and aided by fish-eye camera. IEEE Transactions on Vehicular Technology, 68(11), 1065110662.CrossRefGoogle Scholar
Wu, Y. (2014). Versatile Land Navigation using Inertial Sensors and Odometry: Self-Calibration, In-motion Alignment and Positioning. 2014 DGON Inertial Sensors and Systems (ISS). Karlsruhe, Germany, 1–19.Google Scholar
Wu, Y. X., Goodall, C. and El-Sheimy, N. (2010). Self-calibration for IMU/Odometer Land Navigation: Simulation and Test Results. Proceedings of the 2010 International Technical Meeting of The Institute of Navigation, USA: San Diego, 839849.Google Scholar
Yuan, C., Lai, J. Z., Lyu, P., Shi, P., Zhao, W. and Huang, K. (2018). A novel fault-tolerant navigation and positioning method with stereo-camera/micro electro mechanical systems inertial measurement unit (MEMS-IMU) in hostile environment. Micromachines, 9(12), 626.CrossRefGoogle Scholar
Zhang, P. H., Hancock, C. M., Lau, L., Roberts, G. W. and de Ligt, H. (2019). Low-cost IMU and odometer tightly coupled integration with Robust Kalman filter for underground 3-D pipeline mapping. Measurement, 137, 454463.CrossRefGoogle Scholar
Zhao, H. S., Miao, L. J. and Shao, H. J. (2017). Adaptive two-stage Kalman filter for SINS/odometer integrated navigation systems. The Journal of Navigation, 70(2), 242261.CrossRefGoogle Scholar
Figure 0

Figure 1. The structure of the proposed method

Figure 1

Figure 2. IMU and odometer frame

Figure 2

Table 1. Definitions of typical symbols

Figure 3

Figure 3. Graph structure of the proposed method

Figure 4

Table 2. Statistical results in the simulation

Figure 5

Table 3. Setup of simulation parameters in KITTI datasets

Figure 6

Figure 4. Comparison of scale factor of odometer

Figure 7

Figure 5. Comparison of misalignments in attitude: (a) $X$-axis misalignment in attitude; (b) $Z$-axis misalignment in attitude

Figure 8

Figure 6. Comparison of misalignments in position: (a) $X$-axis misalignment in position; (b) $Y$-axis misalignment in position

Figure 9

Figure 7. The trajectory comparison of IMU/odometer fusion using parameters given by the filter-based method and proposed method

Figure 10

Figure 8. The horizontal position error comparison of IMU/odometer fusion using parameters given by the filter-based method and proposed method

Figure 11

Figure 9. The trajectory comparison of IMU/odometer fusion using parameters given by traditional preintegration methods and the proposed method

Figure 12

Figure 10. The horizontal position error comparison of IMU/odometer fusion using parameters given by traditional preintegration methods and the proposed method

Figure 13

Figure 11. Baidu autonomous driving development kit

Figure 14

Figure 12. The trajectory of the experimental tests

Figure 15

Table 4. IMU sensor specifications

Figure 16

Table 5. GPS sensor specifications

Figure 17

Figure 13. Comparison of scale factor of odometer

Figure 18

Figure 14. Comparison of misalignments in attitude: (a) $X$-axis misalignment in attitude; (b) $Z$-axis misalignment in attitude

Figure 19

Figure 15. Comparison of misalignments in position: (a) $X$-axis misalignment in position; (b) $Y$-axis misalignment in position

Figure 20

Figure 16. The horizontal position error comparison of IMU/odometer fusion using parameters given by the filter-based method and proposed method

Figure 21

Figure 17. The horizontal position error comparison of IMU/odometer fusion using parameters given by the filter-based method and proposed method