# Stop Line Aided Cooperative Positioning
of Connected Vehicles

Xingqi Wang, Chaoyang Jiang, Shuxuan Sheng, Yanjie Xu, and Yifei Jia This work
was supported by the National Natural Science Foundation of China(No.52002026,
No.U1764257, and No.U20A20333)(Corresponding author: Chaoyang Jiang). The
authors are with the School of Mechanical Engineering, Beijing Institute of
Technology, Beijing, China, 100081<EMAIL_ADDRESS>cjiang@bit.edu.cn;)

###### Abstract

This paper develops a stop line aided cooperative positioning framework for
connected vehicles, which creatively utilizes the location of the stop-line to
achieve the positioning enhancement for a vehicular ad-hoc network (VANET) in
intersection scenarios via Vehicle-to-Vehicle (V2V) communication. Firstly, a
self-positioning correction scheme for the first stopped vehicle is presented,
which applied the stop line information as benchmarks to correct the GNSS/INS
positioning results. Then, the local observations of each vehicle are fused
with the position estimates of other vehicles and the inter-vehicle distance
measurements by using an extended Kalman filter (EKF). In this way, the
benefits of the first stopped vehicle are extended to the whole VANET. Such a
cooperative inertial navigation (CIN) framework can greatly improve the
positioning performance of the VANET. Finally, experiments in Beijing show the
effectiveness of the proposed stop line aided cooperative positioning
framework.

###### Index Terms:

Cooperative positioning, stop line, V2V communication, cooperative inertial
navigation, extended Kalman filter.

##  I Introduction

Reliable and accurate positioning is essential for path planning, trajectory
tracking, and the safe operation of autonomous vehicles in all traffic
conditions [1]. Intersection scenarios are of great significance for
autonomous driving. The maturing vehicle-to-everything (V2X) communication has
the potential to upgrade separate autonomous vehicles to connected vehicles,
which constitute vehicular ad hoc networks and can greatly improve their
positioning performance, road safety and passenger convenience, especially for
intersection scenarios [2]. Hence, positioning for connected vehicles in
intersection scenarios has increasingly attracted recent attention.

Global navigation satellite system (GNSS) can provide the global position for
a vehicle with a GNSS receiving terminal, which is broadest-used for vehicle
positioning. But its performance in highly urbanized areas is severely
degraded due to multi-path effects [3, 4, 5], and the GNSS signal can even be
denied in some special areas. With the help of the real-time kinematic (RTK)
technique, GNSS can provide more accurate results. Integrated with an inertial
navigation system (INS), both accuracy and availability of the positioning
information can be enhanced [6, 7]. However, the current GNSS/INS integrated
navigation systems with RTK still cannot meet the safety requirements of
autonomous vehicles in many urbanized scenarios [1, 8, 9, 10]. Fortunately,
rich environmental information is available in urbanized areas such as
intersection areas, and 5G communication infrastructure is being rapidly built
in many countries like China. Both the environmental correction and the
cooperative positioning based on the V2X communication have the potential to
improve the accuracy and reliability of vehicular positioning, especially in
GNSS-challenged areas [11, 12, 13].

Environmental information such as lane lines, traffic signs, and pedestrians
can be exploited to enhance vehicle position estimation [11, 14, 15]. Suhr et
al. [11] introduced the observations of symbolic road markings into a GNSS/INS
integrated navigation system via a particle filter with which the localization
error was decreased. Lanes were formulated to constraints and introduced to a
constrained Kalman filter in [14, 15], which significantly reduced the lateral
errors. Wang et al. [16] fused the visually recognized traffic lights with the
INS information and enhanced the vehicle localization in an intersection
scenario. Welzel et al. [17] utilized the location of traffic signs to correct
the GNSS measurements in a Bayesian filtering framework. Similarly, Qu et al.
[18] applied geo-referenced traffic signs and developed a localization
enhancement method that reduced the accumulated drifts of visual sensors.
Considering the environmental information, all the works mentioned above can
provide remarkable improvement in terms of positioning accuracy. However, they
all focused on self-positioning and has no help for the vehicles that cannot
observe the particular environmental markings.

With the V2X communication, vehicles can interact with other vehicles and
receive information from roadside units (RSU), which allows cooperative
positioning and improves the overall positioning performance of the VANET [19,
20]. Song et al. [21] applied the traffic signs as benchmarks to correct the
GNSS observations with the aid of laser radar, and then broadcast the
estimated GNSS error to all vehicles in the VANET. Alam et al. [22] and Li et
al. [23] developed an RSU-assisted GNSS positioning method to achieve lane-
level positioning. They identified the driving lane by utilizing the carrier
frequency offset (CFO) and received signal strength (RSS) broadcast from
roadside infrastructure beacons, respectively. Chen et al. [24] utilized the
observations of roadside cameras via vehicle-to-infrastructure (V2I)
communication to correct the GNSS positioning, and transmitted the GNSS
correction information to the connected vehicles via V2V communications.
However, these studies do not consider any inter-vehicle measurements that are
significant for the error reduction of cooperative positioning. In [25], the
azimuths of partial vehicles with a single active RSU were observed. Both V2V
and V2I communications were applied in a novel EKF-based cooperative
positioning algorithm and then the positioning performance of all vehicles in
the VANET is improved.

However,the RSUs introduce additional costs, which limits their density in
urban areas and the practical application shortly. Hence, it is interesting to
consider the cooperative positioning for connected vehicles in the RSUs-absent
environment. Hoang et al. [26] built a V2V communication network via ultra-
wideband (UWB) devices and integrated the local GNSS observations with the
vehicle-to-vehicle distance measurements, which provided a remarkable
positioning enhancement. Meyer et al. [27] and Soatti et al. [28] presented an
implicit cooperative positioning algorithm and fused the local features such
as pedestrians, traffic lights, and inactive vehicles by a consensus
methodology to refine vehicular positioning of a VANET. Liu et al. [29]
integrated the dedicated short-range communication (DSRC) and GNSS
observations and applied a modified robust cubature Kalman filter to improve
the cooperative positioning robustness and adaptive performance. Wang et al.
[30] and Yang et al. [31] presented a two-layer structure for a multi-sensor
multi-vehicle localization scenario, in which the local filter integrated
local onboard sensor measurements and the global filter fused the observations
from other vehicles. Simulation results demonstrated that the localization
accuracy could be greatly improved by cooperation among multiple sensors and
vehicles.

In this work, we apply the location of known stop-lines to correct the
position estimation of the first stopped vehicle and extend the enhancement to
all vehicles in the VANET via a well-designed cooperative inertial navigation
(CIN) framework. The onboard observations of each vehicle, the environmental
information (i.e., the location of stop-lines), and the inter-vehicle distance
observations are fused for positioning enhancement of the whole VANET. The
main contributions of this paper are as follows:

(1)The stop lines are utilized to assist the GNSS/INS integrated positioning
system of the first stopped vehicle, which not only corrects the estimation of
the self-position but also provides a better initialization for the
positioning filter after the traffic light turns green. To the best of our
knowledge, it is the first work for vehicle positioning enhancement with the
help of stop lines.

(2)We apply the V2V communications and the inter-vehicle distance measurements
to extend the improvement of the first stopped vehicle to the whole VANET. The
proposed stop line aided cooperative positioning framework can improve the
vehicle positioning performance for intersection scenarios.

(3)UWB sensors are used to build the V2V communication networks and to measure
the inter-vehicle distances. Experimental results show that the proposed
framework remarkably improves the positioning performance of the vehicles in
an intersection scenario, especially for the vehicles directly communicating
with the first stopped vehicle.

The rest of this paper is organized as follows. The problem formulation is
given in Section II. Section III presents the stop line aided self-positioning
correction scheme in detail. In Section IV, we propose the cooperative
inertial navigation framework. The experimental results and analysis of the
proposal are discussed in section V. Finally, Section VI concludes this paper.

##  II Problem Statement

In high-traffic intersection scenarios, vehicles can be classified into moving
vehicles and stopped vehicles. According to the traffic rules, the first
stopped vehicle is usually next to the stop line, as the two red cars in
Fig.1. The locations of the stop lines are commonly a prior known. The first
stopped vehicle will be directly localized if we can obtain the relative
location with the stop line. Even if the relative location is unknown and
simply assumed that the first stopped vehicle is close to the stop line, we
can still estimate the longitudinal position, which can be used to enhance
positioning results of the GNSS/INS integrated navigation system especially in
highly urbanized areas.

It is known that with V2V communications and inter-vehicle distance and/or
angle measurements, the positioning performance of the vehicles in a VANET can
be enhanced. The first stopped vehicle can be viewed as an anchor, and we hope
that the positioning improvement of the first stopped vehicle can be extended
via a cooperative positioning algorithm. Therefore, in this paper, we focus on
the following two problems.

