Localization Error Modeling for Autonomous Driving in GPS Denied Environment

Abstract

Precise localization plays a crucial role in autonomous driving applications. As Global Position System (GPS) signals are often susceptible to interference or even not fully available, odometry sensors can precisely calculate positions in urban environments. However, the cumulative error is thus originated with time increasing. This paper proposes an effective empirical formula to model such unbounded cumulative errors from noisy relative measurements. Furthermore, a recursive cumulative error expression has been established by calculating the first and second moments of the Ackermann model. Finally, based on the developed formula, numerical experiments have also been conducted to verify the validity of the proposed model.

1. Introduction

Interest in autonomous driving has grown exponentially in past decades [1,2]. As the Global Position System (GPS) signal is susceptible to interference or even not fully available in typical urban environments, precise localization is considered as a supplement solution to localize vehicles with multi-source heterogeneous sensors e.g., laser sensors [3], Wireless Local Area Network (WLAN) sensors [4] and inertial sensors [5].

The current localization framework could be mainly divided into SLAM and dead-reckoning-based technologies. The former has high accuracy but requires loop-closure (the vehicle must visit the same places) during the navigation procedure [6,7,8,9,10,11]. In contrast, the latter has faster performance but is limited to unbounded drift (also called cumulative error) originating from noisy odometry sensors [12,13]. As the vehicle often goes to unknown areas, it is essential to eliminate cumulative errors during dead-reckoning procedures.

Although varied techniques have been developed to decline cumulative error, a rigorous analysis of the growth rate concerning ego-motion is still missing. Typical contributions are summarized as follows: Wan et al. presented a multi-sensor fusion method to eliminate the cumulative error in urban environments [14]. Song et al. also proposed a fusion-based approach to enhance localization accuracy with the camera and laser [15]. However, the cumulative error has already proved to be overgrowing, and the statistical characteristics are rarely considered [16,17]. Moreover, to the best of the author’s knowledge, a precise formula to calculate cumulative error from anonymous trajectories is quite challenging. In contrast to the straightforward analytic solutions, calibration-based methods have thus been investigated. Liu et al. proposed a framework for error calibration based on the RFID techniques [18], whereas Alwin et al. analyzed the error propagations by utilizing ultra-wideband measurement [19]. Furthermore, with the development of artificial intelligence (AI) techniques, deep learning-based approaches have also been investigated [20]. For example, Brossard et al. proposed an AI-based dead-reckoning model to improve the localization performance [21], where the neural network is utilized to learn the cumulative errors. Meanwhile, Shit et al. designed an AI crowdsource-based localization approach for intelligent transportation systems [22]. Since all learning-based methods inevitably rely on deep neural networks, the poor interpretability seriously limits its potential in autonomous driving applications [23]. In addition, as the learning-based strategy is not comprehensive enough and contains huge hyper-parameters, different scenarios may significantly reduce localization performance [24].

It is concluded that no methodology is perfect for autonomous driving; especially in large-scale urban environments, the cumulative error grows exponentially. Our previous work introduced an analytical method to analyze the cumulate error; however, only a simple dead-reckoning model was used, and it hardly contributed to actual driving scenarios [25].

This paper proposes an effective empirical formula to estimate moments of the cumulative error regarding autonomous driving. Independent scenarios could quickly implement the proposed methodology to regular and irregular trajectories and be verified numerically through Monte Carlo simulations. Furthermore, in contrast to the state-of-the-art, the proposed strategy could recursively estimate the cumulative error without prior information, which has fantastic potential in autonomous driving.

The contents of this paper are organized as follows: Section 1 briefly introduces the background of the problem. Section 2 investigates the statistical properties of the localization errors. Section 3 presents experimental results with Monte-Carlo simulations. Finally, this paper is concluded in Section 4.

2. Mathematical Model and Estimation

An autonomous vehicle localizes itself by receiving GPS signals during driving scenarios. However, in GPS denied scenarios, dead-reckoning models are often applied to calculate the position using odometry measurements. Meanwhile, as the relative noises are distributed with Independent and Identical Distribution (IID) in consecutive frames, the vehicle must calibrate cumulative errors frequently. Hence, the main challenge for autonomous driving is how to model the nonlinear and unbounded drift in urban scenarios precisely.

2.1. Problem Statement

To precisely control and localize the movement of an autonomous vehicle, a digital model of the vehicle ego-motion should be established. This paper utilizes a typical kinematic model to reflect the vehicle characteristics accurately, also called the Ackermann steering geometry. Figure 1a is a schematic diagram of the Ackermann model. Here, O is the rotation center of the vehicle, $\beta$ is the slip angle, $\theta$ is the heading angle, and v is the velocity.

