High Precision Outdoor and Indoor Reference State Estimation for Testing Autonomous Vehicles ^†

Abstract

A current trend in automotive research is autonomous driving. For the proper testing and validation of automated driving functions a reference vehicle state is required. Global Navigation Satellite Systems (GNSS) are useful in the automation of the vehicles because of their practicality and accuracy. However, there are situations where the satellite signal is absent or unusable. This research work presents a methodology that addresses those situations, thus largely reducing the dependency of Inertial Navigation Systems (INSs) on the SatNav. The proposed methodology includes (1) a standstill recognition based on machine learning, (2) a detailed mathematical description of the horizontation of inertial measurements, (3) sensor fusion by means of statistical filtering, (4) an outlier detection for correction data, (5) a drift detector, and (6) a novel LiDAR-based Positioning Method (LbPM) for indoor navigation. The robustness and accuracy of the methodology are validated with a state-of-the-art INS with Real-Time Kinematic (RTK) correction data. The results obtained show a great improvement in the accuracy of vehicle state estimation under adverse driving conditions, such as when the correction data is corrupted, when there are extended periods with no correction data and in the case of drifting. The proposed LbPM method achieves an accuracy closely resembling that of a system with RTK.

1. Introduction

Autonomous driving has become a popular trend in automotive research. The motivation for autonomous driving ranges from comfort or practicality functions to safety critical applications. Independently of the final use, the fact that the vehicles are machines that can cause serious harm to humans in the event of a malfunction has to be taken into consideration. Therefore, autonomous driving functions need to be subjected to an extensive process of testing and validation which requires a highly accurate reference vehicle state.

A common practice to generate such a highly accurate reference vehicle state is the fusion of data from Inertial Measurement Units (IMU) with measurements from external sensors. Given the extensive scientific community working on these systems, their practicality, and especially their high accuracy, Satellite Navigation (SatNav) receivers have become the most common choice of sensor to fuse with IMUs. Even consumer-grade receivers are capable of acquiring information from various GNSS (such as GPS, GLONASS, Galileo, Beidou, etc.), which enables the receivers to accurately estimate several state variables. Unfortunately, there are common driving situations where satellite signals are either absent or corrupted to an extent that renders them unusable. Some examples of these driving situations are tunnels, bridge underpasses, parking structures or testing halls.

The objective of the present work is to reduce the dependency of INSs on external sensors, while still being able to generate a reference vehicle state. Six key aspects of the estimation of the vehicle state are addressed: (i) the standstill recognition (Section 2.1), (ii) the horizontation of IMU measurements (Section 2.2), (iii) sensor fusion (Section 2.3), (iv) drift detection (Section 2.4), (v) outlier detection for correction data (Section 2.5), and (vi) highly accurate indoor navigation (Section 2.6). It will be shown that the proposed methods are able to provide important improvements in the estimation of the vehicle state even when used individually. Furthermore, if used in combination, the proposed methods allow for an accurate enough estimation of the vehicle state to test and validate autonomous driving functions even in indoor environments.

For an adequate estimation of the vehicle state by means of IMUs, it is essential to ascertain when the vehicle is not moving because it is at this time that both the velocity over ground and the proper acceleration are equal to zero [1]. Even though, historically, vehicle speed sensors were designed to measure velocity rather than when a vehicle is at a standstill [2], they are now able to accurately measure a standstill. Nonetheless, there could be disadvantages if external devices continuously read the vehicle sensor data in real time (for example, through the OBDII interface). Even when various methods [3,4,5] and tools used by manufacturers [6] theoretically allow a CAN-bus use of ≈80% [7], recent studies suggest that CAN-bus messages can still miss their deadlines [8,9,10]. The traffic within a CAN-bus would increment if an external device continuously accesses the vehicle sensor data in real time, which could further increase the risk that the CAN-bus messages miss their deadlines. Consequently, more research is required before considering fusing the vehicle on-board sensors with external IMUs. Alternative sensors for measuring the velocity over ground, such as the Wheel Pulse Transducer (WPT) [11] or the Correvit [12,13] increase testing costs and complexity.

To the best of authors’ knowledge, the only method specifically designed for standstill detection is patented by Robert Bosch GmbH [14]. This method compares the position of an object in consecutive frames of a video provided by a camera in the vehicle, and an algorithm estimates the velocity of the vehicle relative to the object. As with all camera-based methods, there is a trade-off between the spatial resolution and the distance to the recorded objects, which negatively affects the standstill recognition.

The method proposed in this work for standstill recognition is based on a machine learning classifier, and has the key advantage that all required inputs are generated solely with IMU measurements. No additional sensors or infrastructural elements are required, which reduces testing complexity, improves the robustness of the method and makes it highly adequate for indoor navigation.

The state of the vehicle can be estimated once the algorithm has reliably established that the vehicle is moving. If Gauss-Markov stochastic processes are assumed, the most computationally efficient method to estimate the optimum state of moving objects is the Kalman Filter (KF) [15]. The KF algorithm can fuse sensor measurements with the vehicle state predicted by means of a motion model. Since the aim of this work is to generate a reference vehicle state for testing and validation of autonomous driving functions, the motion model used is especially designed for accurate performance under tractive driving.

Two critical parameters for the KF are the measurement noise and the system noise, because they are required to calculate the Kalman gain. Therefore, two methods are proposed to monitor both values, and thus to improve the robustness of the KF: a drift recognition and an outlier detector.

The drift recognition tackles the task of monitoring the vehicle state to detect when the vehicle is drifting. This information is extremely valuable because the motion model used in this research work is optimized for tractive-driving situations. When the vehicle starts to drift, the previously estimated system noise is no longer valid. Knowing when the vehicle is drifting enables the possibility of adjusting the system noise or switching to a better suited motion model.

Other works found in the literature address the issue of drifting too, however, they tend to limit the scope of their research to simulation environments [16,17] or require additional parameters that cannot easily be estimated with an IMU [18]. Conversely, the drift recognition in this work is designed to operate by performing consistency checks between two estimated state variables: the lateral acceleration and the sideslip angle. As demonstrated later, these state variables provide enough information to perform a robust drift detection.

The outlier detector serves as a complement to the drift detector. Although the drift detector monitors the validity of the motion model, the outlier detector does the same for the measurements. This is especially beneficial for SatNav measurements due to the multi-path effect. This effect appears when the signal received from the satellites does not follow a straight path, but bounces off other objects before reaching the SatNav receiver. The multi-path effect can occur as the vehicle drives near objects that are able to reflect the SatNav signal, such as trailers, tall buildings or metallic structures, and causes problems for the methods that measure the vehicle state (trilateration, carrier-phase measurements, etc.). A relevant consequence of the multi-path effect is that it reduces the accuracy of the measured vehicle state. It is quite common to see significant jumps in the measured state variables while the multi-path effect is present. There are several approaches in the literature to deal with the multi-path problem, such as [19,20,21,22,23]. However, to implement these approaches would require access to the pseudo-ranges of the SatNav receivers which is not possible for most consumer-grade devices. Therefore, the problem is addressed by filtering outliers. This filtering is achieved by a consistency check of the measurement vector before it is used as correction data.

In this research work a dynamic low-pass filter is designed to refine the outlier detection method by means of taking the measurement noise into account. This dynamic low-pass filter is highly beneficial for all sensors that estimate a quality indicator for their own measurements, as is the case of the “dilution of precision”, “diminution of precision” [24,25] or standard deviation of the SatNav receivers. Many sensors can track the accuracy associated with the measurements that they perform, but are not always capable of detecting estimation errors of this self-reported accuracy.

The investments made in GNSS [26,27] have provided various advantages, such as (i) high accuracy [28,29], (ii) a low cost for end users [30], and (iii) an extensive scientific community that works on refining its measurements and overcoming its limitations [31]. Arguably, these three advantages make the GNSS the best choice for external sensors in open-sky use-cases. However, there are still situations where the GNSS are not available or usable, as is the case in tunnels, parking structures or urban canyons. Moreover, there is an interest in testing in indoor halls because this allows engineers to test sensors, algorithms and systems under simulated and reproducible weather conditions such as day, night, rain, fog or even SatNav outages.

Due to the aforementioned reasons, it is necessary to investigate approaches for acquiring feedback in closed-sky situations. Comprehensive surveys on indoor positioning systems can be found in [32,33]. These approaches are based on the combination of diverse methods and sensors, such as WiFi, sonar, radar, or proprietary technologies such as the iBeacon [34].

The LiDAR-based approach with the best performance is presented on [35] with a mean localization error of 11.5 cm and standard deviation of 5.4 cm. The accuracy is evaluated by comparing the outputs of the algorithm to “human-labeled” ground-truth data. The indoor positioning system with the best performance that was found is the “Active Bat” [36,37] with a mean localization error of 3 cm, but the evaluation methodology is not clearly specified [38]. The biggest disadvantage of the Active Bat is the numerous receivers required which must be placed with separations of 1.2 m and must cover the complete measurement area. Therefore, the installation of such a system might not be practical. Other indoor positioning systems already on the market [39,40,41] are designed for applications that do not require a centimeter-precise localization. Alternative navigation methods that use LiDARs are presented in [42,43,44]. However, the existing methods still face a series of challenges, such as the positioning accuracy, the number of state variables they can measure or the algorithm runtime.

2. Materials and Methods

The present work deals with the measurements provided by different sensors, such as IMUs, LiDARs, SatNav receivers, etc. For simplification purposes, it is assumed that all sensors output their measurements in the Local Car Plane (LCP). The LCP is the vehicle reference frame and is defined analogue to the ISO 8855:2011 norm [45]. The LCP is composed by the mutually perpendicular $x_{\mathrm{LCP}}$, $y_{\mathrm{LCP}}$, and $z_{\mathrm{LCP}}$ axes, with $\overrightarrow{z_{\mathrm{LCP}}}=\overrightarrow{x_{\mathrm{LCP}}}\times \overrightarrow{y_{\mathrm{LCP}}}$ and origin $o_{\mathrm{LCP}}$ at the Center of sprung Mass (CoM) of the vehicle. The $x_{\mathrm{LCP}}$ axis is parallel to the longitudinal axis of the vehicle and points towards the hood, the $z_{\mathrm{LCP}}$ axis points upwards, and the $y_{\mathrm{LCP}}$ axis is given according to the right-hand rule. The LCP is illustrated in Figure 1.

The vehicles move on the Local Tangent Plane (LTP). The LTP is a Cartesian reference frame, composed by the mutually perpendicular $x_{\mathrm{LTP}}$, $y_{\mathrm{LTP}}$ and $z_{\mathrm{LTP}}$ axes, with $\overrightarrow{z_{\mathrm{LTP}}}=\overrightarrow{x_{\mathrm{LTP}}}\times \overrightarrow{y_{\mathrm{LTP}}}$; and with origin $o_{\mathrm{LTP}}$ at an arbitrary location on the surface of the Earth. The axis orientation of the LTP is similar to the East-North-Up (ENU) reference frame, where the $x_{\mathrm{LTP}}\times y_{\mathrm{LTP}}$-plane is perpendicular to the gravitational pull of the Earth, the $y_{\mathrm{LTP}}$ axis points to the true north of the Earth, and the $z_{\mathrm{LTP}}$ axis is parallel to the gravitational pull of the Earth, and points upwards.

2.1. Standstill Recognition

The first step for estimating the vehicle state is the standstill detection. The strategy to achieve this, is to generate features from the IMU signals that can describe whether the vehicle is standing still or not. The generated features are then classified by means of machine learning. The proposed method uses the Random Forest (RF) [46] for classifying standstill or motion according to the features generated from the IMU signals as inputs. The classes used are shown in

Table 1. Standstill classification.

	Classified State	Standstill	Motion
True State
Standstill		True positive	False negative
Motion		False positive	True negative

. False positives are unacceptable because they can affect greatly the estimation of the vehicle state, especially if they occur at highway velocities. False negatives can be tolerated because, as shown later, their effect is negligible for the estimation of the vehicle state. If this holds, the standstill classification is considered robust.

The use of the RF is justified mainly because it is not possible to manually determine thresholds for the IMU signals that allow a robust standstill classification. This happens because some driving conditions are almost equal to a standstill, such as cruising on a highway or driving at walking velocity. This fact is further strengthened when considering the numerous configurations possible for the same vehicle chassis (engine, suspension, tires, etc.), and that even small components, such as the engine mounts, can have a significant impact on the vibrations and rotations sensed by the IMU.

The RF not only enables the implementation of a robust standstill recognition, but a 10 min dataset is also enough for a robust classification, and the training process is done within seconds. Also, given that the RF is real-time capable, it allows the online use of the proposed method. Further details about the RF can be found in [46]. The procedure to recognize a standstill consists of three steps: (i) generation of the training dataset, (ii) training of the RF, and (iii) classification of the vehicle state. The specifics applicable to the present research work is detailed below.

The first step to recognize a standstill, is to generate a training dataset for the RF. The 10 min-long dataset that is required to generate the features for the RF is composed by the proper acceleration vector $a_{\mathrm{LCP}}$ and the composition of rotations ${\dot{\mathit{\theta }}}_{\mathrm{LCP}}$ that are measured by the IMU, these are such that

$$a_{\mathrm{LCP}}={\left[\begin{array}{ccc}{a}_{x,\mathrm{LCP}}& a_{y,\mathrm{LCP}}& a_{z,\mathrm{LCP}}\end{array}\right]}^{\mathrm{T}},$$

(1)

$${\dot{\mathit{\theta }}}_{\mathrm{LCP}}={\left[\begin{array}{ccc}{\dot{\theta }}_{\mathrm{roll}}& {\dot{\theta }}_{\mathrm{pitch}}& {\dot{\theta }}_{\mathrm{yaw}}\end{array}\right]}^{\mathrm{T}}.$$

(2)

The training dataset for the RF is equally distributed between three conditions: standstill, moving at walking pace and random driving.

For the standstill condition, the vehicle should be as motionless as possible, but ready to drive off. In the case of vehicles with automatic transmission, “drive” should be selected; and vehicles with manual transmission should be either in neutral or in gear and with the clutch engaged. The reason for this, is to let the IMU sense engine and drivetrain vibrations as well. During the standstill, all dead loads are negligible, and all live loads are undesirable. Except for engine and drivetrain, all actions that provoke chassis vibration or motion should be avoided: passenger movement, vehicle loading, opening or closing doors, trunk or hood, etc.

For the condition of moving at walking pace, the vehicle is driven as slow as possible and on a straight line. Contact with pedals or steering wheel should be avoided. This allows one to generate samples with the “motion” label but that are very similar to a “standstill”, which is highly relevant for differentiating between both.