###### Problem 1

For intersection scenarios, given the observations of GNSS and INS of the
first stopped vehicle, the location of the stop lines, how can we estimate the
position of the first stopped vehicle from the GNSS/INS integrated navigation
system, and enhance the estimation with the aid of the stop line?

###### Problem 2

In intersection scenarios, the enhanced position estimation of the first
stopped vehicle, the V2V communication, and the distance measurements of
neighbor inter-vehicles are available. With the aid of the positioning
enhancement of the first stopped vehicle, design a cooperative positioning
algorithm to improve the position estimation of all vehicles in the VANET.

Figure 1: A typical intersection scenario for connected vehicles.

##  III Stop line aided self-positioning enhancement

In this section, we first introduce a GNSS/INS integration method for vehicle
self-positioning and then develop a stop line aided GNSS/INS integration
solution to enhance the positioning performance of the first stopped vehicle
in intersection scenarios.

###  III-A GNSS/INS integration for vehicle positioning

For simplicity, we apply the GNSS/INS loosely coupled fusion system for
vehicle self-positioning. The differences between GNSS-measured position and
velocity and those derived from INS are used to estimate INS errors. The
estimated navigation errors, including the position error, the velocity error,
and the angle error, are introduced to refine the INS-derived solutions.
Furthermore, the estimated biases of the gyroscope and accelerometer are
feedback to correct raw IMU measurements. The system architecture is shown in
Fig.2.

Figure 2: The architecture of the GNSS/INS integration for vehicle self-
positioning.

The state prediction equation is as follows,

${\bf{x}}(k)={\bf{F}}(k-1){\bf{x}}(k-1)+{\bf{w}}(k),$ (1)

with

$\begin{split}{\bf{x}}(k)&=[\delta{\phi_{\rm{R}}},\delta{\phi_{\rm{P}}},\delta{\phi_{Y}},\delta{\rm{v_{N}}},\delta{\rm{v_{E}}},\delta{\rm{v_{D}}},\delta
L,\delta\lambda,\delta h,\\\
&{\varepsilon_{x}},{\varepsilon_{y}},{\varepsilon_{z}},{\Delta_{x}},{\Delta_{y}},{\Delta_{z}}]^{\rm{{\rm
T}}}\end{split}$ (2)

where ${\bf{x}}(k)$ is the state vector with fifteen state variables,
$\delta{\phi_{\rm{R}}}$, $\delta{\phi_{\rm{P}}}$, and $\delta{\phi_{\rm{Y}}}$
are roll, pitch, and yaw angle errors, respectively;$\delta{v_{\rm{N}}}$,
$\delta{v_{\rm{E}}}$, and $\delta{v_{\rm{D}}}$ are velocity errors from the
direction of north, east, and down, respectively; $\delta L$, $\delta\lambda$,
and $\delta h$ are position errors of latitude, longitude, and height in the
geodetic coordinate system, respectively. ${\varepsilon_{x}}$,
${\varepsilon_{y}}$, and ${\varepsilon_{z}}$ denote constant gyro drifts, and
${\Delta_{x}}$ , ${\Delta_{y}}$ and ${\Delta_{z}}$ denote constant
accelerometer drifts. Besides, the process noise vector ${\bf{w}}(k)$ is a
Gaussian random vector having covariance matrix ${\bf{Q}}(k)$. ${\bf{F}}(k-1)$
is the state transition matrix, which connects the INS solution and IMU error
sources. More details about the state transition matrix can be found in ref
[32][33], which is well-known in inertial theory.

The velocity and position obtained from the INS solution and GNSS receiver are
defined as ${{\bf{v}}_{{\rm{INS}}}}$, ${{\bf{r}}_{{\rm{INS}}}}$,
${{\bf{v}}_{{\rm{GNSS}}}}$ and ${{\bf{r}}_{{\rm{GNSS}}}}$ respectively.

${{\bf{v}}_{{\rm{INS}}}}={\bf{v}}+\delta{{\bf{v}}_{{\rm{INS}}}},$ (3)
${{\bf{r}}_{{\rm{INS}}}}={\bf{r}}+\delta{{\bf{r}}_{{\rm{INS}}}},$ (4)
${{\bf{v}}_{{\rm{GNSS}}}}={\bf{v}}{\rm{-}}\delta{{\bf{v}}_{{\rm{GNSS}}}},$ (5)
${{\bf{r}}_{{\rm{GNSS}}}}={\bf{r}}-\delta{{\bf{r}}_{{\rm{GNSS}}}},$ (6)

with

$\delta{{\bf{v}}_{{\rm{INS}}}}{\rm{=[}}\begin{array}[]{*{20}{c}}{\delta{v_{\rm{N}}}}&{\begin{array}[]{*{20}{c}}{\delta{v_{\rm{E}}}}&{\delta{v_{\rm{D}}}}\end{array}}\end{array}{{\rm{]}}^{\rm{T}}},$
$\delta{{\bf{r}}_{{\rm{INS}}}}{\rm{=[}}\begin{array}[]{*{20}{c}}{\delta
L}&{\delta\lambda}&{\delta h}\end{array}{{\rm{]}}^{\rm{T}}},$

where ${\bf{v}}$ and ${\bf{r}}$ represent the true velocity and the position
in the geodetic coordinate system, respectively;
$\delta{{\bf{v}}_{{\rm{GNSS}}}}$ and $\delta{{\bf{r}}_{{\rm{GNSS}}}}$
represent the velocity and positions of the GNSS receiver, respectively.
Consequently, the observation equation can be formulated by

$\displaystyle{\bf{z}}(k)$
$\displaystyle=\left[{\begin{array}[]{*{20}{c}}{{{\bf{v}}_{{\rm{INS}}}}{\rm{-}}{{\bf{v}}_{{\rm{GNSS}}}}}\\\
{{{\bf{r}}_{{\rm{INS}}}}{\rm{-}}{{\bf{r}}_{{\rm{GNSS}}}}}\end{array}}\right]{\rm{=}}\left[{\begin{array}[]{*{20}{c}}{\delta{{\bf{v}}_{{\rm{INS}}}}{\rm{+}}\delta{{\bf{v}}_{{\rm{GNSS}}}}}\\\
{\delta{{\bf{r}}_{{\rm{INS}}}}{\rm{+}}\delta{{\bf{r}}_{{\rm{GNSS}}}}}\end{array}}\right]$
(7)
$\displaystyle={{\bf{H}}_{{\rm{sp}}}}(k){\bf{x}}(k){\rm{+}}{{\bf{v}}_{{\rm{sp}}}}\left(k\right),$

with

${{\bf{H}}_{{\rm{sp}}}}(k){\rm{=}}\left[{\begin{array}[]{*{20}{c}}{{{\bf{0}}_{3\times
3}},{\mathop{\rm
diag}\nolimits}[\begin{array}[]{*{20}{c}}1&1&1\end{array}],{{\bf{0}}_{3\times
3}},{{\bf{0}}_{3\times 6}}}\\\ {{{\bf{0}}_{3\times 3}},{{\bf{0}}_{3\times
3}},{\mathop{\rm
diag}\nolimits}[\begin{array}[]{*{20}{c}}1&1&{-1}\end{array}],{{\bf{0}}_{3\times
6}}}\end{array}}\right],$
${{\bf{v}}_{{\rm{sp}}}}\left(k\right)=\left[{\begin{array}[]{*{20}{c}}{\delta{{\bf{v}}_{{\rm{GNSS}}}}}\\\
{\delta{{\bf{r}}_{{\rm{GNSS}}}}}\end{array}}\right]$

where the subscript ’sp’ implies the ’self-positioning’, the measurement noise
${{\bf{v}}_{{\rm{sp}}}}\left(k\right)$ is assumed following zero-mean Gaussian
distribution with the covariance matrix ${\bf{R}}(k)$. Eq.(1) and Eq. (7)
constitute a typical linear system. With a Kalman filter, we can estimate the
real-time vehicular position.

###  III-B Stop line aided GNSS/INS integration method

