Open AccessArticle

A Novel IMU Extrinsic Calibration Method for Mass Production Land Vehicles

Control Systems Group, Department of Electrical Engineering and Computer Science, Technische Universität Berlin, D-10587 Berlin, Germany

Research and Development, Daimler AG, 71059 Sindelfingen, Germany

Author to whom correspondence should be addressed.

Sensors 2021, 21(1), 7; https://doi.org/10.3390/s21010007

Submission received: 17 November 2020 / Revised: 13 December 2020 / Accepted: 16 December 2020 / Published: 22 December 2020

(This article belongs to the Special Issue On-Board and Remote Sensors in Intelligent Vehicles)

Download

Browse Figures

Versions Notes

Abstract

Multi-modal sensor fusion has become ubiquitous in the field of vehicle motion estimation. Achieving a consistent sensor fusion in such a set-up demands the precise knowledge of the misalignments between the coordinate systems in which the different information sources are expressed. In ego-motion estimation, even sub-degree misalignment errors lead to serious performance degradation. The present work addresses the extrinsic calibration of a land vehicle equipped with standard production car sensors and an automotive-grade inertial measurement unit (IMU). Specifically, the article presents a method for the estimation of the misalignment between the IMU and vehicle coordinate systems, while considering the IMU biases. The estimation problem is treated as a joint state and parameter estimation problem, and solved using an adaptive estimator that relies on the IMU measurements, a dynamic single-track model as well as the suspension and odometry systems. Additionally, we show that the validity of the misalignment estimates can be assessed by identifying the misalignment between a high-precision INS/GNSS and the IMU and vehicle coordinate systems. The effectiveness of the proposed calibration procedure is demonstrated using real sensor data. The results show that estimation accuracies below 0.1 degrees can be achieved in spite of moderate variations in the manoeuvre execution.

Keywords:

extrinsic calibration; motion estimation; automotive industry; nonlinear systems; inertial sensors; Kalman filter; odometry; autonomous driving; simultaneous state and parameter estimation; systems and control engineering

1. Introduction

Accurate and reliable information of the vehicle ego-motion is pivotal for the proper operation of active safety and automated driving functionalities. Hence, nowadays vehicles are equipped with sensors devoted to the estimation of the vehicle motion. In order to meet the high accuracy and reliability demands, multi-modal sensor fusion has become ubiquitous in the field of vehicle motion estimation [1,2,3]. Sensors relying on different technologies, with complementary benefits and deficiencies, are combined in order to increase the overall accuracy and reliability of the motion estimation solution.

In a multi-modal sensor fusion set-up, the individual information sources supply motion quantities that generally relate to different points within the vehicle and are expressed in different coordinate systems. A consistent fusion of the measurements demands that the supplied motion variables relate to and are represented in a common coordinate system. Hence, the transformations between the individual coordinate systems need to be known. In automotive series projects, the position and orientation of the sensors is usually precisely defined in a computer aided design (CAD) of the vehicle. Nevertheless, tolerances in manufacturing technologies may cause the location and orientation of the sensors to significantly differ for the real vehicles. For instance, from our experience, in car mass production, IMU mounting errors of few millimetres and two to three degrees can be expected. These inaccuracies in the transformations between the individual coordinate systems deteriorate the performance of fusion algorithms and may even cause the estimator to diverge [4]. Therefore, many applications require calibration procedures that estimate the transformations between the different coordinate systems. This process is also known in the literature by extrinsic calibration [5,6].

In the last few decades, the topic of extrinsic calibration has received increasing attention due to its significance in relevant engineering fields such as robotics. A considerable amount of literature covering the extrinsic calibration of systems involving perception sensors has been produced, e.g., [7,8,9,10,11,12]. Comparatively, the extrinsic calibration of systems not reliant on perception sensors has received considerably less attention. Motion estimation schemes fusing information from odometers (based on wheel speed and steering angle sensors), vehicle models and inertial sensors have been broadly used in active safety and automated driving technologies [3,13,14]. In such a set-up, the IMU angular rates and specific forces are given in the coordinate system of the IMU while the information supplied by the combination of odometers with kinematic [15] and dynamic models [16] is customarily expressed in a vehicle coordinate system. The identification of the relative pose between the IMU and the vehicle coordinate system is rarely addressed in the available vehicle motion estimation algorithms. Instead, a perfect alignment or known extrinsic calibration parameters are commonly assumed [15,17,18]. While, considering the low angular motion of land vehicles, the positioning errors may be neglected, the IMU misalignment errors have a significant impact on the motion estimates [19].

1.1. Brief Review of Previous Approaches

Although scarce, some literature has been produced to address the estimation of the IMU–vehicle extrinsic calibration. Most of the proposed approaches rely on the non-holonomic constraint [20,21,22,23,24], which assumes zero lateral and vertical velocity in the rear axle. However, differences are found with regard to the sensor portfolio and the quality of the sensors used, as well as the parameters identified.

The methods presented in [20] address the estimation of the mounting misalignments of an IMU (roll, pitch and yaw) using the accelerometer signals together with GPS-derived velocities and the non-holonomic constraint. First, the pitch and roll misalignment angles are estimated. Then, these estimates are used for the estimation of the yaw misalignment. Accelerometer biases, tilted roads and accelerations during the roll/pitch calibration phase will result in errors in the estimated misalignment angles. Estimation errors of up to 2 degrees are reported, which are prohibitively large for applications requiring highly accurate motion state estimates.

Some researchers have proposed methods to identify the misalignment angles between the IMU and vehicle coordinate system using IMU, odometry, GNSS information and the non-holonomic constraint [21,22]. For instance, [22] describes an approach that estimates the IMU pitch and yaw misalignment angles using a GNSS/INS aided dead reckoning approach. The method is experimentally tested with data obtained from typical navigation-grade, tactical-grade and low-cost (automotive-grade) IMUs. For the navigation- and tactical-grade IMUs, the misalignment estimates rapidly converge. However, the estimates for the low-cost IMU display a less steady behaviour. Despite its widespread use, GNSS is susceptible to interference and it lacks reliability in some environments. Moreover, not all land vehicles are equipped with an accurate GNSS receiver.

Aware of these drawbacks, some research groups have developed self-contained methods that perform the IMU–vehicle extrinsic calibration, solely reliant on odometry and IMU signals [23,24,25]. In [23], Xue et al. propose an in-motion alignment algorithm that estimates the vehicle-IMU misalignment (pitch and yaw) together with a unique scaling factor for the entire odometry. The method relies on the non-holonomic constraint, odometry measurements and a high-precision 6D IMU that allows north alignment via gyrocompassing. Furthermore, it assumes perfect alignment between the road plane and the vehicle body. A similar approach is proposed in [24], which, based on the same information sources, additionally estimates the IMU position with respect to the vehicle’s rear axle. Despite the accuracy shown in the experimental results of [23,24], tactical- or navigation-grade IMUs are prohibitively costly for car series projects, limiting their use for research and development purposes.

The previously presented approaches rely on the non-holonomic constraint. The main drawbacks of such methods is that when the vehicle does not comply with the non-holonomic constraint, e.g., when a side-slip angle builds during cornering or a vertical movement is caused by the suspension, errors are transferred to the misalignments estimates [22]. Furthermore, in most cases, the roll misalignment is neglected since, due to the non-holonomic constraint, it is unobservable. One of the few works that is not based on the non-holonomic constraint and relies on an automotive-grade IMU is [25]. The approach proposed in this work estimates the longitudinal and lateral accelerometer biases as well as the IMU–vehicle yaw misalignment using FIR-Filter modulating functions and an algebraic observer. Instead of applying the non-holonomic constraints, vertical and lateral velocity measurements are used. These measurements are built upon suspension signals and a single-track model. Nevertheless, the supplied estimates exhibit a relatively unsteady behaviour, which raises questions regarding noise robustness and parameter identifiability.

All in all, there is a scarcity of methods which, not relying on accurate GNSS information and based on an automotive-grade IMU, provide accurate estimates of all three misalignment angles (roll, pitch and yaw). Another shortcoming of the state-of-the-art is that most of the previous approaches assume a perfect alignment between vehicle body and road surface. Changes in the suspension state, due to effects such as variations in the load distribution or speed, are taken as variations in the IMU pitch misalignment, e.g., [22,24]. This demands a continuous estimation of the misalignment angles. Therefore, a method that takes into account suspension information would be desirable. By doing so, an adjustment of the IMU misalignment estimation would only be required after structural changes in the platform, such as a replacement of the IMU or modifications in the steering system.

Besides the already discussed challenges, one of the aspects to consider is the fact that the real IMU misalignment is usually not known. Hence, there is a lack of a ground-truth reference against which the estimates can be directly compared. [22] proposes to validate the estimation by checking that the estimated velocity (expressed in the vehicle coordinate system) has no significant lateral and vertical velocity components when the misalignments have been compensated. This validation is consistent as long as, in the analysed data, the non-holonomic constraint is satisfied, which will not be the case when turning or driving on a road with significant bank angles. Another option is presented in [24], in which the correctness of the estimated calibration parameters is assessed by analysing the improvement of the position error of the dead-reckoning solution when the calibration parameters have been corrected. However, this approach does not provide a direct way to assess the errors of the misalignment estimates.

1.2. Current Approach and Main Contributions

This manuscript proposes an estimation approach that addresses the identification of the 3D rotation between the IMU and vehicle coordinate systems, which is fully determined by three misalignment angles (roll, pitch and yaw). In order to avoid estimation inaccuracies stemming from the IMU biases, these are compensated in the algorithm. The approach is aimed at automotive series projects and, hence, the use of prohibitively expensive sensors is avoided. Therefore, the sensor portfolio is comprised by an automotive-grade 6D IMU and series chassis sensors.

Additionally, for analysis and evaluation purposes, test vehicles are commonly equipped with high accuracy measurement systems, which supply ground-truth motion signals. In our test vehicle, a high-precision INS/GNSS deeply coupled inertial navigation solution is mounted. This article also presents a method to estimate the 3D rotation between the ground-truth and both the IMU and vehicle coordinate systems. These estimates are not only necessary to assess the performance of motion estimation algorithms but also serve as a validation of the 3D rotation estimate between the IMU and the vehicle coordinate systems. Please note that the misalignment between the IMU and the vehicle coordinate systems may be obtained from the 3D rotation between the ground-truth and IMU coordinate systems, and the 3D rotation between the ground-truth and the vehicle coordinate systems.

Based upon the foregoing, the main contributions of this article are listed below.

The proposed estimation scheme estimates all three misalignment angles while considering the IMU biases. This contrasts with most previously proposed approaches, in which the roll misalignment is neglected.
A calibration manoeuvre is proposed for which a persistent excitation condition for the identification of the misalignment angles is experimentally validated. Furthermore, it is shown that the estimation results remain consistent despite moderate variations in the manoeuvre execution.
The estimation scheme relies on cost-efficient automotive sensors, and it is not dependent on neither perception nor GNSS information. Experimental results show that, in spite of not using high-precision measurement systems, the approach supplies accurate and reliable misalignment estimates.
Instead of relying on the non-holonomic constraint, information from the suspension system and a single-track model is used. Hence, the approach considers vertical movements caused by the suspension and side-slip angles in the rear axle, e.g., built during cornering or while driving on a road with significant bank angles.
The pose of the vehicle with respect to the road plane is not seen as an IMU misalignment. Hence, readjustment of the misalignment estimates is not required due to, for instance, load redistribution or changes in the speed. This reduces the need for a continuous extrinsic calibration. A re-estimation of the misalignment angles would just be required after structural modifications in the platform, e.g., replacement of the IMU or alterations in the odometry.
An approach to directly assess the validity of the misalignment estimates is proposed. This is based on the identification of the 3D rotation between a high-precision INS/GNSS, taken as ground-truth, and the IMU and vehicle coordinate systems.

In short, the current work presents an extrinsic calibration procedure that supplies the 3D rotations between the ground-truth, IMU and vehicle coordinate systems (see Figure 1).

The paper is organised as follows. In Section 2, the required coordinate system definitions, the used notation as well as some preliminaries are introduced. The ground-truth–vehicle, ground-truth–IMU and IMU–vehicle extrinsic calibration procedures are presented in Section 3, Section 4 and Section 5, respectively. Each one of the proposed estimation schemes is experimentally validated in its corresponding section. Finally, a summary and some important remarks conclude this article (Section 6).

2. Coordinate Systems and Preliminaries

2.1. Coordinate Systems

In order to describe the position, orientation and motion of a vehicle based on the information supplied by the available sensors and vehicle models, some coordinate systems (CS) need to be introduced, see Figure 2 and Figure 3. Besides coordinate systems commonly used in the navigation literature [26], i.e., the inertial coordinate system (i), the Earth-Centred Earth-Fixed coordinate system (e) as well as the local navigation coordinate system (East-North-Up) (n), some additional coordinate systems are defined.

The vehicle coordinate system (v) is fixed to the vehicle sprung mass and its origin is located on the longitudinal symmetry plane of the vehicle at the mid-wheelbase point at the height of the center of mass (in reference loading conditions). Regarding the orientation of the axes, the

x_{v}

- and

y_{v}

-axes lie on a plane defined in the CAD design of the vehicle, and the

z_{v}

-axis is normal to this plane pointing upwards. The

y_{v}

-axis points to the left, and the

x_{v}

-axis points forward. Nevertheless, unlike [27], the

x_{v}

-axis does not necessarily lie on the longitudinal symmetry plane of the vehicle. This specific orientation is determined by the used dynamic model. In the present work, it is aligned with the direction of travel while driving on a horizontal road with no vertical angular rate, conditions under which the single-track model supplies zero lateral velocity. Note that, due to asymmetries in the vehicle, this direction of travel does not necessarily lie on the theoretical symmetry plane of the vehicle.

The IMU and ground-truth coordinate systems (m and g, respectively) are fixed to the sprung mass of the vehicle. Their origins and axes orientations are determined by the mounting position and orientation of the IMU and ground-truth systems, respectively. In the test vehicle used in this work, the x-, y-, and z-axes point in the forward direction of the vehicle, to the left and upward, respectively. Due to mounting errors, their axes are not aligned.

The origin of the road coordinate system (r) is located on the road plane (The road plane is a plane that represents the road surface, on which the tyres are supported and which produces the friction required to move the vehicle. In case of planar road surfaces, the road plane is coincident with the road surface. Nevertheless, if the road surface contours have a wavelength similar to or less than the size of the vehicle, an equivalent road plane is determined, which is an approximation of the actual road surface. The road plane is determined by a best fit approximation through the tire contact patches.) maintaining the symmetry with respect to the vehicle wheels. The