For the condition of random driving, the vehicle is driven with random accelerations, velocities and directions. The more diverse, the better for the dataset.

Next, the features for the RF are generated in the time and frequency domain. The features in the time domain are: (i) $a_{x,\mathrm{LCP}}^2+a_{y,\mathrm{LCP}}^2$, (ii) $a_{z,\mathrm{LCP}}^2$, (iii) ${\dot{\theta }}_{\mathrm{roll}}^2+{\dot{\theta }}_{\mathrm{pitch}}^2$, and (iv) ${\dot{\theta }}_{\mathrm{yaw}}^2$.

The features in the frequency domain are created from the features in the time domain by generating the Discrete Fourier Transform (DFT) of each feature in the time domain. In the present work, the DFT is generated with a MATLAB Toolbox that is based on [47,48,49]. The frequency range that the DFT uses is given as in Equation (3).

$$\begin{array}{c}\hfill [0:\frac{F_{\mathrm{s}}}{n_{\mathrm{DFT}}}:F_{\mathrm{s}}],\end{array}$$

(3)

where $F_{\mathrm{s}}$ is the sampling frequency and $n_{\mathrm{DFT}}$ is the number of samples used for the DFT. The highest meaningful frequency from the DFT is $\frac{F_{\mathrm{s}}}{2}$ [50,51], and a 0 Hz frequency (direct current) is not relevant in this work. Therefore, the used frequency range is given by Equation (4).

$$\begin{array}{c}\hfill [\frac{F_{\mathrm{s}}}{n_{\mathrm{DFT}}}:\frac{F_{\mathrm{s}}}{n_{\mathrm{DFT}}}:\lfloor \frac{n_{\mathrm{DFT}}}{2}\rfloor ·\frac{F_{\mathrm{s}}}{n_{\mathrm{DFT}}}].\end{array}$$

(4)

The IMU sampling rate is set to $F_{\mathrm{s}}=100\mathrm{Hz}$, and a time window of ${\tau }_{\mathrm{DFT}}=170\mathrm{m}\mathrm{s}$ is used. Therefore, $n_{\mathrm{DFT}}=17$ samples are used each time for the DFT generation: the latest values of the features in the time domain, and their values from the previous $n_{\mathrm{DFT}}-1$ samples. Therefore, the used DFT frequencies are (1) 5.8824 Hz, (ii) 11.7647 Hz, (iii) 17.6471 Hz, (iv) 23.5294 Hz, (v) 29.4118 Hz, (vi) 35.2941 Hz, (vii) 41.1765 Hz, and (viii) 47.0588 Hz. The single-sided amplitude of these 8 frequencies are the features in the frequency domain for a total of 36 features: 4 in the time domain and 32 in the frequency domain. It should be noted that due to the sliding window, it is not possible to generate a DFT for the first $n_{\mathrm{DFT}}-1$ samples. In this case, the amplitudes of their frequencies are set to zero.

By analyzing the features of the training datasets of different vehicles, it can be noticed that the dominant frequencies at standstill can be associated with the vehicle motorization. For the vehicles with internal combustion engines, to the idling Revolutions Per Minute (RPM) and the number of cylinders. When the vehicles move at walking velocity, the dominant frequency can be associated mainly with the vehicle differential. Finally, when the vehicle moves faster than walking velocity, the dominant frequencies come from the live loads of the vehicle, such as road imperfections and suspension travel, among others.

As shown in Table 1, “standstill” and “motion” are the two labels used for classification. The labeling of the training dataset can be done by using other sensors as reference, by manually analyzing the IMU signals, or a combination of both. Whichever way, it is important to notice that the labeling directly affects the classification performance of the RF.

A best practice for labeling the training datasets is to include the jerks that appear at drive-off into the “motion” class. When the vehicle comes to a stop, the “standstill” class should start only once the chassis has stopped moving and there are no more jerks or rotations present in the IMU signals. In case of uncertainty, it is preferable to label a sample as “motion” rather than “standstill” because, as is shown later, false negatives can be tolerated, but false positives are unacceptable.

The algorithm initialization and the transition between states are two effects of the proposed method to keep in mind. As for the initialization, $n_{\mathrm{DFT}}-1$ samples are needed for algorithm initialization. This is because, as explained above, the DFT can be generated only once $n_{\mathrm{DFT}}$ samples from the IMU are obtained. The single-sided amplitudes for the previous samples are set to zero, which might not be representative of the vehicle state.

Regarding the transition between states, as a vehicle goes from one state to the other (motion to standstill or vice versa), it might happen that the samples used for the DFT belong to different classes. The smaller the sliding window ${\tau }_{\mathrm{DFT}}$, the faster all samples for the DFT fall within a single class. Otherwise, as the sliding window ${\tau }_{\mathrm{DFT}}$ gets bigger, more features can be used for the classification, but this also increases the lapse where samples from both classes are fed to the RF. The current size of ${\tau }_{\mathrm{DFT}}$ is found to be adequate for a robust standstill recognition, and is determined after testing the RF with several hours of test datasets, and by comparing the classification performance of the RF with different values for ${\tau }_{\mathrm{DFT}}$.

Once the features in time and frequency domain are generated and labeled, the second step to recognize a standstill, is to train the RF. The training parameters used in this work are (i) number of trees: 12, (ii) stopping criteria: minimum leaf size, and (iii) minimum leaf size: 5. These parameters provide a robust classification and can be used for a variety of land vehicles.

It should be noted that the trained RF is not specific to the vehicle used to generate the training dataset, but to all vehicles with the same configuration (chassis, engine, transmission, suspension, etc.). This means that the training occurs only once, and the trained RF can be used for as long as the vehicle configuration remains the same.

Once the RF is trained, the third step to recognize a standstill, is to use the RF for the standstill recognition on test data. For this, the 36 features required as inputs for the RF are generated as the vehicle drives. The first step to do so is to buffer $n_{\mathrm{DFT}}$ IMU samples, i.e., the most recent IMU measurements and the previous $n_{\mathrm{DFT}}-1$. The latest IMU measurements are the features in the time domain ($a_{x,\mathrm{LCP}}^2+a_{y,\mathrm{LCP}}^2$, $a_{z,\mathrm{LCP}}^2$, ${\dot{\theta }}_{\mathrm{roll}}^2+{\dot{\theta }}_{\mathrm{pitch}}^2$, and ${\dot{\theta }}_{\mathrm{yaw}}^2$), and the features in the frequency domain are generated with all $n_{\mathrm{DFT}}$ IMU buffered samples. When all 36 features (4 in the time domain and 32 in the frequency domain) are generated, they are used as input for the RF.

If a standstill is recognized, all change rates (velocities, accelerations and rotation rates) are set to zero. Only the position and orientation are carried forward from the previous time step. Otherwise, if the vehicle moves, its state is estimated as is detailed later.

2.2. Horizontation of IMU Measurements

The horizontation is understood as the transformation of the IMU measurements, as if the $x_{\mathrm{LCP}}\times y_{\mathrm{LCP}}$ and $x_{\mathrm{LTP}}\times y_{\mathrm{LTP}}$ planes were parallel, and the $z_{\mathrm{LCP}}$ axis had the same orientation and direction as the $z_{\mathrm{LTP}}$ axis. The horizontation is required mainly because most vehicle motion models found in the literature depict the movement of cars from a bird’s-eye perspective. Also, it is not a practical solution to physically maintain the $x_{\mathrm{LCP}}\times y_{\mathrm{LCP}}$ and $x_{\mathrm{LTP}}\times y_{\mathrm{LTP}}$ planes parallel when the vehicles are in motion. The horizontation requires tracking of the LTP axes relative to the LCP, so that the IMU measurements can be projected on the tracked LTP axes. This process varies depending on whether the vehicle is standing still or not. Both cases are detailed in what follows.

With exception of the engine vibrations, there are no live loads present in a vehicle during a standstill. Therefore, the gravitational pull G is the only force sensed by the IMU. This is given at standstill by

$$\begin{array}{c}\hfill G=\Vert a_{\mathrm{LCP}}\Vert =\sqrt{a_{x,\mathrm{LCP}}^2+a_{y,\mathrm{LCP}}^2+a_{z,\mathrm{LCP}}^2}\approx 9.81\frac{\mathrm{m}}{{\mathrm{s}}^2}.\end{array}$$

(5)

By using the gravitational pull as reference, the roll and pitch angles are calculated as follows

$$\begin{array}{c}\hfill {\theta }_{\mathrm{roll}}=arccos\left(\frac{a_{y,\mathrm{LCP}}}{G}\right),\end{array}$$

(6)

$$\begin{array}{c}\hfill {\theta }_{\mathrm{pitch}}=arccos\left(\frac{a_{x,\mathrm{LCP}}}{G}\right).\end{array}$$

(7)

Then, the easiest manner to obtain an initial value of the yaw angle ${\theta }_{\mathrm{yaw}}$, is to equip the vehicle with a sensor that measures the Course Over Ground (COG), such as a SatNav receiver, and to drive the vehicle on a straight line. This is so because driving on a straight line results in ${\theta }_{\mathrm{yaw}}=\mathrm{COG}$.

Two rotations are necessary to transform the LTP to the LCP: one rotation around an arbitrary axis and one rotation around the $z_{\mathrm{LTP}}$ axis. The first rotation aligns the $z_{\mathrm{LCP}}$ axis with the $z_{\mathrm{LTP}}$ axis. The second rotation aligns the $x_{\mathrm{LCP}}$ axis with the $x_{\mathrm{LTP}}$ axis. The two parameters of the first rotation are the arbitrary axis $r_{{\mathrm{h}}_0}$ and the rotation angle ${\theta }_{\mathrm{h}}$. They are expressed as follows

$$\begin{array}{c}\hfill {\theta }_{\mathrm{h}}=arccos\left(\frac{a_{z,\mathrm{LCP}}}{G}\right),\end{array}$$

(8)

$$r_{{\mathrm{h}}_0}=\left[\begin{array}{ccc}{a}_{x,\mathrm{LCP}}& a_{y,\mathrm{LCP}}& a_{z,\mathrm{LCP}}\end{array}\right]\times \left[\begin{array}{ccc}0& 0& 1\end{array}\right]=\left[\begin{array}{ccc}{r}_{x,\mathrm{h}}& r_{y,\mathrm{h}}& r_{z,\mathrm{h}}\end{array}\right].$$

(9)

The rotation matrix ${\mathit{R}}_{\mathrm{LCP}}^{{\mathrm{LCP}}^{\prime }}$ that aligns the $z_{\mathrm{LCP}}$ axis with the $z_{\mathrm{LTP}}$ axis is then given by:

$${\mathit{R}}_{\mathrm{LCP}}^{{\mathrm{LCP}}^{\prime }}=\left[\begin{array}{ccc}\frac{r_{x,\mathrm{h}}^2+\left(r_{y,\mathrm{h}}^2+r_{z,\mathrm{h}}^2\right)cos\left({\theta }_{\mathrm{h}}\right)}{r_{\mathrm{m}}}& \frac{r_{x,\mathrm{h}}{r}_{y,\mathrm{h}}{r}_{\mathrm{c}}-r_{z,\mathrm{h}}\sqrt{r_{\mathrm{m}}}sin\left({\theta }_{\mathrm{h}}\right)}{r_{\mathrm{m}}}& \frac{r_{x,\mathrm{h}}{r}_{z,\mathrm{h}}{r}_{\mathrm{c}}+r_{y,\mathrm{h}}\sqrt{r_{\mathrm{m}}}sin\left({\theta }_{\mathrm{h}}\right)}{r_{\mathrm{m}}}\\ \frac{r_{x,\mathrm{h}}{r}_{y,\mathrm{h}}{r}_{\mathrm{c}}+r_{z,\mathrm{h}}\sqrt{r_{\mathrm{m}}}sin\left({\theta }_{\mathrm{h}}\right)}{r_{\mathrm{m}}}& \frac{r_{y,\mathrm{h}}^2+\left(r_{x,\mathrm{h}}^2+r_{z,\mathrm{h}}^2\right)cos\left({\theta }_{\mathrm{h}}\right)}{r_{\mathrm{m}}}& \frac{r_{y,\mathrm{h}}{r}_{z,\mathrm{h}}{r}_{\mathrm{c}}-r_{x,\mathrm{h}}\sqrt{r_{\mathrm{m}}}sin\left({\theta }_{\mathrm{h}}\right)}{r_{\mathrm{m}}}\\ \frac{r_{x,\mathrm{h}}{r}_{z,\mathrm{h}}{r}_{\mathrm{c}}-r_{y,\mathrm{h}}\sqrt{r_{\mathrm{m}}}sin\left({\theta }_{\mathrm{h}}\right)}{r_{\mathrm{m}}}& \frac{r_{y,\mathrm{h}}{r}_{z,\mathrm{h}}{r}_{\mathrm{c}}+r_{x,\mathrm{h}}\sqrt{r_{\mathrm{m}}}sin\left({\theta }_{\mathrm{h}}\right)}{r_{\mathrm{m}}}& \frac{r_{z,\mathrm{h}}^2+\left(r_{x,\mathrm{h}}^2+r_{y,\mathrm{h}}^2\right)cos\left({\theta }_{\mathrm{h}}\right)}{r_{\mathrm{m}}}\end{array}\right],$$

(10)

$$\begin{array}{c}\hfill r_{\mathrm{c}}=1-cos\left({\theta }_{\mathrm{h}}\right),\end{array}$$

(11)

$$\begin{array}{c}\hfill r_{\mathrm{m}}=r_{x,\mathrm{h}}^2+r_{y,\mathrm{h}}^2+r_{z,\mathrm{h}}^2.\end{array}$$

(12)

The Equation (10) is known as the “Rodriguez’ Formula”, and its step-by-step derivation can be found in [52]. A graphical representation of a rotation around an arbitrary axis is shown in Figure 2. It should be noted that Equation (10) can present a singularity if ${\theta }_{\mathrm{roll}}=0$ and ${\theta }_{\mathrm{pitch}}=0$, because in that case $r_{\mathrm{m}}=0$ as well. However, this would mean that the $z_{\mathrm{LCP}}$ axis and the $z_{\mathrm{LTP}}$ axis are aligned. Hence, ${\mathit{R}}_{\mathrm{LCP}}^{{\mathrm{LCP}}^{\prime }}={\mathit{I}}_{3\times 3}$, where $\mathit{I}$ is the identity matrix.

The matrix that tracks the LTP with respect to the LCP is then given by