The first stopped vehicle, i.e., the first vehicle waiting for the green light
in an intersection scenario, is usually next to the stop line. It is not too
close or too far from the stop line and maintains a safe distance to
pedestrians. Generally, vehicles usually travel along the lane central area.
Therefore, we assume that the distance from the head of the vehicle to the
stop line, which is denoted by ${d_{\rm{s}}}$, and the distance from
longitudinal median plane of the vehicle to the left lane line, which is
denoted by ${d_{\rm{l}}}$, follow Gaussian distribution, respectively. We can
collect data of first stopped vehicles from which we can estimate the
statistical feature of the two distances, i.e.,
${d_{\rm{s}}}\sim\mathcal{N}{\rm{(}}{m_{{\rm{xb}}}}{\rm{,}}{\sigma^{2}_{{\rm{xb}}}}{\rm{)}}$
and
${d_{\rm{l}}}\sim\mathcal{N}{\rm{(}}{m_{{\rm{yb}}}}{\rm{,}}{\sigma^{2}_{{\rm{yb}}}}{\rm{)}}$.

Figure 3: Illustration of the stop line aided positioning for the first
stopped vehicle.

No additional sensor is required to estimate the two distances, which is a
great merit of the proposed method. The distance ${d_{\rm{s}}}$ and
${d_{\rm{l}}}$ can also be obtained from visual sensors, which may provide
better estimation but is out of scope of this paper.

In the local north-east-down (NED) coordinate system as shown in Fig. 3, the
stop line and the left lane line can be formulated as following two equations,
respectively.

${y}={k_{\rm{s}}}{x}+{b_{\rm{s}}},$ ${y}={k_{\rm{l}}}{x}+{b_{\rm{l}}},$

where the slopes and the intercepts of both lines are a prior known.

As shown in Fig.3, ${d_{\rm{e}}}$, i.e., the distance from the origin
$O_{\rm{b}}$ of the vehicle body coordinate system to the stop line, is given
by

${d_{\rm{e}}}{\rm{=}}{d_{\rm{s}}}{\rm{+}}{l_{0}}$

where ${l_{0}}$ represents the distance between the origin $O_{\rm b}$ and the
vehicle head. In this paper, stop line based positioning is to estimate the
position of the origin $O_{\rm b}$ in the local NED coordinate system. We
denote the results of the stop line based positioning by
$({p_{{\rm{N\\_sl}}}},{p_{{\rm{E\\_sl}}}})$. Considering the equation for the
distance from point to line, we can obtain

${d_{e}}=\frac{{\left|{{k_{s}}{p_{{\rm{N\\_sl}}}}{\rm{-}}{p_{{\rm{E\\_sl}}}}+{b_{s}}}\right|}}{{\sqrt{{k_{s}}^{2}+1}}},$
(8)
${d_{l}}=\frac{{\left|{{k_{l}}{p_{{\rm{N\\_sl}}}}{\rm{-}}{p_{{\rm{E\\_sl}}}}+{b_{l}}}\right|}}{{\sqrt{{k_{l}}^{2}+1}}}.$
(9)

From the above two equations, we can directly solve the vehicle position
$({p_{{\rm{N\\_sl}}}},{p_{{\rm{E\\_sl}}}})$.

Fusing with the observations of INS, we can achieve the following observation
equation in the local NED coordinate system:

$\displaystyle{{\bf{z}}_{{\rm{sl}}}}(k)$
$\displaystyle=\left[{\begin{array}[]{*{20}{c}}{{p_{{\rm{N\\_INS}}}}-{p_{{\rm{N\\_sl}}}}}\\\
{{p_{{\rm{E\\_INS}}}}-{p_{{\rm{E\\_sl}}}}}\end{array}}\right]=\left[{\begin{array}[]{*{20}{c}}{\delta{p_{{\rm{N\\_INS}}}}{\rm{-}}\delta{p_{{\rm{N\\_sl}}}}}\\\
{\delta{p_{{\rm{E\\_INS}}}}{\rm{-}}\delta{p_{{\rm{E\\_sl}}}}}\end{array}}\right]$
(10)
$\displaystyle={{\bf{H}}_{{\rm{sl}}}}(k){\bf{x}}(k){\rm{+}}{{\bf{v}}_{{\rm{sl}}}}\left(k\right)$

with

${{\bf{H}}_{{\rm{sl}}}}(k){\rm{=}}\left[{\begin{array}[]{*{20}{c}}{{{\bf{0}}_{1\times
6}},{R_{\rm{M}}}{\rm{+}}h,{{\bf{0}}_{1\times 8}}}\\\ {{{\bf{0}}_{1\times
7}},{R_{\rm{N}}},{{\bf{0}}_{1\times 7}}}\end{array}}\right],$

${{\bf{v}}_{{\rm{sl}}}}\left(k\right)=\left[{\begin{array}[]{*{20}{c}}-\delta{p_{{\rm{N\\_sl}}}}\\\
-\delta{p_{{\rm{E\\_sl}}}}\end{array}}\right]$

where ${p_{{\rm{N\\_INS}}}}$ and ${p_{{\rm{E\\_INS}}}}$ represent the INS-
derived positioning results; $\delta{p_{{\rm{N\\_INS}}}}$ and
$\delta{p_{{\rm{E\\_INS}}}}$ denote the INS-derived positioning errors;
$\delta{p_{{\rm{N\\_sl}}}}$ and $\delta{p_{{\rm{E\\_sl}}}}$ are the errors of
north and east position obtained from Eq. (8) and Eq. (9). They are all
formulated in the NED coordinate frame. The matrix ${{\bf{H}}_{{\rm{sl}}}}$ is
used to transform the errors of the NED frame to those of the geodetic
coordinate system. ${{R_{\rm{M}}}}$ and ${{R_{\rm{N}}}}$ are the long and
short semi-axis radius of the Earth individually, and $h$ is the altitude of
the vehicle in the geodetic coordinate system. The noise vector
${{\bf{v}}_{{\rm{sl}}}}$ is assumed following the zero-mean Gaussian
distribution with the covariance matrix

${{\bf{\Sigma}}_{{\rm{sl}}}}=\left[{\begin{array}[]{*{20}{c}}{\sigma({x_{\rm{N}}},{x_{\rm{N}}})}&{\sigma({x_{\rm{N}}},{x_{\rm{E}}})}\\\
{\sigma({x_{\rm{E}}},{x_{\rm{N}}})}&{\sigma({x_{\rm{E}}},{x_{\rm{E}}})},\end{array}}\right]$

where $\sigma({x_{\rm{N}}},{x_{\rm{N}}})$ and
$\sigma({x_{\rm{E}}},{x_{\rm{E}}})$ denote the error variance in the north and
east direction, respectively;
$\sigma({x_{\rm{N}}},{x_{\rm{E}}}){\rm{=}}\sigma({x_{\rm{E}}},{x_{\rm{N}}})$
represents the error covariance in the north and east direction.
${{\bf{\Sigma}}_{{\rm{sl}}}}$ can be obtained by coordinate conversion from
${\bf{\Sigma}}$ representing the covariance matrix of the observed longitude
distance ${d_{\rm{s}}}$ and lateral distance ${d_{\rm{l}}}$. Apparently, the
errors in the longitudinal and lateral direction of the road are uncorrelated,
but the errors are correlated in the north and east directions unless the road
direction is due north or due east. Denoting the coordinate transformation
matrix by ${\bf{T}}$ and the angle between the road and due north by $\theta$
as shown in Fig. 3, we have

${{\bf{\Sigma}}_{{\rm{sl}}}}={\bf{T\Sigma}}{{\bf{T}}^{\rm{T}}},$ (11)

where

${\bf{T}}=\left[{\begin{array}[]{*{20}{c}}{\cos\theta}&{-\sin\theta}\\\
{\sin\theta}&{\cos\theta}\end{array}}\right],\quad\quad{\bf{\Sigma}}{\rm{=}}\left[{\begin{array}[]{*{20}{c}}{{\sigma_{{\rm{xb}}}}^{2}}&0\\\
0&{{\sigma_{{\rm{yb}}}}^{2}}\end{array}}\right].$

Combining with observations from the GNSS receivers, the observation equation
can be finally formulated to be

${\bf{Z}}(k)=\left[{\begin{array}[]{*{20}{c}}{{p_{{\rm{N\\_INS}}}}-{p_{{\rm{N\\_sl}}}}}\\\
{{p_{{\rm{E\\_INS}}}}-{p_{{\rm{E\\_sl}}}}}\\\
{{{\bf{v}}_{{\rm{INS}}}}{\rm{-}}{{\bf{v}}_{{\rm{GNSS}}}}}\\\
{{{\bf{r}}_{{\rm{INS}}}}{\rm{-}}{{\bf{r}}_{{\rm{GNSS}}}}}\end{array}}\right]={{\bf{H}}_{{\rm{sp\\_sl}}}}(k){\bf{x}}(k){\rm{+}}{{\bf{v}}_{{\rm{sp\\_sl}}}}\left(k\right).$
(12)

Applying the Kalman filter for the linear system consisting of Eq.(1) and
Eq.(12), we can obtain the stop-line corrected vehicular position. The
architecture of the stop line aided positioning enhancement can be found from
Fig. 4.