z_{r}

-axis of the road coordinate system is normal to the road plane and its

x_{r}

-axis points in the direction of the

x_{v}

-axis.

2.2. Notation

The present paper adopts the notation commonly used in the navigation systems literature [26,28]. Specifically, three indices are employed to characterize motion variables, such as velocities

v

or angular rates

ω

. Let us illustrate this with the following angular rate:

ω_{i n}^{v} = (\begin{matrix} ω_{i n, x}^{v} \\ ω_{i n, y}^{v} \\ ω_{i n, z}^{v} \end{matrix})

(1)

The superscript index specifies the coordinate system in which the motion quantity is decomposed (resolving coordinate system). The right lower index indicates the coordinate system whose motion is described (object coordinate system). Finally, the left lower index denotes the coordinate system with which the motion is respect to (resolving coordinate system). Hence,

ω_{i n}^{v}

depicts the angular rate of the local navigation coordinate system with respect to the inertial coordinate system resolved in the vehicle coordinate system.

The relative attitude between two coordinate systems may be represented via Euler angles. Particularly, intrinsic Euler angles with the ZYX convention are adopted in the present work. Attitude is fully described by the object and reference coordinate systems and, therefore, Euler angles are typically denoted using two indices. For example:

ψ_{n v} = (\begin{matrix} ϕ_{n v} \\ θ_{n v} \\ ψ_{n v} \end{matrix})

(2)

represents the Euler rotation from the local navigation coordinate system to the vehicle coordinate system. Here, the first rotation,

ψ_{n v}

, called yaw, is performed about

z^{n}

-axis; the second rotation,

θ_{n v}

, known as pitch, is performed about the once rotated y-axis; the third rotation,

ϕ_{n v}

, also named roll, is performed about the twice rotated x-axis. The corresponding rotation matrix

R_{n}^{v}

transforms a motion quantity from the set of resolving axes of the coordinate system n to those of the coordinate system v:

ω_{i v}^{v} = R_{n}^{v} ω_{i v}^{n}

(3)

For small rotations, for which the small-angle approximation holds (

sin α \approx 0

cos α \approx 1

sin α sin α \approx α α \approx 0

), some approximations can be made:

R_{n}^{v} \approx (\begin{matrix} 1 & ψ_{n v} & - θ_{n v} \\ - ψ_{n v} & 1 & ϕ_{n v} \\ θ_{n v} & - ϕ_{n v} & 1 \end{matrix}) where (\begin{matrix} ϕ_{n v} \\ θ_{n v} \\ ψ_{n v} \end{matrix}) \approx (\begin{matrix} - ϕ_{v n} \\ - θ_{v n} \\ - ψ_{v n} \end{matrix})

(4)

2.3. Inertial Measurement Unit: Preliminaries

A 6D IMU is a sensor supplying measurements of the angular rate and specific force of the vehicle body (at the mounting location) with respect to the inertial coordinate system, i.e.,

ω_{i m}^{m}

and

f_{i m}^{m}

. While the angular rate is the same within a rigid body (e.g.,

ω_{i m}^{m} = ω_{i v}^{m} = ω_{i g}^{m}

, hereafter

ω^{m}

), the specific force of a rotating body depends on the particular point within the body (e.g.,

f_{i m}^{m} \neq f_{i v}^{m} \neq f_{i g}^{m}

). When the relative location between a point of interest and the mounting position of an IMU is known, the specific force may be transformed to the point of interest using a simple kinematic equation [17]. Additionally, the signals supplied by the IMU are corrupted by different error sources such as biases, scaling factors and non-orthogonalities [29]. This work assumes that most errors have been corrected at an end-of-line calibration of the IMU [30]. However, for the automotive-grade IMU, biases are further considered due to the significant turn-on and in-run terms [26]. Hence, the following IMU error model is considered:

\begin{matrix} {\tilde{f}}_{i m}^{m} & = f_{i m}^{m} + b_{f} \end{matrix}

(5)

\begin{matrix} {\tilde{ω}}^{m} & = ω^{m} + b_{ω}, \end{matrix}

(6)

where

{\tilde{f}}_{i m}^{m}

and

{\tilde{ω}}^{m}

denote the measured specific force and angular rates, and

b_{f}

and

b_{ω}

the accelerometer and gyro biases, respectively.

3. Ground-Truth–Vehicle Misalignment Estimation

As previously stated, the precise knowledge of the ground-truth–vehicle relative pose is not only essential for analysis and evaluation purposes but, together with the ground-truth–IMU pose, provides a reference for the IMU–vehicle misalignment estimation. In this section, a method to estimate the misalignment angles between the ground-truth and vehicle coordinate systems (

ϕ_{v g}

θ_{v g}

ψ_{v g}

) is proposed and experimentally validated. The method can be divided into two parts, i.e., the estimation of the pitch and roll misalignments, and the estimation of the yaw misalignment.

The employed ground-truth is a high-precision INS/GNSS deeply coupled inertial navigation system, which provides accurate velocities, orientation angles (

ϕ_{n g}

θ_{n g}

ψ_{n g}

), specific forces and angular rates.

3.1. Roll and Pitch Misalignment

The roll and pitch misalignment angles (

ϕ_{v g}

θ_{v g}

) may be determined while standing still, thanks to the high-precision IMU installed in the ground-truth system, as well as the vehicle level measurements.

The spring deflections, supplied by the vehicle level sensors, together with the tyre deflections, provided by a simple tyre vertical model, are used to compute the vehicle levels over-road

z_{i}

[3]. Here,

i \in [FL, FR, RL, RR]

, where FL, FR, RL, and RR denotes to the front left, front right, rear left and rear right wheels, respectively. Therewith, the pitch and roll angles with respect to the road plane are obtained (see Figure 4). In most vehicles, these angles remain very small during standstill phases. Hence, the small-angle approximation is valid,

sin α \approx α

and

cos α \approx 1

\begin{matrix} θ_{r v} & = \frac{z_{RL} + z_{RR} - z_{FL} - z_{FR}}{2 l} \\ ϕ_{r v} & = \frac{1}{2} (\frac{z_{FL} - z_{FR}}{t_{f}} + \frac{z_{RL} - z_{RR}}{t_{r}}) \end{matrix}

(7)

where l,

t_{F}

and

t_{R}

are respectively the wheel base and the front and rear track-widths.

Assuming that the vehicle is standing on a road plane with small bank and slope angles (

ϕ_{n r}

θ_{n r}

), and bearing in mind that the misalignment angles and the vehicle angles with respect to the road plane are small, the following relationship holds:

\begin{matrix} ϕ_{n g} & = ϕ_{n r} + ϕ_{r v} + ϕ_{v g} \\ θ_{n g} & = θ_{n r} + θ_{r v} + θ_{v g}, \end{matrix}

(8)

where

ϕ_{n g}

and

θ_{n g}

are obtained from the ground-truth, and

ϕ_{r v}

and

θ_{r v}

from the vehicle levels according to (7). If the vehicle stands on a horizontal road (

θ_{n r}, ϕ_{n r} = 0

), the misalignment angles may be directly obtained using the following relationships:

\begin{matrix} ϕ_{v g} & = ϕ_{n g} - ϕ_{r v} \\ θ_{v g} & = θ_{n g} - θ_{r v} \end{matrix}

(9)

However, horizontal road surfaces are not always available. In such case, the unknown slope and bank angle can be cancelled out by using two standstill phases at the exact same position but with the vehicle facing opposite directions (see Figure 5, where Figure 5a corresponds with pose 1 and Figure 5b with pose 2):

\begin{matrix} ϕ_{n g, 1} & = ϕ_{n r, 1} + ϕ_{r v, 1} + ϕ_{v g} \end{matrix}

(10)

\begin{matrix} ϕ_{n g, 2} & = ϕ_{n r, 2} + ϕ_{r v, 2} + ϕ_{v g} = - ϕ_{n r, 1} + ϕ_{r v, 2} + ϕ_{v g} \end{matrix}

(11)

\begin{matrix} θ_{n g, 1} & = θ_{n r, 1} + θ_{r v, 1} + θ_{v g} \end{matrix}

(12)

\begin{matrix} θ_{n g, 2} & = θ_{n r, 2} + θ_{r v, 2} + θ_{v g} = - θ_{n r, 1} + θ_{r v, 2} + θ_{v g} \end{matrix}

(13)

Adding (10) and (11), as well as (12) and (13), and reorganising the terms yields

\begin{matrix} ϕ_{v g} & = \frac{ϕ_{n g, 1} + ϕ_{n g, 2} - ϕ_{r v, 1} - ϕ_{r v, 2}}{2} \end{matrix}

(14)

\begin{matrix} θ_{v g} & = \frac{θ_{n g, 1} + θ_{n g, 2} - θ_{r v, 1} - θ_{r v, 2}}{2} \end{matrix}

(15)

3.2. Yaw Misalignment

Unlike for the pitch and roll cases, vehicle motion is required to determine the yaw misalignment between the ground-truth and the vehicle coordinate systems (

ψ_{v g}

). Note that the used single-track model partially defines the vehicle coordinate system (see Section 2):

\begin{matrix} v_{e r, y}^{r} & \approx l_{r} ω_{z}^{r} - S G v_{e r, x}^{r} f_{i r, y}^{r}, \end{matrix}

(16)

where

v_{e r, x}^{r}

ω_{z}^{r}

and

f_{i r, y}^{r}

represent the over-road lateral velocity, vertical angular rate and lateral specific force, respectively, and

S G

and

l_{r}

are the sideslip angle gradient and the distance between the center of gravity and the rear axle. Using the roll angle with respect to the road plane, the velocity of the vehicle coordinate system resolved in vehicle coordinates may be expressed as follows:

v_{e v, y}^{v} = v_{e r, y}^{r} + v_{r v, z}^{r} ϕ_{r v} - h_{r} {\dot{ϕ}}_{r v}

(17)

When driving straight ahead with constant speed over a well-paved horizontal road, we have

ω_{z}^{r}, v_{r v, z}^{r}, {\dot{ϕ}}_{r v}, f_{i r, y}^{r} \approx 0

, and, hence,

v_{e v, y}^{v} \approx 0

. Therefore, under these conditions, the projection of the velocity vector onto the

z_{v}

-plane (plane normal to the

z_{v}

vector) is completely aligned with

x_{v}

(see Figure 6). Please note that, due to asymmetries in the vehicle, the

x_{v}

-axis does not necessarily lie on the longitudinal symmetry plane. In such situation,

ψ_{v g}

can be determined by the direction in which the ground-truth perceives the velocity vector:

ψ_{v g} = - β_{g},

(18)

where

β_{g} ≜ arctan \frac{v_{e v, y}^{g}}{v_{e v, x}^{g}}

can directly be obtained from the ground-truth velocity measurements.

Unfortunately, not all test tracks possess a perfectly horizontal road. Most seemingly horizontal roads have some kind of bank angle for water drainage purposes. Due to gravity, even small road bank angles cause

f_{i r, y}^{r} \neq 0

, which in turn, through the term

- S G f_{i r, y}^{r} v_{e r, x}^{r}

causes

v_{e v, y}^{v} \neq 0

. If neglected, the lateral velocity caused by the bank angle is taken as a yaw misalignment.

In order to compensate the sideslip angle (

β_{v} ≜ arctan \frac{v_{e v, y}^{v}}{v_{e v, x}^{v}}

) caused by the road bank, one can drive on the same lane twice, but in opposite directions (see Figure 7). In each drive, the sideslip angle caused by the gravity takes opposite directions:

\begin{matrix} ψ_{v g} & = - β_{g, 1} + β_{bank} \end{matrix}

(19)

\begin{matrix} ψ_{v g} & = - β_{g, 2} - β_{bank}, \end{matrix}

(20)

where

β_{bank} \approx f_{i r, y}^{r} S G

, and 1 and 2 refer to the first and second drive, respectively. Hence, adding (19) and (20) yields

\begin{matrix} ψ_{v g} & = - \frac{β_{g, 1} + β_{g, 2}}{2}, \end{matrix}

(21)

where

β_{g, 1}

and

β_{g, 2}

are obtained from the ground-truth measurements.

3.3. Results

The calibration to determine the relative pose between the ground-truth and vehicle coordinate systems was performed on a test-track three times (on different dates). The road bank and slope angles were below 2 degrees. For the pitch and roll angle misalignment procedure, the vehicle was kept at a standstill for at least

30 s

in each direction. In order to avoid movement in the car, the driver and passengers got out of the car in both standstill phases. The values

θ_{n g, 1}

θ_{n g, 2}

θ_{r v, 1}

and

θ_{r v, 2}

were obtained by averaging over the standstill phase.

For the yaw misalignment manoeuvre, the vehicle was accelerated to 30 km/h and kept at constant speed for at least 100 m. Afterwards, the vehicle was turned 180 deg and driven back along the same lane at the same velocity (see Figure 7). The values

β_{g, 1}

and

β_{g, 2}

were obtained by averaging over the samples corresponding to the constant speed phase. The results are depicted in Table 1:

The maximum difference among the misalignment estimates within the three calibration procedures lies below

0.03

\deg

for both the roll

{\hat{ϕ}}_{v g}

and pitch

{\hat{θ}}_{v g}

misalignment angles. A bit larger is the variability of the yaw misalignment estimate

{\hat{ψ}}_{v g}

, presenting a maximum difference of

0.055

\deg

. In any case, the variability remains low and, hence, it can be concluded that the estimation results are consistent.

4. Ground-Truth–IMU Misalignment Estimation

One of the main challenges faced when addressing the estimation of the misalignment between an automotive-grade and a ground-truth IMU is the fact that the biases of the automotive-grade IMU must be considered. Hence, the complexity of the problem rises to that of estimating both the biases and misalignment angles. In the present section, an approach is presented that addresses the misalignment estimation between the IMU (m) and the ground-truth (g) coordinate systems while considering the biases of the automotive-grade IMU.

4.1. Method

The proposed calibration procedure consists of two successive phases, in which the outputs of both the automotive-grade and ground-truth IMUs are used. First, during a standstill phase, the angular rate biases are determined and linear constraints between the accelerometer biases and the misalignment angles are identified. Then, a calibration manoeuvre is performed, which provides enough excitation to estimate the misalignment angles (see Figure 8). The fact that the automotive-grade and ground-truth IMUs are physically mounted at different positions within the vehicle body is taken into account. The ground-truth specific force signals (

f_{i g}^{g}

) are transformed to the mounting position of the automotive-grade IMU (

f_{i m}^{g}

) in a pre-processing step.