Figure 1b illustrates both the Ackermann model and the position $(x_n^m,y_n^m)$ calculated by relative noisy measurements $({\theta }_n^m,{\beta }_n^m,v_n^m)$, where $\mathit{n}$ and $\mathit{m}$ express both the step and measurement, respectively. v, $\theta$ and $\beta$ represent the corresponding velocity, heading angle, and slip angle in consecutive frames. Definitions of notations are exhibited in

Table 1. Notations definition.

Notation	Definition	Notation	Definition
v	velocity	${\overline{\theta }}_n$	true value
$\theta$	heading angle	${\tilde{\theta }}_n$	error
$\beta$	slip angle	${\theta }_n^m$	noisy measurements
$E(x_n^m)$	expectation	$var(x_n^m)$	variance

. The pose measurements ${\overline{\theta }}_n$, ${\overline{\beta }}_n$ and ${\overline{v}}_n$ are defined as the ground truths. ${\tilde{\theta }}_n$, ${\tilde{\beta }}_n$ and ${\tilde{v}}_n$ are noisy errors assumed to be independent with zero mean and standard deviation ${\delta }_{\theta }$, ${\delta }_{\beta }$ and ${\delta }_v$. Hence, measurements on each step are acquired as follows

$${\theta }_n^m={\overline{\theta }}_n+{\tilde{\theta }}_n;{\beta }_n^m={\overline{\beta }}_n+{\tilde{\beta }}_n;v_n^m={\overline{v}}_n+{\tilde{v}}_n.$$

(1)

where corresponding errors are IID. Hence, the vehicle’s localization is calculated by dead-reckoning with:

$$x_n^m=\sum _{i=1}^n(v_i^{m}cos(\sum _{j=1}^i{\theta }_j^m+{\beta }_i^m));y_n^m=\sum _{i=1}^n(v_i^{m}sin(\sum _{j=1}^i{\theta }_j^m+{\beta }_i^m)).$$

(2)

2.2. True Error Statistics

The localization error could also be represented by expanding for autonomous driving scenarios.

$$\begin{array}{cc}\hfill x_n^m={\overline{x}}_n+{\tilde{x}}_n& =\sum _{i=1}^n[({\overline{v}}_i+{\tilde{v}}_i)cos(\sum _{j=1}^i({\overline{\theta }}_j+{\tilde{\theta }}_j)+({\overline{\beta }}_i+{\tilde{\beta }}_i))]\hfill \\ \hfill & =[\sum _{i=1}^n{\overline{v}}_i+\sum _{i=1}^n{\tilde{v}}_i]·[cos(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)cos(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)\hfill \\ \hfill & -sin(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)sin(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)],\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill y_n^m={\overline{y}}_n+{\tilde{y}}_n& =\sum _{i=1}^n[({\overline{v}}_i+{\tilde{v}}_i)sin(\sum _{j=1}^i({\overline{\theta }}_j+{\tilde{\theta }}_j)+({\overline{\beta }}_i+{\tilde{\beta }}_i))]\hfill \\ \hfill & =[\sum _{i=1}^n{\overline{v}}_i+\sum _{i=1}^n{\tilde{v}}_i]·[sin(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)cos(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)\hfill \\ \hfill & +cos(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)sin(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)].\hfill \end{array}$$

(4)

where ${\overline{x}}_n={\sum }_{i=1}^n[{\overline{v}}_{i}cos(({\sum }_{j=1}^i{\overline{\theta }}_j)+{\overline{\beta }}_i)]$ and ${\overline{y}}_n={\sum }_{i=1}^n[{\overline{v}}_{i}sin(({\sum }_{j=1}^i{\overline{\theta }}_j)+{\overline{\beta }}_i)]$ are the ground-truth positions. Furthermore, by rearranging the above equations, we have:

$$\begin{array}{cc}\hfill {\tilde{x}}_n=& \sum _{i=1}^n{\overline{v}}_i[cos(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)(cos(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)-1)-sin(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)sin(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)]\hfill \\ \hfill & +\sum _{i=1}^n{\tilde{v}}_i[cos(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)cos(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)-sin(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)sin(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)],\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill {\tilde{y}}_n=& \sum _{i=1}^n{\overline{v}}_i[sin(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)(cos(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)-1)+cos(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)sin(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)]\hfill \\ \hfill & +\sum _{i=1}^n{\tilde{v}}_i[sin(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)cos(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)+cos(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)sin(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)].\hfill \end{array}$$