Figure 4: The architecture of the stop line aided GNSS/INS integration for
vehicle self-positioning.

##  IV Cooperative Inertial Navigation Approach

For connected vehicles in intersection scenarios, with the positioning
improvement of the first stopped vehicle, we aim to further extend the
enhancement to all vehicles in the VANET via a cooperative inertial navigation
framework.

In such a framework, if vehicle $i$ interacts with other vehicles, we run a
cooperative positioning algorithm for vehicle $i$. The elements of the state
vector ${\bf x}^{(i)}(k)$ are the same as those of the state vector ${\bf
x}(k)$ in Eq. (2). The cooperative positioning algorithm has two stages, and
the first stage is the following one-step prediction:

${\bf{x}}^{(i)}(k|k-1)={\bf{F}}(k\left|{k-1}\right.){\bf{x}}^{(i)}(k{\rm{-}}1),$
(13) ${\bf{P}}^{(i)}(k|k-1)={\bf{F}}(k-1){\bf{P}}^{(i)}(k-1){\bf{F}}^{\rm
T}{(k-1)}{\rm{+}}{\bf{Q}}{\rm{(}}k{\rm{-1)}}.$ (14)

The arrival of new information triggers the second stage, i.e., the update
stage. Except for the periodic GNSS observation, vehicle $i$ also measures the
inter-node distances and receives beacon packets from neighbors both via V2V
communication. Similar to the common GNSS/INS integration method, we adopt
newly arrived information to correct INS errors. However, it unnecessarily
receives all above-mentioned information during a time horizon due to the
working frequencies difference between the GNSS and the V2V communications. As
shown in Fig.5, the filter can be triggered by the reception of local GNSS
readings or the received beacon packets from neighbors. Consequently, the
update stage at the $k$ instant has the following three cases:

  * •

Case1:only the local GNSS readings are available;

  * •

Case2:the beacon packets and the inter-node distances are available;

  * •

Case3:the local GNSS readings, the beacon packets, and the inter-node
distances are all available.

Figure 5: Illustration of the observation sequences.

###  IV-A Case1:Only the local GNSS readings are available

Except for the INS observations, if only the local GNSS observations are
available, it is a typical self-positioning problem.

####  IV-A1 Vehicle $i$ is the first stopped vehicle

As discussed in the last section, the INS-derived solutions can be corrected
by the stop line, we use the Eq. (12) as the observation equation, i.e.,

${\bf{z}}(k){\rm{=}}{{\bf{z}}^{(i)}_{{\rm{sp\\_sl}}}}(k){\rm{=}}{{\bf{H}}^{(i)}_{{\rm{sp\\_sl}}}}(k){\bf{x}}(k){\rm{+}}{{\bf{v}}^{(i)}_{{\rm{sp\\_sl}}}}\left(k\right).$
(15)

####  IV-A2 Vehicle $i$ is not the first stopped vehicle

this case is a typical GNSS/INS integrated navigation system, and the
observation equation is the Eq.(7), i.e.,

${\bf{z}}(k){\rm{=}}{{\bf{z}}^{(i)}_{{\rm{sp}}}}(k){\rm{=}}{{\bf{H}}^{(i)}_{{\rm{sp}}}}(k){\bf{x}}(k){\rm{+}}{{\bf{v}}^{(i)}_{{\rm{sp}}}}\left(k\right).$
(16)

###  IV-B Case2:Beacon packets and inter-node distances are available

We denote the INS calculation and true location of vehicle $i$ by
${\bf{p}}_{{}_{{\rm{INS}}}}^{(i)}(k){\rm{=}}{\left[{{p_{{\rm{N\\_INS}}}},{p_{{\rm{E\\_INS}}}}}\right]^{\rm{T}}}$
and ${{\bf{p}}^{(i)}}(k)$, respectively. Like Eq. $(\ref{eq3})\sim\eqref{eq-
rgnss}$, we have

${\bf{p}}_{{}_{{\rm{INS}}}}^{(i)}(k){\rm{=}}{{\bf{p}}^{(i)}}(k){\rm{+}}\delta{{\bf{p}}^{(i)}}(k)$
(17)

where
$\delta{{\bf{p}}^{(i)}}={\left[{\delta{p_{{\rm{N\\_INS}}}},\delta{p_{{\rm{E\\_INS}}}}}\right]^{\rm{T}}}$
is the INS positioning error in the NED coordinate system.
$\delta{{\bf{p}}^{(i)}}$ can be transformed into the geodetic coordinate
system, i.e.,

$\delta{{\bf{p}}^{(i)}}{\rm{=[}}({R_{\rm{M}}}{\rm{+}}h){\bf{x}}(7),{R_{\rm{N}}}{\bf{x}}(8)].$
(18)

We assume vehicle $i$ receives beacon packets and relative distances from its
$n$ neighbor vehicles, and denote the neighborhood set by ${S_{i}}$. In the
NED coordinate frame, $\forall j\in{S_{i}},j=1,2,...n$, we denote the self-
positioning estimation and true location of vehicle $j$ by
${{\bf{\hat{p}}}^{(j)}}(k)$ and ${{\bf{p}}^{(j)}}(k)$, respectively. We
introduce ${\tilde{z}_{j\to i}}$, the difference between the estimated
position of vehicle $i$ and that of vehicle $j$, and the relative distance
${\hat{z}_{j\to i}}$ measured from the V2V communication. Then we arrive at
the following V2V-based observation equation:

$\begin{split}{\rm{z}}(k)&={\rm{z}}_{{}_{{\rm{v2v}}}}^{(j)}{\rm{(}}k{\rm{)}}\\\
&={\tilde{z}_{j\to i}}-{\hat{z}_{j\to i}}\\\
&=\left\|{{\bf{p}}_{{}_{{\rm{INS}}}}^{(i)}{\rm{(}}k{\rm{)-}}{{{\bf{\hat{p}}}}^{(j)}}(k)}\right\|-{\hat{z}_{j\to
i}}\\\
&=\left\|{[{{\bf{p}}^{{\rm{(}}i{\rm{)}}}}{\rm{(}}k{\rm{)}}+\delta{{\bf{p}}^{{\rm{(}}i{\rm{)}}}}{\rm{(}}k{\rm{)}}]{\rm{-[}}{{\bf{p}}^{{\rm{(}}j{\rm{)}}}}{\rm{(}}k{\rm{)+}}\delta{{\bf{p}}^{{\rm{(}}j{\rm{)}}}}{\rm{(}}k{\rm{)}}]}\right\|\\\
&-(\left\|{{{\bf{p}}^{{\rm{(}}i{\rm{)}}}}{\rm{(}}k{\rm{)-}}{{\bf{p}}^{{\rm{(}}j{\rm{)}}}}{\rm{(}}k{\rm{)}}}\right\|-{n_{j\to
i}})\\\ &={\mathop{\rm h}\nolimits}_{j\to
i}^{\rm{p}}(\delta{{\bf{p}}})+{n_{j\to i}},\end{split}$ (19)

with

$\delta{\bf{p}}=[\begin{array}[]{*{20}{c}}{\delta{{\bf{p}}^{{\rm{(}}i{\rm{)}}}}}&{\delta{{\bf{p}}^{{\rm{(}}j{\rm{)}}}}}\end{array}]$

where
$\delta{{\bf{p}}^{{\rm{(}}j{\rm{)}}}}{\rm{(}}k{\rm{)=}}{{\bf{\hat{p}}}^{(j)}}(k){\rm{-}}{{\bf{p}}^{(j)}}(k)$
is the self-positioning error of the vehicle $j$, and ${n_{j\to i}}$ is the
V2V ranging error. Since ${\mathop{\rm h}\nolimits}_{j\to
i}^{\rm{p}}(\delta{\bf{p}})$ is a nonlinear function of $\delta{{\bf{p}}}$,
linearizing ${\mathop{\rm h}\nolimits}_{j\to i}^{\rm{p}}(\delta{\bf{p}})$
yields

