# Graph-Based Cooperative Localization Using Symmetric Measurement Equations

^{1}

^{2}

^{3}

^{*}

*Keywords:*cooperative localization; factor graphs; SME; SLAM

Next Article in Journal

Next Article in Special Issue

Next Article in Special Issue

Previous Article in Journal

Robotics and Embedded Systems, Technische Universität München, Boltzmannstraße 3, 85748 Garching bei München, Germany

School of Marine Science and Technology, Northwestern Polytechnical University, 710072 Xi’an, China

Cogsense Technologies Limited, Berkshire RG14 1QL, UK

Author to whom correspondence should be addressed.

Received: 28 April 2017
/
Revised: 13 June 2017
/
Accepted: 13 June 2017
/
Published: 17 June 2017

(This article belongs to the Special Issue Sensors, Wireless Connectivity and Systems for Autonomous Vehicles and Smart Mobility)

Precise localization is a key requirement for the success of highly assisted or autonomous vehicles. The diminishing cost of hardware has resulted in a proliferation of the number of sensors in the environment. Cooperative localization (CL) presents itself as a feasible and effective solution for localizing the ego-vehicle and its neighboring vehicles. However, one of the major challenges to fully realize the effective use of infrastructure sensors for jointly estimating the state of a vehicle in cooperative vehicle-infrastructure localization is an effective data association. In this paper, we propose a method which implements symmetric measurement equations within factor graphs in order to overcome the data association challenge with a reduced bandwidth overhead. Simulated results demonstrate the benefits of the proposed approach in comparison with our previously proposed approach of topology factors.

The large-scale proliferation of digital technology has resulted in an increasing interest in sensors in the environment. With improvements in vehicle-to-vehicle (V2V) and vehicle-to-infrastructure (V2I) technologies, these sensors are increasingly connected through real-time communication systems. Hence, the task of cooperative localization (CL) poses a feasible solution for autonomous vehicles [1].

Cooperative localization is not a new concept. Roumeliotis et. al. and Karam et al. [2,3] demonstrated the use the Kalman filter and its family algorithms to undertake CL. Other research has provided novel solutions, including maximum a posteriori estimation (MAP) [4], particle filters [5], Markov localization [6], split covariance intersection filter [7], and the random finite set framework (RFS) [8].

As a specific example, Howard et. al. [9] use maximum likelihood estimation (MLE) to achieve the CL by combining relative measurements between robots in a least square formulation. Ahmad et. al. [10] do the same but also introduces moving landmarks to provide an improved estimate. Gulati et. al. [11] formulate the CL as a graphical model, adding a topology factor to govern the inter-node distance within a system. This is implemented as a factor graph within the Georgia Tech Smoothing and Mapping (GTSAM) [12] framework.

Successful CL depends on the correct association of sensor measurements observed by different nodes. In the case where multiple targets are present, the data association must be undertaken using semantic information uniquely identifying the target, or using computationally intensive track-to-track association.

In terms of data association, various novel solutions exist to solve this problem. Fortmann et al. [13] introduced joint probabilistic data association in the early 1980s, and it is proven to be a reliable technique. It is a widely used solution and as recently as 2015 Rezatofighi et al. [14] demonstrated its derivative to solve a challenge.

Reid [15] came up with the multi-hypothesis tracking filter where each detected target is spawned into a new track. With an increase in the received measurements, the probabilities of joint hypotheses are updated recursively using all the available information. The newly created branches are then updated and the dead branches are removed based on these probabilities. Streit et. al. [16] proposed a probabilistic multi-hypothesis tracking algorithm and Giannopoulos et. al. [17] extended it for the multi-sensor scenarios.

In addition to the concepts discussed above, further techniques exist for estimating the measurement-to-track association as part of the state estimation process. Mahler successfully addressed the issue by extending the constant-gain Kalman filter for the multi-target environments using the probability hypothesis density (PHD) filter [18]. Kamen [19] proposed an symmetric measurement equation (SME) filter for multiple target tracking based on symmetric measurement equations. Zhang et. al. [20] further use the SME Filter and topology to perform the CL.

However, within the frameworks presented by most of the techniques mentioned above, the computational complexity increases exponentially with the number of targets (and hence becomes a non-linear system). Many of the state-of-the-art approaches fuse the measurements from an external sensor to get the final state estimates for the CL, performing both data association and coordinate transformation. In our previous work [11] we successfully avoid the task of data association and coordinate transformation by adding the sum of inter-vehicular distances as constraints between the various vehicles for each state.

The concept in [11] can be better understood by an example system of three vehicles, ${V}_{1},{V}_{2}$ and ${V}_{3}$, in the field of view of an external sensor like RADAR. If the inter-vehicle distance between ${V}_{i}$ and ${V}_{j}$ is ${d}_{ij}$, then ${d}_{12}$, ${d}_{23}$ and ${d}_{31}$ are the possible inter-vehicle distances for the system. Important properties of the distance ${d}_{ij}$ are: (1) it is a scalar value, independent of the coordinate system of the censor; and (2) the sum of all the ${d}_{ij}$ is independent of the track-to-measurement association. Now at any given state we can form a geometric topology by connecting all the participating vehicles. Therefore, to address the challenge of data association and coordinate transformation, we convert the original measurement into the constraint of a topology factor consisting of sum of all the inter-vehicular distances. Our proposed solution performs better than the optimal Kalman filter and simultaneously scales optimally. However, formulating the topology factor results in a loss of information. This is because the topology factor converts any two participating vehicles’ coordinate positions into an inter-vehicle distance. Therefore we have lost the actual location, as the resulting scalar value has no position information.