(6)

Regarding the formulas mentioned above, the expectation and variance of the accumulated error are then calculated. Notice that each cumulative error depends on the ground truth, where the statistical properties of corresponding errors are explicitly represented as follows:

Assuming $\tilde{\theta }\backsim N(0,{\delta }_{\theta }^2)$, $\tilde{\beta }\backsim N(0,{\delta }_{\beta }^2)$ and $\tilde{v}\backsim N(0,{\delta }_v^2)$, we can obtain:

$$\sum _{i=1}^n\tilde{\theta }\backsim N(0,i{\delta }_{\theta }^2);\sum _{i=1}^n\tilde{\beta }\backsim N(0,i{\delta }_{\beta }^2);\sum _{i=1}^n\tilde{v}\backsim N(0,i{\delta }_v^2),$$

(7)

Concerning the Ackermann model, since both the heading angle and slip angle are independently distributed, equations are also calculated as follows:

$$((\sum _{i=1}^n\tilde{\theta })+\tilde{\beta })\backsim N(0,i{\delta }_{\theta }^2+{\delta }_{\beta }^2).$$

(8)

In this case, the following equations are acquired

$$\begin{array}{cc}\hfill & E(cos((\sum _{j=1}^i{\tilde{\theta }}_j)+{\tilde{\beta }}_i))=e^{(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)/2},\hfill \\ \hfill & E(sin((\sum _{j=1}^i{\tilde{\theta }}_j)+{\tilde{\beta }}_i))=0,\hfill \\ \hfill & E({cos}^2((\sum _{j=1}^i{\tilde{\theta }}_j)+{\tilde{\beta }}_i))=\frac{1}{2}(e^{2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}+1),\hfill \\ \hfill & E({sin}^2((\sum _{j=1}^i{\tilde{\theta }}_j)+{\tilde{\beta }}_i))=\frac{1}{2}(-e^{2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}+1),\hfill \\ \hfill & E(cos((\sum _{j=1}^i{\tilde{\theta }}_j)+{\tilde{\beta }}_i)sin((\sum _{j=1}^i{\tilde{\theta }}_j)+{\tilde{\beta }}_i))=0.\hfill \end{array}$$

(9)

where Equation (9) exhibits the expected value of parameters $\theta$ and $\beta$. Calculating the expectation of ${\tilde{x}}_n$ and ${\tilde{y}}_n$ respectively, and placing formula (9) into the corresponding equation for simplification, the expectations are thus acquired as follows:

$${\mu }_t(\overline{\theta },\overline{\beta },\overline{v})=\left[\begin{array}{c}E[{\tilde{x}}_n]\\ E[{\tilde{y}}_n]\end{array}\right]=\left[\begin{array}{c}\sum _{i=1}^n\overline{v}[cos((\sum _{j=1}^i{\tilde{\theta }}_j)+{\tilde{\beta }}_i)(e^{-\frac{i{\delta }_{\theta }^2+{\delta }_{\beta }^2}{2}-1})]\\ \sum _{i=1}^n\overline{v}[sin((\sum _{j=1}^i{\tilde{\theta }}_j)+{\tilde{\beta }}_i)(e^{-\frac{i{\delta }_{\theta }^2+{\delta }_{\beta }^2}{2}-1})]\end{array}\right].$$

(10)

The Cauchy–Schwarz inequality has to be utilized afterward to calculate the second-order moment of the cumulative error.

$${(\sum _{i=1}^n{x}_i{y}_i)}^2⩽(\sum _{i=1}^n{x}_i^2)(\sum _{i=1}^n{y}_i^2).$$

(11)

Based on the above equation, we have

$$\begin{array}{cc}\hfill E{({\tilde{x}}_n-E({\tilde{x}}_n))}^2& =E[\sum _{i=1}^n{\overline{v}}_i[cos(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)(cos(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)-e^{-\frac{i{\delta }_{\theta }^2+{\delta }_{\beta }^2}{2}})\hfill \\ & -sin(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)sin(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)]+\sum _{i=1}^n{\tilde{v}}_i[cos(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)cos(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)\hfill \\ & -sin(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)sin(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i){]]}^2\hfill \\ & ⩽(1^2+1^2)·E[\sum _{i=1}^n{\overline{v}}_i[cos(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)(cos(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)-e^{-\frac{i{\delta }_{\theta }^2+{\delta }_{\beta }^2}{2}-1})\hfill \\ & -sin(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)sin(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i){]}^2+[\sum _{i=1}^n{\tilde{v}}_i[cos(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)cos(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)\hfill \\ & -sin(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)sin(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i){]]}^2]\hfill \\ & =2·E(A)+2·E(B).\hfill \end{array}$$