$\begin{split}{{\bf{z}}^{(j)}_{{\rm{v2v}}}}\left(k\right)&={\mathop{\rm
h}\nolimits}_{j\to i}^{\rm{p}}(\delta{\bf{p}})+{n_{j\to i}}\\\
&\approx{\bf{H}}_{j\to
i}^{p}\delta{{\bf{p}}^{(i)}}{\rm{(}}k{\rm{)+}}{\bf{H}}_{i\to
j}^{\rm{p}}\delta{{\bf{p}}^{{\rm{(}}j{\rm{)}}}}{\rm{(}}k{\rm{)+}}{n_{j\to
i}}\\\ &={{\bf{H}}^{(j)}_{{\rm{v2v}}}}{\bf{x}}{\rm{(}}k{\rm{)+}}{\bf{H}}_{i\to
j}^{p}\delta{{\bf{p}}^{{\rm{(}}j{\rm{)}}}}{\rm{(}}k{\rm{)+}}{n_{j\to i}}\\\
&={{\bf{H}}^{(j)}_{{\rm{v2v}}}}{\bf{x}}{\rm{(}}k{\rm{)}}+{u_{j}}{\rm{+}}{n_{j\to
i}}\\\
&={{\bf{H}}^{(j)}_{{\rm{v2v}}}}{\bf{x}}{\rm{(}}k{\rm{)+}}{{\bf{v}}^{(j)}_{{\rm{v2v}}}}{\rm{(}}k{\rm{)}}\end{split}$
(20)

where

${\bf{H}}_{j\to i}^{\rm{p}}{\rm{=-}}{\bf{H}}_{i\to
j}^{\rm{p}}{\rm{=}}{\left.{\frac{{\partial{\mathop{\rm h}\nolimits}_{j\to
i}^{\rm{p}}(\delta{\bf{p}})}}{{\partial\delta{{\bf{p}}^{(i)}}}}}\right|_{\delta{\bf{p}}=[\delta{{\bf{p}}^{(i)}}(k\left|{k-1}\right.),\delta{{\bf{p}}^{(j)}}(k\left|{k-1}\right.)]}}$

is the Jacobian matrix of ${\mathop{\rm h}\nolimits}_{j\to
i}^{\rm{p}}(\delta{{\bf{p}}})$ at the current state prediction which can be
obtained from Eq.(13), and the measurement matrix
${{\bf{H}}^{(j)}_{{\rm{v2v}}}}{\rm{=}}\left[{{{\bf{0}}_{1\times
6}},({R_{\rm{M}}}{\rm{+}}h){\bf{H}}_{j\to
i}^{\rm{p}}(1),{R_{\rm{N}}}{\bf{H}}_{j\to i}^{\rm{p}}(2),{{\bf{0}}_{1\times
7}}}\right].$ In Eq. (20), ${u_{j}}={\bf{H}}_{i\to
j}^{\rm{p}}\delta{{\bf{p}}^{{\rm{(}}j{\rm{)}}}}{\rm{(}}k{\rm{)}}$ follows a
Gaussian distribution since
$\delta{{\bf{p}}^{{\rm{(}}j{\rm{)}}}}{\rm{(}}k{\rm{)}}$ follows a Gaussian
distribution. Denote ${u_{j}}\sim\mathcal{N}{\rm{(0,}}{r_{j}}{\rm{)}}$, we
have

$\begin{split}{r_{j}}&={\bf{H}}_{i\to
j}^{\rm{p}}\mathds{E}[\delta{{\bf{p}}^{{\rm{(}}j{\rm{)}}}}{\rm{(}}k{\rm{)(}}\delta{{\bf{p}}^{{\rm{(}}j{\rm{)}}}}{\rm{(}}k{\rm{)}}{{\rm{)}}^{\rm{T}}}]{({\bf{H}}_{i\to
j}^{\rm{p}})^{\rm{T}}}\\\ &={\bf{H}}_{i\to
j}^{\rm{p}}{{\bf{P}}^{(j)}}(k){({\bf{H}}_{i\to
j}^{\rm{p}})^{\rm{T}}}.\end{split}$ (21)

Here
${{\bf{P}}^{(j)}}(k)=\mathds{E}[\delta{{\bf{p}}^{{\rm{(}}j{\rm{)}}}}{\rm{(}}k{\rm{)(}}\delta{{\bf{p}}^{{\rm{(}}j{\rm{)}}}}{\rm{(}}k{\rm{)}}{{\rm{)}}^{\rm{T}}}]$
is the covariance matrix of the self-positioning result of vehicle $j$. The
measurement noise of the relative distance between two vehicles is also
assumed as a Gaussian distribution, i.e., ${n_{j\to
i}}\sim\mathcal{N}{\rm{(0,}}\sigma_{{\rm{V2V}}}^{2}{\rm{)}}$, and the variance
$\sigma^{2}_{\rm{V2V}}$ can be estimated from a prior observed data-sets for
V2V-based distance measurements.

If vehicle $j$ is the first stopped vehicle, the position estimation will be
corrected with the aid of the stop line, and consequently has a smaller
covariance ${{\bf{P}}^{(j)}}(k)$, which leads to a smaller $r_{j}$. In that
case, vehicle $j$ can be viewed as a temporary anchor such that its neighbors
can apply the proposed cooperative positioning framework to achieve better
positioning results.

If vehicle $i$ can receive beacon packets and relative distances from all its
$n$ neighbor nodes, we generalize the above mentioned vehicle $j$ to all
vehicles in $S_{i}$, and arrive at the observation equation (22).

${\bf{z}}\left(k\right)=\left[{\begin{array}[]{*{20}{c}}{{\rm{z}}_{{}_{{\rm{v2v}}}}^{(1)}\left(k\right)}\\\
{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{{\rm{z}}_{{}_{{\rm{v2v}}}}^{(2)}\left(k\right)}\\\
{...}\end{array}}\end{array}}\\\
{{\rm{z}}_{{}_{{\rm{v2v}}}}^{(n)}\left(k\right)}\end{array}}\end{array}}\right]{\rm{=}}\left[{\begin{array}[]{*{20}{c}}{{\bf{H}}_{{}_{{\rm{v2v}}}}^{(1)}\left(k\right)}\\\
{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{{\bf{H}}_{{}_{{\rm{v2v}}}}^{(2)}\left(k\right)}\\\
{...}\end{array}}\end{array}}\\\
{{\bf{H}}_{{}_{{\rm{v2v}}}}^{(n)}\left(k\right)}\end{array}}\end{array}}\right]{\bf{x}}(k)){\rm{+}}\left[{\begin{array}[]{*{20}{c}}{{\bf{v}}_{{}_{{\rm{v2v}}}}^{(1)}\left(k\right)}\\\
{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{{\bf{v}}_{{}_{{\rm{v2v}}}}^{(2)}\left(k\right)}\\\
{...}\end{array}}\end{array}}\\\
{{\bf{v}}_{{}_{{\rm{v2v}}}}^{(n)}\left(k\right)}\end{array}}\end{array}}\right]$
(22)
${\bf{z}}\left(k\right)=\left[{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{{{\bf{z}}^{(i)}_{{\rm{sp\\_sl}}}}\left(k\right)\;\;\mathrm{or}\;\;{{\bf{z}}^{(i)}_{{\rm{sp}}}}\left(k\right)}\\\
{{{\bf{z}}^{(1)}_{{\rm{v2v}}}}\left(k\right)}\end{array}}\\\
{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{{{\bf{z}}^{(2)}_{{\rm{v2v}}}}\left(k\right)}\\\
{...}\end{array}}\end{array}}\\\
{{{\bf{z}}^{(n)}_{{\rm{v2v}}}}\left(k\right)}\end{array}}\end{array}}\right]{\rm{=}}\left[{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{{{\bf{H}}^{(i)}_{{\rm{sp\\_sl}}}}\;\;\mathrm{or}\;\;{{\bf{H}}^{(i)}_{{\rm{sp}}}}}\\\
{{{\bf{H}}^{(1)}_{{\rm{v2v}}}}}\end{array}}\\\
{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{{{\bf{H}}^{(2)}_{{\rm{v2v}}}}}\\\
{...}\end{array}}\end{array}}\\\
{{{\bf{H}}^{(n)}_{{\rm{v2v}}}}}\end{array}}\end{array}}\right]{\bf{x}}(k){\rm{+}}\left[{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{{{\bf{v}}^{(i)}_{{\rm{sp\\_sl}}}}\left(k\right)\;\;\mathrm{or}\;\;{{\bf{v}}^{(i)}_{{\rm{sp}}}}\left(k\right)}\\\
{{{\bf{v}}^{(1)}_{{\rm{v2v}}}}\left(k\right)}\end{array}}\\\
{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{\begin{array}[]{*{20}{c}}{{{\bf{v}}^{(2)}_{{\rm{v2v}}}}\left(k\right)}\\\
{...}\end{array}}\end{array}}\\\
{{{\bf{v}}^{(n)}_{{\rm{v2v}}}}\left(k\right)}\end{array}}\end{array}}\right]$
(23) Figure 6: The stop line aided cooperative positioning framework.

###  IV-C Case3: local GNSS readings, beacon packets, and inter-node distances
are all available