In this paper we propose the use of the SME constraint factor, which adds full information into a factor graph to perform CL for semi or fully autonomous vehicles That is, it uses the measurements without calculating the difference, in a factor graph to perform CL for semi or fully autonomous vehicles. Our simulation indicates that the new SME factor performs better than the topology factor. In the process we address the challenge of data association and do not require an expensive data association solution.

The rest of this paper is organized as follows. Section 2 defines the problem with its constraints. Section 3 gives the overview of the SME, which is followed by explanation of factor graphs and non-linear least square optimization methods in Section 4. Section 5 presents the evaluation and Section 6 concludes the paper.

A typical scenario is shown in Figure 1a, highlighting the vehicle-infrastructure CL. The assumptions are as follows:

- Each vehicle has an odometry and a Global Positioning System (GPS) sensor to localize itself in an absolute reference and can broadcasts its measurements.
- The infrastructure sensor that can derive the global position of all the vehicles in its field of view, but cannot uniquely identify the vehicles. This introduces a challenge from the perspective of data association. A typical example of such sensor is a RADAR. RADAR has been extensively used by the military for surveillance [21]. Since the mid 1990s, it has been researched as an active [22] or passive [23] component of the Intelligent Vehicle Highway System (IVHS). Recently however, because of lowering of costs, it has gained a lot of traction as an infrastructure sensor for smart highways [24,25,26,27].
- The vehicles and the infrastructure sensor can communicate in both directions without any timing delay or data error.
- The environment has no clutter and there are no missed detections.

The the goal of vehicle-infrastructure CL is to improve the precision of position estimates of the participating vehicles. Our goal of CL is achieved by fusing the measurements from internal sensors (Assumption 1) with the measurements from an external sensor (Assumption 2). To address the data association challenge from the external sensor (Assumption 2) we use symmetric measurement equations (SMEs). The concepts of SMEs are introduced in Section 3. To represent the problem of CL we use a graph-based Simultaneous Localization and Mapping (SLAM) approach, implemented within factor graphs and use non-linear least square optimization to optimize the graph for generating the posterior state estimates. The concepts of factor graphs and non-linear least square optimization are introduced in the Section 3.

As mentioned in the problem description, SMEs are primarily used to avoid the data association for the external sensor. This is possible because we use SMEs to generate ’pseudo-measurements’ using symmetric functions from the original measurements [19]. This is achieved by adding or multiplying the original sensor measurements to generate new measurements having values from all the targets. The resulting measurements are independent of the targets, thereby avoiding a computationally expensive data-association step.

The above can be understood by considering the case for two targets in one dimension. A sensor results in two measurements, ${m}_{1}$ and ${m}_{2}$. Without data association, it is not possible to associate the measurements with any of the targets. However, a pseudo-measurement $p{m}_{1}$ can be generated by adding the original sensor measurements as [${m}_{1}+{m}_{2}$]. Another pseudo-measurement $p{m}_{2}$ can be generated by adding the values resulting from squares of the original sensor measurements as [${m}_{1}^{2}+{m}_{2}^{2}$]. Now these two new sets of measurements [$p{m}_{1}$, $p{m}_{2}$] are called sum-of-powers. The second pseudo-measurement $p{m}_{2}^{\prime}$ can also be generated by multiplying the original sensor measurements as [${m}_{1}\xb7{m}_{2}$]. These two new sets of measurements [$p{m}_{1}$, $p{m}_{2}^{\prime}$] are called the sum-of-product.

Similarly following are the two SME representations of the three targets in one dimension:

- Sum-of-powers:$${S}_{pow}=\left[\begin{array}{c}{m}_{1}+{m}_{2}+{m}_{3}\\ {m}_{1}^{2}+{m}_{2}^{2}+{m}_{3}^{2}\\ {m}_{1}^{3}+{m}_{2}^{3}+{m}_{3}^{3}\end{array}\right]$$
- Sum-of-product:$${S}_{prod}=\left[\begin{array}{c}{m}_{1}+{m}_{2}+{m}_{3}\\ {m}_{1}{m}_{2}+{m}_{2}{m}_{3}+{m}_{1}{m}_{3}\\ {m}_{1}{m}_{2}{m}_{3}\end{array}\right]$$

where ${m}_{i}$ is the measurement from the $ith$ target.

As can be seen from the above examples, the original participating measurements result in the same number of ’pseudo-measurements’ which are independent of the original measurements. However, this also increases the non-linearity of the system as the new equations are now of the $nth$ degree for n targets, square for the two- and cubic for the three-target system. Thus, we trade one challenging problem of ’data association’ with another of non-linearity. The next section describes the concept of ’factor graphs’ which we use to optimize the joint probability function and address the traded in challenge.

Factor graphs are used as a framework for addressing the problem described in Section 2. They are a graphical notation and lend themselves to logically represent the problem herein. After formulating as graphs we use a non-linear least square optimization to extract the estimated state vectors. In this section we provide brief overview of these methodologies and how we use them to achieve our goal.

A factor graph is a bipartite graph ${G}_{k}=({F}_{k},{V}_{k},{E}_{k})$ with two types of nodes: factor nodes ${f}_{i}\in {F}_{k}$ and variable nodes ${v}_{j}\in {V}_{k}$. Edges ${e}_{ij}\in {E}_{k}$ can exist only between factor nodes and variable nodes, and are present if and only if the factor ${f}_{i}$ involves a variable ${v}_{j}$ [28]. They can also represent the probabilistic graphical models (PGM) and are used to implement Bayesian networks [29] and Markov random fields [30].