4.1.1. Standstill Phase

During standstill, the angular rate biases can be determined relatively easily [31]. However, this is not the case for the accelerometer biases. On the one hand, the pitch and roll angles of the IMU coordinate system m with respect to the local navigation coordinate system n are not perfectly known and, on the other hand, the discrepancies between the outputs of the ground-truth and the automotive-grade IMUs stem from a combination of bias and misalignment errors. Even though the accelerometer biases cannot be directly identified during standstill, a relationship between the bias and misalignment errors can be identified.

The misalignment angles are assumed to be small. Hence, using the small angle approximation, the rotation matrix

R_{m}^{g}

is defined by

R_{m}^{g} = (\begin{matrix} 1 & ψ_{m g} & - θ_{m g} \\ - ψ_{m g} & 1 & ϕ_{m g} \\ θ_{m g} & - ϕ_{m g} & 1 \end{matrix})

(22)

Furthermore, using the coordinate transformation of the specific forces from the resolving coordinate system g to the resolving coordinate system m yields:

\begin{matrix} (\begin{matrix} f_{i m, x}^{m} \\ f_{i m, y}^{m} \\ f_{i m, z}^{m} \end{matrix}) = (\begin{matrix} 1 & - ψ_{m g} & θ_{m g} \\ ψ_{m g} & 1 & - ϕ_{m g} \\ - θ_{m g} & ϕ_{m g} & 1 \end{matrix}) (\begin{matrix} f_{i m, x}^{g} \\ f_{i m, y}^{g} \\ f_{i m, z}^{g} \end{matrix}), \end{matrix}

(23)

where

{(f_{i m, x}^{g}, f_{i m, y}^{g}, f_{i m, z}^{g})}^{T}

represents the specific force resolved in ground-truth coordinates, obtained from the ground-truth measurement system, and

{(f_{i m, x}^{m}, f_{i m, y}^{m}, f_{i m, z}^{m})}^{T}

the specific force resolved in IMU coordinates, which is not directly accessible due to the IMU accelerometer biases. Instead, the bias-corrupted specific force signals

{({\tilde{f}}_{i m, x}^{m}, {\tilde{f}}_{i m, y}^{m}, {\tilde{f}}_{i m, z}^{m})}^{T}

are available:

\begin{matrix} {\tilde{f}}_{i m, x}^{m} & = f_{i m, x}^{m} + b_{f_{x}} \\ {\tilde{f}}_{i m, y}^{m} & = f_{i m, y}^{m} + b_{f_{y}} \\ {\tilde{f}}_{i m, z}^{m} & = f_{i m, z}^{m} + b_{f_{z}} . \end{matrix}

(24)

Combining (24) and (23), the biases are expressed as a function of the misalignment angles as follows:

\begin{matrix} (\begin{matrix} b_{f_{x}} \\ b_{f_{y}} \\ b_{f_{z}} \end{matrix}) = (\begin{matrix} {\tilde{f}}_{i m, x}^{m} \\ {\tilde{f}}_{i m, y}^{m} \\ {\tilde{f}}_{i m, z}^{m} \end{matrix}) - (\begin{matrix} f_{i m, x}^{g} \\ f_{i m, y}^{g} \\ f_{i m, z}^{g} \end{matrix}) + (\begin{matrix} 0 & - f_{i m, z}^{g} & f_{i m, y}^{g} \\ f_{i m, z}^{g} & 0 & - f_{i m, x}^{g} \\ - f_{i m, y}^{g} & f_{i m, x}^{g} & 0 \end{matrix}) (\begin{matrix} ϕ_{m g} \\ θ_{m g} \\ ψ_{m g} \end{matrix}) . \end{matrix}

(25)

This relationship between the accelerometer biases and misalignment angles is exploited during a standstill phase in order to set the following constraints:

\begin{matrix} (\begin{matrix} b_{f_{x}} \\ b_{f_{y}} \\ b_{f_{z}} \end{matrix}) = (\begin{matrix} {\tilde{f}}_{i m, x 0}^{m} \\ {\tilde{f}}_{i m, y 0}^{m} \\ {\tilde{f}}_{i m, z 0}^{m} \end{matrix}) - (\begin{matrix} f_{i m, x 0}^{g} \\ f_{i m, y 0}^{g} \\ f_{i m, z 0}^{g} \end{matrix}) + (\begin{matrix} 0 & - f_{i m, z 0}^{g} & f_{i m, y 0}^{g} \\ f_{i m, z 0}^{g} & 0 & - f_{i m, x 0}^{g} \\ - f_{i m, y 0}^{g} & f_{i m, x 0}^{g} & 0 \end{matrix}) (\begin{matrix} ϕ_{m g} \\ θ_{m g} \\ ψ_{m g} \end{matrix}), \end{matrix}

(26)

where (

{\tilde{f}}_{i m, x 0}^{m}

{\tilde{f}}_{i m, y 0}^{m}

{\tilde{f}}_{i m, z 0}^{m}

) and (

f_{i m, x 0}^{g}

f_{i m, y 0}^{g}

and

f_{i m, z 0}^{g}

) respectively represent the mean of the IMU and ground-truth specific force measurements during the standstill phase.

Despite the time-varying nature of the biases, it can be assumed that, in the absence of sensor malfunctions and strong changes in the environment, the biases practically remain constant within intervals of several minutes. Hence, it is a sensible assumption that the biases and constraints hold during the subsequent calibration manoeuvre.

4.1.2. Calibration Manoeuvre

The misalignment angles between the IMU and ground-truth coordinate systems are estimated during a calibration manoeuvre using an estimator based on the recursive least squares (RLS) algorithm. The estimator exploits both the bias-corrected angular rates from the automotive-grade IMU, as well as the constraints identified during the standstill phase.

Applying the coordinate transformation of the angular rates and specific forces from the set of resolving axes m to the set of resolving axes g leads to the following equations:

(\begin{matrix} ω_{x}^{g} \\ ω_{y}^{g} \\ ω_{z}^{g} \end{matrix}) - (\begin{matrix} ω_{x}^{m} \\ ω_{y}^{m} \\ ω_{z}^{m} \end{matrix}) = (\begin{matrix} 0 & - ω_{z}^{m} & ω_{y}^{m} \\ ω_{z}^{m} & 0 & - ω_{x}^{m} \\ - ω_{y}^{m} & ω_{x}^{m} & 0 \end{matrix}) (\begin{matrix} ϕ_{m g} \\ θ_{m g} \\ ψ_{m g} \end{matrix})

(27)

(\begin{matrix} f_{i m, x}^{g} \\ f_{i m, y}^{g} \\ f_{i m, z}^{g} \end{matrix}) - (\begin{matrix} f_{i m, x}^{m} \\ f_{i m, y}^{m} \\ f_{i m, z}^{m} \end{matrix}) = (\begin{matrix} 0 & - f_{i m, z}^{m} & f_{i m, y}^{m} \\ f_{i m, z}^{m} & 0 & - f_{i m, x}^{m} \\ - f_{i m, y}^{m} & f_{i m, x}^{m} & 0 \end{matrix}) (\begin{matrix} ϕ_{m g} \\ θ_{m g} \\ ψ_{m g} \end{matrix}),

(28)

where

{(ω_{x}^{g}, ω_{y}^{g}, ω_{z}^{g})}^{T}

and

{(ω_{x}^{m}, a n d ω_{y}^{m}, ω_{z}^{m})}^{T}

respectively correspond with the bias-free angular rates, supplied by the ground-truth, and the bias-corrected angular rates of the automotive-grade IMU.

Substituting (24) and (26) in (28) yields

\begin{matrix} (\begin{matrix} f_{i m, x}^{g} - {\tilde{f}}_{i m, x}^{m} + {\tilde{f}}_{i m, x 0}^{m} - f_{i m, x 0}^{g} \\ f_{i m, y}^{g} - {\tilde{f}}_{i m, y}^{m} + {\tilde{f}}_{i m, y 0}^{m} - f_{i m, y 0}^{g} \\ f_{i m, z}^{g} - {\tilde{f}}_{i m, z}^{m} + {\tilde{f}}_{i m, z 0}^{m} - f_{i m, z 0}^{g} \end{matrix}) = (\begin{matrix} 0 & - {\tilde{f}}_{i m, z}^{m} + f_{i m, z 0}^{g} & {\tilde{f}}_{i m, y}^{m} - f_{i m, y 0}^{g} \\ {\tilde{f}}_{i m, z}^{m} - f_{i m, z 0}^{g} & 0 & - {\tilde{f}}_{i m, x}^{m} + f_{i m, x 0}^{g} \\ - {\tilde{f}}_{i m, y}^{m} + f_{i m, y 0}^{g} & {\tilde{f}}_{i m, x}^{m} - f_{i m, x 0}^{g} & 0 \end{matrix}) (\begin{matrix} ϕ_{m g} \\ θ_{m g} \\ ψ_{m g} \end{matrix}), \end{matrix}

(29)

where the terms corresponding to the multiplication of the biases and misalignment angles have been assessed to be negligible.

The combination of (27) and (29) leads to

\begin{matrix} (\begin{matrix} ω_{x}^{g} - ω_{x}^{m} \\ ω_{y}^{g} - ω_{y}^{m} \\ ω_{z}^{g} - ω_{z}^{m} \\ f_{i m, x}^{g} - {\tilde{f}}_{i m, x}^{m} + {\tilde{f}}_{i m, x 0}^{m} - f_{i m, x 0}^{g} \\ f_{i m, y}^{g} - {\tilde{f}}_{i m, y}^{m} + {\tilde{f}}_{i m, y 0}^{m} - f_{i m, y 0}^{g} \\ f_{i m, z}^{g} - {\tilde{f}}_{i m, z}^{m} + {\tilde{f}}_{i m, z 0}^{m} - f_{i m, z 0}^{g} \end{matrix}) = (\begin{matrix} 0 & - ω_{z}^{m} & ω_{y}^{m} \\ ω_{z}^{m} & 0 & - ω_{x}^{m} \\ - ω_{y}^{m} & ω_{x}^{m} & 0 \\ 0 & - {\tilde{f}}_{i m, z}^{m} + f_{i m, z 0}^{g} & {\tilde{f}}_{i m, y}^{m} - f_{i m, y 0}^{g} \\ {\tilde{f}}_{i m, z}^{m} - f_{i m, z 0}^{g} & 0 & - {\tilde{f}}_{i m, x}^{m} + f_{i m, x 0}^{g} \\ - {\tilde{f}}_{i m, y}^{m} + f_{i m, y 0}^{g} & {\tilde{f}}_{i m, x}^{m} - f_{i m, x 0}^{g} & 0 \end{matrix}) (\begin{matrix} ϕ_{m g} \\ θ_{m g} \\ ψ_{m g} \end{matrix}), \end{matrix}

(30)

which may be treated as a linear identification problem of the following form:

y_{k} = H_{k} ρ + w_{k},

(31)

where

w_{k}

represents the measurement noise, which is assumed to be Gaussian, white, zero-mean and independent with a constant covariance matrix R. Please note that the first three diagonal elements of the measurement noise covariance R are directly linked to the ground-truth and IMU gyro noises, while the last three elements relate to the noise characteristics of the accelerometers.

The well-know recursive least squares (RLS) algorithm represents a logical choice to solve this identification problem. Exponential convergence in estimation algorithms is a relevant property since it generally leads to enhanced robustness against noise and modelling errors, as well as an improved performance in the non-stationary case [32]. The integration of an exponential forgetting factor

λ \in (0, 1)

in the RLS algorithm is necessary to achieve exponential convergence and, additionally, it prevents the loss of estimation capability [33]. Therefore, the incorporation of an exponential forgetting factor in the RLS algorithm is common practice. Nevertheless, a sufficient excitation property is usually required to guarantee exponential convergence.

Definition 1.

The sequence

H_{k}

is persistently exciting if there exist an integer

h > 0

and a real constant

α > 0

such that, for all integer

k \geq 0

\sum_{s = 0}^{h - 1} H_{k} H_{k}^{T} \geq α I .

(32)

If the sequence

H_{k}

is persistently exciting, the estimates of the RLS with forgetting factor converge exponentially to the true parameters [33]. However, if the excitation is poor, the estimation problem becomes ill-conditioned, leading to unreliable estimates strongly affected by noise and model uncertainties.

In order to increase the robustness of the estimator against periods of poor excitation, a regularised recursive least squares algorithm with exponential forgetting factor is used for the identification of the misalignment between the ground-truth and the IMU (see Algorithm 1).

Algorithm 1: Regularised recursive least squares [34].

1: $k = k + 1$
input: $H_{k}$ , $P_{k | k - 1}$ , $λ$ , $Σ_{R}$
2: $K_{k} = P_{k | k - 1} H_{k}^{T} (R + H_{k} P_{k | k - 1} H_{k}^{T})$ %Gain as in RLS
3: $P_{k | k} = P_{k | k - 1} + K_{k} H_{k} P_{k | k - 1}$ %Covariance update as in RLS
4: ${\bar{P}}_{k + 1 | k} = \frac{1}{λ} P_{k | k}$ %Covariance after forgetting
5: $P_{k + 1 | k} = {\bar{P}}_{k + 1 | k} {[I + (1 - λ) Σ_{R} {\bar{P}}_{k + 1 | k}]}^{- 1}$ %Covariance after regularization
6: ${\hat{ρ}}_{k} = {\hat{ρ}}_{k - 1} + K_{k} (y_{k} - H_{k} ρ_{k - 1})$ %Update parameter estimate
output: ${\hat{ρ}}_{k}$ , $P_{k + 1 | k}$

The suggested calibration manoeuvre is a figure eight drive since it provides a high level of excitation and stimulates the individual IMU elements equally in both directions. However, other manoeuvres that satisfy the persistent excitation condition of Defination 1 may also lead to satisfactory estimation results.

4.2. Experimental Validation

The performance of the algorithm is evaluated using data collected from figure eight drives. The ground-truth angular rates and specific forces are supplied by the INS/GNSS deeply coupled measurement system. This accurate equipment contains a high-precision IMU with MEMS accelerometers and fibre optic gyros (FOG), with biases specified as low as

0.01 m / s^{2}

and

1 \deg / h

, respectively. As for the IMU angular rates and specific forces, they are obtained from a significantly less accurate automotive-grade IMU.

Please note that the real misalignment between the INS/GNSS system and the automotive-grade IMU is not known. Hence, the efficacy of the algorithm is analysed based on the following three criteria:

The algorithm ability to follow artificially introduced misalignment errors.
The misalignment estimation robustness against noise.
The consistency of the misalignment estimates obtained from a set of different measurements.