$${\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP}}={\left(\left[\begin{array}{ccc}cos\left({\theta }_{\mathrm{yaw}}\right)& -sin\left({\theta }_{\mathrm{yaw}}\right)& 0\\ sin\left({\theta }_{\mathrm{yaw}}\right)& cos\left({\theta }_{\mathrm{yaw}}\right)& 0\\ 0& 0& 1\end{array}\right]{\mathit{R}}_{\mathrm{LCP}}^{{\mathrm{LCP}}^{\prime }}\right)}^{\mathrm{T}}.$$

(13)

Once the vehicle pose at standstill is determined, it must be updated as the vehicle moves. The update is done primarily with the gyroscopes because even when cruising in a straight line, random forces act on road vehicles. Some sources of these forces could be road irregularities, bumps, road joints or the wind. These forces are sensed by the IMU, which complicates the update of ${\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP}}$ with the accelerometers. Therefore, the composition of rotations is used instead. This is detailed in what follows.

With the instantaneous rotation rates ${\dot{\theta }}_{\mathrm{roll}}$, ${\dot{\theta }}_{\mathrm{pitch}}$ and ${\dot{\theta }}_{\mathrm{yaw}}$ around the $x_{\mathrm{LCP}}$, $y_{\mathrm{LCP}}$, and $z_{\mathrm{LCP}}$ axes respectively, the instantaneous composition of rotations is expressed as follows

$$\mathit{S}\left({\dot{\mathit{\theta }}}_{\mathrm{LCP},{\tau }_2}^{\mathrm{LCP},{\tau }_1}\right)=\left[\begin{array}{ccc}0& -{\dot{\theta }}_{\mathrm{yaw}}& {\dot{\theta }}_{\mathrm{pitch}}\\ {\dot{\theta }}_{\mathrm{yaw}}& 0& -{\dot{\theta }}_{\mathrm{roll}}\\ -{\dot{\theta }}_{\mathrm{pitch}}& {\dot{\theta }}_{\mathrm{roll}}& 0\end{array}\right],$$

(14)

$${\dot{\mathit{\theta }}}_{\mathrm{LCP},{\tau }_2}^{\mathrm{LCP},{\tau }_1}=\left[\begin{array}{ccc}{\dot{\theta }}_{\mathrm{roll}}& {\dot{\theta }}_{\mathrm{pitch}}& {\dot{\theta }}_{\mathrm{yaw}}\end{array}\right].$$

(15)

Next, the instantaneous composition of rotations is integrated as if it was a vector and added to ${\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP}}$. The step-by-step derivation of this procedure is shown below.

$$\begin{array}{c}\hfill {\widehat{\mathit{R}}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_2}={\mathit{R}}_{\mathrm{LCP},{\tau }_1}^{\mathrm{LCP},{\tau }_2}{\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_1},\end{array}$$

(16)

$$\begin{array}{c}\hfill {\widehat{\mathit{R}}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_2}=\left({\mathit{R}}_{\mathrm{LCP},{\tau }_1}^{\mathrm{LCP},{\tau }_1}+\Delta \tau {\dot{\mathit{R}}}_{\mathrm{LCP},{\tau }_1}^{\mathrm{LCP},{\tau }_2}\right){\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_1},\end{array}$$

(17)

$$\begin{array}{c}\hfill {\widehat{\mathit{R}}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_2}=\left({\mathit{R}}_{\mathrm{LCP},{\tau }_1}^{\mathrm{LCP},{\tau }_1}+\Delta \tau \mathit{S}\left({\dot{\mathit{\theta }}}_{\mathrm{LCP},{\tau }_1}^{\mathrm{LCP},{\tau }_2}\right){\mathit{R}}_{\mathrm{LCP},{\tau }_1}^{\mathrm{LCP},{\tau }_2}\right){\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_1},\end{array}$$

(18)

$$\begin{array}{c}\hfill {\widehat{\mathit{R}}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_2}=\left({\mathit{R}}_{\mathrm{LCP},{\tau }_1}^{\mathrm{LCP},{\tau }_1}+\Delta \tau \mathit{S}\left(-{\dot{\mathit{\theta }}}_{\mathrm{LCP},{\tau }_2}^{\mathrm{LCP},{\tau }_1}\right){\mathit{R}}_{\mathrm{LCP},{\tau }_1}^{\mathrm{LCP},{\tau }_2}\right){\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_1},\end{array}$$

(19)

$$\begin{array}{c}\hfill {\widehat{\mathit{R}}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_2}=\left({\mathit{I}}_{3\times 3}+\Delta \tau {\left(\mathit{S}\left({\dot{\mathit{\theta }}}_{\mathrm{LCP},{\tau }_2}^{\mathrm{LCP},{\tau }_1}\right)\right)}^{\mathrm{T}}{\mathit{I}}_{3\times 3}\right){\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_1},\end{array}$$

(20)

$$\begin{array}{c}\hfill {\widehat{\mathit{R}}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_2}=\left(\Delta \tau \mathit{S}{\left({\dot{\mathit{\theta }}}_{\mathrm{LCP},{\tau }_2}^{\mathrm{LCP},{\tau }_1}\right)}^{\mathrm{T}}+{\mathit{I}}_{3\times 3}\right){\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_1},\end{array}$$

(21)

where $\Delta \tau ={\tau }_2-{\tau }_1$. Equation (21) is used each time that a new IMU measurement is available to keep updating ${\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP}}$.

To add rotations as vectors is an approximation that is valid only for rotations that tend to zero [53]. This is because ${\widehat{\mathit{R}}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_2}$ is not an orthonormal matrix. Therefore, ${\widehat{\mathit{R}}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_2}$ must be adjusted to make its axes perpendicular to each other. The procedure is shown in the following.

$${\widehat{\mathit{R}}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_2}=\left[\begin{array}{ccc}{\mathit{r}}_{{\mathrm{h}}_1}^{\prime }& {\mathit{r}}_{{\mathrm{h}}_2}^{\prime }& {\mathit{r}}_{{\mathrm{h}}_3}^{\prime }\end{array}\right],$$

(22)

$$\begin{array}{c}\hfill {\mathit{r}}_{{\mathrm{h}}_3}^{″}={\mathit{r}}_{{\mathrm{h}}_3}^{\prime },\end{array}$$

(23)

$$\begin{array}{c}\hfill {\mathit{r}}_{{\mathrm{h}}_1}^{″}={\mathit{r}}_{{\mathrm{h}}_2}^{\prime }\times {\mathit{r}}_{{\mathrm{h}}_3}^{″},\end{array}$$

(24)

$$\begin{array}{c}\hfill {\mathit{r}}_{{\mathrm{h}}_2}^{″}={\mathit{r}}_{{\mathrm{h}}_3}^{″}\times {\mathit{r}}_{{\mathrm{h}}_1}^{″},\end{array}$$

(25)

$${\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_2}=\left[\begin{array}{ccc}\frac{{\mathit{r}}_{{\mathrm{h}}_1}^{\prime \prime }}{∥{\mathit{r}}_{{\mathrm{h}}_1}^{″}∥}& \frac{{\mathit{r}}_{{\mathrm{h}}_2}^{″}}{∥{\mathit{r}}_{{\mathrm{h}}_2}^{″}∥}& \frac{{\mathit{r}}_{{\mathrm{h}}_3}^{″}}{∥{\mathit{r}}_{{\mathrm{h}}_3}^{″}∥}\end{array}\right].$$

(26)

The next step is to project on the LTP the acceleration vector ${\mathit{a}}_{\mathrm{LCP}}$ and the composition of rotations ${\dot{\mathit{\theta }}}_{\mathrm{LCP}}$ measured by the IMU. This is done by using the tracker matrix ${\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_2}$ as follows

$${\mathit{a}}_{\mathrm{LTP},{\tau }_2}={\left({\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_2}\right)}^{\mathrm{T}}{\mathit{a}}_{\mathrm{LCP}}=\left[\begin{array}{ccc}{a}_{x,\mathrm{LTP},{\tau }_2}& a_{y,\mathrm{LTP},{\tau }_2}& a_{z,\mathrm{LTP},{\tau }_2}\end{array}\right],$$

(27)

$${\dot{\mathit{\theta }}}_{\mathrm{LTP},{\tau }_2}={\left({\mathit{R}}_{\mathrm{LTP}}^{\mathrm{LCP},{\tau }_2}\right)}^{\mathrm{T}}{\dot{\mathit{\theta }}}_{\mathrm{LCP}}=\left[\begin{array}{ccc}{\dot{\theta }}_{x,\mathrm{LTP},{\tau }_2}& {\dot{\theta }}_{y,\mathrm{LTP},{\tau }_2}& {\dot{\theta }}_{z,\mathrm{LTP},{\tau }_2}\end{array}\right],$$

(28)

where ${\mathit{a}}_{\mathrm{LTP},{\tau }_2}$ is the acceleration vector in LTP at time instance ${\tau }_2$, ${\dot{\mathit{\theta }}}_{\mathrm{LTP},{\tau }_2}$ is the composition of rotations in LTP at time instance ${\tau }_2$, $a_{x,\mathrm{LTP},{\tau }_2}$, $a_{y,\mathrm{LTP},{\tau }_2}$ and $a_{z,\mathrm{LTP},{\tau }_2}$ are the accelerations at time instance ${\tau }_2$ along the $x_{\mathrm{LTP}}$, $y_{\mathrm{LTP}}$ and $z_{\mathrm{LTP}}$ axes accordingly; and ${\dot{\theta }}_{x,\mathrm{LTP},{\tau }_2}$, ${\dot{\theta }}_{y,\mathrm{LTP},{\tau }_2}$ and ${\dot{\theta }}_{z,\mathrm{LTP},{\tau }_2}$ are the rotations at time instance ${\tau }_2$ around the $x_{\mathrm{LTP}}$, $y_{\mathrm{LTP}}$ and $z_{\mathrm{LTP}}$ axes correspondingly.

The acceleration vector and composition of rotations are then projected on the $x_{\mathrm{LTP}}\times y_{\mathrm{LTP}}$-plane. The magnitude $a_{\mathrm{OG}}$ and direction ${\theta }_{\mathrm{aOG}}$ of the acceleration over ground are calculated as follows

$$\begin{array}{c}\hfill a_{\mathrm{OG}}=\sqrt{a_{x,\mathrm{LTP},{\tau }_2}^2+a_{y,\mathrm{LTP},{\tau }_2}^2},\end{array}$$

(29)

$$\begin{array}{c}\hfill {\theta }_{\mathrm{aOG}} = arctan2\left(a_{y,\mathrm{LTP},{\tau }_2},a_{x,\mathrm{LTP},{\tau }_2}\right).\end{array}$$

(30)

The magnitude ${\dot{\theta }}_{\mathrm{OG}}$ and direction ${\theta }_{\mathrm{rOG}}$ of the composition of rotations over ground are given by

$$\begin{array}{c}\hfill {\dot{\theta }}_{\mathrm{OG}}=\sqrt{{\dot{\theta }}_{x,\mathrm{LTP},{\tau }_2}^2+{\dot{\theta }}_{y,\mathrm{LTP},{\tau }_2}^2},\end{array}$$

(31)

$$\begin{array}{c}\hfill {\theta }_{\mathrm{rOG}} = arctan2\left({\dot{\theta }}_{y,\mathrm{LTP},{\tau }_2},{\dot{\theta }}_{x,\mathrm{LTP},{\tau }_2}\right).\end{array}$$

(32)

Once the accelerations along and rotations around the LTP axes are known, they are projected on the projection on the $x_{\mathrm{LTP}}\times y_{\mathrm{LTP}}$-plane of the longitudinal axis of the vehicle, i.e., the acceleration vector ${\mathit{a}}_{\mathrm{LTP},{\tau }_2}$ and composition of rotations ${\dot{\mathit{\theta }}}_{\mathrm{LTP},{\tau }_2}$ are projected on a vector located on the $x_{\mathrm{LTP}}\times y_{\mathrm{LTP}}$-plane and with ${\theta }_{\mathrm{yaw}}$ orientation. Therefore,

$$\left[\begin{array}{c}{a}_{x,\mathrm{h}}\\ a_{y,\mathrm{h}}\end{array}\right]=\left[\begin{array}{c}{a}_{\mathrm{OG}}cos\left({\theta }_{\mathrm{aOG}}-{\theta }_{\mathrm{yaw}}\right)\\ a_{\mathrm{OG}}sin\left({\theta }_{\mathrm{aOG}}-{\theta }_{\mathrm{yaw}}\right)\end{array}\right],$$

(33)

$$\left[\begin{array}{c}{\dot{\theta }}_{\mathrm{roll}}\\ {\dot{\theta }}_{\mathrm{pitch}}\end{array}\right]=\left[\begin{array}{c}{\dot{\theta }}_{\mathrm{OG}}cos\left({\theta }_{\mathrm{rOG}}-{\theta }_{\mathrm{yaw}}\right)\\ {\dot{\theta }}_{\mathrm{OG}}sin\left({\theta }_{\mathrm{rOG}}-{\theta }_{\mathrm{yaw}}\right)\end{array}\right],$$

(34)

where $a_{x,\mathrm{h}}$ and $a_{y,\mathrm{h}}$ are called “horizontal accelerations”. The measurement vector of the Extended Kalman Filter (EKF) that is designed in this research work (Section 2.3) includes ${\dot{\theta }}_{z,\mathrm{LTP}}$, $a_{x,\mathrm{h}}$ and $a_{y,\mathrm{h}}$ as measurement variables. The process from Equation (14) to Equation (34) is repeated with each new inertial measurement while the vehicle is in motion to keep updating the values of the measurement vector of the EKF.

2.3. Statistical Filtering

The next step is to estimate the vehicle state. This is done by using a motion model to predict the vehicle motion and by using exteroceptive sensors to get feedback.

If one assumes Gauss-Markov stochastic processes, the most computationally efficient method to estimate the optimum state of moving objects is the Kalman Filter. The Kalman Filter fuses the predicted vehicle motion with observed measurements. Its detailed derivation can be found in [15], and the specifics for the present research work are given in what follows.

The measurement vector is defined in the present work as

$${\mathit{z}}_{\mathrm{h}}={\left[\begin{array}{cccccccc}{x}_{\mathrm{h}}& y_{\mathrm{h}}& {\theta }_{z,\mathrm{LTP}}& {\dot{\theta }}_{z,\mathrm{LTP}}& v_{\mathrm{h}}& {\beta }_{\mathrm{h}}& a_{x,\mathrm{h}}& a_{y,\mathrm{h}}\end{array}\right]}^{\mathrm{T}},$$

(35)