Figure 1b is an example of a factor graph with variables $w,x,y$ and z, and functions ${f}_{1}$ and ${f}_{2}$ with factorization: $f(w,x,y,z)={f}_{1}(w,x,y)\ast {f}_{2}(y,z)$. Using PGM, the example can also be represented as $P(w,x,y,z)=P(w,x,y)\ast P(y,z)$. Similarly we use the factorized probability distribution to represent the entire trajectories of all the participating vehicles as an optimization problem. The localization can then be represented by estimating the trajectory $\mathbf{x}=\{{x}_{i}|i\in 0,\dots ,n\}$, for a given set of measurements from various sensors. For example, if we have three different sensors, odometry, sensor 1, and sensor 2, then the measurements from the chosen sensors can be written as (here we choose only odometry as known sensor since we want to calculate the trajectory; others could include any form of sensors such as GPS, RADAR, LIDAR, camera):

- from the odometry sensor, $\mathbf{u}=\{{u}_{i}|i\in 0,\dots ,n\}$
- from the generic sensor 1, ${\mathbf{z}}^{1}=\{{z}_{i}^{1}|i\in 0,\dots ,n\}$
- from the generic sensor 2, ${\mathbf{z}}^{2}=\{{z}_{i}^{2}|i\in 0,\dots ,n\}$

Thus, the joint density for the data obtained from the above three sensors can be represented as:
where ${z}_{k}\in \{{\mathbf{z}}^{1},{\mathbf{z}}^{2}\}$ denotes the measurement, originating from either sensor 1 or sensor 2.

$$P(\mathbf{x},{\mathbf{z}}^{1},{\mathbf{z}}^{2},\mathbf{u})\propto P({x}_{0})\prod _{i}^{n}P({x}_{i+1}|{x}_{i},{u}_{i})\prod _{k}^{m}P({z}_{k}|{x}_{ik})$$

Now the factorization is undertaken based on Gaussian distributions for the process and measurement models as:
where h and f denote the measurement and process models, and v and w are the corresponding noises with covariance matrices ${\Sigma}_{k}$ and ${\Gamma}_{i}$.