(12)

The above formula is the second-order moment of the cumulative error in the X direction regarding the variance. However, it is still not entirely simplified. By expressing it with $A_x$ and $B_x$, the Cauchy-Schwarz inequality is then utilized again.

$A_x={[{\sum }_{i=1}^n{\overline{v}}_i[cos({\sum }_{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)(cos({\sum }_{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)-e^{-\frac{i{\delta }_{\theta }^2+{\delta }_{\beta }^2}{2}})-sin({\sum }_{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)sin({\sum }_{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)]]}^2$ and $B_x={[{\sum }_{i=1}^n{\tilde{v}}_i[cos({\sum }_{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)(cos{\sum }_{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)-sin({\sum }_{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)sin({\sum }_{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)]]}^2$

Similar, we also have

$$\begin{array}{c}\hfill E(A_x)=n·\sum _{i=1}^n{\overline{v}}_i^2[{cos}^2(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-{cos}^2(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)e^{-(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-\frac{e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2}],\end{array}$$

(13)

$$\begin{array}{cc}\hfill E(B_x)& =n·E(\sum _{i=1}^n{\tilde{v}}_i^2)[{cos}^2(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-\frac{e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2}]\hfill \\ \hfill & =n·{\delta }_v^2\sum _{i=1}^n({cos}^2(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-\frac{e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2}).\hfill \end{array}$$

(14)

Finally, $var({\tilde{x}}_n)$ becomes

$$\begin{array}{cc}\hfill var({\tilde{x}}_n)& ⩽2n·\sum _{i=1}^n{\overline{v}}_i^2[{cos}^2(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-{cos}^2(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)e^{-(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}\hfill \\ & -\frac{e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2}]+2n·{\delta }_v^2\sum _{i=1}^n({cos}^2(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-\frac{e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2}),\hfill \end{array}$$

(15)

The $var({\tilde{x}}_n)$ and the $var({\tilde{y}}_n)$ are calculated in the same way. The variance of ${\tilde{y}}_n$ can also be written as:

$$\begin{array}{cc}\hfill var({\tilde{y}}_n)& ⩽2n·\sum _{i=1}^n{\overline{v}}_i^2[{sin}^2(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-{sin}^2(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)e^{-(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}\hfill \\ & -\frac{e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2}]+2n·{\delta }_v^2\sum _{i=1}^n({sin}^2(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-\frac{e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2}).\hfill \end{array}$$

(16)

2.3. Error Statistics in Practice

Equations (10), (15) and (16) are theoretically proofed representations for error statistics. However, they are conditional on the ground truth. Hence, the expected values must be conditioned on noisy measurements to extend the potential usages in practice.

$$E[{\mu }_t|{\theta }^m,{\beta }^m,v^m]={\mu }_m,$$

(17)

$$E[var({\tilde{x}}_n)|{\theta }^m,{\beta }^m,v^m]=var({\tilde{x}}_n^m).$$

(18)

$$E[var({\tilde{y}}_n)|{\theta }^m,{\beta }^m,v^m]=var({\tilde{y}}_n^m).$$

(19)

By expanding the above equations with Equation (1), we have

$$\begin{array}{cc}\hfill E[{\tilde{x}}_n^m]& =E[E[{\tilde{x}}_n]|{\theta }^m,{\beta }^m,v^m]\hfill \\ \hfill & =E[\sum _{i=1}^n{\overline{v}}_i[cos(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)(e^{-\frac{(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}{2}}-1)]]\hfill \\ \hfill & =E[\sum _{i=1}^n(v_i^m-{\tilde{v}}_i)[cos(\sum _{j=1}^i({\theta }_j^m-{\tilde{\theta }}_j)+({\beta }_i^m-{\tilde{\beta }}_i))(e^{-(i\frac{{\delta }_{\theta }^2+{\delta }_{\beta }^2}{2})}-1)]]\hfill \\ \hfill & =\sum _{i=1}^n{v}_i^{m}E[(cos((\sum _{j=1}^i{\theta }_j^m+{\beta }_i^m)-(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i)))·(e^{-\frac{(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}{2}}-1)]\hfill \\ \hfill & =\sum _{i=1}^n{v}_i^m(e^{-(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-e^{\frac{-(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}{2}})cos(\sum _{j=1}^i{\theta }_j^m+{\beta }_i^m).\hfill \end{array}$$

(20)

Meanwhile, the second-order moment of the cumulative error has also been calculated with the Cauchy–Schwarz inequality as follows:

$$\begin{array}{cc}\hfill var({\tilde{x}}_n^m)& ⩽E[2n·\sum _{i=1}^n{\overline{v}}_i^2[{cos}^2(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-{cos}^2(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)e^{-(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}\hfill \\ & -\frac{e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2}]]+E[2n·{\delta }_v^2\sum _{i=1}^n({cos}^2(\sum _{j=1}^i{\overline{\theta }}_j+{\overline{\beta }}_i)e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-\frac{e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2})]\hfill \\ & ⩽2n(1^2+{(-1)}^2)E[\sum _{i=1}^n({(v_m^i)}^2-{({\overline{v}}_i)}^2)[{cos}^2((\sum _{j=1}^i{\theta }_j^m+{\beta }_i^m)-(\sum _{j=1}^i{\tilde{\theta }}_j+{\tilde{\beta }}_i))]·e^{-(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}\hfill \\ & ·(e^{-2(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-1)]+2n·{\delta }_v^2\sum _{i=1}^n[{cos}^2((\sum _{j=1}^i{\theta }_j^m+{\beta }_i^m))e^{-4(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-\frac{e^{-4(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2}].\hfill \end{array}$$

(21)

Then, continuing to simplify $var({\tilde{x}}_n^m)$, we can have:

$$\begin{array}{cc}\hfill var({\tilde{x}}_n^m)& ⩽4n\sum _{i=1}^n[{(v_m^i)}^2+({\delta }_v^2)][{cos}^2(\sum _{j=1}^i{\theta }_j^m+{\beta }_i^m)e^{-4(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-{cos}^2(\sum _{j=1}^i{\theta }_j^m+{\beta }_i^m)e^{-3(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}\hfill \\ & -\frac{e^{-4(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{e^{-3(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}-\frac{e^{-(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2}]\hfill \\ & +2n{\delta }_v^2\sum _{i=1}^n({cos}^2(\sum _{j=1}^i{\theta }_j^m+{\beta }_i^m)e^{-4(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-\frac{e^{-4(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2}).\hfill \end{array}$$

(22)

Equations (20) and (22) are the expectation and variance of the cumulative error in the X direction, respectively. With the same manner, corresponding values of ${\tilde{y}}_n$ can also be calculated:

$$\begin{array}{c}\hfill E[{\tilde{y}}_n^m]=\sum _{i=1}^n{v}_i^m(e^{-(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-e^{\frac{-(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}{2}})sin(\sum _{j=1}^i{\theta }_j^m+{\beta }_i^m).\end{array}$$

(23)

$$\begin{array}{cc}\hfill var({\tilde{y}}_n^m)& ⩽4n\sum _{i=1}^n[{(v_m^i)}^2+({\delta }_v^2)][{sin}^2(\sum _{j=1}^i{\theta }_j^m+{\beta }_i^m)e^{-4(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-{sin}^2(\sum _{j=1}^i{\theta }_j^m+{\beta }_i^m)e^{-3(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}\hfill \\ & -\frac{e^{-4(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{e^{-3(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}-\frac{e^{-(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2}]\hfill \\ & +2n{\delta }_v^2\sum _{i=1}^n({sin}^2(\sum _{j=1}^i{\theta }_j^m+{\beta }_i^m)e^{-4(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-\frac{e^{-4(i{\delta }_{\theta }^2+{\delta }_{\beta }^2)}}{2}+\frac{1}{2}).\hfill \end{array}$$

(24)

Notice that the corresponding values are still complicated; nevertheless, these equations are practically helpful.

3. Experimental Results

In this section, the error expectation and variance have been evaluated by using the Monte-Carlo simulation. Especially for autonomous driving scenarios, the well-known Kitti dataset [26] has also been utilized, which offers the pose measurements in urban applications. In the Kitti dataset, 30 different GPS/IMU values are stored in text files, including altitude, global positioning, speed, acceleration, angular rate, accuracy, and satellite information. The Monte-Carlo simulation is conducted by selecting the parameters regarding odometry sensors, such as forwarding speed, heading angle, and the angular speed in the dataset. Meanwhile, Gaussian white noise is manually added to obtain the relative noisy measurements obtained in the actual scene. As shown in

Table 2. Parameters for simulation.

Parameters	Value
${\delta }_v^2$	0.01 m/s
${\delta }_{\theta }^2$	0.0005 rad
${\delta }_{\beta }^2$	0.005 rad

, the deviation concerning velocity noise, the slip angle, and the heading angle are considered as 0.01 m, 0.005 rad, and 0.0005 rad, respectively. Figure 2 exhibits four original trajectories measured by the GPS/IMU system.

During the experiment, the statistical properties of the cumulative regarding different trajectories are calculated with the following steps (here, only concerning the first moment in X-direction).

$$\begin{array}{cc}\hfill & E({\tilde{x}}_1^m)=v_1^m(e^{-({\delta }_{\theta }^2+{\delta }_{\beta }^2)}-e^{\frac{-({\delta }_{\theta }^2+{\delta }_{\beta }^2)}{2}})cos(\sum _{j=1}^1{\theta }_j^m+{\beta }_1^m),\hfill \\ \hfill & E({\tilde{x}}_2^m)=E({\tilde{x}}_1^m)+v_2^m(e^{-(2{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-e^{\frac{-(2{\delta }_{\theta }^2+{\delta }_{\beta }^2)}{2}})cos(\sum _{j=1}^2{\theta }_j^m+{\beta }_2^m),\hfill \\ \hfill & E({\tilde{x}}_3^m)=E({\tilde{x}}_2^m)+v_3^m(e^{-(3{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-e^{\frac{-(3{\delta }_{\theta }^2+{\delta }_{\beta }^2)}{2}})cos(\sum _{j=1}^3{\theta }_j^m+{\beta }_3^m),\hfill \\ \hfill & ⋮\hfill \\ \hfill & E({\tilde{x}}_n^m)=E({\tilde{x}}_{n-1}^m)+v_n^m(e^{-(n{\delta }_{\theta }^2+{\delta }_{\beta }^2)}-e^{\frac{-(n{\delta }_{\theta }^2+{\delta }_{\beta }^2)}{2}})cos(\sum _{j=1}^n{\theta }_j^m+{\beta }_n^m).\hfill \end{array}$$

(25)

Figure 3, Figure 4, Figure 5, Figure 6, Figure 7, Figure 8, Figure 9 and Figure 10 illustrate the results by using Monte-Carlo simulation. Figure 3, Figure 4, Figure 5 and Figure 6 are the expectation of cumulative error in the X direction and Y direction. Figure 7, Figure 8, Figure 9 and Figure 10 show the cumulative error variances in X and Y directions. From Figure 3, Figure 4, Figure 5 and Figure 6, the blue line and red line are represented as the proposed model and the ground truth, which illustrates that most estimated results are approximately equal to the true cumulative error by Monte-Carlo simulations. However, the estimated variance has deviated in contrast to the ground truth, which is caused by high-order nonlinear transformation issues.

Regarding trajectory 1 (Figure 2a), Figure 3 and Figure 7 are the expectation and variance of its cumulative error, respectively. From Figure 2a, we can see that the vehicle moves with a round-trip trajectory in the X-direction and multiple circles in the Y-direction. Hence, the expectation of its cumulative error concerning the X-direction first decreases and then increases, whereas the value in the Y-direction increases and decreases frequently. The numerical change in the corresponding direction is consistent with the movement. However, nonlinear changes lead to inaccurate estimation results in the variance estimation process. Figure 4 and Figure 8 are the expectation and variance of the cumulative error with trajectory 2 (Figure 2b). It is observed that the vehicle has made multiple round-trip movements in both directions, respectively. Noticed that the number of numerical changes is consistent with the number of round-trip movements. Similarly, trajectory 3 and 4 (Figure 2c,d) have also been evaluated in the experiment. However, variance estimation always has inaccurate results caused by nonlinear changes.

In summary, experimental results have demonstrated that the proposed model fits the statistical characteristics well regarding the first-order moment. Hence, it could provide a mathematical solution for autonomous driving in GPS denied environments.

4. Conclusions

Localization uncertainty estimation from odometry measurements in the urban environment has great potential for autonomous driving, especially in GPS denied environments. In this paper, the localization error is modeled to approximate its first and second-order moments based on characteristics of odometry sensors. The proposed approach recursively estimates both the bias and uncertainty without ground truths compared to the related work. Numerical results demonstrate the validity of the proposed formula by using Monte-Carlo simulations. Future work focuses on applying the proposed approach by considering 6-DOF information from the odometry sensor.