The algorithm ability to track the misalignment angles can be illustrated by introducing artificial misalignment errors in the original data and validate whether these errors are tracked. As for the second aspect, the addition of different noise sequences with similar characteristics as the original measurement noise should not imply a significant change in the estimation results. Finally, since the misalignment angles do not change as long as the equipment is not unmounted, the variability of the estimates obtained from datasets collected during manoeuvres performed at different points in time should be low.

The angular rate and specific force measurements supplied by the automotive-grade IMU and the high-performance INS/GNSS system are displayed in Figure 9. Table 2 outlines the parameters from Algorithm 1 employed to produce the presented experimental results. The misalignment estimates are presented in Figure 10. The figure displays the results based on the original data (

{\hat{ϕ}}_{m g 1}

{\hat{θ}}_{m g 1}

{\hat{ψ}}_{m g 1}

) as well as the estimates obtained from the data corrupted by artificially added misalignment errors (

{\hat{ϕ}}_{m g 2}

{\hat{θ}}_{m g 2}

{\hat{ψ}}_{m g 2}

). The artificially added errors are

0.7 \deg

for

ϕ_{m g}

0.5 \deg

for

θ_{m g}

and

1.2 \deg

for

ψ_{m g}

. Additionally, the IMU specific force signals are corrupted by artificial accelerometer biases of

(b_{f_{x}}, b_{f_{y}}, b_{f_{z}}) = (0.1, - 0.1, - 0.2) m / s^{2}

. This aims at showing that the misalignment angles are correctly estimated despite the accelerometer biases. As shown in Figure 10, the estimator is able to track the artificially added misalignment errors despite the added accelerometer biases, presenting increments between the estimates obtained from the misalignment-non-corrupted and misalignment-corrupted data of 0.71, 0.50 and 1.22 deg.

Furthermore, the determinant of the excitation matrix

W_{p, k} = H_{k} H_{k}^{T}

is far from zero, which experimentally shows that the persistence excitation condition is satisfied and the estimation problem is well-conditioned. This is confirmed by the analysis of the robustness against noise (see Figure 11). The original IMU angular rates and specific forces have been corrupted by 100 different sequences of additive Gaussian white noise with standard deviation

σ = 0.001

rad / s

and

σ = 0.05

m / s^{2}

, respectively. One can see that the misalignment estimates remain consistent despite the use of different additive noise sequences, which shows the algorithm robustness against noise. This claim is also supported by the variability of the misalignment estimates obtained from a set of different figure eight manoeuvres (Table 3). The estimation results are consistent throughout the different datasets, presenting the maximum differences in the estimated

{\hat{ϕ}}_{m g}

{\hat{θ}}_{m g}

and

{\hat{ψ}}_{m g}

0.008

0.007

and

0.067

\deg

, respectively.

5. IMU–Vehicle Misalignment Estimation

This section presents the main contribution of the paper, i.e., a method for the estimation of all three IMU–vehicle misalignment angles, which relies on series chassis sensors, an automotive-grade IMU and a single-track model.

On the one hand, the IMU specific forces and angular rates are motion variables expressed in the IMU coordinate system (m). On the other hand, the information stemming from the odometry, suspension and single-track model constitutes a 3D velocity information source, which can be expressed in the defined vehicle coordinate system (v). Unlike for the ground-truth–IMU (g–m) and ground-truth–vehicle (g–v) extrinsic calibration, there is not a direct correlation between the measurements expressed in the IMU coordinate system (m) and the information represented in the vehicle coordinate system (v). Note that the specific forces and angular rates are not directly related to the velocities but to their time derivatives. This entails a significant increase in complexity, for which the previous approaches are no longer suitable. Notwithstanding this, the IMU–vehicle extrinsic calibration can be addressed as a joint parameter and state estimation problem. An adaptive estimator, i.e., the regularized adaptive Kalman filter, is proposed to estimate the vehicle velocities and attitude angles (states) as well as the IMU accelerometer biases and the IMU–vehicle misalignment angles (parameters). Furthermore, in order to bolster the performance of the estimator, additional standstill information is included in the form of constraints, see Figure 12.

The rest of this section is organized as follows. First, the model is derived, upon which the adaptive estimator is built. Afterwards, the estimator design is thoroughly described and the conditions that guarantee the convergence of the estimates are discussed. Finally, the validity of the proposed calibration procedure is experimentally exemplified.

5.1. Model

As in the case of the ground-truth–IMU (g–m) misalignment estimation, when addressing the IMU–vehicle extrinsic calibration, the IMU biases must be taken into account. Hence, the estimation problem of the IMU–vehicle misalignment also extends to that of estimating both the IMU biases and the misalignment angles. Please note that, in the absence of sensor faults and drastic changes in the environment, the biases remain nearly constant within intervals of few minutes. Furthermore, since the angular rate biases can easily be determined during standstill phases, the challenge is to estimate the accelerometer biases together with the misalignment angles.

In the derivation of the system equations, the following considerations are taken into account:

Misalignment angles and IMU biases are small, which implies that second-order products involving these variables are negligible.
Bearing in mind that the car angular motion remains small, particularly for pitch and roll movements, $α {\dot{ϕ}}_{r v}$ and $α {\dot{θ}}_{r v}$ may be neglected (where $α$ represents a small angle).
The misalignment angles remain within the range of $[- 3, 3] \deg$ and, hence, the small-angle approximation holds ( $sin α \approx α$ , $cos α \approx 1$ ).

Keeping in mind that the vehicle and IMU coordinate systems are not aligned, the kinematic differential equations based on the 6D IMU signals are expressed in the IMU coordinate system [3]:

\begin{matrix} \begin{matrix} {\dot{x}}_{1} & = ω_{y}^{m} x_{6} - ω_{z}^{m} x_{2} \\ {\dot{x}}_{2} & = ω_{x}^{m} x_{6} + ω_{z}^{m} x_{1} \\ {\dot{x}}_{3} & = ω_{z}^{m} x_{4} - ω_{y}^{m} x_{5} + g x_{1} + {\tilde{f}}_{i v, x}^{m} - b_{f, x}^{m} \\ {\dot{x}}_{4} & = - ω_{z}^{m} x_{3} + ω_{x}^{m} x_{5} - g x_{2} + {\tilde{f}}_{i v, y}^{m} - b_{f, y}^{m} \\ {\dot{x}}_{5} & = ω_{y}^{m} x_{3} - ω_{x}^{m} x_{4} - g x_{6} + {\tilde{f}}_{i v, z}^{m} - b_{f, z}^{m} \\ {\dot{x}}_{6} & = - ω_{y}^{m} x_{1} - ω_{x}^{m} x_{2}, \end{matrix} \end{matrix}

(33)

where

\begin{matrix} x & = {(\begin{matrix} sin θ_{n m}, sin ϕ_{n m} cos θ_{n m}, v_{e v, x}^{m}, v_{e v, y}^{m}, v_{e v, z}^{m}, cos θ_{n m} cos ϕ_{n m} \end{matrix})}^{T} \end{matrix}

(34)

As in [3], considering the accuracy of automotive-grade IMUs, the rotation of the Earth as well as the transport rate are neglected. Note that

ω^{m} = ω_{i v}^{m}

instead of

ω_{n v}^{m}

is used. Furthermore, the IMU specific force signals are transformed to the origin of the vehicle coordinate system, note that

{\tilde{f}}_{i v}^{m}

instead of

{\tilde{f}}_{i m}^{m}

is used.

Additionally, using the IMU–vehicle misalignment angles (

ϕ_{m v}

θ_{m v}

ψ_{m v}

), the vehicle-to-road orientation (

ϕ_{r v}

θ_{r v}

), the single-track model, and the over-road longitudinal and vertical velocities (

v_{e r, x}^{r}

v_{r v, z}^{r}

), the following measurement model is obtained (see Appendix A for a thorough derivation):

\begin{matrix} y_{1} = [\begin{matrix} {\tilde{v}}_{x}^{v} + h_{p} {\dot{θ}}_{r v} \end{matrix}] & = v_{e v, x}^{m} + ψ_{m v} [{\tilde{v}}_{y}^{v}] - θ_{m v} [{\tilde{v}}_{z}^{v}] \\ y_{2} = [\begin{matrix} {\tilde{v}}_{y}^{v} - h_{r} {\dot{θ}}_{r v} \end{matrix}] & = v_{e v, y}^{m} - ψ_{m v} [{\tilde{v}}_{x}^{v} + S G v_{e r, x}^{r} {\tilde{f}}_{i v, x}^{m}] + ϕ_{m v} [{\tilde{v}}_{z}^{v} + S G v_{e r, x}^{r} ({\tilde{f}}_{i v, z}^{m} + {\tilde{f}}_{i v, y}^{m} ϕ_{r v})] \\ - θ_{m v} [S G v_{e r, x}^{r} ({\tilde{f}}_{i v, z}^{m} ϕ_{r v})] - b_{f_{y}} [S G v_{e r, x}^{r}] + b_{f_{z}} [S G v_{e r, x}^{r} ϕ_{r v}] \\ y_{3} = [{\tilde{v}}_{z}^{v}] & = v_{e v, z}^{m} + θ_{m v} [{\tilde{v}}_{x}^{v} + S G v_{e r, x}^{r} {\tilde{f}}_{i v, x}^{m} ϕ_{r v}^{2}] \\ - ϕ_{m v} [{\tilde{v}}_{y}^{v} + S G v_{e r, x}^{r} ({\tilde{f}}_{i v, z}^{m} ϕ_{r v} + {\tilde{f}}_{i v, y}^{m} ϕ_{r v}^{2})] + ψ_{m v} [S G v_{e r, x}^{r} {\tilde{f}}_{i v, x}^{m} ϕ_{r v}] \end{matrix}

(35)

where

S G

is the side-slip angle gradient,

h_{r}

and

h_{p}

are the height of the origin of the vehicle coordinate system with respect to the roll and pitch axes, and

\begin{matrix} {\tilde{v}}_{x}^{v} & = v_{e r, x}^{r} - v_{r v, z}^{r} θ_{r v} \end{matrix}

(36)

\begin{matrix} {\tilde{v}}_{y}^{v} & = l_{r} ω_{z}^{m} - S G v_{e r, x}^{r} ({\tilde{f}}_{i v, y}^{m} - {\tilde{f}}_{i v, z}^{m} ϕ_{r v}) + v_{r v, z}^{r} ϕ_{r v} \end{matrix}

(37)

\begin{matrix} {\tilde{v}}_{z}^{v} & = v_{r v, z}^{r} + v_{e r, x}^{r} θ_{r v} - (l_{r} ω_{z}^{m} - S G v_{e r, x}^{r} ({\tilde{f}}_{i v, y}^{m} - {\tilde{f}}_{i v, z}^{m} ϕ_{r v})) ϕ_{r v} \end{matrix}

(38)

In (35), the terms in the square brackets (

[. . .]

) are known, and the unknown states and parameters are highlighted in blue and red, respectively.

Combining the differential equations in (33) with the measurement model (35), the system can be represented in a state-affine form as follows:

\begin{matrix} \{\begin{matrix} \dot{x} & = A (u) x + b (u) + Φ ρ \\ y & = C x + Ψ (y) ρ \end{matrix}, \end{matrix}

(39)

where

\begin{matrix} x & = {(sin θ_{n m}, sin ϕ_{n m} cos θ_{n m}, v_{e v, x}^{m}, v_{e v, y}^{m}, v_{e v, z}^{m}, cos θ_{n m} cos ϕ_{n m})}^{T} \end{matrix}

(40)

\begin{matrix} y & = {({\tilde{v}}_{x}^{v} + h_{p} {\dot{θ}}_{r v}, {\tilde{v}}_{y}^{v} - h_{r} {\dot{θ}}_{r v}, {\tilde{v}}_{z}^{v})}^{T} \end{matrix}

(41)

\begin{matrix} u & = {(ω_{x}^{m}, ω_{y}^{m}, ω_{z}^{m}, {\tilde{f}}_{i v, x}^{m}, {\tilde{f}}_{i v, y}^{m}, {\tilde{f}}_{i v, z}^{m})}^{T} \end{matrix}

(42)

\begin{matrix} ρ & = {(ϕ_{m v}, θ_{m v}, ψ_{m v}, b_{f_{x}}, b_{f_{y}}, b_{f_{z}})}^{T} \end{matrix}

(43)

5.2. Estimator Design

In this section, an adaptive estimator, i.e., the regularised adaptive Kalman filter [35,36], is proposed to address the extrinsic calibration between the IMU and vehicle coordinate systems. First, a discretisation is applied to transform the continuous-time system into a discrete-time system and, then, the adaptive estimator is designed based on the discretized system. Furthermore, the conditions that guarantee the convergence of the estimates are outlined. Finally, the integration of additional information beyond the one explicitly described in the system model is discussed. Specifically, the use of constraints determined during a standstill phase is proposed in order to improve the performance of the estimator.

5.2.1. Discrete-Time Implementation

Most systems in the real world are characterized with continuous-time dynamics. Nevertheless, estimators are customarily run in digital computers and, hence, continuous-time system are frequently discretised. Bearing in mind that the car body motion commonly lies within the range of 1–2 Hz [37], 100 Hz is chosen as the sampling frequency. Assuming that the specific force and angular rates remain constant between samples, the exact discretisation of (39) may be obtained as described in [3]. However, in order to reduce the computational load, the discretisation may be simplified in practice by using a first-order approximation or a lookup table.

The resulting discretised system is presented below:

\begin{matrix} \{\begin{matrix} x_{k} = A_{d} (u_{k}) x_{k - 1} + b_{d} (u_{k}) + Φ_{d} (u_{k}) ρ \\ y_{k} = C x_{k} + Ψ (y_{k}) ρ \end{matrix}, \end{matrix}

(44)

where

A_{d}

b_{d}

and

Φ_{d}

are the matrices that describe the discrete-time system, and

x_{k} = x (t_{k})

u_{k} = u (t_{k})

and

y_{k} = y (t_{k})

Note that

u_{k}

and

y_{k}

are known at any

t_{k}

and, therefore, the system can be considered as a discrete-time linear time-varying system [38]:

\begin{matrix} \{\begin{matrix} x_{k} = A_{d, k} x_{k - 1} + b_{d, k} + Φ_{d, k} ρ \\ y_{k} = C x_{k} + Ψ_{k} ρ \end{matrix} \end{matrix}

(45)

5.2.2. Algorithm Description

The proposed algorithm to estimate the misalignment between the IMU and the vehicle coordinate systems is built upon the discrete-time state-space representation in (45). On the one hand, the angular rates and specific forces supplied by the IMU are used for the state propagation, based on the kinematic differential equations from (33). On the other hand, the measurement model (35) uses the IMU–vehicle misalignment in order to relate the information provided by the over-road longitudinal and vertical velocities, as well as the single-track model to the propagated velocities. The resulting discrete-time system is represented in a state-affine form, allowing thereby the application of the regularised adaptive Kalman filter.

The structure of the regularised adaptive Kalman filter can be broken down into two parts. First, the algorithm is built upon a linear Kalman filter, which carries out the estimation of the states assuming known parameters. Second, the estimator is bolstered by an RLS-based adaptation law, which addresses the estimation of the parameters. The algorithm is shown in Algorithm 2. The state propagation, (a.1)–(a.2), is equivalent to that of the linear Kalman filter. In this part, the IMU signals are used to compute the predicted state estimates. The computation of the Kalman gain and updated error covariance takes place in (a.3)–(a.4), which is identical to the Kalman filter. The RLS-based adaptation law can be found in (a.5)–(a.13), where the gain

Γ_{k}

for the parameter correction is computed. Finally, the state and parameter estimates are updated based on the measured signals in (a.14)–(a.16). Then, the recursion repeats using the next measurement.

Algorithm 2 Regularised Adaptive Kalman Filter (RAKF)
	INITIALIZATION
1	$P_{0}^{+} = P_{0}$ ; $Υ_{0} = 0$ ; $S_{0 \| 0} = S_{1 \| 0} = S_{0}$ ; ${\hat{x}}_{0 \| 0} = {\hat{x}}_{0}$ ; ${\hat{ρ}}_{0} = {\hat{ρ}}_{0}$
	RECURSION
2	$k = k + 1$
	Prediction
3	${\hat{x}}_{k \| k - 1} = A_{k} {\hat{x}}_{k - 1 \| k - 1} + B_{k} u_{k} + Φ_{k} {\hat{ρ}}_{k - 1}$ %State prediction as in the KF	(a.1)
4	$P_{k \| k - 1} = A_{k} P_{k - 1 \| k - 1} A_{k}^{T} + Q_{k}$ %Covariance prediction as in the KF	(a.2)
	Innovation
5	$Σ_{k} = C_{k} P_{k \| k - 1} C_{k}^{T} + R_{k}$ %Predicted measurement covariance as in the KF	(a.3)
6	$K_{k} = P_{k \| k - 1} C_{k}^{T} Σ_{k}^{- 1}$ %Kalman Gain as in the KF	(a.4)
7	$P_{k \| k} = [I_{n} - K_{k} C_{k}] P_{k \| k - 1}$ %Covariance innovation as in KF	(a.5)
8	$Ω_{k} = C_{k} A_{k} Υ_{k - 1} + C_{k} Φ_{k} + Ψ_{k}$ %Aux. var. (represents excitation for parameter estimation)	(a.6)
9	$Λ_{k} = {[Σ_{k} + Ω_{k} S_{k \| k - 1} Ω_{k}^{T}]}^{- 1}$ %Var. for simplification of (a.8) and (a.9)	(a.7)
10	$Γ_{k} = S_{k \| k - 1} Ω_{k}^{T} Λ_{k}$ %Parameter gain as in RLS	(a.8)
11	$S_{k \| k} = S_{k \| k - 1} - S_{k \| k - 1} Ω_{k}^{T} Λ_{k} Ω_{k} S_{k \| k - 1}$ %Parameter covariance update as in the RLS	(a.9)
12	${\bar{S}}_{k + 1 \| k} = \frac{1}{λ} S_{k \| k}$ %Parameter covariance after forgetting	(a.10)
13	$S_{k + 1 \| k} = {\bar{S}}_{k + 1 \| k} {[I + (1 - λ) Σ_{R} {\bar{S}}_{k + 1 \| k}]}^{- 1}$ %Parameter covariance after regularization	(a.11)
14	$Υ_{k} = [I - K_{k} C_{k}] A_{k} Υ_{k - 1} + [I - K_{k} C_{k}] Φ_{k} - K_{k} Ψ_{k}$ %Aux. var.	(a.12)
15	${\tilde{y}}_{k} = y_{k} - C_{k} {\hat{x}}_{k \| k - 1} - Ψ_{k} {\hat{ρ}}_{k - 1}$ %Measurement error (meas-pred)	(a.13)
16	$Δ ρ_{k} = Γ_{k} {\tilde{y}}_{k}$ %Parameter correction	(a.14)
17	${\hat{ρ}}_{k} = {\hat{ρ}}_{k - 1} + Δ ρ_{k}$ %Parameter update	(a.15)
18	${\hat{x}}_{k \| k} = {\hat{x}}_{k \| k - 1} + K_{k} {\tilde{y}}_{k} + Υ_{k} Δ ρ_{k}$ %State update	(a.16)
	${\hat{x}}_{k \| k - 1}$ , ${\hat{x}}_{k \| k}$ : A priori and a posteriori state estimates.
	$P_{k \| k - 1}$ , $P_{k \| k}$ : A priori and a posteriori state error covariance matrices.

The exponential convergence of the state and parameter estimates is guaranteed provided that the

[A_{k}, C_{k}]

pair is uniformly completely observable, the

[A_{k}, Q_{k}^{\frac{1}{2}}]

pair is uniformly completely controllable [39], and the system is being persistently excited:

Definition 2.

The matrices

A_{k}

C_{k}

Φ_{k}

and

Ψ_{k}

are persistently exciting if there exist an integer

h > 0

and a real constant

α > 0

such that, for all integer

j \geq 0

, the matrix sequence

Ω_{k}

driven by

A_{k}

C_{k}

Φ_{k}

and

Ψ_{k}

through the linear system (a.6) and (a.12), satisfies

\sum_{k = j + 1}^{j + h} W_{p, k} \geq α I, where W_{p, k} ≜ Ω_{k}^{T} Σ_{k}^{- 1} Ω_{k}

(46)

The regularization algorithm integrated in the adaptation law (a.10) enhances the robustness of the estimator so that, despite poor excitation, acceptable estimates are supplied. More precisely, it prevents the so-called covariance wind-up phenomenon. During phases of poor excitation, when the information is insufficient to determine all parameters, some elements of

S_{k | k}

tend to grow unlimited, which may lead to an aggressive behaviour and large estimation errors. The regularization algorithm sets bounds to this growth while keeping the exponential convergence properties of the system when excitation is available. This results in a more reliable and accurate estimation of both states and parameters.

In [3], it was already shown that, assuming known parameters, the system is always observable. Hence, the

[A_{k}, C_{k}]

pair is uniformly completely observable. Furthermore, it was also shown that, since

Q_{k}

is a positive definite matrix, the

[A_{k}, Q_{k}^{\frac{1}{2}}]

pair is uniformly completely controllable. In spite of regularization, sufficient excitation is required for the parameter convergence. Ensuring that, during a calibration manoeuvre, the persistent excitation condition is met guarantees the convergence of the parameter estimates. This may easily be investigated by computing the excitation matrix

W_{p, k}

online and analysing how close it is to singularity. For this purpose, the determinant is used. A

\det (W_{p, k})

close to zero indicates that the excitation is poor while large values of

\det (W_{p, k})

imply that the information is sufficient to estimate all the parameters.

5.2.3. Estimator with Constraints

The specific force during standstill corresponds with the reaction to the acceleration due to gravity:

\begin{matrix} f_{i v}^{v} = - g^{v} \end{matrix}

(47)

Hence, if the attitude angles (pitch and roll) of the vehicle coordinate system are known, the values of each specific force component can be deduced. This relationship can be exploited during the calibration procedure to incorporate additional information to the estimator. Before or after the calibration manoeuvre, the vehicle could remain stationary in an area where it is feasible to identify its attitude angles. An option would be to stand still on a horizontal surface (normal to gravity) and infer the specific force components in the vehicle coordinate system from the vehicle levels:

\begin{matrix} f_{i v, x 0}^{v} & = - g sin θ_{r v} \\ f_{i v, y 0}^{v} & = g sin ϕ_{r v} cos θ_{r v} \\ f_{i v, z 0}^{v} & = g cos ϕ_{r v} cos θ_{r v} \end{matrix}

(48)

Another option could be to identify the attitude angles of the vehicle coordinate system using perception sensors.

This information yields a relationship between biases and misalignment angles:

\begin{matrix} (\begin{matrix} b_{f_{x}} \\ b_{f_{y}} \\ b_{f_{z}} \end{matrix}) = (\begin{matrix} {\tilde{f}}_{i v, x 0}^{m} \\ {\tilde{f}}_{i v, y 0}^{m} \\ {\tilde{f}}_{i v, z 0}^{m} \end{matrix}) - (\begin{matrix} f_{i v, x 0}^{v} \\ f_{i v, y 0}^{v} \\ f_{i v, z 0}^{v} \end{matrix}) + (\begin{matrix} 0 & - f_{i v, z 0}^{v} & f_{i v, y 0}^{v} \\ f_{i v, z 0}^{v} & 0 & - f_{i v, x 0}^{v} \\ - f_{i v, y 0}^{v} & f_{i v, x 0}^{v} & 0 \end{matrix}) (\begin{matrix} ϕ_{m v} \\ θ_{m v} \\ ψ_{m v} \end{matrix}), \end{matrix}

(49)

which can be integrated in the estimator in the form of constraints to improve the estimation performance. One way of doing this is to integrate the parameter equality constraint as an artificial measurement [40]:

\begin{matrix} y_{4} & = {\tilde{f}}_{i v, x 0}^{m} - f_{i v, x 0}^{v} = f_{i v, z 0}^{v} θ_{m v} - f_{i v, y 0}^{v} ψ_{m v} + b_{f_{x}} \\ y_{5} & = {\tilde{f}}_{i v, y 0}^{m} - f_{i v, y 0}^{v} = - f_{i v, z 0}^{v} ϕ_{m v} + f_{i v, x 0}^{v} ψ_{m v} + b_{f_{y}} \\ y_{6} & = {\tilde{f}}_{i v, z 0}^{m} - f_{i v, z 0}^{v} = f_{i v, y 0}^{v} ϕ_{m v} - f_{i v, x 0}^{v} θ_{m v} + b_{f_{z}} \end{matrix}

(50)

Please note that these additional equations are affine with respect to the parameters. Hence, their integration in the system will not distort the system structure of (45). The resulting system will therefore take the following form:

\begin{matrix} \{\begin{matrix} x_{k} = A_{d, k} x_{k - 1} + b_{d, k} + Φ_{d, k} ρ \\ y_{c, k} = C_{c} x_{k} + Ψ_{c, k} ρ \end{matrix}, \end{matrix}

(51)

where

y_{c, k} = ({\tilde{v}}_{x, k}^{v} + h_{p} {\dot{θ}}_{r v, k}, {\tilde{v}}_{y, k}^{v} - h_{r} {\dot{θ}}_{r v, k}, {\tilde{v}}_{z, k}^{v}, y_{4, k}, y_{5, k}, y_{6, k})

5.3. Experimental Results

In order to evaluate the performance of the described algorithm, an experimental validation is conducted. The test vehicle is a rear-wheel drive car equipped with the ground-truth system (described in Section 1.2), an automotive-grade MEMS 6D IMU and series chassis sensors. The analysed data have been generated during figure eight manoeuvres, which provide a high level of excitation and stimulate the individual axes equally in both directions. Unlike for the ground-truth–IMU relative orientation estimation, a restriction for the maximum lateral excitation is set in the execution of the manoeuvre so that the vehicle remains within the validity region of the single-track model (

| {\tilde{f}}_{i v, y}^{m} | ⪅ 4 \frac{m}{s^{2}}

Two different estimator designs are compared:

(A): The regularized adaptive Kalman filter (Algorithm 2) with artificial measurement constraints, according to (51).
(B): The regularized adaptive Kalman filter (Algorithm 2) without constraints, according to (45).

The aim is to illustrate the benefits of exploiting the additional standstill information, which is not explicitly given by the original system model.

Regarding the parametrisation of the estimator, the choice of the regularized adaptive Kalman filter parameters (Algorithm 2) is shown in Table 4. The parametrisation was selected based on the sensor specifications as well as data-driven evaluations of the proposed adaptive estimator. Moreover, a first-order approximation has been used to obtain the discretised system.

In order to validate the results of the misalignment estimates, these are compared against the misalignment angles obtained from the ground-truth–IMU and –vehicle extrinsic calibration, shown in Table 5.

The inputs of the estimator are depicted in Figure 13. Please note that the plotted angular rates (

ω_{x}

ω_{y}

ω_{z}

) are bias-compensated. Furthermore, note that the lateral specific force mostly remains within the region of

| {\tilde{f}}_{i v, y}^{m} | < 4 \frac{m}{s^{2}}

. The corresponding estimation results are shown in Figure 14.

First, let us focus on the estimator without constraints, i.e., (B). Especially conspicuous in the plot is the fact that the determinant of the persistence excitation matrix is close to zero for this estimator (see last sub-plot), indicating that there is not sufficient information in order to properly infer all parameters. This may explain the unsteady behaviour of the misalignment estimates. Furthermore, the parameter estimates do not converge to the values computed from the ground-truth–IMU and –vehicle extrinsic calibration from Table 5. Hence, one can conclude that the outputs of (45) do not carry sufficient information to reach a satisfactory estimation of all the parameters, additional knowledge is required. In view of these results, estimator (B) is not considered in further analyses.

Things look different for the estimator with constraints, i.e., (A). Particularly noteworthy is the determinant of the persistence excitation matrix, which is not close to zero any more. This indicates that the estimator has enough information to properly infer all parameters. This may well be the reason for the steady behaviour exhibited by the parameter estimates, which stands in stark contrast with the results of the estimator without constraints, i.e., (B). As for the comparison of the results with respect to the values computed from the ground-truth–IMU and –vehicle extrinsic calibration (Table 5), the final estimated parameters are fairly close. The differences in

ϕ_{m v}

θ_{m v}

and

ψ_{m v}

are as low as 0.004, 0.056 and 0.067 deg, respectively. Hence, one can conclude that the integration of the additional standstill information in the form of constraints has improved the estimation results by making the system persistently excited during the calibration manoeuvre.

Additionally, in order to assess the ability of (A) to follow artificially introduced misalignments, the algorithm has also been run on corrupted data, where misalignments of

(0.7, 0.5, 1.2) \deg

and accelerometer biases of

(0.1, - 0.1, - 0.2) m / s^{2}

have been added to the original data. For the corrupted dataset, the misalignment estimates increased by 0.670, 0.503 and 1.212

\deg

for

ϕ_{m v}

θ_{m v}

and

ψ_{m v}

, respectively. Hence, the results show that, despite the artificial biases, the algorithm is able to track the added misalignments.

Finally, the estimator (A) has been run using two additional data sets collected on different days: a less dynamic figure eight drive with

| {\tilde{f}}_{y \max}^{m} | \approx 3

\frac{m}{s^{2}}

(

M 2

) and a more dynamic one in which

| {\tilde{f}}_{y \max}^{m} | \approx 5

\frac{m}{s^{2}}

(

M 3

). This aims at analysing the variability of the misalignment estimates within a collection of different figure eight drives and, moreover, at assessing the effect of moderate deviations in the manoeuvre execution. The resulting misalignment estimates from these manoeuvres together with the ones obtained from the manoeuvre in Figure 13 (

M 1

) are displayed in Table 6. The maximum differences are as low as

0.035

0.0530

and

0.021 \deg

for

ϕ_{m v}

θ_{m v}

and

ψ_{m v}

, respectively. Furthermore, all results remain very close to the misalignments obtained from the

m - g

and

v - g

extrinsic calibration. Hence, one concludes that the estimation results remain consistent in spite of variations in the manoeuvre execution.

6. Conclusions

The present paper addresses the extrinsic calibration of a vehicle equipped with series chassis sensors and an automotive-grade IMU. Specifically, it proposes a method to estimate the misalignment between the IMU and vehicle coordinate systems. Unfortunately, due to tolerances in the manufacturing processes, the transformation between the IMU and vehicle coordinate systems is not perfectly known—while, due to the low angular motion of land vehicles, the positioning errors may be neglected, and misalignment errors have a significant impact on the motion estimates.

One of the challenges that arises when evaluating the performance of such an extrinsic calibration procedure is the fact that there is no simple way to measure the real misalignment. In this work, a reference is proposed based on a high-precision INS/GNSS system, which is taken as a ground-truth for the motion variables. Two calibration procedures to estimate the 3D rotation between the ground-truth and both the IMU and vehicle coordinate systems have been described and experimentally validated. The combination of these two rotations yields a reference for the 3D rotation between the IMU and vehicle coordinate systems.

The IMU–vehicle misalignment estimation has been addressed as a joint state and parameter estimation problem represented in a state-affine form. The approach relies on measurements from odometry, suspension and an automotive-grade IMU fused in a regularised adaptive Kalman filter. It has been experimentally shown with figure eight drives that the sole use of odometry, suspension and a single-track model does not supply enough excitation to determine all states and parameters. Additional information is required. In order to improve this aspect, a standstill phase, in which the vehicle attitude is known, is used to incorporate further information in the form of artificial measurements. Experimental results show that integrating this additional information decisively improves the performance of the estimator. The persistence excitation condition is satisfied and the estimated misalignments are consistent with the results obtained in the ground-truth–IMU and –vehicle calibration procedures. Additionally, the estimator is able to follow artificial variations in the misalignment angles and the variability of the estimates remains low despite alterations in the manoeuvre execution.

The results presented in this article show that the proposed calibration procedure is a robust and industrially viable method for the IMU–vehicle misalignment estimation. Unlike other errors, such as the time-varying IMU biases, the misalignment between the IMU and the vehicle is unlikely to change over time. In a series product, not equipped with a ground-truth system, the proposed IMU–vehicle misalignment estimation may be integrated into the end-of-line calibration of the vehicle. The calibration manoeuvre can be standardized and driven manually or automatically at the end of the production process. Moreover, the same method may also be used as a re-calibration procedure in the case of structural modifications in the vehicle or its sensors, such as after replacing the IMU. The identified misalignment angles may then be incorporated as parameters in a vehicle motion estimation module, which, based on misalignment-compensated IMU signals, supplies estimates of motion quantities during operation. It is left to the vehicle motion estimation algorithm running online to account for time-varying errors, such as the IMU in-run biases or the biases induced by ageing. This topic takes up a central question in [3], in which a method for the simultaneous estimation of the vehicle motion states and IMU biases is proposed and experimentally validated.

The usefulness of the knowledge of the 3D rotation between the ground-truth and both the IMU and vehicle coordinate systems goes beyond that of supplying a reference for the IMU–vehicle misalignment estimates. These transformations are required for a consistent evaluation of motion estimation algorithms based on the ground-truth.

In the proposed calibration procedure, GNSS and perception sensor information have not been used. However, if the vehicle is equipped with cameras, lidars, radars or accurate GNSS receivers, their information may be integrated in the calibration procedure in order to enhance its performance. However, this would come at a cost. A considerable increase in complexity and quite some effort in guaranteeing the reliability of the information supplied by these additional sensors is to be expected.

One of the limitations of the proposed approach is the fact that not all IMU systematic error contributions are considered, such as scaling factor, non-orthogonality or nonlinearity error components. Certainly, if these contributions are significant, the accuracy of the misalignment estimates may degrade. In order to mitigate the effects of these error contributions, an IMU end-of-line calibration procedure should be conducted by the sensor supplier prior to the IMU assembly in the vehicle [41,42]. Furthermore, the specific orientation of the vehicle coordinate system within the vehicle body may slightly vary due to effects such as tire wear, load distribution or suspension warp. These uncertainties have been neglected in this work since significant changes are not expected in normal operation conditions (no faults nor extreme wear), which has also been supported by the analysed data. However, a thorough analysis on a relatively large vehicle fleet over a large time span could be carried out to investigate these effects. Conclusions may be drawn regarding the frequency in which a misalignment calibration is needed and the particular events that should trigger it. This may be the subject of future work.

Author Contributions

Conceptualization, V.R.M., J.K.; Experiment design and execution, V.R.M., J.K.; Formal analysis, V.R.M., J.K., T.S.; Data analysis, V.R.M.; Software, V.R.M.; Writing–Original draft preparation, V.R.M.; Writing–Review & editing, J.K., T.S., J.R.; Supervision, J.K., T.S., J.R. All authors have read and agreed to the published version of the manuscript.

Funding

We acknowledge the support by the German Research Foundation and the Open Access Publication Fund of TU Berlin.

Conflicts of Interest

The authors declare no conflict of interest.

Appendix A. Derivation of (35)

Using the pitch and roll angles (

θ_{r v}

ϕ_{r v}

) with respect to the road plane and bearing in mind that they remain small, the vehicle velocities may be obtained from the over-road velocities as follows [3]:

\begin{matrix} \begin{matrix} v_{e v, x}^{v} & = v_{e r, x}^{r} - v_{r v, z}^{r} θ_{r v} + h_{p} {\dot{θ}}_{r v} \\ v_{e v, y}^{v} & = v_{e r, y}^{r} + v_{r v, z}^{r} ϕ_{r v} - h_{r} {\dot{ϕ}}_{r v} \\ v_{e v, z}^{v} & = v_{r v, z}^{r} + v_{e r, x}^{r} θ_{r v} - v_{e r, y}^{r} ϕ_{r v} \end{matrix} \end{matrix}

(A1)

where

h_{r}

and

h_{p}

are the height of the origin of the vehicle coordinate system with respect to the roll and pitch axes, respectively.

Both

v_{e r, x}^{r}

and

v_{r v, z}^{r}

are primarily obtained from the odometry and suspension signals, respectively.

v_{e r, y}^{r}

could be provided by a sensor measuring the over-road lateral velocity, such as an optic sensor. Nevertheless, in the current work, we assume that such sensor is not available. Instead, this information is obtained from a single-track model, which provides a simplified but plausible description of the vehicle lateral motion within the region of lateral accelerations up to 4

m / s^{2}

under dry road conditions [37]:

v_{e r, y}^{r} = l_{r} ω_{z}^{r} - S G v_{e r, x}^{r} f_{i r, y}^{r},

(A2)

where the relationship between

f_{i r, y}^{r}

and

f_{i v}^{v}

may be expressed as follows

f_{i r, y}^{r} = f_{i v, y}^{v} - f_{i v, z}^{v} ϕ_{r v} + {\ddot{ϕ}}_{r v} h_{r}

(A3)

Bearing in mind the low angular motion of land vehicles, particularly in the pitch and roll direction:

\begin{matrix} ω_{z}^{r} & \approx ω_{z}^{m} \end{matrix}

(A4)

\begin{matrix} {\ddot{ϕ}}_{r v} & \approx 0 \end{matrix}

(A5)

\begin{matrix} f_{i r, y}^{r} & \approx f_{i v, y}^{r} = f_{i v, y}^{v} - f_{i v, z}^{v} ϕ_{r v} \end{matrix}

(A6)

Due to the misalignment error, the specific forces and angular rates supplied by the IMU are not expressed in v but in m. Using the rotation matrix

R_{v}^{m}

, the specific force is transformed from the set of resolving axes of coordinate system m to the set of resolving axes of coordinate system v

\begin{matrix} f_{i v, y}^{v} = f_{i v, y}^{m} - ψ_{m v} f_{i v, x}^{m} + ϕ_{m v} f_{i v, z}^{m} \\ f_{i v, z}^{v} = f_{i v, z}^{m} - ϕ_{m v} f_{i v, y}^{m} + θ_{m v} f_{i v, x}^{m} \end{matrix}

(A7)

Furthermore, substituting (A7) in (A4) results in

f_{i v, y}^{r} = (f_{i v, y}^{m} - ψ_{m v} f_{i v, x}^{m} + ϕ_{m v} f_{i v, z}^{m}) - (f_{i v, z}^{m} - ϕ_{m v} f_{i v, y}^{m} + θ_{m v} f_{i v, x}^{m}) ϕ_{r v},

(A8)

Now, substituting (A8) in (A2) yields

v_{e r, y}^{r} = l_{r} ω_{z}^{m} - S G v_{e r, x}^{r} (f_{i v, y}^{m} - ψ_{m v} f_{i v, x}^{m} + ϕ_{m v} f_{i v, z}^{m} - f_{i v, z}^{m} ϕ_{r v} + ϕ_{m v} f_{i v, y}^{m} ϕ_{r v} - θ_{m v} f_{i v, x}^{m} ϕ_{r v}),

(A9)

and combining (A9) and (A1) gives

\begin{matrix} v_{e v, x}^{v} & = v_{e r, x}^{r} - v_{r v, z}^{r} θ_{r v} + h_{p} {\dot{θ}}_{r v} \\ v_{e v, y}^{v} & = l_{r} ω_{z}^{m} - S G v_{e r, x}^{r} (f_{i v, y}^{m} - ψ_{m v} f_{i v, x}^{m} + ϕ_{m v} f_{i v, z}^{m} - f_{i v, z}^{m} ϕ_{r v} + ϕ_{m v} f_{i v, y}^{m} ϕ_{r v} - θ_{m v} f_{i v, x}^{m} ϕ_{r v}) + v_{r v, z}^{r} ϕ_{r v} - h_{r} {\dot{ϕ}}_{r v} \\ v_{e v, z}^{v} & = v_{r v, z}^{r} + v_{e r, x}^{r} θ_{r v} \\ - (l_{r} ω_{z}^{m} - S G v_{e r, x}^{r} (f_{i v, y}^{m} - ψ_{m v} f_{i v, x}^{m} + ϕ_{m v} f_{i v, z}^{m} - f_{i v, z}^{m} ϕ_{r v} + ϕ_{m v} f_{i v, y}^{m} ϕ_{r v} - θ_{m v} f_{i v, x}^{m} ϕ_{r v})) ϕ_{r v}, \end{matrix}

(A10)

On the other hand, using the rotation matrix

R_{v}^{m}

and applying the small-angle approximation, the following relationship is obtained:

\begin{matrix} v_{e v, x}^{m} & = v_{e v, x}^{v} - ψ_{m v} v_{e v, y}^{v} + θ_{m v} v_{e v, z}^{v} \\ v_{e v, y}^{m} & = v_{e v, y}^{v} + ψ_{m v} v_{e v, x}^{v} - ϕ_{m v} v_{e v, z}^{v} \\ v_{e v, z}^{m} & = v_{e v, z}^{v} - θ_{m v} v_{e v, x}^{v} + ϕ_{m v} v_{e v, y}^{v}, \end{matrix}

(A11)

Rearranging (A11) yields

\begin{matrix} v_{e v, x}^{v} & = v_{e v, x}^{m} + ψ_{m v} v_{e v, y}^{v} - θ_{m v} v_{e v, z}^{v} \\ v_{e v, y}^{v} & = v_{e v, y}^{m} - ψ_{m v} v_{e v, x}^{v} + ϕ_{m v} v_{e v, z}^{v} \\ v_{e v, z}^{v} & = v_{e v, z}^{m} + θ_{m v} v_{e v, x}^{v} - ϕ_{m v} v_{e v, y}^{v} \end{matrix}

(A12)

Substituting (A10) in (A12) results in

\begin{matrix} v_{x}^{v^{'}} + h_{p} {\dot{θ}}_{r v} & = v_{e v, x}^{m} + ψ_{m v} v_{y}^{v^{'}} - θ_{m v} v_{z}^{v^{'}} \\ v_{y}^{v^{'}} - h_{r} {\dot{ϕ}}_{r v} - S G v_{e r, x}^{r} (- ψ_{m v} f_{i v, x}^{m} + ϕ_{m v} f_{i v, z}^{m} + ϕ_{m v} f_{i v, y}^{m} ϕ_{r v} - θ_{m v} f_{i v, x}^{m} ϕ_{r v}) & = v_{e v, y}^{m} - ψ_{m v} v_{x}^{v^{'}} + ϕ_{m v} v_{z}^{v^{'}} \\ v_{z}^{v^{'}} + S G v_{e r, x}^{r} (- ψ_{m v} f_{i v, x}^{m} + ϕ_{m v} f_{i v, z}^{m} + ϕ_{m v} f_{i v, y}^{m} ϕ_{r v} - θ_{m v} f_{i v, x}^{m} ϕ_{r v}) ϕ_{r v} & = v_{e v, z}^{m} + θ_{m v} v_{x}^{v^{'}} - ϕ_{m v} v_{y}^{v^{'}}, \end{matrix}

(A13)

where

\begin{matrix} v_{x}^{v^{'}} & = v_{e r, x}^{r} - v_{r v, z}^{r} θ_{r v} \end{matrix}

(A14)

\begin{matrix} v_{y}^{v^{'}} & = l_{r} ω_{z}^{m} - S G v_{e r, x}^{r} (f_{i v, y}^{m} - f_{i v, z}^{m} ϕ_{r v}) + v_{r v, z}^{r} ϕ_{r v} \end{matrix}

(A15)

\begin{matrix} v_{z}^{v^{'}} & = v_{r v, z}^{r} + v_{e r, x}^{r} θ_{r v} - (l_{r} ω_{z}^{m} - S G v_{e r, x}^{r} (f_{i v, y}^{m} - f_{i v, z}^{m} ϕ_{r v})) ϕ_{r v}, \end{matrix}

(A16)

Finally, after taking into account the accelerometer biases, neglecting second-order error terms and rearranging, the measurement equations of (35) are obtained.

References

Farrell, J.A.; Roysdon, P.F. Advanced vehicle state estimation: A tutorial and comparative study. IFAC-Pap. OnLine 2017, 50, 15971–15976. [Google Scholar] [CrossRef]
Singh, K.B.; Arat, M.A.; Taheri, S. Literature review and fundamental approaches for vehicle and tire state estimation. Veh. Syst. Dyn. 2018, 57, 1643–1665. [Google Scholar] [CrossRef]
Marco, V.R.; Kalkkuhl, J.; Raisch, J.; Scholte, W.J.; Nijmeijer, H.; Seel, T. Multi-modal sensor fusion for highly accurate vehicle motion state estimation. Control Eng. Pract. 2020, 100, 104409. [Google Scholar] [CrossRef]
Mirzaei, F.M.; Roumeliotis, S.I. A Kalman filter-based algorithm for IMU-camera calibration: Observability analysis and performance evaluation. IEEE Trans. Robot. 2008, 24, 1143–1156. [Google Scholar] [CrossRef]
Brookshire, J.; Teller, S. Extrinsic calibration from per-sensor egomotion. Robot. Sci. Syst. VIII 2013, 5, 504–512. [Google Scholar]
Heng, L.; Li, B.; Pollefeys, M. Camodocal: Automatic intrinsic and extrinsic calibration of a rig with multiple generic cameras and odometry. In Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo, Japan, 3–7 November 2013; pp. 1793–1800. [Google Scholar]
Carrera, G.; Angeli, A.; Davison, A.J. SLAM-based automatic extrinsic calibration of a multi-camera rig. In Proceedings of the 2011 IEEE International Conference on Robotics and Automation, Shanghai, China, 9–13 May 2011; pp. 2652–2659. [Google Scholar]
Mirzaei, F.M.; Kottas, D.G.; Roumeliotis, S.I. 3-D lidar–camera intrinsic and extrinsic calibration: Identifiability and analytical least-squares-based initialization. Int. J. Robot. Res. 2012, 31, 452–467. [Google Scholar] [CrossRef] [Green Version]
Gao, C.; Spletzer, J.R. On-line calibration of multiple lidars on a mobile vehicle platform. In Proceedings of the 2010 IEEE International Conference on Robotics and Automation, Anchorage, AK, USA, 3–7 May 2010; pp. 279–284. [Google Scholar]
Le Gentil, C.; Vidal-Calleja, T.; Huang, S. 3-D lidar–IMU calibration based on upsampled preintegrated measurements for motion distortion correction. In Proceedings of the 2018 IEEE International Conference on Robotics and Automation (ICRA), Brisbane, QLD, Australia, 21–25 May 2018; pp. 2149–2155. [Google Scholar]
Censi, A.; Franchi, A.; Marchionni, L.; Oriolo, G. Simultaneous calibration of odometry and sensor parameters for mobile robots. IEEE Trans. Robot. 2013, 29, 475–492. [Google Scholar] [CrossRef]
Wu, J.; Sun, Y.; Wang, M.; Liu, M. Hand-Eye Calibration: 4-D Procrustes Analysis Approach. IEEE Trans. Instrum. Meas. 2019, 69, 2966–2981. [Google Scholar] [CrossRef]
Brunker, A.; Wohlgemuth, T.; Frey, M.; Gauterin, F. Odometry 2.0: A slip-adaptive EIF-based four-wheel- odometry model for parking. IEEE Trans. Intell. Veh. 2018, 4, 114–126. [Google Scholar] [CrossRef]
Klier, W.; Reim, A.; Stapel, D. Robust Estimation of Vehicle Sideslip Angle—An Approach w/o Vehicle and Tire Models. In Proceedings of the SAE World Congress & Exhibition, Detroit, MI, USA, 14–17 April 2008. [Google Scholar]
Selmanaj, D.; Corno, M.; Panzani, G.; Savaresi, S.M. Vehicle sideslip estimation: A kinematic based approach. Control Eng. Pract. 2017, 67, 1–12. [Google Scholar] [CrossRef]
Grip, H.F.; Imsland, L.; Johansen, T.A.; Fossen, T.I.; Kalkkuhl, J.C.; Suissa, A. Nonlinear vehicle side-slip estimation with friction adaptation. Automatica 2008, 44, 611–622. [Google Scholar] [CrossRef]
Katriniok, A.; Abel, D. Adaptive EKF-Based Vehicle State Estimation With Online Assessment of Local Observability. IEEE Trans. Control Syst. Technol. 2016, 24, 1368–1381. [Google Scholar] [CrossRef]
Grip, H.F.; Imsland, L.; Johansen, T.A.; Kalkkuhl, J.C.; Suissa, A. Vehicle sideslip estimation. IEEE Control Syst. Mag. 2009, 29, 36–52. [Google Scholar]
Syed, Z.F.; Aggarwal, P.; Niu, X.; El-Sheimy, N. Civilian vehicle navigation: Required alignment of the inertial sensors for acceptable navigation accuracies. IEEE Trans. Veh. Technol. 2008, 57, 3402–3412. [Google Scholar] [CrossRef]
Vinande, E.; Axelrad, P.; Akos, D. Mounting-angle estimation for personal navigation devices. IEEE Trans. Veh. Technol. 2009, 59, 1129–1138. [Google Scholar] [CrossRef]
Wu, Y.; Goodall, C.; El-Sheimy, N. Self-calibration for IMU/odometer land navigation: Simulation and test results. In Proceedings of the ION International Technical Meeting, San Diego, CA, USA, 25–27 January 2010; Volume 2527. [Google Scholar]
Chen, Q.; Zhang, Q.; Niu, X. Estimate the Pitch and Heading Mounting Angles of the IMU for Land Vehicular GNSS/INS Integrated System. IEEE Trans. Intell. Transp. Syst. 2020. early access. [Google Scholar]
Xue, H.; Guo, X.; Zhou, Z.; Wang, K. In-motion alignment algorithm for vehicle carried sins based on odometer aiding. J. Navig. 2017, 70, 1349–1366. [Google Scholar] [CrossRef]
Wu, Y. Versatile land navigation using inertial sensors and odometry: Self-calibration, in-motion alignment and positioning. In Proceedings of the 2014 DGON Inertial Sensors and Systems (ISS), Karlsruhe, Germany, 16–17 September 2014; pp. 1–19. [Google Scholar]
Huttner, F.; Kalkkuhl, J.; Reger, J. Offset and Misalignment Estimation for the Online Calibration of an MEMS-IMU Using FIR-Filter Modulating Functions. In Proceedings of the 2018 IEEE Conference on Control Technology and Applications (CCTA), Copenhagen, Denmark, 21–24 August 2018; pp. 1427–1433. [Google Scholar]
Groves, P.D. Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems; Artech House: Norwood, MA, USA, 2013. [Google Scholar]
ISO 8855. Road Vehicles—Vehicle Dynamics and Road-Holding Ability– Vocabulary; Tech. Rep. Geneva; CH: International Organization for Standardization: Geneva, Switzerland, 2011. [Google Scholar]
Wendel, J. Integrierte Navigationssysteme: Sensordatenfusion, GPS und Inertiale Navigation; Oldenbourg Verlag: München, Germany, 2011. [Google Scholar]
Quinchia, A.G.; Falco, G.; Falletti, E.; Dovis, F.; Ferrer, C. A Comparison between Different Error Modeling of MEMS Applied to GPS/INS Integrated Systems. Sensors 2013, 13, 9549–9588. [Google Scholar] [CrossRef] [Green Version]
Amirsadri, A.; Kim, J.; Petersson, L.; Trumpf, J. Practical considerations in precise calibration of a low-cost MEMS IMU for road-mapping applications. In Proceedings of the 2012 American Control Conference (ACC), Montreal, QC, Canada, 27–29 June 2012; pp. 881–888. [Google Scholar]
Rodrigo Marco, V.; Kalkkuhl, J.; Raisch, J. EKF for simultaneous vehicle motion estimation and IMU bias calibration with observability-based adaptation. In Proceedings of the 2018 Annual American Control Conference (ACC), Milwaukee, WI, USA, 27–29 June 2018; pp. 6309–6316. [Google Scholar]
Anderson, B.D.; Johnson, C.R., Jr. Exponential convergence of adaptive identification and control algorithms. Automatica 1982, 18, 1–13. [Google Scholar] [CrossRef]
Johnstone, R.M.; Johnson, C.R., Jr.; Bitmead, R.R.; Anderson, B.D. Exponential convergence of recursive least squares with exponential forgetting factor. Syst. Control Lett. 1982, 2, 77–82. [Google Scholar] [CrossRef]
Rhode, S. Robust and Regularized Algorithms for Vehicle Tractive Force Prediction and Mass Estimation; KIT Scientific Publishing: Karlsruhe, Germany, 2018; Volume 62. [Google Scholar]
Rodrigo Marco, V.; Kalkkuhl, J.; Raisch, J.; Seel, T. Regularized adaptive Kalman filter for non-persistently excited systems. 2020. Submitted to Automatica. [Google Scholar]
Zhang, Q. Adaptive Kalman filter for actuator fault diagnosis. Automatica 2018, 93, 333–342. [Google Scholar] [CrossRef]
Schramm, D.; Hiller, M.; Bardini, R. Vehicle Dynamics—Modeling and Simulation; Springer: Berlin/Heidelberg, Germany, 2018. [Google Scholar]
Ticlea, A. Techniques D’Immersion Pour l’Estimation Non Linéaire-Application aux Systèmes de Puissance. Ph.D. Thesis, Institut National Polytechnique de Grenoble-INPG, Grenoble, France, 2006. [Google Scholar]
Jazwinski, A.H. Stochastic Processes and Filtering Theory; Dover Publications, Inc.: Mineola, NY, USA, 2007. [Google Scholar]
Simon, D. Kalman filtering with state constraints: A survey of linear and nonlinear algorithms. IET Control Theory Appl. 2010, 4, 1303–1318. [Google Scholar] [CrossRef] [Green Version]
Rohac, J.; Sipos, M.; Simanek, J. Calibration of low-cost triaxial inertial sensors. IEEE Instrum. Meas. Mag. 2015, 18, 32–38. [Google Scholar] [CrossRef]
Tedaldi, D.; Pretto, A.; Menegatti, E. A robust and easy to implement method for IMU calibration without external equipments. In Proceedings of the 2014 IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, China, 31 May–7 June 2014; pp. 3042–3049. [Google Scholar]

Figure 1. The present work proposes an approach to estimate the 3D rotation between the IMU and vehicle coordinate systems (

R_{m}^{v}

). Additionally, calibration procedures are described to determine the 3D rotation between a ground-truth measurement system and both the IMU and vehicle coordinate systems (

R_{g}^{m}

and

R_{g}^{v}

, respectively). These represent a reference to directly assess the accuracy of the estimated IMU–vehicle 3D rotation.

Figure 1. The present work proposes an approach to estimate the 3D rotation between the IMU and vehicle coordinate systems (

R_{m}^{v}

). Additionally, calibration procedures are described to determine the 3D rotation between a ground-truth measurement system and both the IMU and vehicle coordinate systems (

R_{g}^{m}

and

R_{g}^{v}

, respectively). These represent a reference to directly assess the accuracy of the estimated IMU–vehicle 3D rotation.

Figure 2. Representation of the inertial (i), ECEF (e), local navigation (n), vehicle (v) and road (r) coordinate systems. For a clear depiction, a world and vehicle perspectives are displayed, which corresponds with (a,b), respectively. The black and yellow planes respectively represent the road plane and the plane tangent to the Earth’s ellipsoid, which is considered to be normal to gravity. Please note that n and v share the same origin, and the origin of r lies on the road plane.

Figure 3. Representation of the ground-truth (g), IMU (m), vehicle (v) and road (r) coordinate systems. Please note that the origin of the ground-truth and IMU coordinate systems is not necessarily coincident with that of the vehicle coordinate system. This representation has been chosen to highlight the misalignment between the different coordinate systems.

Figure 4. Vehicle-to-road pose [3]. (a) shows the roll model, where

i \in [F, R]

; (b) depicts the pitch model where

i \in [L, R]

Figure 4. Vehicle-to-road pose [3]. (a) shows the roll model, where

i \in [F, R]

; (b) depicts the pitch model where

i \in [L, R]

Figure 5. Calibration procedure on non-horizontal road (roll angle perspective). (a) depicts the first standstill pose and (b) the second, which is equal to the first pose but facing the opposite direction. g, v and r denote the ground-truth, vehicle and road coordinate systems, respectively. (a,b) correspond with (10) and (11), respectively. The yellow line represents the horizontal plane. The circle on the upper left shows the direction of positive roll angles, and the arrows in it represent the y-axes of the corresponding coordinate systems. Note that, in the circles, the angle between the yellow line and the black arrow correspond with

ϕ_{n r}

. It can then be seen that

ϕ_{n r, 2} = - ϕ_{n r, 1}

. The corresponding pitch angle perspective would be equivalent. It is omitted for the sake of conciseness.

ϕ_{n r}

. It can then be seen that

ϕ_{n r, 2} = - ϕ_{n r, 1}

. The corresponding pitch angle perspective would be equivalent. It is omitted for the sake of conciseness.

Figure 6. Velocities of a vehicle while driving straight ahead with constant speed over a well-paved horizontal road.

Figure 7. Yaw calibration procedure on a non-horizontal road. The dashed and continuous lines respectively represent the visual division between lanes and the road limit. The vehicle drives on the same lane twice but in opposite directions. The upper left and right circles show the direction of positive yaw angles for the corresponding driving directions. The red, green and blue arrows in the circle depict the directions of the velocity vector, the x-axis of the vehicle CS and the x-axis of the ground-truth CS, respectively.

β_{bank}

is the side-slip angle stemming from the road bank and

β_{g}

the direction in which the ground-truth perceives the velocity vector.

β_{bank}

is the side-slip angle stemming from the road bank and

β_{g}

the direction in which the ground-truth perceives the velocity vector.

Figure 8. Calibration procedure for the estimation of the misalignment between the ground-truth and IMU coordinate systems.

Figure 9. Measurements during a figure eight manoeuvre. The ground-truth angular rates (

ω_{x}^{g}

ω_{y}^{g}

ω_{z}^{g}

) and specific forces (

f_{i m, x}^{g}

f_{i m, y}^{g}

f_{i m, z}^{g}

) obtained from the high-performance INS/GNSS system and transformed to the mounting position of the automotive-grade IMU, and the bias-corrected angular rates (

ω_{x}^{m}

ω_{y}^{m}

ω_{z}^{m}

) and bias-corrupted specific forces (

{\tilde{f}}_{i m, x}^{m}

{\tilde{f}}_{i m, y}^{m}

{\tilde{f}}_{i m, z}^{m}

) supplied by the automotive-grade IMU.

Figure 9. Measurements during a figure eight manoeuvre. The ground-truth angular rates (

ω_{x}^{g}

ω_{y}^{g}

ω_{z}^{g}

) and specific forces (

f_{i m, x}^{g}

f_{i m, y}^{g}

f_{i m, z}^{g}

) obtained from the high-performance INS/GNSS system and transformed to the mounting position of the automotive-grade IMU, and the bias-corrected angular rates (

ω_{x}^{m}

ω_{y}^{m}

ω_{z}^{m}

) and bias-corrupted specific forces (

{\tilde{f}}_{i m, x}^{m}

{\tilde{f}}_{i m, y}^{m}

{\tilde{f}}_{i m, z}^{m}

) supplied by the automotive-grade IMU.

Figure 10. Estimation results for the figure eight manoeuvre of Figure 9.

{\hat{ϕ}}_{m g 1}

{\hat{θ}}_{m g 1}

{\hat{ψ}}_{m g 1}

are the parameter estimates obtained based on the original data.

{\hat{ϕ}}_{m g 2}

{\hat{θ}}_{m g 2}

{\hat{ψ}}_{m g 2}

are the parameter estimates obtained based on the data corrupted by the artificial misalignment error

(0.7, 0.5, 1.2) \deg

for

ϕ_{m g}

θ_{m g}

and

ψ_{m g}

, respectively. Finally, the determinant of the excitation matrix

W_{p} = H_{k} H_{k}^{T}

is depicted in the last sub-plot.

Figure 10. Estimation results for the figure eight manoeuvre of Figure 9.

{\hat{ϕ}}_{m g 1}

{\hat{θ}}_{m g 1}

{\hat{ψ}}_{m g 1}

are the parameter estimates obtained based on the original data.

{\hat{ϕ}}_{m g 2}

{\hat{θ}}_{m g 2}

{\hat{ψ}}_{m g 2}

are the parameter estimates obtained based on the data corrupted by the artificial misalignment error

(0.7, 0.5, 1.2) \deg

for

ϕ_{m g}

θ_{m g}

and

ψ_{m g}

, respectively. Finally, the determinant of the excitation matrix

W_{p} = H_{k} H_{k}^{T}

is depicted in the last sub-plot.

Figure 11. Estimation robustness against noise. One hundred different sequences of additive Gaussian white noises with standard deviations

0.001 rad / s

and

0.05 m / s^{2}

have been added to the original IMU angular rate and specific force data, respectively. The estimator has been run for each sequence. The last estimated values of each run have been collected in these histograms. The red curves represent the corresponding best Gaussian distribution fits. The corresponding mean

μ

and standard deviation

σ

are added on top of each plot.

Figure 11. Estimation robustness against noise. One hundred different sequences of additive Gaussian white noises with standard deviations

0.001 rad / s

and

0.05 m / s^{2}

μ

and standard deviation

σ

are added on top of each plot.

Figure 12. Estimation approach for the IMU–vehicle misalignment estimation. The proposed regularized adaptive Kalman filter relies on the specific forces (

{\tilde{f}}_{i m}^{m}

) supplied by an automotive-grade 6D IMU, its bias-compensated angular rates (

ω_{i m}^{m}

) as well as over-road longitudinal, lateral and vertical velocity information (

v_{e r, x}^{r}

v_{e r, y}^{r}

v_{r v, z}^{r}

) mainly provided by the odometry, single-track model and suspension, respectively. Furthermore, the vehicle-to-road orientation (

ϕ_{r v}

θ_{r v}

), computed from the suspension signals, is used to relate the over-road velocities to the vehicle velocity (

v_{e v}^{v}

), see Figure 4. Finally, constraints stemming from a standstill phase are used to enhance the performance of the estimator.

Figure 12. Estimation approach for the IMU–vehicle misalignment estimation. The proposed regularized adaptive Kalman filter relies on the specific forces (

{\tilde{f}}_{i m}^{m}

) supplied by an automotive-grade 6D IMU, its bias-compensated angular rates (

ω_{i m}^{m}

) as well as over-road longitudinal, lateral and vertical velocity information (

v_{e r, x}^{r}

v_{e r, y}^{r}

v_{r v, z}^{r}

) mainly provided by the odometry, single-track model and suspension, respectively. Furthermore, the vehicle-to-road orientation (

ϕ_{r v}

θ_{r v}

), computed from the suspension signals, is used to relate the over-road velocities to the vehicle velocity (

v_{e v}^{v}

), see Figure 4. Finally, constraints stemming from a standstill phase are used to enhance the performance of the estimator.

Figure 13. Estimator inputs during a figure eight drive. The bias-free angular velocities (

ω_{x}

ω_{y}

ω_{z}

), the uncorrected specific forces (

{\tilde{f}}_{i v, x}

{\tilde{f}}_{i v, y}

{\tilde{f}}_{i v, z}

), the measurements (

y_{1}

y_{2}

y_{3}

Figure 13. Estimator inputs during a figure eight drive. The bias-free angular velocities (

ω_{x}

ω_{y}

ω_{z}

), the uncorrected specific forces (

{\tilde{f}}_{i v, x}

{\tilde{f}}_{i v, y}

{\tilde{f}}_{i v, z}

), the measurements (

y_{1}

y_{2}

y_{3}

Figure 14. Estimation results during the figure eight manoeuvre of Figure 13 for both estimators (A) and (B). Additionally, in green, the parameter estimates obtained with estimator (A) based on the data corrupted by the artificial misalignment error

(0.7, 0.5, 1.2) \deg

for

ϕ_{m v}

θ_{m v}

and

ψ_{m v}

, respectively, and the artificial bias errors

(0.1, 0.1, - 0.2) m / s^{2}

for

b_{f_{x}}

b_{f_{y}}

and

b_{f_{z}}

, respectively. Finally, the determinant of the excitation matrix

W_{p, k} = Ω_{k}^{T} Σ_{k}^{- 1} Ω_{k}

is depicted in the last sub-plot.

(0.7, 0.5, 1.2) \deg

for

ϕ_{m v}

θ_{m v}

and

ψ_{m v}

, respectively, and the artificial bias errors

(0.1, 0.1, - 0.2) m / s^{2}

for

b_{f_{x}}

b_{f_{y}}

and

b_{f_{z}}

, respectively. Finally, the determinant of the excitation matrix

W_{p, k} = Ω_{k}^{T} Σ_{k}^{- 1} Ω_{k}

is depicted in the last sub-plot.

Table 1. Estimation results of the v–g misalignment obtained from three calibration procedures performed on different dates. Results expressed in degrees.

Parameter	M1	M2	M3	Mean
${\hat{ϕ}}_{v g}$	0.100	0.128	0.122	0.117
${\hat{θ}}_{v g}$	−1.178	−1.158	−1.177	−1.171
${\hat{ψ}}_{v g}$	0.265	0.292	0.320	0.292

Table 2. Parameter choice, units according to the International System of Units.

Parameter	Value
R	diag([2 × 10 $^{- 6}$ , 2 × 10 $^{- 6}$ , 2 × 10 $^{- 6}$ , 1 × 10 $^{- 3}$ , 1 × 10 $^{- 3}$ , 1 × 10 $^{- 3}$ ])
$P_{0}$	diag([3.0625 × 10 $^{- 4}$ , 3.0625 × 10 $^{- 4}$ , 3.0625 × 10 $^{- 4}$ ])
${\hat{ρ}}_{0}$	[0, 0, 0]
$Σ_{R}$	diag([3.2653 × 10 $^{3}$ , 3.2653 × 10 $^{3}$ , 3.2653 × 10 $^{3}$ ])
$λ$	$0.9998$

Table 3. Variability misalignment estimation with angular rates and specific forces. Results expressed in degrees.

Parameter	M1	M2	M3	M4	M5	M6	M7	Max. Diff.	Mean
${\hat{ϕ}}_{m g}$	−0.187	−0.188	−0.193	−0.193	−0.191	−0.193	−0.195	0.008	−0.192
${\hat{θ}}_{m g}$	0.011	0.012	0.006	0.006	0.007	0.008	0.005	0.007	0.009
${\hat{ψ}}_{m g}$	0.434	0.373	0.367	0.367	0.402	0.401	0.394	0.067	0.396

Table 4. Estimator scheme parameter choice, units according to the International System of Units.

Parameter	Value
Q	diag([ $2 \times 10^{- 6}, 2 \times 10^{- 6}, 1 \times 10^{- 3}, 1 \times 10^{- 3}, 1 \times 10^{- 3}, 1 \times 10^{- 8}$ ])
R (A)	diag([ $1 \times 10^{- 2}, 1 \times 10^{- 1}, 1 \times 10^{- 2}, 1 \times 10^{- 4}, 1 \times 10^{- 4}, 1 \times 10^{- 4}$ ])
R (B)	diag([ $1 \times 10^{- 2}, 1 \times 10^{- 1}, 1 \times 10^{- 2}$ ])
$P_{0}$	diag([ $1.2 \times 10^{- 3}, 1.2 \times 10^{- 3}, 4 \times 10^{- 3}, 4 \times 10^{- 3}, 4 \times 10^{- 3}, 1.2 \times 10^{- 3}$ ])
$S_{0}$	diag([ $8 \times 10^{- 3}, 8 \times 10^{- 3}, 8 \times 10^{- 3}, 1 \times 10^{- 2}, 1 \times 10^{- 2}, 1 \times 10^{- 2}$ ])
${\hat{x}}_{0}$	${(0, 0, 0, 0, 0, 1)}^{T}$
${\hat{ρ}}_{0}$	${(0, 0, 0, 0, 0, 0)}^{T}$
$Σ_{R}$	diag([ $1250, 1250, 1250, 125, 125, 125$ ])
$λ$	$0.9996$

Table 5. Misalignment angles computed from the identified m-g and v-g misalignments in Section 3 and Section 4 (see Table 1 and Table 3). Results expressed in degrees.

Misalignment Angle	$mg$	$vg$	$mv$
$ϕ$	−0.192	0.117	≈−0.309
$θ$	0.009	−1.171	≈1.180
$ψ$	0.396	0.292	≈0.104

Table 6. Estimation results of (A) for three different figure eight manoeuvres driven on different days.

M 1

is the manoeuvre displayed in Figure 13 with

| {\tilde{f}}_{i v, y \max}^{m} | \approx 4

\frac{m}{s^{2}}

M 2

represents a less dynamic Figure 8 drive with

| {\tilde{f}}_{i v, y \max}^{m} | \approx 3

\frac{m}{s^{2}}

and

M 3

corresponds with a more dynamic Figure 8 drive with

| {\tilde{f}}_{i v, y \max}^{m} | \approx 5

\frac{m}{s^{2}}

Table 6. Estimation results of (A) for three different figure eight manoeuvres driven on different days.

M 1

is the manoeuvre displayed in Figure 13 with

| {\tilde{f}}_{i v, y \max}^{m} | \approx 4

\frac{m}{s^{2}}

M 2

represents a less dynamic Figure 8 drive with

| {\tilde{f}}_{i v, y \max}^{m} | \approx 3

\frac{m}{s^{2}}

and

M 3

corresponds with a more dynamic Figure 8 drive with

| {\tilde{f}}_{i v, y \max}^{m} | \approx 5

\frac{m}{s^{2}}

Misalignment Angle	$M 1$	$M 2$	$M 3$
$ϕ_{m v}$	−0.313	−0.278	−0.300
$θ_{m v}$	1.236	1.184	1.183
$ψ_{m v}$	0.037	0.058	0.045

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

© 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

Share and Cite

MDPI and ACS Style

Rodrigo Marco, V.; Kalkkuhl, J.; Raisch, J.; Seel, T. A Novel IMU Extrinsic Calibration Method for Mass Production Land Vehicles. Sensors 2021, 21, 7. https://doi.org/10.3390/s21010007

AMA Style

Rodrigo Marco V, Kalkkuhl J, Raisch J, Seel T. A Novel IMU Extrinsic Calibration Method for Mass Production Land Vehicles. Sensors. 2021; 21(1):7. https://doi.org/10.3390/s21010007

Chicago/Turabian Style

Rodrigo Marco, Vicent, Jens Kalkkuhl, Jörg Raisch, and Thomas Seel. 2021. "A Novel IMU Extrinsic Calibration Method for Mass Production Land Vehicles" Sensors 21, no. 1: 7. https://doi.org/10.3390/s21010007

APA Style

Rodrigo Marco, V., Kalkkuhl, J., Raisch, J., & Seel, T. (2021). A Novel IMU Extrinsic Calibration Method for Mass Production Land Vehicles. Sensors, 21(1), 7. https://doi.org/10.3390/s21010007

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Menu

A Novel IMU Extrinsic Calibration Method for Mass Production Land Vehicles

Abstract

1. Introduction

1.1. Brief Review of Previous Approaches

1.2. Current Approach and Main Contributions

2. Coordinate Systems and Preliminaries

2.1. Coordinate Systems

2.2. Notation

2.3. Inertial Measurement Unit: Preliminaries

3. Ground-Truth–Vehicle Misalignment Estimation

3.1. Roll and Pitch Misalignment

3.2. Yaw Misalignment

3.3. Results

4. Ground-Truth–IMU Misalignment Estimation

4.1. Method

4.1.1. Standstill Phase

4.1.2. Calibration Manoeuvre

4.2. Experimental Validation

5. IMU–Vehicle Misalignment Estimation

5.1. Model

5.2. Estimator Design

5.2.1. Discrete-Time Implementation

5.2.2. Algorithm Description

5.2.3. Estimator with Constraints

5.3. Experimental Results

6. Conclusions

Author Contributions

Funding

Conflicts of Interest

Appendix A. Derivation of (35)

References

Share and Cite

Article Metrics

Article Access Statistics

Further Information

Guidelines

MDPI Initiatives

Follow MDPI