If the local GNSS readings, beacon packets, and inter-node distances are all
available, adding the observation equation of self-positioning to Eq.(22)
yields the observation equation (23). The observation equation of self-
positioning has two cases: if vehicle $i$ is the first-stopped vehicle, the
observation is the ${{\bf{z}}^{(i)}_{{\rm{sp\\_sl}}}}\left(k\right)$ in Eq.
(15); otherwise the self-positioning observation is the
${{{\bf{z}}^{(i)}_{{\rm{sp}}}}\left(k\right)}$ in Eq. (16).

###  IV-D Measurement update of the proposed stop line aided cooperative
positioning framework

The measurement data sequence is shown in Fig. 5, and the stop line aided
cooperative positioning framework is shown in Fig.6. Based on the types of the
received observations, we select the corresponding observation equation from
Eq. (15), (16), (22), (23), respectively. For simplicity, we describe the four
observation equations to be the following general form:

$\mathbf{z}(k)=\mathbf{H}(k)\mathbf{x}(k)+\mathbf{v}(k)$

which is actually one of the above-mentioned four equations.

With the observation equations, we can obtain the Kalman gain

$\begin{split}{{\bf{K}}^{(i)}}(k)&={{\bf{P}}^{(i)}}(k|k-1){\bf{H}}{(k)^{\rm{T}}}[{\bf{H}}(k){{\bf{P}}^{(i)}}(k|k-1){\bf{H}}{(k)^{\rm{T}}}\\\
&+{{\bf{R}}^{(i)}}(k){]^{-1}},\end{split}$ (24)

the a posterior state estimation

${\bf{\hat{x}}}^{(i)}(k)={\bf{x}}^{(i)}(k|k-1)+{\bf{K}}^{(i)}(k)\left[{{\bf{z}}\left(k\right){\rm{-}}{\bf{H}}(k){\bf{x}}^{(i)}(k|k-1)}\right],$
(25)

and the corresponding covariance matrix

$\begin{split}{\bf{P}}^{(i)}(k)&=({\bf{I}}-{\bf{K}}^{(i)}(k){\bf{H}}(k)){\bf{P}}^{(i)}(k){({\bf{I}}-{\bf{K}}^{(i)}(k){\bf{H}}(k))^{\rm{T}}}\\\
&+{\bf{K}}^{(i)}(k){\bf{R}}^{(i)}(k){\bf{K}}^{(i)}{(k)^{\rm{T}}}.\end{split}$
(26)

##  V Experimental Validation

###  V-A Experimental Setup and Scenario Description

Two scenarios with three vehicles were conducted at an intersection with
traffic lights in Beijing, China, as shown in Fig.7. We used a high-accuracy
Real-Time Kinematic GNSS/INS integrated navigation system(Novatel PwrPak7D-E1)
to provide the positioning ground truth. In addition, each vehicle was
equipped with a Sinan M100W GNSS navigation device, XSENS Mti630 IMU, Nooploop
UWB Node, and a laptop. The M100W acted as an ordinary GNSS receiver and
collected data at a frequency of 5Hz. The XSENS Mti630 IMU collected data at
100Hz. The UWB device was used for V2V communication at 5 Hz and inter-vehicle
ranging at 100 Hz.

Figure 7: Experimental vehicles and the scenario setup.The red box shows the
area of the intersection. Weigongcun Road features a two-way with 4-lane motor
traffic and Minzu University West Road has one lane each way.

We compare the following four positioning approaches to show the effectiveness
of the proposed framework:

(1)Self-positioning (SP): GNSS/IMU loosely coupled fusion for vehicle self-
positioning.

(2)Stop line aided self-positioning (SL-SP): we apply the stop line as a
measurement for the first stopped vehicle and fuse the measurement with the
GNSS/IMU positioning system.

(3)Cooperative positioning (CP): the local IMU and GNSS observation, position-
related information from other vehicles, and inter-vehicle ranging are applied
for localization.

(4)Stop line aided cooperative positioning (SL-CP): the proposed framework.

###  V-B Experimental results for scenario 1

This scenario shows the basic idea of the stop-line aided self-positioning
enhancement and how the improvement extends to all connected vehicles. In this
scenario, two vehicles travel straight from south to north on Minzu University
West Road, and one vehicle turns right into Minzu University West Road from
Weigongcun Road.

Fig.8 provides the key scenes of this scenario. Fig.8(a) shows that at the
${t_{0}}$ instant, vehicle V0 was running on the Weigongcun Road and ready to
turn right into the Minzu University West Road. Vehicle V1 and V2 were both
running on the Minzu University West Road. Fig.8(b) shows that at the
${t_{s1}}$ instant, vehicle V1 became the first stopped vehicle while vehicles
V0 and V2 were still running. At ${t_{s2}}$, as shown in Fig.8(c), both V1 and
V2 were waiting for the green light while V0 was still running. At ${t_{e}}$,
as shown in Fig.8(d), both V1 and V2 were still waiting for the green light
while V0 was far away from the intersection. In this scenario, the three
vehicles communicated via the UWB sensors. After the ${t_{e}}$ instant, the
communications between V0 and the other two vehicles might not be stable due
to the limitation of communication distance.

(a) the $t_{0}$ instant

(b) the ${t_{s1}}$ instant

(c) the ${t_{s2}}$ instant

(d) the $t_{e}$ instant

Figure 8: The key scenes of the scenario 1. Orange represents the vehicle is
stopped. Blue represents the vehicle is moving.

(a) V0

(b) V1

(c) V2

Figure 9: The positioning errors of the three vehicles in scenario 1.

Positioning errors of the three vehicles using the four approaches as
mentioned above are shown in Fig.9. There was no stopped vehicle during
${t_{0}}\sim{t_{s1}}$. Hence, the results of SL-CP were the same as those of
the cooperative positioning (CP) method during ${t_{0}}\sim{t_{s1}}$. Compared
with self-positioning, using SL-CP or CP greatly reduced the positioning error
of V0 while sightly increasing the errors of V1 and V2. The reason is that the
self-positioning error of V1 and V2 were both much smaller than that of V0. A
vehicle with low positioning performance can greatly benefit from the
cooperative positioning strategy.

TABLE I: POSITIONING ERROR (RMSE) during ${t_{s1}}\sim{t_{e}}$ |  V0 |  V1 |  V2
---|---|---|---
SP |  9.15 m |  5.18 m |  4.91 m
SL-SP |  / m |  0.24 m |  / m
CP |  5.10 m |  7.10 m |  6.02 m
SL-CP |  1.93 m |  0.25 m |  3.84 m

As shown in Fig.8(b), V1 was the first vehicle stopped due to the red light.
It was motionless during ${t_{s1}}\sim{t_{e}}$. TABLE I shows the positioning
errors, i.e., the root mean square error (RMSE), of the three vehicles during
this period, respectively. Fig.9(b) shows that at the ${t_{s1}}$ instant, the
positioning error of V1 using the SL-SP method droped from about 5 m to less
than 0.5 m. As can be seen from TABLE I, with the help of the stop line
information, during ${t_{s1}}\sim{t_{e}}$, SL-SP reduced the RMSE of V1 from
5.18 m to 0.24 m. Both Fig.9(b) and TABLE I indicate that stop line
information can greatly help the self-positioning for the first stopped
vehicle.

As previously mentioned, compared with the self-positioning method, CP sightly
increased the errors of V1 without the stop line information. But the SL-CP
significantly reduced the positioning error of V1 from around 6 m to less than
0.5 m during ${t_{s1}}\sim{t_{e}}$, which is clearly shown in Fig.9(b). As
shown from TABLE I, the RMSE of V1 during this period decreased from 7.10 m to
0.25m, which achieved a significant improvement from the CP method. More
importantly, the improvement of V1 can further help the other two vehicles
with the proposed SL-CP framework. Fig.9(a) and (c) show that during
${t_{s1}}\sim t_{e}$, SL-CP dramatically improved the positioning performance
of both V0 and V2. Their RMSEs (see TABLE I) reduced from 5.10 m and 6.02 m to
1.93 m and 3.84 m, respectively.

The experimental results of this scenario indicate that the stop line can be
used for the positioning enhancement of the first stopped vehicle. With the
proposed SL-CP framework, the enhancement can be extended to the whole VANET.

(a) the $t_{0}$ instant

(b) the ${t_{s1}}$ instant

(c) the ${t_{s2}}$ instant

(d) the ${t_{s3}}$ instant

(e) the ${t_{s4}}$ instant

(f) the ${t_{s5}}$ instant

(g) the ${t_{e}}$ instant

Figure 10: The key scenes of the scenario 2. Orange represents the vehicle is
stopped. Blue represents the vehicle is moving.