where $\left(x_{\mathrm{h}},y_{\mathrm{h}}\right)$ are the $\left(x,y\right)$ coordinates of $o_{\mathrm{LCP}}$ in LTP, ${\theta }_{z,\mathrm{LTP}}$ is the yaw angle of the vehicle, $v_{\mathrm{h}}$ is the velocity over ground of the vehicle, and ${\beta }_{\mathrm{h}}$ is the sideslip angle of the vehicle. The used measurement noise covariance matrix ${\sigma }_{\mathrm{z}}$ and state vector ${\mathit{x}}_{\mathrm{s}}$ are defined as follows

$${\mathit{\sigma }}_{\mathrm{z}}=\mathrm{diag}\left(\begin{array}{cccccccc}{\sigma }_{x,\mathrm{h}}^2& {\sigma }_{y,\mathrm{h}}^2& {\sigma }_{{\theta }_z,\mathrm{LTP}}^2& {\sigma }_{{\dot{\theta }}_z,\mathrm{LTP}}^2& {\sigma }_{v,\mathrm{h}}^2& {\sigma }_{\beta ,\mathrm{h}}^2& {\sigma }_{a_x,\mathrm{h}}^2& {\sigma }_{a_y,\mathrm{h}}^2\end{array}\right),$$

(36)

$${\mathit{x}}_{\mathrm{s}}={\left[\begin{array}{cccccccccc}{x}_{\mathrm{s}}& y_{\mathrm{s}}& {\theta }_{\mathrm{yaw},\mathrm{s}}& {\dot{\theta }}_{\mathrm{yaw},\mathrm{s}}& v_{\mathrm{s}}& {\beta }_{\mathrm{sv}}& {\dot{\beta }}_{\mathrm{sv}}& {\beta }_{\mathrm{sp}}& a_{x,\mathrm{sh}}& a_{y,\mathrm{sh}}\end{array}\right]}^{\mathrm{T}},$$

(37)

where $\left(x_{\mathrm{s}},y_{\mathrm{s}}\right)$ are the $\left(x,y\right)$ coordinates of $o_{\mathrm{LCP}}$ in LTP, ${\theta }_{\mathrm{yaw},\mathrm{s}}$ is the yaw angle, ${\dot{\theta }}_{\mathrm{yaw},\mathrm{s}}$ is the yaw rate, $v_{\mathrm{s}}$ is the velocity over ground, ${\beta }_{\mathrm{sv}}$ is the first estimation of the vehicle sideslip angle, ${\dot{\beta }}_{\mathrm{sv}}$ is the rate of change of the vehicle sideslip angle, ${\beta }_{\mathrm{sp}}$ is the second estimation of the vehicle sideslip angle, and $a_{x,\mathrm{sh}}$ and $a_{y,\mathrm{sh}}$ are the vehicle horizontal accelerations. Two sideslip estimators are used to improve the prediction of the vehicle motion. The first sideslip estimator (${\beta }_{\mathrm{sv}}$ and ${\dot{\beta }}_{\mathrm{sv}}$) is based on the balance of moments of inertia and provides a better performance for the estimation of the vehicle velocity. The second sideslip estimator (${\beta }_{\mathrm{sp}}$) is based on a geometrical model under the assumption of tractive driving, and gives a better performance for the position estimation.

With a distance $l_r$ along the longitudinal axis of the vehicle from $o_{\mathrm{LCP}}$ to the rear transaxle, and additive system noise ${\mathit{\eta }}_{\mathrm{s}}$, the used state equations are given by

$$\begin{array}{c}\hfill {\mathit{x}}_{{\tau }_{i+1},\mathrm{s}}=f\left({\mathit{x}}_{{\tau }_i,\mathrm{s}}\right)+{\mathit{\eta }}_{\mathrm{s}},\end{array}$$

(38)

$$f_{{}_{\begin{array}{c}\hfill 1-10\end{array}}}\left({\mathit{x}}_{{\tau }_i,\mathrm{s}}\right)=\left[\begin{array}{c}{x}_{\mathrm{s}}+v_{\mathrm{s}}\mathrm{c}\left({\theta }_{\mathrm{yaw},\mathrm{s}}+{\beta }_{\mathrm{sp}}\right)\Delta \tau +\left(a_{x,\mathrm{sh}}\mathrm{c}\left({\theta }_{\mathrm{yaw},\mathrm{s}}\right)-a_{y,\mathrm{sh}}\mathrm{s}\left({\theta }_{\mathrm{yaw},\mathrm{s}}\right)\right)\frac{\Delta {\tau }^2}{2}\\ y_{\mathrm{s}}+v_{\mathrm{s}}\mathrm{s}\left({\theta }_{\mathrm{yaw},\mathrm{s}}+{\beta }_{\mathrm{sp}}\right)\Delta \tau +\left(a_{x,\mathrm{sh}}\mathrm{s}\left({\theta }_{\mathrm{yaw},\mathrm{s}}\right)+a_{y,\mathrm{sh}}\mathrm{c}\left({\theta }_{\mathrm{yaw},\mathrm{s}}\right)\right)\frac{\Delta {\tau }^2}{2}\\ {\theta }_{\mathrm{yaw},\mathrm{s}}+{\dot{\theta }}_{\mathrm{yaw},\mathrm{s}}\Delta \tau \\ {\dot{\theta }}_{\mathrm{yaw},\mathrm{s}}\\ v_{\mathrm{s}}+\left(a_{x,\mathrm{sh}}\mathrm{c}\left({\beta }_{\mathrm{sv}}\right)+a_{y,\mathrm{sh}}\mathrm{s}\left({\beta }_{\mathrm{sv}}\right)\right)\Delta \tau \\ {\beta }_{\mathrm{sv}}+{\dot{\beta }}_{\mathrm{sv}}\Delta \tau \\ \frac{1}{v_{\mathrm{s}}}\left(a_{y,\mathrm{sh}}\mathrm{c}\left({\beta }_{\mathrm{sv}}\right)-a_{x,\mathrm{sh}}\mathrm{s}\left({\beta }_{\mathrm{sv}}\right)\right)-{\dot{\theta }}_{\mathrm{yaw},\mathrm{s}}\\ {tan}^{-1}\left(\frac{l_r{\dot{\theta }}_{\mathrm{yaw},\mathrm{s}}}{v_{\mathrm{s}}}\right)\\ a_{x,\mathrm{sh}}\\ a_{y,\mathrm{sh}}\end{array}\right].$$

(39)

From $f_7\left({\mathit{x}}_{\mathrm{s}}\right)$ and $f_8\left({\mathit{x}}_{\mathrm{s}}\right)$, one deducts that as the vehicle drives slower than $1.5\frac{\mathrm{m}}{\mathrm{s}}$, the sideslip estimators lead to a mathematical indetermination. To solve this, when $v_{\mathrm{s}}<1.5\frac{\mathrm{m}}{\mathrm{s}}$, then ${\mathit{x}}_{\mathrm{s}}\left(6\right)$, ${\mathit{x}}_{\mathrm{s}}\left(7\right)$ and ${\mathit{x}}_{\mathrm{s}}\left(8\right)$ are set to zero. Therefore, no sideslip is considered for the velocity or position estimation. However, given the low velocities where the indetermination happens, the integration error is negligible.

2.4. Drift Recognition

The geometrical model from which $f_8\left({\mathit{x}}_{\mathrm{s}}\right)$ is derived, assumes tractive driving. Hence, the performance of the motion model to predict the vehicle motion decays during a drift. By recognizing a drift, it is possible to (i) identify when the performance of the motion model decays, and (ii) take measures such as adjusting the system noise, or switching to other motion models. Therefore, an objective of this research work is to create a drift detector. Other approaches achieve this with multiple SatNav receivers or with specific sensors, such as the Correvit. A drift is recognized in this work by checking the consistency between state variables to reduce the dependence of INSs on external sensors.

The tire longitudinal and lateral slips are a consequence of the forces that act on the tires as well as their mechanical properties. As such, both slips indicate how much force can the tires transmit to the ground. When classified by the tire slips, the driving state of a vehicle can be classified as either “tractive” or “non-tractive” [54]. The tractive driving is characterized by high lateral and longitudinal tire tractive forces and by tire slips usually less than 10${}^{\circ }$ sideslip and less than 10% longitudinal slip. The non-tractive driving is characterized by low tire tractive forces and by high slips that are usually greater than 10${}^{\circ }$ sideslip and greater than 10% longitudinal slip.

Hence, the first pair of state variables to compare to detect a drift, are both sideslip estimators. During tractive driving, ${\beta }_{\mathrm{sv}}\approx {\beta }_{\mathrm{sp}}$. However, as the vehicle starts drifting, the difference between both gets bigger because ${\beta }_{\mathrm{sp}}$ is derived under the assumption of tractive driving. Therefore, the difference between ${\beta }_{\mathrm{sv}}$ and ${\beta }_{\mathrm{sp}}$ is a strong indicator that shows whether the vehicle is drifting or not.

The second pair of variables that are used for the drift recognition are the estimated lateral acceleration $a_{y,\mathrm{sh}}$ and the expected lateral acceleration $a_{y,\mathrm{sc}}$. The latter results from assuming a tire sideslip of ${\beta }_t\approx 0$. This would mean that the path of a turning vehicle describes an arch with constant radius, so that $a_{y,\mathrm{sc}}={\dot{\theta }}_{\mathrm{yaw},\mathrm{s}}{v}_{\mathrm{s}}$. Therefore, $a_{y,\mathrm{sh}}$ estimates the lateral acceleration in reality, and $a_{y,\mathrm{sc}}$ describes how the lateral acceleration should ideally be. During tractive driving, $a_{y,\mathrm{sh}}\approx a_{y,\mathrm{sc}}$. And as the vehicle starts drifting, the difference between $a_{y,\mathrm{sh}}$ and $a_{y,\mathrm{sc}}$ becomes statistically significant.

In the present work, it is said that the vehicle is drifting if $|a_{y,\mathrm{sh}}-a_{y,\mathrm{sc}}|\ge 2.1\frac{\mathrm{m}}{{\mathrm{s}}^2}$ and $|{\beta }_{\mathrm{sv}}-{\beta }_{\mathrm{sp}}|\ge 0.2\mathrm{rad}$. These thresholds are determined after comparing the corresponding state variables as measured during drifting tests, and as estimated during tractive driving on open roads.

2.5. Outlier Detection

The next step is to fuse the predicted vehicle motion with measurements from external sensors (correction data). However, the sensor information could be corrupted, which would affect negatively the estimation of the vehicle state. To avoid this, the state vector is used as reference to filter out flawed sensor measurements. This is because, by definition, the state vector represents the last known optimal state of the vehicle, and because, assuming that both the motion model and the sensors depict reality with 100% fidelity, the measurement vector is equal to the predicted vehicle state.

The consistency check is done with two consecutive sensor measurements made at time instances ${\tau }_1$ and ${\tau }_2$, which are compared to the corresponding predicted state variable. Therefore, the difference between two consecutive velocity measurements could be expressed as follows

$$\begin{array}{c}\hfill v_{{\tau }_2,\mathrm{h}}-v_{{\tau }_1,\mathrm{h}}=\left(a_{x,\mathrm{sh}}\mathrm{c}\left({\beta }_{\mathrm{sv}}\right)+a_{y,\mathrm{sh}}\mathrm{s}\left({\beta }_{\mathrm{sv}}\right)\right)\Delta \tau ,\end{array}$$

(40)

and to consider the inaccuracies of the sensor and the motion model, this can be rewritten as:

$$\begin{array}{c}\hfill v_{{\tau }_2,\mathrm{h}}-v_{{\tau }_1,\mathrm{h}}\approx \left(a_{x,\mathrm{sh}}\mathrm{c}\left({\beta }_{\mathrm{sv}}\right)+a_{y,\mathrm{sh}}\mathrm{s}\left({\beta }_{\mathrm{sv}}\right)\right)\Delta \tau ,\end{array}$$

(41)

Since the purpose is to filter outliers, an additional margin is given by means of a factor $\kappa =3$. Also, only the absolute difference is taken to consider both positive and negative accelerations of the vehicle. Therefore, the measured velocity $v_{{\tau }_2,\mathrm{h}}$ at time instance ${\tau }_2$ is used as correction data if

$$\begin{array}{c}\hfill \left|v_{{\tau }_2,\mathrm{h}}-v_{{\tau }_1,\mathrm{h}}\right|\le \left|\left(a_{x,\mathrm{sh}}\mathrm{c}\left({\beta }_{\mathrm{sv}}\right)+a_{y,\mathrm{sh}}\mathrm{s}\left({\beta }_{\mathrm{sv}}\right)\right)\Delta \tau \kappa \right|.\end{array}$$

(42)

The value of $\kappa$ is determined after analyzing hours of real-world data, where the behavior of the SatNav measurements during regular operation is compared to the behavior of the measurements during periods of time where the multi-path effect is present.

Since ${\theta }_{\mathrm{yaw}}\approx \mathrm{COG}$ during tractive driving, the COG measured by the SatNav receivers can be used as correction data for ${\theta }_{\mathrm{yaw},\mathrm{s}}$ when ${\dot{\theta }}_{\mathrm{yaw},\mathrm{s}}\approx 0$. Analogue to the velocity measurement, the measured COG ${\theta }_{{\tau }_2,\mathrm{yaw},\mathrm{h}}$ at time instance ${\tau }_2$ is used as correction data if

$$\begin{array}{c}\hfill \left|{\theta }_{{\tau }_2,\mathrm{yaw},\mathrm{h}}-{\theta }_{{\tau }_1,\mathrm{yaw},\mathrm{h}}\right|\le \left|{\dot{\theta }}_{\mathrm{yaw},\mathrm{s}}\Delta \tau \kappa \right|.\end{array}$$

(43)

Analogue to the velocity and the COG, the SatNav location measurement $\left(x_{{\tau }_2,\mathrm{h}},y_{{\tau }_2,\mathrm{h}}\right)$ at time instance ${\tau }_2$ is used as correction data if

$$\begin{array}{c}\hfill \sqrt{{\left(x_{{\tau }_2,\mathrm{h}}-x_{{\tau }_1,\mathrm{h}}\right)}^2+{\left(y_{{\tau }_2,\mathrm{h}}-y_{{\tau }_1,\mathrm{h}}\right)}^2}\le \left|\left(v_{\mathrm{s}}+\left(a_{x,\mathrm{sh}}\mathrm{c}\left({\beta }_{\mathrm{sv}}\right)+a_{y,\mathrm{sh}}\mathrm{s}\left({\beta }_{\mathrm{sv}}\right)\right)\Delta \tau \right)\Delta \tau \kappa \right|.\end{array}$$

(44)

The same method to detect outliers can be applied for ${\beta }_{\mathrm{sv}}$ and ${\beta }_{\mathrm{sp}}$ if necessary.