$$\begin{array}{c}\hfill {x}_{i}={f}_{i}({x}_{i-1},{u}_{i})-{w}_{i}\iff P({x}_{i+1}|{x}_{i},{u}_{i})\propto \mathrm{exp}(-\frac{1}{2}||{f}_{i}({x}_{i-1}-{x}_{i}{||}_{{\Gamma}_{i}}^{2})\end{array}$$

$$\begin{array}{c}\hfill {z}_{k}={h}_{k}({x}_{ik})-{v}_{k}\iff P({z}_{k}|{x}_{ik})\propto \mathrm{exp}(-\frac{1}{2}||{h}_{k}({x}_{ik})-{z}_{k}{||}_{{\Sigma}_{k}}^{2})\end{array}$$

In this paper, the goal is to calculate the maximum likelihood estimate (MLE) by using the non-linear least square method:

$$\begin{array}{c}\overline{\theta}=\mathrm{argmax}P(\theta |\mathbf{z},\mathbf{u})=\mathrm{argmin}\{{\displaystyle \sum _{i=1}^{n}}||{f}_{i}({x}_{i-1}-{u}_{i})-{x}_{i}{||}_{{\Gamma}_{i}}^{2}+{\displaystyle \sum _{k=1}^{m}}||{h}_{k}({x}_{ik})-{z}_{k}{||}_{{\Sigma}_{k}}^{2}\}\hfill \end{array}$$

The above method can be extended for any number and type of sensors. In terms of factor graphs, factors are a synonym for this measurement model. For a Gaussian noise model, the corresponding measurement factor for any $Sensor-S$ from Equation (5) can be written as:
where ${h}_{k}$ is the measurement model as the function of state variable ${X}_{k}$, ${z}_{k}$ is the measurement from the sensor and operator $d(\xb7)$ represents the cost function.

$$\begin{array}{c}\hfill {f}^{Sensor-S}({X}_{k})=d[{h}_{k}({X}_{k})-{z}_{k}]\end{array}$$

The first assumption for the problem description in Section 2 is the availability of odometry and GPS measurements directly from the participating vehicles. The second assumption is the availability of infrastructure sensor measurements, provided naively without any knowledge of the relatve correlation or association. As explained in Section 2, we make use of SMEs on the measurements obtained from the external sensor to solve the problem of data association. As explained above, various sensor measurements are represented as factors in a factor graph. Therefore, let us take a look at the formulations of various measurements as factors for the factor graph.

For a constant velocity model, the measurement equation for an odometry factor is given by:
where ${h}^{o}$ is the function to calculate the odometry measurement at time t, ${z}_{t}^{o}$ is the measurement at time t and ${n}^{o}$ is the measurement noise. The error function of the binary factor ${f}^{ODOM}$ between the states ${X}_{t},{X}_{t-1}$ is:

$${z}_{t}^{o}={h}^{o}({z}_{t-1}^{o})+{n}^{o}$$

$${f}^{ODOM}({X}_{t},{X}_{t-1})\triangleq d({z}_{t}^{o}-{h}^{o}({z}_{t-1}^{o}))$$

The odometry measurement from the sensor is used directly, hence the noise model provided by the sensor manufacture in the form of covariance matrices is used while formulating the corresponding factors.

The GPS measurement equation is:
where ${n}^{g}$ is the measurement noise, and ${h}^{g}$ is the measurement function, providing the relation between the measurement ${z}_{t}^{g}$ and the position of the vehicle ${z}_{t}$ at time t. Equation (10) gives an unary factor ${f}^{GPS}$ for a state ${X}_{t}$, which is written as:

$${z}_{t}^{g}={h}^{g}({z}_{t})+{n}^{g},$$

$${f}^{GPS}({X}_{t})\triangleq d({z}_{t}^{g}-{h}^{g}({z}_{t}))$$

The GPS measurement from the sensor is used directly, hence the noise model provided by the sensor manufacture in the form of a covariance matrix is used while formulating the corresponding factors.

To perform CL, the node running the fusion receives the following:

- The odometry and GPS measurements from all/other vehicles;
- Absolute positions, in global coordinates, of all vehicles in the field of view of a configured RADAR. Here by configuration of RADAR we imply that it knows its position in global coordinates and hence is able to perform a coordinate transformation of the measurements of the detected targets in its local coordinates to the global coordinates.

As the RADAR does not perform any data association, it will not associate the calculated positions to the individual vehicles. To incorporate such information in the factor graph, we construct the SME factor. Using (1) of the SME transformation, we convert the received values from RADAR for n vehicles at time t, as:
where ${x}_{i}$ is the absolute position received from RADAR in the x dimension for the $ith$ vehicle, ${h}^{{s}_{x}}$ is the new measurement function and ${n}^{sme}$ is the measurement noise. Similarly y can be written as:
where ${y}_{i}$ is the absolute position received from RADAR in the y dimension for the $ith$ vehicle. ${h}^{{s}_{y}}$ is the new measurement function and ${n}^{sme}$ is the measurement noise.

$${z}_{t}^{{s}_{x}}=\left[\begin{array}{c}{\sum}_{i=1}^{n}{x}_{i}\\ {\sum}_{i=1}^{n}{x}_{i}^{2}\\ \cdots \\ {\sum}_{i=1}^{n}{x}_{i}^{n}\end{array}\right]+{n}^{sme}={h}^{{s}_{x}}({x}_{0},\cdots ,{x}_{n})+{n}^{sme}$$

$${z}_{t}^{{s}_{y}}={h}^{{s}_{y}}({y}_{0},\cdots ,{y}_{n})+{n}^{sme}$$

Equations (12) and (13) result in the following N-ary factor:
Figure 2 shows factor graphs with different kinds of factors.

$$\begin{array}{c}\hfill {f}^{SME}({({X}_{0},\cdots ,{X}_{N})}_{t})\triangleq d({z}_{t}^{s}-{h}^{s}({z}_{0},\cdots ,{z}_{N}))\end{array}$$

The SME measurement can be considered a pseudo measurement, hence we must also calculate a covariance matrix for it.

If ${\sigma}_{x}^{2}$ and ${\sigma}_{y}^{2}$ are the x and y variances, respectively, for the RADAR, then we have:

$$Cov(x,y)=\mathrm{diag}\left[\begin{array}{c}{\sigma}_{{x}_{1}}^{2},\cdots ,{\sigma}_{{x}_{n}}^{2},{\sigma}_{{y}_{1}}^{2},\cdots ,{\sigma}_{{y}_{n}}^{2}\end{array}\right]$$

We follow [31] where the covariance for the new SME measurement can be defined as:
where M is a $1X2N$ matrix as follows:

$${\sigma}_{sm{e}_{x,y}}^{2}=M\xb7Cov(x,y)\xb7{M}^{T}$$

$$M=\left[\begin{array}{c}\frac{\partial}{\partial {x}_{1}}({z}_{t}^{s}),\cdots ,\frac{\partial}{\partial {x}_{n}}({z}_{t}^{s}),\frac{\partial}{\partial {y}_{1}}({z}_{t}^{s}),\cdots ,\frac{\partial}{\partial {y}_{n}}({z}_{t}^{s})\end{array}\right]$$

The non-linear problem formulated using the factor graph is solved using the Levenberg Marquardt linearization algorithm. This is a Gauss–Newton style non-linear optimizer. Using an initial estimate ${x}_{0}$ it iteratively finds an update $\Delta $ from the linearized system:
where $J({x}_{0})$ is the sparse Jacobian matrix at the current linearization point ${x}_{0}$ and $b({x}_{0})=f({x}_{0})-z$ is the residual for given the measurement z. The Jacobian matrix is equivalent to a linearized version of the factor graph, and its block structure reflects the structure of the factor graph. After solving (18), the linearization point is updated to the new estimate ${x}_{0}+\Delta $. Further details on this process are presented within [32].

$$\underset{\Delta}{arg\; min}J({x}_{0})\Delta -b({x}_{0})$$

As seen in Equation (18) we need to calculate the Jacobians to obtain the optimal state. The Jacobian for the odometry is calculated from the Equation (8). Now, the measurement ${z}_{t}^{o}$ in the equation represents x and y position in a $2-D$ plane. As such, Equation (8) becomes,

$${(x,y)}_{t}^{o}={h}^{o}({(x,y)}_{t-1}^{o})+{n}^{o}$$

The Jacobian for ${h}^{o}(x,y)$ (constant velocity is a linear function) is obtained with $\partial x$ and $\partial y$ as:

$${J}^{O}=\frac{\partial ({h}^{o}({z}^{o})+{n}^{o})}{\partial x\partial y}=\frac{\partial ({h}^{o}(x,y))}{\partial x\partial y}=\left[\begin{array}{cc}\frac{\partial ({h}^{o}(x,y))}{\partial x}& 0\\ 0& \frac{\partial ({h}^{o}(x,y))}{\partial y}\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$$

Similarly the Jacobian used for the GPS factor can be calculated from Equation (10) and is same as that of the odometry.

From Equations (12) and (14), it can be seen that for the N participating vehicles, there will be a total of N factors between the state nodes. The Jacobian for each of them (${J}_{1}^{S},\cdots ,{J}_{N}^{S}$) can be written as:

$${J}_{1}^{S}=\left[\begin{array}{cc}\frac{\partial ({\sum}_{i=1}^{N}({x}_{i}))}{\partial x}& 0\\ 0& \frac{\partial ({\sum}_{i=1}^{N}({y}_{i}))}{\partial y}\end{array}\right],{J}_{2}^{S}=\left[\begin{array}{cc}\frac{\partial ({\sum}_{i=1}^{N}({x}_{i}^{2}))}{\partial x}& 0\\ 0& \frac{\partial ({\sum}_{i=1}^{N}({y}_{i}^{2}))}{\partial y}\end{array}\right],\cdots ,{J}_{N}^{S}=\left[\begin{array}{cc}\frac{\partial ({\sum}_{i=1}^{N}({x}_{i}^{N}))}{\partial x}& 0\\ 0& \frac{\partial ({\sum}_{i=1}^{N}({y}_{i}^{N}))}{\partial y}\end{array}\right]$$

This can be generalized as:

$${J}_{p}^{S}=\left[\begin{array}{cc}\frac{\partial ({\sum}_{i=1}^{N}({x}_{i}^{p}))}{\partial x}& 0\\ 0& \frac{\partial ({\sum}_{i=1}^{N}({y}_{i}^{p}))}{\partial y}\end{array}\right]\forall p=1\cdots N$$

To address the primary challenge of the data association or the track-to-measurement association, the data from internal sensors from multiple vehicles and external infrastructure sensor is naively shared between various participating nodes and the fusion node. Unlike the other methods the proposed methodology does not require the corresponding covariances to be exchanged. Based on these requirements, we define the following scenario sets for an evaluation:

- Two vehicles with random trajectories on a ground plane with an infrastructure RADAR.
- Three vehicles with a constant turn model on a ground plane with an infrastructure RADAR.
- An intersection with five vehicles with an infrastructure RADAR mounted at the center of intersection (Figure 3). We assume the infrastructure RADAR has an equal field of view for all the four directions. This test uses the trajectories with a constant velocity model.
- Monte Carlo simulations for 1000 iterations for 2, 3 and 4 vehicles. This test also reflects the trajectories with a constant velocity model.

We simulate each of the sets for the 200 steps. The above sets are further divided into one with and one without GPS sSensor measurements. Georgia Tech Smoothing and Mapping (GTSAM) [12] is used to implement the factor graphs.

Simulated vehicles provide their own odometry measurements and the location in global coordinates. Infrastructure RADAR provides global coordinates for the vehicles in its field of view without performing any data association. The covariances are assumed as $diag\phantom{\rule{0.166667em}{0ex}}[1.0,1.0]$, $diag\phantom{\rule{0.166667em}{0ex}}[10.0,10.0]$ and $diag\phantom{\rule{0.166667em}{0ex}}[0.5,0.5]$ for odometry, GPS and RADAR, respectively.

Results from the simulation of the two sets are compared three ways, between:

- the fused trajectory only using odometry;
- the fused trajectory for odometry and topology factor [11]; and
- the fused trajectory for the odometry and SME factor (proposed in this work).

As stated before, the experiments are performed with and without a GPS factor (constructed from GPS measurements), therefore a similar three-way comparison is performed with GPS measurements too.

The performance is measured by calculating root-mean-square error (RMSE) for the complete system. The total error is the sum of the RMSE of each vehicle for n steps:

$$Error=\sqrt{\frac{{\sum}_{j=1}^{n}{\sum}_{i=1}^{2}{[{({x}_{{i}_{est}}-{x}_{{i}_{real}})}^{2}+{({y}_{{i}_{est}}-{y}_{{i}_{real}})}^{2}]}^{j}}{n}}$$

Figure 4a shows the results from the first set for two vehicles, each with a random trajectory and without GPS measurements. Figure 4b shows the corresponding total system RMSE. It can be seen that the trajectories generated by using the SME filter are closer to the ground truth. Also, it can be seen that the total system RMSE with SME factors is far superior than the other two. In fact, only odometry performs better than the topology factor. This is because the topology factor adds only relative inter-vehicle constraints and does not consider the true position. As the RADAR has a lower error covariance, the topology measurements resulting from it have a higher weight and forces the graph to converge to an incorrect solution. Hence, this results in a negative performance. On the other hand, the SME factor incorporates full information as it uses the full global coordinates obtained from the RADAR and hence gives better performance.

Figure 5a also includes the GPS measurement for the same set of trajectories as of Figure 4a. Figure 5b shows the corresponding RMSE. Here after fusing of the GPS sensor information, the topology factor performs better than the one with only odometry and GPS. This happens because GPS information forces the topology to converge to the true position. However, the GPS information further improves the results from the SME factor and its performance remains superior than for the other two.

Next, the effectiveness of the solution can be seen from the second scenario of three vehicles using a constant turn model. Figure 6a shows the results without the GPS sensor. These are similar to the Figure 4a. The trajectory generated using the SME filter is closer to the ground truth. Due to the nearness of the trajectories of the three vehicles in the system, the result can be better analyzed from the corresponding RMSE values, as highlighted in Figure 6b. The RMSE also shows similar performance to that in Figure 4b. Figure 7a shows results with the GPS sensor information and Figure 7b shows the corresponding complete system RMSE. Both the trajectories and the RMSE in Figure 7 show similar characteristics to those of Figure 5.

To further evaluate and demonstrate the scalability of the proposed solution we simulate the third scenario of an intersection as modeled in Figure 3. Here the vehicles travel with a constant velocity. Figure 8a shows the results without the GPS sensor. Again it can be seen that as in Figure 4a, trajectories resulting from the use of an SME factor are closer to the ground truth. Due to a large number of trajectories in the system, the result can be better analyzed from the corresponding RMSE values in Figure 8b. The RMSE also shows superior performance of the SME factor to that of Figure 4b. Figure 9a shows results with the GPS sensor included and Figure 9b shows the corresponding complete system RMSE. Again these show that the SME factor outperforms the other two as seen in Figure 5 and Figure 7.

Lastly we evaluate the stability of the proposed solution by running a Monte Carlo simulation of 1000 iterations for a system with 2 , 3 and 4 vehicle trajectories. We analyze the results by calculating the average RMSE of 1000 iterations for each of the system as shown in Table 1.

Figure 10a–f shows one set of trajectories of the 1000 iterations performed as part of Monte Carlo simulation. It can be observed that the trajectories with an SME factor for 2-, 3- and 4-vehicle systems are closer to the ground truth than the other two. Table 1 shows clear reduction of RMSE error in localization with an addition of the SME factor. However, it also highlights an anomaly for the two-vehicle system, when the SME factor without GPS performs a little better than the one with GPS. However, the difference is very small and rest of the results show considerable improvements with the GPS factor.

All the previous tests show the validity and stability of the proposed solution of SME factors. However, it has been demonstrated using an offline batch Levenberg Marquardt optimizer. We optimized the full graph and assumed that the measurements from all the sensors were available all the time. However, in a real scenario, all the measurements may not be available all the time (for example from the external sensors). They may be only available at run time and for only a small duration (when the vehicles are in the field of view of the external sensor). In that case, the system should be able to automatically incorporate such measurements to calculate the final state estimates. Such scenarios can also be classified as ’plug and play’. The ’plug’ phase adds the new, previously unseen, measurements at the run time when they are available. The ’play’ phase fuses the newly added measurements and results in a better state estimate of the participating vehicles. When the measurements are no longer available the system automatically uses the remaining available measurements.

In such real scenario an online incremental smoothing algorithm like iSAM2 [33] (supported in GTSAM) is required. Further, the GTSAM framework supports the above described plug and play feature [34] thereby providing a efficient platform for such scenarios.

As we have seen, the SME factor is also quite efficient when GPS is unavailable. Hence, it can be used in scenarios like underground parking lots and tunnels when there is no line of sight to the satellites. Also, in scenarios like urban canyons where a reduced number of satellites are available in direct line of sight, addition of the SME factor would improve the CL. This is because the SME factor is formulated from the measurements of a precise infrastructure sensor and hence has a lower error covariance. Therefore, the SME factor has a higher weight during smoothing and arrives at a more precise solution.

In this section we demonstrate a simple scenario of two vehicles on a ground plane for 200 steps. For the first 50 steps the vehicle is assumed to have GPS measurements. At the 51st step it enters a tunnel and loses the GPS connection. At the 151st step it exits the tunnel and GPS is restored without any delay. The tunnel has an infrastructure RADAR which is configured to provide the global coordinates of the vehicles in its field of view. We assume RADAR does not perform any data association.

Figure 11a shows the trajectories of the two vehicles. For the case when the tunnel does not have a RADAR sensor, and hence no SME factor, we can see the fused trajectories of the vehicles drift farther from the ground truth after they enter the tunnel. However, when the tunnel is equipped with the infrastructure RADAR and the SME factor is added as the constraint between the trajectories when the vehicle is inside the tunnel, the fused trajectories remain closer to the ground truth. The trajectory resulting from the SME factor is shown in the red color. For the scenarios with and without the RADAR, the first and last 50 steps have odometry and GPS as factors. Inside the tunnel, without the RADAR, only odometry factors are added to the graph. However, with RADAR, odometry and SME factors are added in the graph. This additional information in form of the SME factor helps in convergence to the true position.

The result can also be analyzed from the RMSE of the total system shown in Figure 11b. Through to the 50th time step, both the systems have the same RMSE values. Starting at 51st time step the RMSE error increases for the system which does not have the infrastructure RADAR. However, for the system with the infrastructure RADAR the RMSE falls as RADAR has a lower error covariance.

In our simulation experiments, original measurements from the RADAR have been used, hence data association is avoided. Also, the original measurements are used to construct the new SME measurements, therefore no information is lost during transformation. Furthermore, the use of factor graph helps in addressing the non-linearity of the systems efficiently. Hence, it scales well with the degree of equations for SME factors, which directly depend on the number of vehicles in the field of view of the RADAR. To perform CL, traditional distributed Gaussian-based fusion methods using Kalman filter, extended Kalman filter, PHD filter [2,3,18], etc. require states and covariances to be sent on the network with each time step to the node performing the fusion. However, for the proposed method only measurements are sent, keeping the bandwidth requirements to minimum.

We successfully avoided the measurement to track association issues in RADAR measurements, albeit with the assumption of a clutter-free environment. In practice this is not the case. Incorrect values in the graph degrade the optimization [35]. Therefore, clutter for the factor graphs can be handled at two levels, either actively at the RADAR [36,37,38], or later during the optimization of factor graphs [39,40]. These ideas can be further explored to tackle the challenges of the clutter.

Although the SME factor performs better than the topology factor, using topology factor we are also able to avoid coordinate transformations. Since we need the absolute position to construct the SME factor, we also require coordinate transformations. Static infrastructure sensors are generally configured only once and hence the coordinate transformation is only a fixed time process.

In this paper we present a novel approach to the challenge of vehicle-infrastructure cooperative localization using a combination of factor graphs and symmetric measurement equations. Specifically, these techniques address the challenges of track-to-measurement association, bandwidth expense and computational scalability which are usually associated with this task. The work presented herein provides improved accuracy over our previous work which added a unique topology factor to the factor graph. However, it should be noted that this work is unable to resolve the coordinate transformation issue which is addressed using the topology factor.

The simulations show that even in the absence of GPS measurements (like in tunnels, urban canyons and underground parking lots), the SME factor remains robust and performs better than the topology factor. The plug and play framework allows the sensor measurements to be incorporated online from sensors outside the ego vehicle. With the use of the factor graphs, the rising non-linearity of the system with an increase in the number of the agents is adequately addressed and the use of the incremental smoothing algorithms ensures that the system remains computationally efficient.

The solution proposed herein has the potential to address some of the major challenges for highly assisted and autonomous vehicles. Specifically, this work holds significant potential in applications where vehicles cooperate with external agents, such as sensors on other vehicles and sensors mounted within the infrastructure in order to improve their localization accuracy. Further research should consider how different sensors such as inertial and wheel encoders can be incorporated into the cooperative localization model, and how external targets can be localized and tracked within this framework. Finally, this work has not yet considered some real-world issues such as clutter, target birth/death and missed detections, which should be investigated in any future work which seeks to test these techniques on real world data.

This work was supported by the German Research Foundation (DFG) and the Technical University of Munich (TUM) in the framework of the Open Access Publishing Program.

Dhiraj Gulati and Feihu Zhang proposed the idea. Dhiraj Gulati performed the experiments. Dhiraj Gulati, Feihu Zhang and Daniel Clarke prepared the manuscript. Daniel Clarke and Alois Knoll provided the supervision and direction of the work.

The authors declare no conflict of interest.

- Kurazume, R.; Nagata, S.; Hirose, S. Cooperative positioning with multiple robots. In Proceedings of the 1994 IEEE International Conference on Robotics and Automation, San Diego, CA, USA, 8–13 May 1994; Volume 2, pp. 1250–1257. [Google Scholar]
- Roumeliotis, S.I.; Bekey, G.A. Distributed multirobot localization. IEEE Trans. Robot. Autom.
**2002**, 18, 781–795. [Google Scholar] [CrossRef] - Karam, N.; Chausse, F.; Aufrere, R.; Chapuis, R. Cooperative multi-vehicle localization. In Proceedings of the 2006 IEEE Intelligent Vehicles Symposium, Tokyo, Japan, 13–15 June 2006; pp. 564–570. [Google Scholar]
- Nerurkar, E.D.; Roumeliotis, S.I.; Martinelli, A. Distributed maximum a posteriori estimation for multi-robot cooperative localization. In Proceedings of the IEEE International Conference on Robotics and Automation, Kobe, Japan, 12–17 May 2009; pp. 1402–1409. [Google Scholar]
- Rekleitis, I.M.; Dudek, G.; Milios, E.E. Multi-robot cooperative localization: A study of trade-offs between efficiency and accuracy. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Lausanne, Switzerland, 30 September–4 October 2002; Volume 3, pp. 2690–2695. [Google Scholar]
- Fox, D.; Burgard, W.; Kruppa, H.; Thrun, S. A probabilistic approach to collaborative multi-robot localization. Auton. Robot.
**2000**, 8, 325–344. [Google Scholar] [CrossRef] - Li, H.; Nashashibi, F. Cooperative Multi-Vehicle Localization Using Split Covariance Intersection Filter. IEEE Intell. Transp. Syst. Mag.
**2013**, 5, 33–44. [Google Scholar] [CrossRef] - Zhang, F.; Stähle, H.; Chen, G.; Buckl, C.; Knoll, A. Multiple vehicle cooperative localization under random finite set framework. In Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo, Japan, 3–7 November 2013; pp. 1405–1411. [Google Scholar]
- Howard, A.; Matark, M.J.; Sukhatme, G.S. Localization for mobile robot teams using maximum likelihood estimation. In Proceedings of the 2002 IEEE/RSJ International Conference on Intelligent Robots and Systems, Lausanne, Switzerland, 30 September–4 October 2002; Volume 1, pp. 434–439. [Google Scholar]
- Ahmad, A.; Tipaldi, G.D.; Lima, P.; Burgard, W. Cooperative robot localization and target tracking based on least squares minimization. In Proceedings of the 2013 IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, 6–10 May 2013; pp. 5696–5701. [Google Scholar]
- Gulati, D.; Zhang, F.; Clarke, D.; Knoll, A. Vehicle infrastructure cooperative localization using Factor Graphs. In Proceedings of the 2016 IEEE Intelligent Vehicles Symposium (IV), Gothenburg, Sweden, 19–22 June 2016; pp. 1085–1090. [Google Scholar]
- GTSAM, Georgia Tech Smoothing and Mapping. Available online: https://collab.cc.gatech.edu/borg/gtsam/ (accessed on 16 June 2017).
- Fortmann, T.E.; Bar-Shalom, Y.; Scheffe, M. Multi-target tracking using joint probabilistic data association. In Proceedings of the 1980 19th IEEE Conference on Decision and Control including the Symposium on Adaptive Processes, Albuquerque, NM, USA, 10–12 December 1980; pp. 807–812. [Google Scholar]
- Rezatofighi, S.H.; Milan, A.; Zhang, Z.; Shi, Q.; Dick, A.; Reid, I. Joint Probabilistic Data Association Revisited. In Proceedings of the 2015 IEEE International Conference on Computer Vision, Santiago, Chile, 13–16 December 2015; pp. 3047–3055. [Google Scholar]
- Reid, D. An algorithm for tracking multiple targets. IEEE Trans. Autom. Control
**1979**, 24, 843–854. [Google Scholar] [CrossRef] - Streit, R.L.; Luginbuhl, T.E. A probabilistic multi-hypothesis tracking algorithm without enumeration and pruning. In Proceedings of the Sixth Joint Service Data Fusion Symposium, Maryland, USA, 14–18 June 1993; pp. 1015–1024. [Google Scholar]
- Giannopoulos, E.; Streit, R.; Swaszek, P. Probabilistic multi-hypothesis tracking in a multi-sensor, multi-target environment. In Proceedings of the First Australian Data Fusion Symposium, Adelaide, Australia, 21–22 November 1996; pp. 184–189. [Google Scholar]
- Mahler, R.P.S. Multitarget Bayes filtering via first-order multitarget moments. IEEE Trans. Aerosp. Electron. Syst.
**2003**, 39, 1152–1178. [Google Scholar] [CrossRef] - Kamen, E.W. Multiple target tracking based on symmetric measurement equations. IEEE Trans. Autom. Control
**1992**, 37, 371–374. [Google Scholar] [CrossRef] - Zhang, F.; Hinz, G.; Gulati, D.; Clarke, D.; Knoll, A. Cooperative Vehicle-Infrastructure Localization Based on the Symmetric Measurement Equation Filter. Geoinformatica
**2016**, 20, 159–178. [Google Scholar] [CrossRef] - Skolnik, M.I. Introduction to Radar Systems; Mc Graw-Hill: New York, NY, USA, 2001. [Google Scholar]
- Higgins, J. Doppler Radar System for Automotive Vehicles. U.S. Patent 5,481,268, 2 January 1996. [Google Scholar]
- Akutsu, E. Vehicle Position Detection System. U.S. Patent 6,081,187, 27 June 2000. [Google Scholar]
- Digital Test Field. Available online: www.siemens.com/presse/digitales-testfeld-a9 (accessed on 16 June 2017).
- Tang, X.; Gao, F.; Xu, G.; Ding, N.; Cai, Y.; Ma, M.; Liu, J. Sensor systems for vehicle environment perception in a highway intelligent space system. Sensors
**2014**, 14, 8513–8527. [Google Scholar] [CrossRef] [PubMed] - Abdel-Aty, M.; Oloufa, A.; Peng, Y.; Shen, T.; Yang, X.; Lee, J.; Copley, R.; Ismail, A.; Eady, F.; Lalchan, R.; et al. Real Time Monitoring and Prediction of Reduced Visibility Events on Florida’s Highways; Report, BDV24 962-01; University of Central Florida: Orlando, FL, USA, 2014. [Google Scholar]
- Saponara, S.; Neri, B. Radar Sensor Signal Acquisition and Multidimensional FFT Processing for Surveillance Applications in Transport Systems. IEEE Trans. Instrum. Meas.
**2017**, 66, 604–615. [Google Scholar] [CrossRef] - Kschischang, F.R.; Frey, B.J.; Loeliger, H.A. Factor graphs and the sum-product algorithm. IEEE Trans. Inf. Theory
**2001**, 47, 498–519. [Google Scholar] [CrossRef] - Loeliger, H.A. An introduction to factor graphs. IEEE Signal Process. Mag.
**2004**, 21, 28–41. [Google Scholar] [CrossRef] - Indelman, V.; Williams, S.; Kaess, M.; Dellaert, F. Information fusion in navigation systems via factor graph based incremental smoothing. Robot. Auton. Syst.
**2013**, 61, 721–738. [Google Scholar] [CrossRef] - Arras, K.O. An Introduction to Error Propagation: Derivation, Meaning and Examples of Equation CY = FX CXFX T; Technical Report; ETH-Zürich: Zürich, Switzerland, 1998. [Google Scholar]
- Dellaert, F.; Kaess, M. Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing. Int. J. Robot. Res.
**2006**, 25, 1181–1203. [Google Scholar] [CrossRef] - Kaess, M.; Johannsson, H.; Roberts, R.; Ila, V.; Leonard, J.; Dellaert, F. iSAM2: Incremental smoothing and mapping with fluid relinearization and incremental variable reordering. In Proceedings of the 2011 IEEE International Conference on Robotics and Automation, Shanghai, China, 9–13 May 2011; pp. 3281–3288. [Google Scholar]
- Chiu, H.P.; Zhou, X.S.; Carlone, L.; Dellaert, F.; Samarasekera, S.; Kumar, R. Constrained optimal selection for multi-sensor robot navigation using plug-and-play factor graphs. In Proceedings of the 2014 IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, China, 31 May–7 June 2014; pp. 663–670. [Google Scholar]
- Sünderhauf, N.; Protzel, P. Towards a robust back-end for pose graph slam. In Proceedings of the 2012 IEEE International Conference on Robotics and Automation (ICRA), Saint Paul, MN, USA, 14–18 May 2012; pp. 1254–1261. [Google Scholar]
- Meller, M. Fast clutter cancellation for noise radars via waveform design. IEEE Trans. Aerosp. Electron. Syst.
**2014**, 50, 2328–2335. [Google Scholar] [CrossRef] - Akcakaya, M.; Sen, S.; Nehorai, A. A Novel Data-Driven Learning Method for Radar Target Detection in Nonstationary Environments. IEEE Signal Process. Lett.
**2016**, 23, 762–766. [Google Scholar] [CrossRef] - Kwon, Y.; Narayanan, R.M.; Rangaswamy, M. Multi-target detection using total correlation for noise radar systems. IEEE Trans. Aerosp. Electron. Syst.
**2013**, 49, 1251–1262. [Google Scholar] [CrossRef] - Gulati, D.; Zhang, F.; Malovetz, D.; Clarke, D.; Knoll, A. Robust Cooperative Localization in a dynamic environment Using Factor Graphs and Probability Data Association Filter. In Proceedings of the 20th International Conference on Information Fusion (FUSION), Xi’an, China, 10–13 July 2017. [Google Scholar]
- Sünderhauf, N.; Protzel, P. Switchable constraints for robust pose graph SLAM. In Proceedings of the 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, Vilamoura, Portugal, 7–12 October 2012; pp. 1879–1884. [Google Scholar]

Number | No Additional Factor | Topology Factor | SME Factor | |||
---|---|---|---|---|---|---|

of Vehicles | Without GPS | With GPS | Without GPS | With GPS | Without GPS | With GPS |

2 | 19.148325 | 4.443266 | 14.525345 | 3.167303 | 1.266813 | 1.291505 |

3 | 23.784680 | 5.462410 | 26.036128 | 4.530059 | 6.811776 | 2.510663 |

4 | 27.664985 | 6.314271 | 30.067669 | 5.490308 | 6.145480 | 2.694074 |

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