(a) V0

(b) V1

(c) V2

Figure 11: The positioning errors of the three vehicles in scenario 2.

###  V-C Experimental Results for Scenario 2

We then implement the other scenario to further validate the proposed
framework. When traffic lights switch, the role of the first stopped vehicle
also shifts from one vehicle to another. In this scenario, two vehicles travel
from south to north on Minzu University West Road, and the third vehicle goes
straight from west to east on Weigongcun Road.

Fig.10 provides the critical scenes of this scenario. Fig.10(a) shows V0 and
V1 were running on the Minzu University West Road, and V2 was moving on the
Weigongcun Road at the ${t_{0}}$ instant. Fig.10(b) shows that V0 became the
first stopped vehicle while V1 and V2 were still running at the ${t_{s1}}$
moment. Fig.10(c) illustrates that V1 stopped to wait for the green light. At
${t_{s3}}$, V2 ran into the line-of-sight (LOS) range of V0 and V1. All three
vehicles could measure relative distances and communicate with each other.
During ${t_{0}}\sim{t_{s3}}$, V2 could not stably communicate with either V0
or V1. At ${t_{s4}}$, V2 became the first stopped vehicle. As shown in Fig.
10(f), at ${t_{s5}}$, the traffic light turned green, V0 was turning left, and
V1 was turning right. Fig.10(g) shows that at ${t_{e}}$, V2 was still stopped,
and both V0 and V1 were far away. After ${t_{e}}$, the communications between
the three vehicles might not be stable due to the limitation of communication
distance.

TABLE II: POSITIONING ERROR (RMSE) of V0 and V1 during ${t_{s1}}\sim{t_{s5}}$ |  SP |  SL-SP |  CP |  SL-CP
---|---|---|---|---
V0 |  2.78 m |  0.27 m |  1.49 m |  0.26 m
V1 |  2.73 m |  / m |  2.09 m |  1.52 m
TABLE III: POSITIONING ERROR (RMSE) of V0 and V1 during ${t_{s5}}\sim{t_{e}}$ |  SP |  SL-SP |  CP |  SL-CP
---|---|---|---|---
V0 |  2.64 m |  0.95 m |  1.88 m |  0.41 m
V1 |  3.20 m |  / m |  2.34 m |  1.23 m
TABLE IV: POSITIONING ERROR (RMSE) of V2 |  SP |  SL-SP |  CP |  SL-CP
---|---|---|---|---
${t_{s1}}\sim{t_{s4}}$ |  2.38 m |  2.38 m |  2.41 m |  2.28 m
${t_{s4}}\sim{t_{e}}$ |  2.39 m |  0.26 m |  2.49 m |  0.28 m

The positioning errors (RMSE) are shown in Fig.11, and the detailed results
are shown in TABLE II $\sim$ IV. During ${t_{0}}\sim{t_{s1}}$, no vehicle was
stopped, and only V0 and V1 could mutually communicate and measure the
relative distance. Hence, the positioning results of V0 and V1 calculated by
the SL-CP and the CP methods were identical (see Fig.11). Compared with self-
positioning, using SL-CP or CP decreased the positioning errors of both V0 and
V1 significantly during that period.

At ${t_{s1}}$, V0 became the first stopped vehicle. With SL-SP, the
localization error directly declined to less than 0.5 m (red curve in
Fig.11(a)). During ${t_{s1}}\sim{t_{s5}}$, V0 waited for the green light, and
the RMSE of the SL-SP was 0.27 m (see TABEL II), which could provide much
better initialization for the localization of V0 after ${t_{s5}}$. At
${t_{s5}}$, V0 started to move. The RMSE of SL-SP for the time slot
${t_{s5}}\sim t_{e}$ was 0.95 m, much smaller than that of the common self-
positioning due to the better initialization. Compared to SL-SP, SL-CP can
further benefit from cooperation with the other two vehicles, especially the
new first stopped vehicle, i.e., V2. The RMSE reduced to 0.41m (see TABEL
III). With the help of the stop line and the SL-CP scheme, V0 significantly
improved the overall positioning performance.

At ${t_{s3}}$, V2 ran into the LOS range of V0 and V1. During
${t_{s3}}\sim{t_{s4}}$, the CP of V2 was slightly worse than the self-
positioning while SL-CP had a dramatic improvement due to the better self-
positioning of V0, i.e., the first stopped vehicle. At ${t_{s4}}$, V2 became
the first stopped vehicle and the positioning error was greatly reduced with
the help of stop line. As can be seen from TABEL IV, the RMSE of SL-SP is 0.26
m and that of SL-CP was 0.28 m during ${t_{s4}}\sim{t_{e}}$. In the same
period, ignoring the stop line information, the positioning errors of SP and
CP were both more than 2 m.

Within the SL-CP framework, V1 can greatly benefit from the other two
vehicles. At ${t_{s1}}$, V0 became the first stopped vehicle, and the
positioning error of V1 via the SL-CP had a modest reduction. At ${t_{s4}}$,
V2 also became the first stopped vehicle, and consequently V1’s positioning
error was further reduced. In the time slot ${t_{s1}}\sim{t_{s5}}$, compared
with the CP method, the RMSE of V1 via SL-CP decreased from 2.09 to 1.52 m. At
${t_{s5}}$, V0 and V1 began to move one after another, and hence V0 could not
adopt the stop line information for self-positioning enhancement. Therefore,
the improvement of V1 introduced by V2V communication experienced a decreasing
trend. Nonetheless, compared with the CP method, the RMSE of V1 via the SL-CP
reduced from 2.34 m to 1.23 m during ${t_{s5}}\sim{t_{e}}$ due to the new
stopped vehicle, i.e., V2.

In this scenario, vehicles successively stopped due to the red light. The role
of the first stopped vehicle shifted from one vehicle to another when the
traffic lights switched. It is a more general scenario in heavy-traffic areas.
The experiment results show the effectiveness of the stop-line-aided self-
positioning and the enhanced performance of the proposed SL-CP framework for
the whole VANET in such a general intersection scenario.

(a) Scenario 1-V0

(b) Scenario 1-V2

(c) Scenario 2-V0

(d) Scenario 2-V1

Figure 12: Illustration for cooperative positioning: square represents the
real position, star represents the self-positioning result, and the circle
represents the cooperative positioning result.

###  V-D Discussion

In scenario 2, compared with the self-positioning method, the positioning
errors of both V0 and V1 were decreased during ${t_{0}}\sim{t_{s3}}$. In
scenario 1, however, CP reduced the positioning error of V0 while increasing
the errors V2 during ${t_{0}}\sim{t_{e}}$. As shown in Fig.12(a), CP method
fuses the self-positioning results with the V2V ranging, i.e., ${\hat{z}_{2\to
0}}$, then the estimated positioning result of V0 is closer to the real
position. But Fig.12(b) shows the opposite case for V2. Fig.12(c) and (d)
illustrate that the CP results of V0 and V1 are both closer to the real
position over the self-positioning method in scenario 2. Fig. 12 indicates
that the error direction of the self-positioning result is related to the
performance of cooperative positioning.

It has the potential to further improve the performance by ignoring certain
vehicles in cooperative positioning algorithm. For example, in scenario 1, it
is better for V2 to ignore V0 in both cooperative positioning methods.
However, this phenomenon is out of the scope of this work and will be
discussed in our further work.

##  VI Concluding remarks and future works

This paper develops a novel stop-line-aided cooperative inertial navigation
framework to enhance the localization of connected vehicles in intersection
scenarios. The stop line information is introduced to improve the first
stopped vehicle, and the CIN framework is applied to further extend the
improvement to the whole VANET. The experimental results indicate

  1. 1.

The stop line information can significantly enhance the self-positioning of
the first stopped vehicle;

  2. 2.

The proposed stop line aided cooperative positioning (SL-CP) framework can
enhance the positioning of the whole VANET in intersection scenarios.

In the further, we will focus on the node-selection mechanism in the
cooperative positioning framework to detect and remove the low informative
nodes, for example, V0 in scenario 1, to further improve the overall
performance of a VANET.