Some sensors can compute quality indicators for the own measurements, such as a dilution of precision or a standard deviation. The problem is that sometimes the sensors do not recognize when they miscalculate their quality measures, as is the case with the standard deviation of SatNav receivers.

To address this, the quality measure is modelled as a PT1 element to dampen it over time. Therefore, let ${\sigma }_{{\tau }_1,\mathrm{h}}$ and ${\sigma }_{{\tau }_2,\mathrm{h}}$ be an element of ${\mathit{\sigma }}_{\mathrm{z}}$ at time instances ${\tau }_1$ and ${\tau }_2$, ${\sigma }_{{\tau }_1,\mathrm{h}}^{\prime }$ and ${\sigma }_{{\tau }_2,\mathrm{h}}^{\prime }$ be the values corresponding to ${\sigma }_{{\tau }_1,\mathrm{h}}$ and ${\sigma }_{{\tau }_2,\mathrm{h}}$ that the sensor calculates, $t_{\mathrm{sat}}=1.5\mathrm{s}$ a saturation parameter, $t_{{\tau }_1}$ and $t_{{\tau }_2}$ a timer at time instances ${\tau }_1$ and ${\tau }_2$. The update of the standard deviation is then modelled as follows

$$t_{{\tau }_2}=\left\{\begin{array}{cc}0,\hfill & {\sigma }_{{\tau }_1,\mathrm{h}}<{\sigma }_{{\tau }_2,\mathrm{h}}^{\prime }\hfill \\ t_{{\tau }_1}+\Delta \tau \hfill & \mathrm{otherwise}\hfill \end{array}\right.,$$

(45)

$${\sigma }_{{\tau }_2,\mathrm{h}}=\left\{\begin{array}{cc}{\sigma }_{{\tau }_2,\mathrm{h}}^{\prime },\hfill & {\sigma }_{{\tau }_1,\mathrm{h}}<{\sigma }_{{\tau }_2,\mathrm{h}}^{\prime }\hfill \\ {\sigma }_{{\tau }_1,\mathrm{h}}{e}^{-\frac{t_{{\tau }_2}}{t_{\mathrm{sat}}}}+{\sigma }_{{\tau }_2,\mathrm{h}}^{\prime }\left(1-e^{-\frac{t_{{\tau }_2}}{t_{\mathrm{sat}}}}\right)\hfill & \mathrm{otherwise}.\hfill \end{array}\right.$$

(46)

The saturation parameter $t_{\mathrm{sat}}$ is optimized for a SatNav receiver by analyzing its measurements with and without the multi-path effect, and can be optimized for other sensors as required.

From Equations (45) and (46), it can be deducted that the strategy is to quickly shift the Kalman gain towards the motion model if the quality of the sensor measurements decays, and to gradually shift the gain towards the sensor if the quality of its measurements continuously improve.

2.6. LiDAR-Based Positioning Method

As stated above, there are various situations where no SatNav is available, but where highly precise correction data is required. A highly precise source of correction data is generated in this work by means of a Velodyne LiDAR HDL-32E [55], and the method is detailed in what follows.

The first step is to identify references or “markers” with the LiDAR. Since recognizing markers by their shape is computationally intensive, they are identified by their reflectivity. It should be noted that the point cloud resolution decreases (i) as the distance d_velo between the LiDAR and the measured object increases, and (ii) as the rotational velocity ω_velo of the LiDAR head increases. The Euclidean distance d_hor,velo between two horizontally adjacent points as a function of d_velo and ω_velo is given by

$$\begin{array}{c}\hfill d_{\mathrm{hor},\mathrm{velo}}=d_{\mathrm{velo}}·sin\left({\omega }_{\mathrm{velo}}·\frac{360}{60}·0.00004608\mathrm{m}\mathrm{s}\right),\end{array}$$

(47)

where $0.00004608\mathrm{m}\mathrm{s}$ is the time required for one firing cycle (a single shot of all lasers for a single LiDAR azimuth). From Equation (47), the relation between $d_{\mathrm{velo}}$, ${\omega }_{\mathrm{velo}}$ and the point cloud resolution can be deducted. This relation can be very relevant as the vehicle velocity increases.

The chosen LiDAR can measure the NIST calibrated reflectivity [56], which ranges from 0 for a lost reflection to 255 for a reflection with no loses. This allows differentiation of highly reflective objects from the rest of the world, but not to identify the highly reflective objects individually.

Once only the points with high reflectivity remain, they are clustered using a time threshold of ${\Gamma }_{\mathrm{cluster}}=1\mathrm{m}\mathrm{s}$. Therefore, the points measured within ${\Gamma }_{\mathrm{cluster}}$ one another, are clustered together. Hence, all points that are within a clustering distance $d_{\mathrm{cluster}}$ from each other are clustered together, so that

$$\begin{array}{c}\hfill d_{\mathrm{cluster}}=d_{\mathrm{velo}}·sin\left({\omega }_{\mathrm{velo}}·\frac{360}{60}·{\Gamma }_{\mathrm{cluster}}\right).\end{array}$$

(48)

The value of $d_{\mathrm{cluster}}$ is important when defining ${\omega }_{\mathrm{velo}}$, ${\Gamma }_{\mathrm{cluster}}$ and the spacing between markers.

The next step is to determine a single set of (x,y) coordinates for the cluster. For this work, the motion of the vehicle is limited to the $x_{\mathrm{LTP}}\times y_{\mathrm{LTP}}$-plane. Therefore, the i-th cluster point ${\mathit{p}}_{i,\mathrm{velo}}$ is defined as

$${\mathit{p}}_{i,\mathrm{velo}}=\left[\begin{array}{c}{x}_{i,\mathrm{velo}}\\ y_{i,\mathrm{velo}}\end{array}\right]=\left[\begin{array}{c}{d}_{i,\mathrm{velo}}·cos\left({\theta }_{i,\mathrm{velo}}\right)\\ d_{i,\mathrm{velo}}·sin\left({\theta }_{i,\mathrm{velo}}\right)\end{array}\right],$$

(49)

and the i-th cluster of LiDAR measurements is then defined as

$${\mathit{P}}_{i,\mathrm{velo}}\in {\mathbb{R}}^{2,n_{\mathrm{pts}}}=\left[\begin{array}{cccc}{x}_{1,\mathrm{velo}}& x_{i,\mathrm{velo}}& ...& x_{n_{\mathrm{pts}},\mathrm{velo}}\\ y_{1,\mathrm{velo}}& y_{i,\mathrm{velo}}& ...& y_{n_{\mathrm{pts}},\mathrm{velo}}\end{array}\right],$$

(50)

where $d_{i,\mathrm{velo}}$ and ${\theta }_{i,\mathrm{velo}}$ are respectively the range and azimuth angle of the i-th measured point, and $n_{\mathrm{pts}}$ is the number of points of the cluster.

The coordinates ${\mathit{p}}_{i,\mathrm{m}}^{\prime }={[x_{i,\mathrm{m}}^{\prime },y_{i,\mathrm{m}}^{\prime }]}^{\mathrm{T}}$ that express the measured position in LCP of the i-th cluster (marker) are then calculated by means of the mid-range arithmetic mean, so that

$${\mathit{p}}_{i,\mathrm{m}}^{\prime }=\left[\begin{array}{c}{x}_{i,\mathrm{m}}^{\prime }\\ y_{i,\mathrm{m}}^{\prime }\end{array}\right]=\left[\begin{array}{c}\frac{max\left({\mathit{P}}_{i,\mathrm{velo}}\left(1,:\right)\right)+min\left({\mathit{P}}_{i,\mathrm{velo}}\left(1,:\right)\right)}{2}\\ \frac{max\left({\mathit{P}}_{i,\mathrm{velo}}\left(2,:\right)\right)+min\left({\mathit{P}}_{i,\mathrm{velo}}\left(2,:\right)\right)}{2}\end{array}\right].$$

(51)

The mid-range arithmetic is applied as well to $d_{\mathrm{velo}}$, ${\theta }_{\mathrm{velo}}$ and the timestamp of the clustered measurements. This allows one to obtain the distance $d_{i,\mathrm{m}}$, the azimuth ${\theta }_{i,\mathrm{m}}$ and the time instance at which the i-th marker is seen. Next, the true location in LTP of the markers must be known. This is defined for the i-th marker as follows

$$\begin{array}{c}\hfill {\mathit{p}}_{i,\mathrm{map}}={[x_{i,\mathrm{map}},y_{i,\mathrm{map}}]}^{\mathrm{T}}.\end{array}$$

(52)

The true location in LTP of all markers is stored in the marker map, which is assumed to be available beforehand. This map can be generated, for example, by means of a tachymeter or by stitching together LiDAR measurements of the test area. The marker map is then defined as follows

$${\mathit{P}}_{\mathrm{map}}\in {\mathbb{R}}^{3,n_{\mathrm{pts}}}=\left[\begin{array}{cccc}1& i& ...& n_{\mathrm{pts}}\\ x_{1,\mathrm{map}}& x_{i,\mathrm{map}}& ...& x_{n_{\mathrm{pts}},\mathrm{map}}\\ y_{1,\mathrm{map}}& y_{i,\mathrm{map}}& ...& y_{n_{\mathrm{pts}},\mathrm{map}}\end{array}\right],$$

(53)

where $n_{\mathrm{pts}}$ is the total number markers, and the first row is a unique identifier for each marker.

In order to identify which marker is being measured, an approximate position ${\mathit{p}}_{\mathrm{app}}$ and an approximate orientation ${\theta }_{\mathrm{app}}$ of the vehicle in LTP is required. This approximate position can be, for example, $\left(x_{\mathrm{s}},y_{\mathrm{s}}\right)$, $\left(x_{\mathrm{h}},y_{\mathrm{h}}\right)$ or initialization values, and is defined as follows

$$\begin{array}{c}\hfill {\mathit{p}}_{\mathrm{app}}={[x_{\mathrm{app}},y_{\mathrm{app}}]}^{\mathrm{T}}.\end{array}$$

(54)

The positioning error ${\eta }_{\mathrm{d},\mathrm{app}}$ is then defined as follows

$$\begin{array}{c}\hfill {\eta }_{\mathrm{d},\mathrm{app}}=\left|{\mathit{p}}_{\mathrm{app}}-{\mathit{p}}_{\mathrm{true}}\right|,\end{array}$$

(55)

were ${\mathit{p}}_{\mathrm{true}}={[x_{\mathrm{true}},y_{\mathrm{true}}]}^{\mathrm{T}}$ is the true position of the vehicle in LTP. Since the location of the vehicle is tracked by means of an EKF, during tractive driving it holds that

$${\mathit{p}}_{\mathrm{true}}\approx {\mathit{p}}_{\mathrm{app}}\approx \left[\begin{array}{c}{x}_{\mathrm{s}}\\ y_{\mathrm{s}}\end{array}\right].$$

(56)

The approximate orientation can be ${\theta }_{\mathrm{yaw},\mathrm{s}}$ or an initialization value, and during tractive driving it holds that

$$\begin{array}{c}\hfill {\theta }_{\mathrm{true}}\approx {\theta }_{\mathrm{app}}\approx {\theta }_{\mathrm{yaw},\mathrm{s}},\end{array}$$

(57)

were ${\theta }_{\mathrm{true}}$ is the true orientation of the vehicle in LTP. The orientation error ${\eta }_{\theta }$ is defined as follows

$$\begin{array}{c}\hfill {\eta }_{\theta }=\left|{\theta }_{\mathrm{app}}-{\theta }_{\mathrm{true}}\right|.\end{array}$$

(58)

So as to prevent ambiguities in the marker identification, ${\eta }_{\mathrm{d},\mathrm{app}}$ and ${\eta }_{\theta }$ are constrained as follows

$$\begin{array}{c}\hfill \left|{\mathit{p}}_{i,\mathrm{m}}^{\prime }\right|sin\left({\eta }_{\theta }\right)+{\eta }_{\mathrm{d},\mathrm{app}}<\frac{min\left({\mathit{d}}_{\mathrm{map}}\right)}{2},\end{array}$$

(59)

where ${\mathit{d}}_{\mathrm{map}}$ is a vector with the Euclidean distance between all possible marker combinations. The markers do not have to be placed in specific patterns, as long as the constraint of Equation (59) holds. The measured position ${\mathit{p}}_{i,\mathrm{m}}={[x_{i,\mathrm{m}},y_{i,\mathrm{m}}]}^{\mathrm{T}}$ in LTP of the i-th marker is then given by

$${\mathit{p}}_{i,\mathrm{m}}={\mathit{p}}_{\mathrm{app}}+\left[\begin{array}{cc}cos\left({\theta }_{\mathrm{app}}\right)& -sin\left({\theta }_{\mathrm{app}}\right)\\ sin\left({\theta }_{\mathrm{app}}\right)& cos\left({\theta }_{\mathrm{app}}\right)\end{array}\right]{\mathit{p}}_{i,\mathrm{m}}^{\prime }.$$

(60)

The element of ${\mathit{P}}_{\mathrm{map}}$ closest to ${\mathit{p}}_{i,\mathrm{m}}$ is then the measured marker.

Once the markers can be individually identified, the measurement variables are calculated. The first one is the velocity over ground $v_{\mathrm{h}}$. To calculate it, it is required that two LiDAR measurements (consecutive or not) point to the same marker. So, let ${\mathit{p}}_{{\tau }_1,\mathrm{m}}^{\prime }$ and ${\mathit{p}}_{{\tau }_2,\mathrm{m}}^{\prime }$ be the measurements in LCP of one marker done at time instances ${\tau }_1$ and ${\tau }_2$ accordingly, so that

$${\mathit{p}}_{{\tau }_1,\mathrm{m}}^{\prime }=\left[\begin{array}{c}{x}_{{\tau }_1,\mathrm{m}}^{\prime }\\ y_{{\tau }_1,\mathrm{m}}^{\prime }\end{array}\right]=\left[\begin{array}{c}{d}_{{\tau }_1,\mathrm{m}}·cos\left({\theta }_{{\tau }_1,\mathrm{m}}\right)\\ d_{{\tau }_1,\mathrm{m}}·sin\left({\theta }_{{\tau }_1,\mathrm{m}}\right)\end{array}\right],$$

(61)

$${\mathit{p}}_{{\tau }_2,\mathrm{m}}^{\prime }=\left[\begin{array}{c}{x}_{{\tau }_2,\mathrm{m}}^{\prime }\\ y_{{\tau }_2,\mathrm{m}}^{\prime }\end{array}\right]=\left[\begin{array}{c}{d}_{{\tau }_2,\mathrm{m}}·cos\left({\theta }_{{\tau }_2,\mathrm{m}}\right)\\ d_{{\tau }_2,\mathrm{m}}·sin\left({\theta }_{{\tau }_2,\mathrm{m}}\right)\end{array}\right].$$

(62)

From ${\mathit{p}}_{{\tau }_1,\mathrm{m}}^{\prime }$ and ${\mathit{p}}_{{\tau }_2,\mathrm{m}}^{\prime }$, the geometry of a cone is constructed as shown in Figure 3.

Since the sum of the internal angles of a triangle is $\left[\pi \right]\mathrm{rad}$, the internal angles of the constructed geometry are calculated as follows

$${\theta }_{1,\mathrm{v}}=\left\{\begin{array}{cc}{\theta }_{{\tau }_1,\mathrm{m}},\hfill & {\theta }_{{\tau }_1,\mathrm{m}}\le \pi \hfill \\ 2\pi -{\theta }_{{\tau }_1,\mathrm{m}},\hfill & {\theta }_{{\tau }_1,\mathrm{m}}>\pi \hfill \end{array}\right.,$$

(63)

$${\theta }_{2,\mathrm{v}}=\left\{\begin{array}{cc}\pi -{\theta }_{{\tau }_2,\mathrm{m}}+{\dot{\theta }}_{\mathrm{yaw},\mathrm{s}}·\Delta \tau ,\hfill & {\theta }_{{\tau }_2,\mathrm{m}}\le \pi \hfill \\ {\theta }_{{\tau }_2,\mathrm{m}}+{\dot{\theta }}_{\mathrm{yaw},\mathrm{s}}·\Delta \tau -\pi ,\hfill & {\theta }_{{\tau }_2,\mathrm{m}}>\pi \hfill \end{array}\right.,$$

(64)

$$\begin{array}{c}\hfill {\theta }_{3,\mathrm{v}}=\pi -{\theta }_{2,\mathrm{v}}-{\theta }_{1,\mathrm{v}}.\end{array}$$

(65)

The vehicle displacement $d_{\mathrm{car}}$ between the time instances ${\tau }_1$ and ${\tau }_2$ is calculated by means of the cosine law. In addition, since the LiDAR measurements have a timestamp, the measured velocity over ground of the vehicle $v_{\mathrm{h}}$ between ${\tau }_1$ and ${\tau }_2$ is calculated as follows

$$\begin{array}{c}\hfill v_{\mathrm{h}}=\frac{d_{\mathrm{car}}}{\Delta \tau }=\frac{\sqrt{d_{{\tau }_1,\mathrm{m}}^2+d_{{\tau }_2,\mathrm{m}}^2-2·d_{{\tau }_1,\mathrm{m}}·d_{{\tau }_2,\mathrm{m}}·cos\left({\theta }_{3,\mathrm{v}}\right)}}{\Delta \tau }.\end{array}$$

(66)

Nevertheless, the cone geometry is not completely defined as it can rotate around the marker. Therefore, it cannot be used to calculate the vehicle location. Instead, both LiDAR measurements must point to different markers. Also, to completely define the geometry shown in Figure 4, ${\mathit{p}}_{{\tau }_2,\mathrm{m}}^{\prime }$ is expressed in the LCP at time instance ${\tau }_1$ as follows

$${\tilde{\mathit{p}}}_{{\tau }_2,\mathrm{m}}={\left[\begin{array}{cc}cos\left({\theta }_{{\tau }_2,\mathrm{yaw},\mathrm{s}}-{\theta }_{{\tau }_1,\mathrm{yaw},\mathrm{s}}\right)& -sin\left({\theta }_{{\tau }_2,\mathrm{yaw},\mathrm{s}}-{\theta }_{{\tau }_1,\mathrm{yaw},\mathrm{s}}\right)\\ sin\left({\theta }_{{\tau }_2,\mathrm{yaw},\mathrm{s}}-{\theta }_{{\tau }_1,\mathrm{yaw},\mathrm{s}}\right)& cos\left({\theta }_{{\tau }_2,\mathrm{yaw},\mathrm{s}}-{\theta }_{{\tau }_1,\mathrm{yaw},\mathrm{s}}\right)\end{array}\right]}^{\mathrm{T}}{\mathit{p}}_{{\tau }_2,\mathrm{m}}^{\prime }+\left[\begin{array}{c}{x}_{{\tau }_2,\mathrm{s}}-x_{{\tau }_1,\mathrm{s}}\\ y_{{\tau }_2,\mathrm{s}}-y_{{\tau }_1,\mathrm{s}}\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_{{\tau }_2,\mathrm{m}}\\ {\tilde{y}}_{{\tau }_2,\mathrm{m}}\end{array}\right].$$

(67)

Next, assuming that ${\mathit{p}}_{{\tau }_1,\mathrm{m}}^{\prime }$ and ${\mathit{p}}_{{\tau }_2,\mathrm{m}}^{\prime }$ point to ${\mathit{p}}_{1,\mathrm{map}}$ and ${\mathit{p}}_{2,\mathrm{map}}$ respectively, the angular offset ${\Xi }_{\theta ,\mathrm{LCP}}$ from the LTP to the LCP at time instance ${\tau }_1$ is given by

$$\begin{array}{c}\hfill {\Xi }_{\theta ,\mathrm{LCP}} = arctan2\left({\tilde{y}}_{{\tau }_2,\mathrm{m}}-y_{{\tau }_1,\mathrm{m}}^{\prime },{\tilde{x}}_{{\tau }_2,\mathrm{m}}-x_{{\tau }_1,\mathrm{m}}^{\prime }\right) - arctan2\left(y_{2,\mathrm{map}}-y_{1,\mathrm{map}},x_{2,\mathrm{map}}-x_{1,\mathrm{map}}\right).\end{array}$$

(68)

Afterwards, the marker measurements in LCP at time instance ${\tau }_1$ are rotated as follows

$${\tilde{\mathit{p}}}_{{\tau }_1,\mathrm{m}}^{*}={\left[\begin{array}{cc}cos\left({\Xi }_{\theta ,\mathrm{LCP}}\right)& -sin\left({\Xi }_{\theta ,\mathrm{LCP}}\right)\\ sin\left({\Xi }_{\theta ,\mathrm{LCP}}\right)& cos\left({\Xi }_{\theta ,\mathrm{LCP}}\right)\end{array}\right]}^{\mathrm{T}}{\mathit{p}}_{{\tau }_1,\mathrm{m}}^{\prime }=\left[\begin{array}{c}{\tilde{x}}_{{\tau }_1,\mathrm{m}}^{*}\\ {\tilde{y}}_{{\tau }_1,\mathrm{m}}^{*}\end{array}\right],$$

(69)

$${\tilde{\mathit{p}}}_{{\tau }_2,\mathrm{m}}^{*}={\left[\begin{array}{cc}cos\left({\Xi }_{\theta ,\mathrm{LCP}}\right)& -sin\left({\Xi }_{\theta ,\mathrm{LCP}}\right)\\ sin\left({\Xi }_{\theta ,\mathrm{LCP}}\right)& cos\left({\Xi }_{\theta ,\mathrm{LCP}}\right)\end{array}\right]}^{\mathrm{T}}{\tilde{\mathit{p}}}_{{\tau }_2,\mathrm{m}}=\left[\begin{array}{c}{\tilde{x}}_{{\tau }_2,\mathrm{m}}^{*}\\ {\tilde{y}}_{{\tau }_2,\mathrm{m}}^{*}\end{array}\right].$$

(70)

Finally, the linear offsets ${\Xi }_{\mathrm{LCP}}$ from the LTP to the LCP at time instance ${\tau }_1$ can be estimated by comparing ${\tilde{\mathit{p}}}_{{\tau }_1,\mathrm{m}}^{*}$ and ${\tilde{\mathit{p}}}_{{\tau }_2,\mathrm{m}}^{*}$ with the true marker location ${\mathit{p}}_{1,\mathrm{map}}$ and ${\mathit{p}}_{2,\mathrm{map}}$ from the marker library. Since both measurements have the same accuracy, the average of both offsets is calculated as follows

$${\Xi }_{\mathrm{LCP}}=\frac{\left({\tilde{\mathit{p}}}_{{\tau }_1,\mathrm{m}}^{*}-{\mathit{p}}_{1,\mathrm{map}}\right)+\left({\tilde{\mathit{p}}}_{{\tau }_2,\mathrm{m}}^{*}-{\mathit{p}}_{2,\mathrm{map}}\right)}{2}=\left[\begin{array}{c}{\Xi }_{x,\mathrm{LCP}}\\ {\Xi }_{y,\mathrm{LCP}}\end{array}\right].$$

(71)

The location and orientation measurement variables at time instances ${\tau }_1$ and ${\tau }_2$ are then given by

$$\left[\begin{array}{c}{x}_{{\tau }_1,\mathrm{h}}\\ y_{{\tau }_1,\mathrm{h}}\\ {\theta }_{{\tau }_1,\mathrm{yaw},\mathrm{h}}\end{array}\right]=-\left[\begin{array}{c}{\Xi }_{x,\mathrm{LCP}}\\ {\Xi }_{y,\mathrm{LCP}}\\ {\Xi }_{\theta ,\mathrm{LCP}}\end{array}\right],$$

(72)

$$\left[\begin{array}{c}{x}_{{\tau }_2,\mathrm{h}}\\ y_{{\tau }_2,\mathrm{h}}\\ {\theta }_{{\tau }_2,\mathrm{yaw},\mathrm{h}}\end{array}\right]=-\left[\begin{array}{c}{\Xi }_{x,\mathrm{LCP}}\\ {\Xi }_{y,\mathrm{LCP}}\\ {\Xi }_{\theta ,\mathrm{LCP}}\end{array}\right]+\left[\begin{array}{c}{x}_{{\tau }_2,\mathrm{s}}-x_{{\tau }_1,\mathrm{s}}\\ y_{{\tau }_2,\mathrm{s}}-y_{{\tau }_1,\mathrm{s}}\\ {\theta }_{{\tau }_2,\mathrm{yaw},\mathrm{s}}-{\theta }_{{\tau }_1,\mathrm{yaw},\mathrm{s}}\end{array}\right].$$

(73)

As can be deducted from the previous equations, the measurement variables for two consecutive time instances are calculated at each iteration, i.e., once the LiDAR measurement from the time instance ${\tau }_2$ is acquired, the vehicle pose for the time instances ${\tau }_1$ and ${\tau }_2$ is calculated. When the LiDAR measurement from the time instance ${\tau }_3$ is acquired, the vehicle pose for the time instances ${\tau }_2$ and ${\tau }_3$ is calculated; and so on. Since this creates an overlap of two pose measurements per time instance, both are averaged. Therefore, let $\left(x_{\mathrm{h},\mathrm{a}},y_{\mathrm{h},\mathrm{a}}\right)$ and $\left(x_{\mathrm{h},\mathrm{b}},y_{\mathrm{h},\mathrm{b}}\right)$ be the measured location of the vehicle at the i-th time instance that is calculated with the LiDAR measurements from the time instances $i-1$ to $i+1$. The measurement variables for the vehicle location at the i-th time instance are then given by

$$\left[\begin{array}{c}{x}_{{\tau }_i,\mathrm{h}}\\ y_{{\tau }_i,\mathrm{h}}\end{array}\right]=\frac{\left[\begin{array}{c}{x}_{\mathrm{h},\mathrm{a}}\\ y_{\mathrm{h},\mathrm{a}}\end{array}\right]+\left[\begin{array}{c}{x}_{\mathrm{h},\mathrm{b}}\\ y_{\mathrm{h},\mathrm{b}}\end{array}\right]}{2}.$$

(74)

Analogously, let ${\theta }_{\mathrm{yaw},\mathrm{h},\mathrm{a}}$ and ${\theta }_{\mathrm{yaw},\mathrm{h},\mathrm{b}}$ be the measured orientation of the vehicle at the i-th time instance that is calculated with the LiDAR measurements from the time instances $i-1$ to $i+1$. The measurement variable for the vehicle orientation at the i-th time instance is then given by

$$\begin{array}{c}\hfill {\theta }_{{\tau }_i,\mathrm{yaw},\mathrm{h}}=\frac{{\theta }_{\mathrm{yaw},\mathrm{h},\mathrm{a}}+{\theta }_{\mathrm{yaw},\mathrm{h},\mathrm{b}}}{2}.\end{array}$$

(75)

As can be deducted from Equations (74) and (75), to average the measurement variables that are obtained using different pairs of LiDAR measurements aids to compensate LiDAR measurement errors, and creates a time-smoothing effect for the measurement vector. A graphical depiction of this process is shown in Figure 5.

To ensure that the measured object is a marker and not any other reflective object, outliers are filtered out. For this, the LiDAR measurements are translated to the LTP to compare them with the true marker position from the marker library. Therefore, the locations ${\widehat{\mathit{p}}}_{{\tau }_i,\mathrm{m}}^{\prime }$ and ${\widehat{\mathit{p}}}_{{\tau }_{i+1},\mathrm{m}}^{\prime }$ in LTP of the LiDAR measurements ${\mathit{p}}_{{\tau }_i,\mathrm{m}}^{\prime }$ and ${\mathit{p}}_{{\tau }_{i+1},\mathrm{m}}^{\prime }$ made at the i-th and $i+1$-th time instance are expressed as follows

$${\widehat{\mathit{p}}}_{{\tau }_i,\mathrm{m}}^{\prime }={\left[\begin{array}{cc}cos\left({\Xi }_{\theta ,\mathrm{LTP}}\right)& -sin\left({\Xi }_{\theta ,\mathrm{LTP}}\right)\\ sin\left({\Xi }_{\theta ,\mathrm{LTP}}\right)& cos\left({\Xi }_{\theta ,\mathrm{LTP}}\right)\end{array}\right]}^{\mathrm{T}}{\mathit{p}}_{{\tau }_i,\mathrm{m}}^{\prime }-{\Xi }_{\mathrm{LTP}},$$

(76)

$${\widehat{\mathit{p}}}_{{\tau }_{i+1},\mathrm{m}}^{\prime }={\left[\begin{array}{cc}cos\left({\Xi }_{\theta ,\mathrm{LTP}}\right)& -sin\left({\Xi }_{\theta ,\mathrm{LTP}}\right)\\ sin\left({\Xi }_{\theta ,\mathrm{LTP}}\right)& cos\left({\Xi }_{\theta ,\mathrm{LTP}}\right)\end{array}\right]}^{\mathrm{T}}{\mathit{p}}_{{\tau }_{i+1},\mathrm{m}}^{\prime }-{\Xi }_{\mathrm{LTP}}.$$

(77)

Assuming that ${\widehat{\mathit{p}}}_{{\tau }_i,\mathrm{m}}^{\prime }$ and ${\widehat{\mathit{p}}}_{{\tau }_{i+1},\mathrm{m}}^{\prime }$ are closest to the i-th and $i+1$-th markers respectively, their measurement errors ${\eta }_{i,\mathrm{m}}$ and ${\eta }_{i+1,\mathrm{m}}$ are given by

$$\begin{array}{c}\hfill {\eta }_{i,\mathrm{m}} = \left|{\widehat{\mathit{p}}}_{{\tau }_i,\mathrm{m}}^{\prime }-{\mathit{p}}_{i,\mathrm{map}}\right| = {\eta }_{i+1,\mathrm{m}} = \left|{\widehat{\mathit{p}}}_{{\tau }_{i+1},\mathrm{m}}^{\prime }-{\mathit{p}}_{i+1,\mathrm{map}}\right|.\end{array}$$

(78)

The previous because, as shown in Equations (71) and (75), the angular and linear offsets are estimated with both LiDAR measurements.

A distance threshold is then used to separate outliers according to the following criteria:

$$\left\{\begin{array}{cc}\mathrm{inlier},\hfill & {\eta }_{i,\mathrm{m}}\le \frac{min\left({\mathit{d}}_{\mathrm{map}}\right)}{2}\hfill \\ \mathrm{outlier},\hfill & {\eta }_{i,\mathrm{m}}>\frac{min\left({\mathit{d}}_{\mathrm{map}}\right)}{2}.\hfill \end{array}\right.$$

(79)

If an outlier is detected, both measurements are discarded.

From what is detailed above, the LbPM can be used only in places with known markers, or an extra effort is needed to place and measure markers in new areas. However, traffic signs have a fixed position and are highly reflective as well. Therefore, they too can be used as markers for Simultaneous Localization and Mapping (SLAM) instead of having a marker library a priori.

Ongoing work of the present research focuses on evaluating the adequacy of the LbPM for SLAM purposes. A critical aspect to evaluate an LbPM-based SLAM, is to identify the sources of error individually. Since the vehicle motion prediction between LiDAR measurements can be approximated as circular movement, the LbPM does not depend on the motion model of the EKF. Hence, it is possible to analyze the motion model with no correction data and the LbPM separately. However, the recursiveness of the prediction-correction process of the EKF and localization-mapping process of the SLAM complicate the isolation of the sources of error when the EKF and the LbPM are combined.

To study separately the sources of error, the mapping performance of the LbPM-based SLAM is inspected under two circumstances: (i) with position, velocity and COG correction data; and (ii) with only velocity correction data. The first variant excludes all possible LbPM feedback errors. The second variant excludes only the velocity feedback from the LbPM as a source of error. The presented method is based on [57,58,59,60], and the specifics applicable to this work are detailed in what follows.

First, some measurement variables are considered instead as inputs to perform the prediction of vehicle state. An input vector ${\mathit{u}}_{\mathrm{h}}$ and the control covariance matrix ${\mathit{Q}}_k$ are then defined as follows

$${\mathit{u}}_{\mathrm{h}}={\left[\begin{array}{cccc}{a}_{x,\mathrm{h}}& a_{y,\mathrm{h}}& {\theta }_{\mathrm{yaw},\mathrm{h}}& v_{\mathrm{h}}\end{array}\right]}^{\mathrm{T}},$$

(80)

$${\mathit{Q}}_k=diag\left(\begin{array}{cccc}{\sigma }_{a_x,\mathrm{h}}^2& {\sigma }_{a_y,\mathrm{h}}^2& {\sigma }_{{\dot{\theta }}_{\mathrm{yaw}}}^2& {\sigma }_{v,\mathrm{h}}^2\end{array}\right).$$

(81)

With this, the vehicle state process covariance matrix can be updated as follows

$$\begin{array}{c}\hfill {\mathbf{P}}_{\mathrm{xx},\mathrm{k}}={\mathbf{F}}_x{\mathbf{P}}_{\mathrm{xx},\mathrm{k}-1}{\mathbf{F}}_x^{\mathrm{T}}+{\mathbf{F}}_u{\mathbf{Q}}_k{\mathbf{F}}_u^{\mathrm{T}},\end{array}$$

(82)

where ${\mathbf{F}}_u$ is the Jacobian matrix of $f\left({\mathit{x}}_{\mathrm{s}}\right)$, derived after the control vector ${\mathit{u}}_{\mathrm{h}}$.

Next, a measurement vector ${\mathit{z}}_{\mathrm{M}}$ is defined as

$${\mathit{z}}_{\mathrm{M}}={\left[\begin{array}{cc}{d}_{i,\mathrm{m}}& {\theta }_{i,\mathrm{m}}\end{array}\right]}^{\mathrm{T}},$$

(83)

where $d_{i,\mathrm{m}}$ and ${\theta }_{i,\mathrm{m}}$ are respectively the range and azimuth angle of the i-th cluster.

The task of the observation model is to calculate the estimated measurement ${\tilde{\mathit{z}}}_{\mathrm{M}}$, for a given vehicle state ${\mathit{x}}_{\mathrm{s}}$ and one specific marker ${\mathit{p}}_{i,\mathrm{m}}^{\prime }$. For this, two cases can be distinguished. If the current marker library is empty, then the marker is added to it with the location derived from ${\mathit{x}}_{\mathrm{s}}$ and ${\mathit{z}}_{\mathrm{M}}$. If the marker library is not empty, then an association check is required to compute the probability that the current observation corresponds to an existing marker. For this, the Mahalanobis Distance [61] Individual Compatibility (IC) check is performed for each marker in the library. The one yielding the minimum Mahalanobis Distance is the marker from the library with the highest probability to be the observed one. For this, the innovation term ${\mathbf{y}}_{\mathrm{M}}$ is calculated as follows

$$\begin{array}{c}\hfill {\mathbf{y}}_{\mathrm{M}}={\mathit{z}}_{\mathrm{M}}-{\tilde{\mathit{z}}}_{\mathrm{M}}.\end{array}$$

(84)

The md is then given by

$$\begin{array}{c}\hfill d_{\mathrm{M}}=\sqrt{{\mathbf{y}}_{\mathrm{M}}^T{\mathbf{S}}_{\mathrm{M}}^{-1}{\mathbf{y}}_{\mathrm{M}}},\end{array}$$

(85)

with ${\mathbf{S}}_{\mathrm{M}}$ being the corresponding covariance matrix for the innovation term ${\mathbf{y}}_{\mathrm{M}}$. Finally, the marker from the library is associated with the observation according to the following criteria

$$\left\{\begin{array}{cc}\mathrm{associated},\hfill & d_{\mathrm{M}}\le {\Gamma }_M\hfill \\ \mathrm{not}\mathrm{associated},\hfill & d_{\mathrm{M}}>{\Gamma }_M,\hfill \end{array}\right.$$

(86)

$$\begin{array}{c}\hfill {\Gamma }_{\mathrm{M}}=3.\end{array}$$

(87)

This is because a threshold ${\Gamma }_{\mathrm{M}}=3$ means a probability of $98.9\%$ that the sensor measurement and the estimated measurement refer to the same marker [62].

3. Results

To validate the performance of the From methods that are detailed above, a series of tests are designed and performed. The From methods are tested with several hours of real-world data. In what follows, the testing details, the evaluation metrics and the most relevant results are presented.

3.1. Standstill Recognition

The performance of the standstill recognition is measured in terms of its classification performance according to Table 1, as well as its robustness against false positives. The four most relevant tests and their results are detailed in what follows.

The results of the 1st test are shown on the Figure 6. Here, a 3rd generation Smart ForTwo electric drive is placed on a test track. The INS is mounted in the trunk of the vehicle. The vehicle is turned on, the front tires are set to point straight ahead, and the gear selector placed on “D”. Some seconds after the data recording starts, the brake pedal is released. The car is driven on the test track for approximately 530 s. The accelerator pedal is not touched at any point, and the brake pedal is used only at the beginning to let the car roll and at the end to stop it. The steering wheel is used only to realign the vehicle towards paths that allow prolonged straight-line driving.

What makes this test special is the combination of (i) the low driving velocity, (ii) the drivetrain of the vehicle (electrical motor and single gear reduction transmission), and (iii) the prolonged driving moments without rotations.

As can be seen, the RF is able to recognize that the vehicle is in motion, even when driving several seconds constantly at walking velocity on a straight line: (17.87–92.53 s) and (434.80–522.00 s). It is seen as well that the RF has no false positives.

The results of the 2nd test are shown on the Figure 7. Here, a 5th generation Audi A4 3.0 L TDI with an S-Tronic 7-gear transmission is driven randomly in a city. The INS is mounted in the trunk of the vehicle. On three occasions, the gear selector is placed on “N”, and the car is allowed to roll. On one of those occasions (22.26 s), the vehicle does come to a brief stop. On the other two occasions (138.10 s and 682.20 s), the car reaches very low velocities ($0.18\frac{\mathrm{m}}{\mathrm{s}}$ and $0.12\frac{\mathrm{m}}{\mathrm{s}}$ respectively), but it does not come to a full stop. A prolonged standstill (302.10–652.00 s) is also included. During this period of time, the gear selector is kept in “S”.

This test shows (i) three rolling instances, and (ii) a prolonged standstill. Diesel engines are known to produce more vibrations than their gasoline counterparts. Also, when the gear selector is placed on “N”, much less vibrations are transmitted to the vehicle chassis because the transmission is not engaged, and because the engine idles. Therefore, the IMU signals when the vehicle rolls closely resemble those when the car is standing still, especially at low velocities. Contrarily, when the gear selector is on “S” instead of “D”, the vibrations transmitted to the chassis increase notably.

As can be seen, in the first rolling instance (22.26 s) the RF is able to recognize the standstill first when the vehicle does come to a stop and not before. Also, the RF can recognize that the vehicle is still in motion at the other two rolling instances (138.10 s and 682.20 s), despite the velocity over ground almost reaching zero. The RF is also mostly able to recognize that the vehicle is standing still (302.10–652.00 s), despite the increased vibrations provoked by selecting “S”. After 720.20 s of true standstill, the estimated total displacement of the vehicle is 0.008 m (8 mm) and the estimated total rotation of the vehicle is 0.011 rad (0.6282 deg). It is seen as well that the RF has no false positives.

The results of the 3rd test are shown on the Figure 8. Here, a Suzuki GSX-R750 K2 is driven on a test track. The INS is mounted by means of a metal plate directly on the chassis of the vehicle. On four occasions (251.80 s, 327.60 s, 391.20 s and 457.10 s), the “N” gear is selected, and the vehicle can roll to a full stop. Various instances of several seconds of standstill are included as well. In two of those instances (43.88–53.75 s and 94.08–108.20 s), the vehicle was let to rest on its side stand, with the motor idling and the gear “N” selected.

What makes this test special are (i) the chassis-drivetrain configuration, and (ii) the rolling instances. By design, the engine mounts of motorcycles are not designed to dampen the engine vibrations as well as the engine mounts of cars. Thus, these vibrations are transmitted in a much more direct manner to the motorcycle chassis. Also, given that the INS is mounted on the chassis with no dampening, it senses the engine vibrations in a much more direct manner. Thus, the INS signals under these conditions, in combination with the rolling instances, echo the signals that are present at standstill.

As can be seen, on two of the occasions where the motorcycle can roll (251.80 s and 327.60 s), false positives appear. On the other two occasions (391.20 s and 457.10 s), the RF detects a standstill first when the vehicle comes to a stop. It is seen as well that the RF has no false positives under regular driving conditions (driving always on gear).

The results of the 4th test are shown on the Figure 9. Here, a 2nd generation Audi Q7 is parked on a test track with the engine idling and the gear “P” selected. The INS is mounted in the trunk of the vehicle. The volume of the sound system is set to the maximum and music with high bass level is played for 254 s, to induce very strong vibrations for prolonged periods of time. The objective is to test if the RF can recognize the standstill regardless of the induced vibrations.

What makes this test special is the constant induction of vibrations which by magnitude is many times bigger than that of when the vehicle is standing still.

As can be seen, the output of the RF toggles much more than in the other tests. However, it is often still able to correctly detect a standstill. The maximum period of time where the vehicle state is wrongly classified as “motion” is 4.20 s, between 190.3 s and 194.50 s. In this test, after 254.01 s of true standstill, the estimated total displacement of the vehicle is 0.12 m (12.12 cm) and the estimated total rotation of the vehicle is 0.018 rad (1.08 deg).

3.2. Horizontation of IMU Measurements

The performance of this module is measured in terms of its capability to describe the pose (roll, pitch and yaw angles) of a vehicle. The motorcycle from the 3rd test of Section 3.1 is used because bigger roll angles can be achieved with it than with a car. The motorcycle is driven randomly on a test track, including a U-turn (772.70–780.00 s), a slalom (783.30–791.00 s), and “8” figures (791.00–843.70 s).

What makes this test special is that it serves as a practical demonstration of the mathematical methods detailed in Section 2.2. The results of the horizontation of the IMU measurements are shown on the Figure 10. As can be seen, the practical implementation of the mathematical methods is able to describe the vehicle pose.

3.3. Statistical Filtering

The results of the statistical filtering are shown on the Figure 11. The performance of this module is measured in terms of its ability to (i) fuse the vehicle path (as estimated with the motion model) with correction data, (ii) estimate the vehicle path despite SatNav shortages or with corrupted SatNav measurements, and (iii) estimate the vehicle path solely by means of the motion model (with no correction data). For this, a vehicle is driven in various types of roads for 1904.15 s. The test starts at a country road (blue), followed by an express way (magenta, thick), then in-city driving (magenta, thin) and a parking structure (green). A standstill instance of 40.00 s inside the parking structure is included as well. The average velocity of the vehicle during the test, including the 40.00 s standstill inside the parking structure, is $8.19\frac{\mathrm{m}}{\mathrm{s}}$.

What makes this test special is (i) the inclusion of various driving conditions, (ii) the inclusion of extended periods of time without or with corrupted SatNav correction data (parking structure), and (iii) the dead-reckoning navigation.

As can be seen, the statistical filtering is able to fuse the estimation of the motion model with the SatNav measurements. It can also be seen that even in total absence of SatNav correction data, the motion model is able to accurately estimate the vehicle path. The test shown in Figure 11 is a representative case of the average accuracy of the motion model because it includes various driving situations. At the end of the test, the cumulative deviation of the position estimated by the motion model from the position measured by the SatNav receiver is 130.00 m, which accounts for a deviation of 245.79 m per driven hour at an average velocity of $8.19\frac{\mathrm{m}}{\mathrm{s}}$.

3.4. Outlier Detection

The performance of this module is measured in terms of its ability to recognize corrupted correction data, even when the sensors suggest otherwise. For this, two segments of the results of Section 3.3 are highlighted.

What makes this test special are the extended periods of time without or with corrupted correction data. As can be seen, the outlier detection recognizes the faulty correction data, thus dynamically adapting the Kalman gains accordingly. This greatly improves the estimation of the vehicle state. Some relevant results of the outlier detection are shown on the Figure 12.

3.5. Drift Recognition

A representative result of the drift recognition is shown on the Figure 13. The performance of this module is measured in terms of its ability to recognize that the vehicle is drifting. For this, a 3rd generation BMW M5 is driven 51 times on a test track, alternating between tractive and non-tractive driving. The test vehicle is equipped with an INS and a Correvit S-Motion. No correction data is used in these tests to test the accuracy of the motion model under these conditions.

The relevance of this test lies in (i) the extended periods of non-tractive driving, and (ii) the exclusion of correction data. As can be seen, when the vehicle enters the first curve, the module recognizes that the vehicle is not drifting. It is only when the sideslip starts to abruptly change (22.98 s) that the module starts to detect a drift. As the vehicle remains on a steady state drift (23.54–26.05 s), the drift bit stabilizes at “drifting”. During the drift transition (26.05–26.83 s), when there are moments of tractive driving, the drift bit toggles between tractive and non-tractive driving. Once the transition is finished, and the vehicle enters again into a steady state drift, the drift bit does not toggle any more. As the vehicle ends the drift (34.31 s) the drift bit toggles for a few milliseconds before stabilizing at tractive driving.

The average deviation of the final position as estimated by the motion model when compared to the position measured by the SatNav receiver for all 51 tests is ≈24.47 s, what accounts for a deviation of 2465.30 m per driven hour at $6.35\frac{\mathrm{m}}{\mathrm{s}}$. However, it should be noted that (i) the average duration of these tests is ≈35 s, (ii) there is tractive and non-tractive driving in each test, (iii) around the first 20 s of each test are solely straight-line driving, and (iv) there is a full reset at the beginning of every test.

3.6. LiDAR-Based Positioning Method

The accuracy results of the LbPM are shown on the

Table 2. Accuracy results for the proposed LbPM. Shown are the maneuvers, maneuverer velocity, mean deviation from the reference and Std. dev. of the corresponding errors.

Manoeuvrers	Positioning Accuracy		Orientation Accuracy		Velocity Accuracy
Drive-by	Mean (m)	Std. dev. (m)	Mean (deg)	Std. dev. (deg)	Mean$\left(\frac{\mathbf{m}}{\mathbf{s}}\right)$	Std. dev.$\left(\frac{\mathbf{m}}{\mathbf{s}}\right)$
5 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.04	0.02	0.73	0.25	0.06	0.08
10 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.03	0.02	0.19	0.20	0.08	0.10
15 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.03	0.02	0.26	0.19	0.07	0.09
20 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.03	0.02	0.37	0.23	0.08	0.09
25 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.04	0.02	0.58	0.23	0.08	0.10
30 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.06	0.02	0.51	0.22	0.08	0.10
35 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.07	0.03	0.44	0.25	0.08	0.11
40 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.08	0.03	0.41	0.26	0.11	0.13
Slalom	Mean (m)	Std. dev. (m)	Mean (deg)	Std. dev. (deg)	Mean$\left(\frac{\mathbf{m}}{\mathbf{s}}\right)$	Std. dev.$\left(\frac{\mathbf{m}}{\mathbf{s}}\right)$
5 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.04	0.02	0.24	0.29	0.08	0.11
10 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.04	0.02	0.40	0.29	0.09	0.12
20 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.04	0.02	0.32	0.36	0.14	0.17
30 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.05	0.02	0.36	0.40	0.18	0.24
40 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.10	0.02	0.53	0.43	0.18	0.22

. The performance of this module is measured in terms of its accuracy to measure the vehicle state. For this, a vehicle is equipped with an INS with RTK and a LiDAR. An array of reflective markers is placed on a test track, and the marker library is generated by measuring the markers with a SatNav receiver with RTK. Two maneuvers (a slalom and a drive-by) are driven with various velocities ranging from $5\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$ and up to $40\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$. The outputs from the LbPM are then compared to those of the INS with RTK. It should be noted that the INS serves as an accurate and objective reference, but no correction data is used for the LbPM.

What makes this test special are (i) the exclusion of correction data, and (ii) the validation with both maneuvers and various velocities. As can be seen, the mean accuracy of the LbPM is almost the same as that of the INS with RTK for all the state variables that the LbPM can measure.

3.7. LbPM Adequacy for SLAM

The adequacy of the LbPM for SLAM is measured in terms of its accuracy to generate the marker library. For this, the same test setup from Section 3.6 is used. Two tests are performed. In the first test, the position, velocity and orientation of an INS with RTK is used as correction data. In the second test, only the velocity from the INS with RTK is used as correction data. In both tests, the location of the markers is computed with the estimated vehicle state and the LiDAR measurements. The observed location of the markers is then compared to their true position from the library.

The relevance of these tests lies in that (i) it allows identify some sources of error, and (ii) it allows evaluation of the identified sources of error individually. The results are presented in

Table 3. Results of the mapping accuracy. Shown are the maneuvers, driving velocity, mean deviation from the observed to the true marker location, std. dev. of the errors and the maximum deviation. The columns (a) show the results when using position, velocity and orientation correction data. The columns (b) show the results when using only velocity correction data.

Manoeuvre	Mapping Accuracy			Manoeuvre	Mapping Accuracy
Drive-by	Mean (m)	Std. dev. (m)	Max (m)	Slalom	Mean (m)	Std. dev. (m)	Max (m)
	(a)/(b)	(a)/(b)	(a)/(b)		(a)/(b)	(a)/(b)	(a)/(b)
5 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.11/0.90	0.04/0.51	0.19/1.56	5 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.33/0.93	0.24/0.51	0.81/1.62
10 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.11/0.13	0.04/0.06	0.19/0.22	10 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.08/0.27	0.03/0.11	0.14/0.42
15 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.12/0.18	0.05/0.07	0.22/0.34	15 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.08/0.53	0.03/0.25	0.16/1.04
20 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.14/0.29	0.06/0.12	0.28/0.48	20 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.08/0.78	0.03/0.56	0.14/1.70
25 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.17/0.11	0.08/0.06	0.32/0.39	25 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.10/0.30	0.05/0.14	0.23/0.64
30 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.14/0.16	0.06/0.09	0.28/0.44	30 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.08/0.67	0.04/0.36	0.21/1.53
35 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.10/0.20	0.05/0.15	0.20/0.69	35 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.11/0.96	0.06/0.52	0.25/1.96
40 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.10/0.38	0.05/0.19	0.23/0.77	40 $\frac{\mathrm{k}\mathrm{m}}{\mathrm{h}}$	0.11/0.26	0.06/0.15	0.25/0.64

. As can be seen, the marker library that is generated when the INS-RTK correction data is present, closely resembles the marker library that is generated with the RTK SatNav receiver. As expected, the marker library that is generated with only velocity correction data is less accurate. However, this inaccuracy is still smaller than that of SatNav with no correction data.

3.8. Runtime

The runtime results of the presented methodology are shown on

Table 4. Shown is the median and the std. dev. of the runtime of the modules presented above.

Module	Median (μs)	Std. dev. (μs)
Standstill classifier	123	50
Statistical filtering	403	98
LbPM-point clustering	43	20
LbPM-velocity estimation	11	14
LbPM-position estimation	42	54

. These values are obtained by executing the Matlab code of each module for 1.1+ million cycles on an Intel i7-6820HQ CPU, and by using the Matlab Profiler to measure the runtime of each module. Given that the IMU horizontation, the outlier and the drift detectors are embedded in the statistical filtering module, the runtime of these four modules is considered to be a single one.

The relevance of this test lies in that it gives a baseline to estimate the possibility of using the proposed methodology in real-time applications. As can be seen, the median runtime of all modules is always in the microsecond area. Even in a worst-case scenario (median plus 3σ, the runtime for the complete methodology is ≈1.33 ms, which remains under a typical IMU sampling time of 10 ms.

4. Discussion

As stated in Section 1, the focus of this research is to generate a reference state for ground vehicles, while reducing the dependency of the INSs on the SatNav. This allows the INSs to bridge SatNav outages for much longer periods of time, or even to function with no SatNav at all. Specifically, for the automotive industry, there are various use-cases that justify the use of SatNav-deprived INSs, such as the navigation in tunnels, underpasses or parking structures. A highly precise and prolonged navigation in places with no SatNav, such as testing halls, is also very relevant for the automotive research. As shown in Section 3, the methods developed in this research work address the aspects where the INSs profit the most.

Starting with the standstill recognition, it is shown that it is possible to classify whether a vehicle is moving or standing still by using only machine learning techniques and IMU measurements. The fact that no additional sensor is required, clearly implies important advantages, such as (i) reduced costs, (ii) less testing complexity, (iii) simplified information processing, and (iv) stand-alone functioning.

The proposed method uses the same features in the frequency domain, regardless of the vehicle. However, a first glance at the Laplace transformation of the features in the time domain, suggests that the choice of frequencies can be further refined if one considers them on a vehicle-specific basis. This could eventually lead to an improvement in the standstill recognition. Therefore, future work could include a vehicle-specific analysis of the features in the frequency domain. The use of the on-board vehicle sensors could also help refine and automate the data labeling.

As for the horizontation of INS measurements, modern INSs do use correction data (typically from the SatNav) to refine the IMU measurements. However, it is shown that the pose of the vehicle can be accurately described with the raw IMU measurements. This implies independence from the SatNav. The inclusion of the detailed mathematics to horizontate IMU measurements, implies an aid to scientists working on similar topics or trying to replicate the results presented here.

One challenging aspect of the horizontation of the IMU measurements is to detect the IMU mounting pose in the vehicle, i.e., it is possible to estimate the pose of the IMU with respect to the gravity vector and the true north of the Earth. However, it is not an easy task to relate the pitch and roll of the IMU with that of the vehicle because the IMU could be mounted in such a manner that its $x\times y$-plane is not parallel to the $x\times y$-plane of the vehicle. Knowing the offsets between both planes could be highly valuable to recognize certain driving situations, such as driving inside parking structures, wheelies or stoppies. Since the acquisition of these offsets on a vehicle-to-vehicle basis is a complicated and time-consuming task, future work could include the investigation of automated methods that allow estimation of these offsets.

The KF is a computationally efficient algorithm for fusing data. However, the accuracy of its output is limited by the accuracy of the information to fuse. Therefore, it is important to refine the sources of information as much as possible before using them as inputs for the KF. In this work, the EKF fuses the state vector estimated by the motion model with the measurements of the vehicle state made with external sensors. Therefore, a lot of effort is put on refining the motion model for tractive-driving applications. As shown above, the proposed motion model can accurately estimate the vehicle state for extended periods of time with no correction data, which implies a high confidence on the vehicle state, even when navigating by dead reckoning.

Given that the motion model is designed to perform best for tractive driving, its performance is reduced during non-tractive driving. Also, given that the drifting tests are designed to test the drift recognition, they are not long enough to objectively determine the accuracy of the motion model during a drift. Therefore, future work could include a deeper investigation of the accuracy of the motion model during non-tractive driving. This could help to estimate adequate values for the system noise, and so improve the performance of the KF during drifting.

As for the outlier detector, it further helps in refining the sources of information before they are fed to the KF. Its effect is especially noticeable when the sensors deliver quality metrics for their own measurements, as is the case of the standard deviation for the SatNav. As shown above, to include a smart outlier detector implies a very useful consistency check of the sensor measurements, thus avoiding fusing information that would negatively affect the estimation of the vehicle state.

This detector uses the latest state vector because it is the last known vehicle state. However, as time passes, the predicted state without correction data might differ so much from the true state that the constraints shown in Section 2.5 filter out meaningful sensor measurements. Future work could include the combination of the system noise with the state vector to better adapt the outlier detector.

Yet another step that is taken to improve the KF is the drift detector. Given that the motion model is designed to function best in tractive driving, it is very useful to differentiate whether the vehicle is drifting or not. As shown above, the drift recognition can make this distinction. This implies that the time instance to make changes in the KF is known. This can be, for example, to adjust the system noise accordingly or to use a more adequate motion model.

The drift recognition here presented is based on the thresholds of two features: the magnitude of the sideslip and the difference between the estimated and expected lateral acceleration. Future work on this method could include the analysis of a bigger dataset that includes vehicles with different drivetrains (front, rear and all-wheel drive). This could help to gain a better understanding on the ideal thresholds for vehicles according to their drivetrain.

One crucial module to reduce the dependency of INSs on the SatNav is the LbPM. The results clearly show that the accuracy of the vehicle state as measured by this method closely resembles that of an INS with RTK correction data. Considering the refresh rate of the LiDAR and the state variables that can be measured by means of the LbPM, this could already be enough for an accurate vehicle indoor navigation. Future work on this module could include the implementation of the method on dedicated hardware to analyze the computing requirements for a real-time implementation.

The results on Table 3, suggest that the LbPM is adequate for SLAM. Given that the LbPM is capable of delivering RTK-like correction data, it could enable the generation of an accurate marker library in unknown environments. It is not clear whether or not a lane-accurate indoor navigation could be possible in places with a reduced marker density. Future work on this module could include adapting the LbPM algorithm for SLAM, and implementing it on dedicated hardware for real-world testing. For this purpose, tailored-made hardware solutions, such as [63,64], could aid in the navigation of complex environments, as can be parking structures. The use of lightweight artificial intelligence techniques, as shown in [65,66] could aid in the implementation of the standstill recognition in hardware with limited computational resources, as are embedded systems. Finally, the sensor fusion with on-board sensors in the vehicles could improve the dead-reckoning navigation in indoor environments.

Regarding the methodology runtime, it should be noted that even with high-level programming code, such as a Matlab, its runtime never exceeded 2 ms. This implies that it is possible to implement the methodology in real-time applications. Future work could include the implementation using a more efficient programming language, such as C++, and the use of dedicated hardware.

5. Conclusions

The objective of generating a reference vehicle state for proper testing and validation of automated driving functions is achieved in a prototypical manner by developing novel methods and by adapting existing ones to the problems at hand. This while reducing the dependency of INSs on external sensors, such as the SatNav. The testing results demonstrate that the proposed methods greatly improve the accuracy of the estimated vehicle state. The measured runtime suggests the possibility of real-time implementation of the proposed methodology.