## References

  * [1] T. G. R. Reid, S. E. Houts, R. Cammarata et al., “Localization Requirements for Autonomous Vehicles,” _SAE International Journal of Connected and Automated Vehicles_ , vol. 2, no. 3, pp. 173-190, 2019.
  * [2] S. M. Farooq, S. M. S. Hussain, and T. S. Ustun. “A Survey of Authentication Techniques in Vehicular Ad-Hoc Networks,” _IEEE Intell. Transp. Syst. Magaz._ , vol. 13, no. 2, pp. 39-52, 2020.
  * [3] W. Wen, X. Bai, Y.-C. Kan et al., “Tightly Coupled GNSS/INS Integration via Factor Graph and Aided by Fish-Eye Camera,” _IEEE Trans. Veh. Technol._ , vol. 68, no. 11, pp. 10651-10662, Nov, 2019.
  * [4] W. Wen, G. Zhang, and L.-T. Hsu, “GNSS NLOS Exclusion Based on Dynamic Object Detection Using LiDAR Point Cloud,” _IEEE Trans. Intell. Transp. Syst._ , vol. 22, no. 2, pp. 853-862, Feb, 2021.
  * [5] E. Zhang, and N. Masoud, “Increasing GPS Localization Accuracy With Reinforcement Learning,” _IEEE Trans. Intell. Transp. Syst._ , vol. 22, no. 5, pp. 2615-2626, May, 2021.
  * [6] Y. Liu, X. Fan, C. Lv et al., “An innovative information fusion method with adaptive Kalman filter for integrated INS/GPS navigation of autonomous vehicles,” _Mech. Syst. Signal Proc._ , vol. 100, pp. 605-616, Feb 1, 2018.
  * [7] H. Nourmohammadi, and J. Keighobadi, “Design and experimental evaluation of indirect centralized and direct decentralized integration scheme for low-cost INS/GNSS system,” _GPS Solut._ , vol. 22, no. 3, Jul, 2018.
  * [8] Z. Cao, D. yang, K. Jiang et al., “Monocular Connected vehicle Position Estimation on Sloping and Uneven Roads,” _IEEE Intell. Transp. Syst. Magaz._ , Early Access, 2020.
  * [9] X. Li, W. Chen, C. Chan et al., “Multi-sensor fusion methodology for enhanced land vehicle positioning,” _Inf. Fusion_ , vol. 46, pp. 51-62, Mar, 2019.
  * [10] W. Jiang, D. Liu, B. Cai et al., “A Fault-Tolerant Tightly Coupled GNSS/INS/OVS Integration Vehicle Navigation System Based on an FDP Algorithm,” _IEEE Trans. Veh. Technol._ , vol. 68, no. 7, pp. 6365-6378, Jul, 2019.
  * [11] J. K. Suhr, J. Jang, D. Min et al., “Sensor Fusion-Based Low-Cost Vehicle Localization System for Complex Urban Environments,” _IEEE Trans. Intell. Transp. Syst._ , vol. 18, no. 5, pp. 1078-1086, May, 2017.
  * [12] B. Zhu, X. Tao, J. Zhao et al., “An Integrated GNSS/UWB/DR/VMM Positioning Strategy for Intelligent Vehicles,” _IEEE Trans. Veh. Technol._ , vol. 69, no. 10, pp. 10842-10853, Oct, 2020.
  * [13] C. Li, Y. Fu, F. R. Yu et al., “Vehicle Position Correction: A Vehicular Blockchain Networks-Based GPS Error Sharing Framework,” _IEEE Trans. Intell. Transp. Syst._ , vol. 22, no. 2, pp. 898-912, Feb, 2021.
  * [14] C. Jiang and Y. A. Zhang., “Some results on linear equality constrained state filtering,” _Int. J. Control_ , vol. 86, no. 12, pp. 2115-2130, 2013.
  * [15] Y. Xu, X. Wang, and C. Jiang., “Adaptive Kalman Filter with Linear Equality Road Constraints for Autonomous Vehicle Localization,” _16th International Conference on Control, Automation, Robotics and Vision_ (ICARCV), 2020, pp. 1341-1346.
  * [16] C. Wang, H. Huang, Y. Ji et al., “Vehicle Localization at an Intersection Using a Traffic Light Map,” _IEEE Trans. Intell. Transp. Syst._ , vol. 20, no. 4, pp. 1432-1441, Apr, 2019.
  * [17] A. Welzel, P. Reisdorf, G. Wanielik et al., ”Improving Urban Vehicle Localization with Traffic Sign Recognition,” _2015 Ieee 18th International Conference on Intelligent Transportation Systems_ , IEEE International Conference on Intelligent Transportation Systems-ITSC, pp. 2728-2732, 2015.
  * [18] Qu X, Soheilian B, Paparoditis N. Vehicle localization using monocamera and georeferenced traffic signs[C] Intelligent Vehicles Symposium. IEEE, 2015.
  * [19] N. Alam, A. Kealy, and A. G. Dempster, “Cooperative Inertial Navigation for GNSS-Challenged Vehicular Environments,” _IEEE Trans. Intell. Transp. Syst._ , vol. 14, no. 3, pp. 1370-1379, Sep, 2013.
  * [20] J. Xiong, J. W. Cheong, Z. Xiong et al., “Integrity for Multi-Sensor Cooperative Positioning,” _IEEE Trans. Intell. Transp. Syst._ , vol. 22, no. 2, pp. 792-807, Feb, 2021.
  * [21] Y. Song, Y. Fu, F. R. Yu et al., “Blockchain-Enabled Internet of Vehicles With Cooperative Positioning: A Deep Neural Network Approach,” _IEEE IEEE Internet Things J._ , vol. 7, no. 4, pp. 3485-3498, Apr, 2020.
  * [22] N. Alam, A. T. Balaei, and A. G. Dempster, “An Instantaneous Lane-Level Positioning Using DSRC Carrier Frequency Offset,” _IEEE Trans. Intell. Transp. Syst._ , vol. 13, no. 4, pp. 1566-1575, Dec, 2012.
  * [23] J. Li, J. Gao, H. Zhang et al., “RSE-Assisted Lane-Level Positioning Method for a Connected Vehicle Environment,” _IEEE Trans. Intell. Transp. Syst._ , vol. 20, no. 7, pp. 2644-2656, Jul, 2019.
  * [24] L.-W. Chen, and Y.-F. Ho, “Centimeter-Grade Metropolitan Positioning for Lane-Level Intelligent Transportation Systems Based on the Internet of Vehicles,” _IEEE IEEE Trans. Ind. Inform._ , vol. 15, no. 3, pp. 1474-1485, Mar, 2019.
  * [25] A. Fascista, G. Ciccarese, A. Coluccia et al., “Angle of Arrival-Based Cooperative Positioning for Smart Vehicles,” _IEEE Trans. Intell. Transp. Syst._ , vol. 19, no. 9, pp. 2880-2892, Sep, 2018.
  * [26] G. M. Hoang, B. Denis, J. Harri et al., “Bayesian fusion of GNSS, ITS-G5 and IR-UWB data for robust cooperative vehicular localization,” _C. R. Phys._ , vol. 20, no. 3, pp. 218-227, Mar-Apr, 2019.
  * [27] F. Meyer, O. Hlinka, H. Wymeersch et al., “Distributed Localization and Tracking of Mobile Networks Including Noncooperative Objects,” _IEEE Trans. Signal Inf. Proc. Netw._ , vol. 2, no. 1, pp. 57-71, Mar, 2016.
  * [28] G. Soatti, M. Nicoli, N. Garcia et al., “Implicit Cooperative Positioning in Vehicular Networks,” _IEEE Trans. Intell. Transp. Syst._ , vol. 19, no. 12, pp. 3964-3980, Dec, 2018.
  * [29] J. Liu, B.-g. Cai, and J. Wang, “Cooperative Localization of Connected Vehicles: Integrating GNSS With DSRC Using a Robust Cubature Kalman Filter,” _IEEE Trans. Intell. Transp. Syst._ , vol. 18, no. 8, pp. 2111-2125, Aug, 2017.
  * [30] D. Wang, K. O’Keefe, M. G. Petovello et al., ”Decentralized Cooperative Navigation for Vehicle-to-Vehicle (V2V) Applications using GPS Integrated with UWB Range,” _Proceedings of the Ion 2013 Pacific Pnt Meeting_ , Proceedings of the Institute of Navigation Pacific PNT Meeting, pp. 793-803, 2013.
  * [31] P. Yang, D. Duan, C. Chen et al., “Multi-Sensor Multi-Vehicle (MSMV) Localization and Mobility Tracking for Autonomous Driving,” _IEEE Trans. Veh. Technol._ , vol. 69, no. 12, pp. 14355-14364, Dec, 2020.
  * [32] Gonzalez R , Catania C A , D Above P , et al. Model validation of an open-source framework for post-processing INS/GNSS systems[C] _International Conference on Geographical Information Systems Theory_. 2017.
  * [33] R. Gonzalez, J. I. Giribet, and H. D. Patino, “NaveGo: a simulation framework for low-cost integrated navigation systems,” _Control Eng. Appl. Inform._ , vol. 17, no. 2, pp. 110-120, Jun, 2015.