Application and Assessment of Cooperative Localization in Three-Dimensional Vehicle Networks

Abstract

The trajectory planning and control of multi-agent systems requires accurate localization, which may not be possible when GPS signals and fixed features required for SLAM are not available. Cooperative Localization (CL) in multi-agent systems offers a short-term solution that may significantly improve vehicle pose estimation. CL algorithms have been mainly developed and assessed for planar mobile robot networks due to complexities and singularities in three-dimensional (3D) motion. In this paper, we develop the required singularity-free equations and apply and assess an EKF-based CL for 3D vehicle networks. We assess the performance of CL with respect to the number of simultaneous and redundant measurements. We further assess CL performance with only relative position measurements available. Finally, experiments are performed to validate the proposed algorithms. We further investigate the effect of absolute position measurements in CL. Conclusions: Cooperative localization is an effective method when applied to 3D vehicle networks. However, CL does not improve localization with only relative position measurements, and thus previously reported results for 2D vehicle models were only effective due to relative orientation measurements. Absolute measurement reduces the overall localization errors much more significantly when there has been CL with prior relative position measurements.

1. Introduction

One of the vital technologies required for autonomous navigation is a localization technique that is capable of recognizing the position and orientation of a vehicle or vehicle networks, especially in GPS-denied environments. This problem is further exacerbated by environments that are often unstructured and lack distinct features (such as deserts), contain features that may change over time due to environment, or as the vehicles move to new areas (such as urban settings). Methods such as feature-based Simultaneous Localization and Mapping (SLAM) [1] are not capable of providing accurate localization in these environments. Collaborative systems of unmanned ground and air vehicles show great promise for various applications [2,3]. Cooperative Localization (CL) can be utilized for these systems to significantly reduce the rate of growth of estimation errors until GPS becomes available again. In addition, CL can help reduce the sample rate requirement for various SLAM methods in structured environments, since these methods require extensive computation. Cooperative SLAM techniques [4,5] have shown great promise for precise localization, especially when combined with other techniques such as the Extended Kalman Filter (EKF) [6]. However, the success of these techniques relies on the knowledge of the environment.

CL can utilize several techniques, including Kalman filters [7,8,9] and particle filters [10,11]. Utilizing these techniques, CL has been suggested as a powerful method to improve pose estimation in vehicle networks capable of inter-agent communication for data sharing [12]. In an early application of CL, an EKF algorithm [13] was developed that distributes vehicle state information while keeping track of the cross-covariance terms. These terms are created by relative measurements of each 2D vehicle’s pose. However, this method requires a completely synchronous and interconnected communication network. A decoupled CL algorithm is proposed [14] that eliminates the need for complete and constant between vehicles. Vehicles are assumed to be equipped with the capability of obtaining their own pose (via equations of motion and onboard sensors), measuring the poses of nearby vehicles relative to their own reference frame, and processing this information and communicating with other vehicles. CL allows a group of vehicles to cooperatively approximate an individual vehicle’s pose by sharing the relative and absolute pose measurements that each vehicle has obtained.

2. Motivation

Research works on multi-agent systems have focused on 2D vehicle models, even treating vehicles as simple nodes [15]. CL methods have focused primarily on maneuvering in 2D space [16,17,18,19] and particularly using wheeled mobile robots [20,21,22] and vehicles [23] due to the complexities involved in the singularity-free representation of 3D orientation. It is, however, critically important to apply CL to three-dimensional (3D) vehicle models to improve the localization of networks with homogeneous and heterogeneous air, marine, and ground vehicles. Others only consider relative localization without any regard to the global reference frames [24,25]. There is also a CL method [26] that only uses range measurements of a vehicle with access to absolute measurements via GPS and therefore does not take advantage of cross-covariances terms. A common theme in these methods is suffering from singularities due to using Euler angles to represent orientation. EKF-based CL with relative pose measurements to improve the absolute pose of networks with 3D vehicles has remained relatively unexplored due to the intricacies involved in the kinematic representation and linearization of state and measurement equations. Alternative relative localization methods [27] have also been explored. Euler angles are normally used to measure orientation, but are associated with singularities in aggressive vehicle maneuvers with relative or absolute angles reaching ${90}^{°}$. Quaternions, on the other hand, do not have any singularities but are not measurable by any sensors. Hence, they are calculated from the rotation matrix, which may be challenging in the case of relative orientation representation.

3. Related Works

EKFs have commonly been used as the standard technique for state estimation in CL. There are EKF-based methods that achieve localization utilizing stereo-vision and sensor fusion [28] and can be further extended to target tracking [29]. However, these methods require maximizing the overlap of each agent’s field of view to enhance localization. Many localization techniques rely on a central system or single/multiple “leader” agents responsible for sensor fusion [30,31,32]. Distributed or decentralized methods have been investigated [33,34,35] to ameliorate the computational bottlenecks associated with centralized systems. While these methods can be efficient and effective, they come at the cost of accuracy. A combination of centralized–decentralized CL in ad hoc networks shows great promise in improving localization without prior knowledge of an environment [14,36]. A distributed CL utilizing an EKF with low communication path requirements can also provide centralized-equivalent performance [37]. It is also possible to model the CL problem as a graph model to further utilize additional algorithms [38].

EKF-based CL algorithms are popular due to their ease of execution and computational efficiency. However, linearizing an extremely nonlinear system can lead to inaccuracies and numerical instabilities. Unscented Kalman Filter (UKF)-based methods were introduced in [39] with the advantage of not requiring any linearization. The UKF is deemed to be more robust [40] since it does not rely on the computation of the Jacobian matrix and is an excellent option for self-localization and tracking [41,42,43,44]. On the other hand, the UKF has a much higher computation cost compared to the EKF, especially considering the Cholesky factorizations at every step. As an alternative, the Squared Root UKF [45] does help mitigate the computation cost. However, implementation of UKF-based CL algorithms and simultaneous measurements is very complex and has not been fully explored.

Table 1. Limitations of related works.

Algorithm	Advantages	Limitations
SLAM	Obtain localization of multiple vehicles, mapping of an environment	Requires knowledge of the environment and designated landmarks, needs assigned “leaders” and “followers” or a central processing station
Particle Filter	Nonlinear applications, low error in ideal situations, no linearization required	Requires individual agent identification, not robust to false-positive detections
Decentralized EKF with Euler angles	No designated central processing center needed	Prone to singularity issues due to gimbal lock, requires linearization (Jacobian)
Current Method	Singularity-free, computationally effective	Quaternions cannot be measured directly, requires linearization

lists some of these related works along with their advantages and limitations.

4. Main Contributions and Novelties

In this paper, we extend a previous conference proceedings publication [46] to include additional subcases, physical experiments, and identifying additional limitations to CL. We extend the centralized EKF-based CL algorithm presented in [13,14] to include 3D vehicle models and assess its performance. Specifically, we utilize quaternions to represent vehicle orientation in order to avoid singularities. For measurements, we introduce quaternions corresponding to relative orientation represented by the rotation matrix and present linearized measurement equations, i.e., the output matrix. We then assess the performance of CL in 3D vehicle networks through a series of Monte Carlo-type simulations. We use a sequential update procedure [47] to assess the effect of simultaneous and redundant measurements on individual vehicles, as well as the overall network. Assuming that absolute orientation can be accurately measured by onboard sensors, we further assess the performance of CL when only relative position measurements are available and investigate the circumstances under which the method could be effective or ineffective. Finally, experiments are preformed to verify the algorithm.

The main contributions and novelties of this publication are outlined as follows:

• The development and application of singularity-free EKF-based CL for networks of three-dimensional vehicles;
• The first comprehensive assessment of EKF-based CL with only relative position measurements;
• The identification of limitations of CL with respect to the number of simultaneous measurements, redundant measurements, and relative position-only measurements;
• The experimental verification of the EKF CL algorithm’s performance and limitations.

The paper is organized as follows. The vehicle model is presented in Section 5. A multi-vehicle system with state and measurement equations and their linearization are presented in Section 6, followed by the CL algorithm in Section 7. Simulations and experiments are presented in Section 8 and Section 9, respectively, followed by the concluding remarks in Section 10.

5. Vehicle Model

5.1. Vehicle Pose

Each vehicle is assigned a body-fixed reference frame $X_v{Y}_v{Z}_v$. Figure 1 depicts the pose of the vehicle, which is then represented by the position of the origin of $X_v{Y}_v{Z}_v$ and its orientation with respect to the inertial reference frame $X_0{Y}_0{Z}_0$. Hence, the vector $r_0^v={\left[x_0^v{y}_0^v{z}_0^v\right]}^{\top }$ represents the position of the vehicle, and the orthogonal rotation matrix $R_0^v$ that transforms the coordinates of any vector in $X_v{Y}_v{Z}_v$ to $X_0{Y}_0{Z}_0$ determines its orientation. $R_0^v$ can be calculated using quaternions, which are favored over Euler angles since they do not suffer from singularities and require less computational overhead. The quaternion vector of the vehicle reference frame ${\overline{q}}^v$ can be presented as:

$${\overline{q}}^v=\left[\begin{array}{c}{q}_0^v\\ q^v\end{array}\right],q^v=\left[\begin{array}{c}{q}_1^v\\ q_2^v\\ q_3^v\end{array}\right]$$

(1)

where there is redundancy due to the four scalar values representing three rotational degrees of freedom. The constraint relating the quaternions is ${\overline{q}}^{v^{\top }}{\overline{q}}^v-1=0$, indicating that ${\overline{q}}^v$ is a unit vector.

The orthogonal rotation matrix is given in terms of the quaternions as [48]:

$$R_0^v\left({\overline{q}}^v\right)=[2{\left(q_0^v\right)}^2-1]I_3+2[q^v{q}^{v^{\top }}+q_0^{v}S\left(q^v\right)]$$

(2)

where $S\left(q^v\right)$ defines a skew-symmetric matrix of a vector such that

$$S\left(q^v\right)=\left[\begin{array}{ccc}0& -q_3^v& q_2^v\\ q_3^v& 0& -q_1^v\\ -q_2^v& q_1^v& 0\end{array}\right]$$

(3)

and $I_3$ is the $3\times 3$ identity matrix. Given the rotation matrix in (2), we can calculate ${\overline{q}}^v$, as described in [46].

5.2. Vehicle Velocity

In the global coordinate frame, $X_0{Y}_0{Z}_0$, the vehicle translational velocity vector, ${\dot{r}}_0^v$, is related to its body-fixed velocities in $X_v{Y}_v{Z}_v$, ${\dot{r}}_v^v$, as [49]:

$${\dot{r}}_0^v=R_0^v\left({\overline{q}}^v\right)v^v$$

(4)

where $v^v\triangleq {\dot{r}}_v^v$. The angular velocity vector of a vehicle about its body-fixed reference frame, ${\omega }^v={\left[{\omega }_x^v{\omega }_y^v{\omega }_z^v\right]}^{\top }$, is used to calculate the time derivative of the quaternion vector, ${\dot{\overline{q}}}^v$, as:

$$\begin{array}{cc}\hfill {\dot{\overline{q}}}^v& =\frac{1}{2}{G}^{v^{\top }}{\omega }^v,{\omega }^v=2G^v{\dot{\overline{q}}}^v\hfill \end{array}$$

(5a)

$$\begin{array}{cc}\hfill G^v& =\left[\begin{array}{c}-q^v,q_0^v{I}_3-S\left(q^v\right)\end{array}\right]\hfill \end{array}$$

(5b)

The unit-length constraint equation that relates the time derivatives of the quaternions is also given as ${\overline{q}}^{v^{\top }}{\dot{\overline{q}}}^v=0$.

5.3. Vehicle State Equations

Considering the single vehicle shown in Figure 1 and as described in the previous section, the pose of the vehicle can be represented by $r_0^v={\left[x_0^v{y}_0^v{z}_0^v\right]}^{\top }$ and ${\overline{q}}^v$. For a kinematic model, we may choose the state vector as a collection of the seven pose variables:

$${\mathsf{x}}^v=\left[\begin{array}{c}{r}_0^v\\ {\overline{q}}^v\end{array}\right]$$

(6)

Using (4) and (5), the driftless state equations may be written as:

$${\dot{\mathsf{x}}}^v=g^v\left({\mathsf{x}}^v\right){\mathsf{u}}^v$$

(7)

where

$$g^v=\left[\begin{array}{cc}{R}_0^v& 0_{3\times 4}\\ 0_{3\times 3}& \frac{1}{2}{G}^{v^T}\end{array}\right],{\mathsf{u}}^v=\left[\begin{array}{c}{v}^v\\ {\omega }^v\end{array}\right]$$

(8)

where $0_{n\times m}$ represents the $n\times m$ zero matrix and the vehicle input vector consists of the body-fixed velocities $v^v$ and ${\omega }^v$.

The $7\times 7$ Jacobian matrix of state equations, $J^v$, necessary for EKF linearization is calculated as:

$$J^v={\displaystyle \frac{\partial \left(g^v\left({\mathsf{x}}^v\right){\mathsf{u}}^v\right)}{\partial {\mathsf{x}}^v}}=\left[\begin{array}{cc}{0}_{3\times 3}& {\displaystyle \frac{\partial {\dot{r}}_0^v}{\partial {\overline{q}}^v}}\\ 0_{4\times 3}& {\displaystyle \frac{\partial {\dot{\overline{q}}}^v}{\partial {\overline{q}}^v}}\end{array}\right]$$

(9)

where the nonzero components are:

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial {\dot{r}}_0^v}{\partial {\overline{q}}^v}}=\left[\begin{array}{cc}{\displaystyle \frac{\partial {\dot{r}}_0^v}{\partial q_0^v}}& {\displaystyle \frac{\partial {\dot{r}}_0^v}{\partial q^v}}\end{array}\right]\hfill \end{array}$$

(10a)

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill & {\displaystyle \frac{\partial {\dot{\overline{q}}}^v}{\partial {\overline{q}}^v}}={\displaystyle \frac{1}{2}}\left[\begin{array}{cc}0& -{\omega }^{v^{\top }}\\ {\omega }^v& -S\left({\omega }^v\right)\end{array}\right]\hfill \end{array}\end{array}$$

(10b)

and

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial {\dot{r}}_0^v}{\partial q_0^v}}=[\left(4q_0^v\right)I_3+2S\left(q^v\right)]v^v\hfill \end{array}$$

(11a)

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial {\dot{r}}_0^v}{\partial q^v}}=2q^v{v}^{v^{\top }}+2\left(q^{v^{\top }}{v}^v\right)I_3-2q_0^{v}S\left(v^v\right)\hfill \end{array}$$

(11b)

It is important to note that several approaches have been proposed to adhere to the unit quaternion constraint during the integration of the state equations [50,51,52].

6. Multi-Vehicle System Model

6.1. State Equations

In order to formulate the CL algorithm, let us first consider a three-vehicle system shown in Figure 2. In the illustrated scenario, a vehicle designated as b or “base vehicle” obtains a pose measurement of another vehicle designated as t or “target vehicle” relative to its own body-fixed reference frame. Referring to (6)–(8), the system may be represented by the 14 states representing absolute poses of the two vehicles. The state equations are written as:

$$\begin{array}{cc}\hfill \dot{\mathsf{x}}& =g\left(\mathsf{x}\right)\mathsf{u}\hfill \\ \hfill \mathsf{x}& ={\left[r_0^{b^{\top }}{\overline{q}}^{b^{\top }}{r}_0^{t^{\top }}{\overline{q}}^{t^{\top }}\right]}^{\top }\hfill \\ \hfill \mathsf{u}& ={\left[v^{b^{\top }}{\omega }^{b^{\top }}{v}^{t^{\top }}{\omega }^{t^{\top }}\right]}^{\top }\hfill \\ \hfill g& =\mathrm{diag}\left[R_0^b\frac{1}{2}{G}^b{R}_0^t\frac{1}{2}{G}^t\right]\hfill \end{array}$$

(12)

The $14\times 14$ Jacobian matrix, J, for the two-vehicle network is simply defined as:

$$J=\mathrm{diag}\left[J^b{J}^t\right]$$

(13)

where $J^b$ and $J^t$ are defined for vehicle b and vehicle t according to (9)–(11).

6.2. Measurement Equations

Consider a base vehicle b acquiring a relative pose measurement of a target vehicle t. A problem with quaternions is that they are not kinematic quantities that can be directly measured by any sensor. Thus, we assume the rotation matrix representing the relative orientation of t with respect to b, perhaps derived from Euler angles, is the measured quantity. We then derive the quaternions corresponding to the relative rotation matrix. Similarly, the relative range, bearing angle, and elevation angle of t with respect to b are the quantities normally measured that may be used to determine the relative position. Here, we define the measurement output vector, $\mathsf{z}$, in terms of the relative position vector, r, and the three components of the quaternion vector q, which are determined from the relative orientation matrix:

$$\mathsf{z}=\left[\begin{array}{c}r\\ q\end{array}\right].$$

(14)

Note that the $q_0$ component is not needed since it can be derived from q using the unit-length constraint equation. Using the system states, the relative position r can be written as:

$$r={R_0^b}^{\top }{r}_0^{bt}={R_0^b}^{\top }(r_0^{\top }-r_0^b)={R_0^b}^{\top }{r}_0$$

(15)

where $r_0=r_0^{\top }-r_0^b$. Quaternions representing the relative orientation are derived from the relative rotation matrix:

$$R\left(\overline{q}\right)={R_0^b}^{\top }\left({\overline{q}}^b\right)R_0^t\left({\overline{q}}^t\right)$$

(16)

Vector q is then derived from the elements of $R\left(\overline{q}\right)$ [46].

Next, we linearize the measurement output equations, $\mathsf{z}=H\mathsf{x}$ where:

$$H=\left[\begin{array}{cc}{H}^b& H^t\end{array}\right]=\left[\begin{array}{cccc}-{R_0^b}^{\top }& {\displaystyle \frac{\partial r}{\partial {\overline{q}}^b}}& {R_0^b}^{\top }& 0_{3\times 4}\\ 0_{3\times 3}& {\displaystyle \frac{\partial q}{\partial {\overline{q}}^b}}& 0_{3\times 3}& {\displaystyle \frac{\partial q}{\partial {\overline{q}}^t}}\end{array}\right].$$

(17)

The term ${\displaystyle \frac{\partial r}{\partial {\overline{q}}^b}}$ can be written in terms of its components as ${\displaystyle \frac{\partial r}{\partial {\overline{q}}^b}}=\left[\begin{array}{cc}{\displaystyle \frac{\partial r}{\partial q_0^b}}& {\displaystyle \frac{\partial r}{\partial q^b}}\end{array}\right]$. These components can then be defined similar to (11) as:

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial r}{\partial q_0^b}}=[\left(4q_0^b\right)I_3+2S\left(q^b\right)]r_0\hfill \end{array}$$

(18a)

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial r}{\partial q^b}}=2q^b{r}_0^{\top }+2\left(q^{b^{\top }}{r}_0\right)I_3-2q_0^{b}S\left(r_0\right).\hfill \end{array}$$

(18b)

Assuming $q_0\ne 0$, the remaining derivative terms can be shown as:

$${\displaystyle \frac{\partial q}{\partial {\overline{q}}^b}}={\displaystyle \frac{1}{4q_0}}{\displaystyle \frac{\partial p}{\partial {\overline{q}}^b}},{\displaystyle \frac{\partial q}{\partial {\overline{q}}^t}}={\displaystyle \frac{1}{4q_0}}{\displaystyle \frac{\partial p}{\partial {\overline{q}}^t}}$$

(19)

where p is represented in terms of the elements of the matrix $R\left(\overline{q}\right)$

$$p={\left[R_{32}-R_{23}{R}_{13}-R_{31}{R}_{21}-R_{12}\right]}^{\top },$$

(20)

and the components of (19) are derived as:

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial p}{\partial q_0^b}}=16q_0^b{q}_0^t{q}^t+2[R_0^t-tr\left(R_0^t\right)I_3]q^b\hfill \\ \hfill & {\displaystyle \frac{\partial p}{\partial q_0^t}}=-16q_0^b{q}_0^t{q}^b-2[R_0^b-tr\left(R_0^b\right)I_3]q^t\hfill \\ & {\displaystyle \frac{\partial p}{\partial q^b}}=2q_0^b[R_0^t-tr\left(R_0^t\right)I_3]-2S\left(q^b\right){R_0^t}^{\top }+2S\left({R_0^t}^{\top }{q}^b\right)\hfill \\ \hfill & {\displaystyle \frac{\partial p}{\partial q^t}}=-2q_0^t[R_0^b-tr\left(R_0^b\right)I_3]+2S\left(q^t\right){R_0^b}^{\top }-2S\left({R_0^b}^{\top }{q}^t\right)\hfill \end{array}$$

(21)

If $q_0=0$, components of ${\displaystyle \frac{\partial q}{\partial q^b}}$ and ${\displaystyle \frac{\partial q}{\partial q^t}}$ are derived as:

$$\begin{array}{cc}\hfill {\displaystyle \frac{\partial q_i}{\partial q_j^b}}& ={\displaystyle \frac{1}{4q_i}}{\displaystyle \frac{\partial R_{ii}}{\partial q_j^b}},q_i\ne 0\hfill \\ \hfill {\displaystyle \frac{\partial q_i}{\partial q_j^t}}& ={\displaystyle \frac{1}{4q_i}}{\displaystyle \frac{\partial R_{ii}}{\partial q_j^t}},q_i\ne 0\hfill \\ \hfill {\displaystyle \frac{\partial q_i}{\partial q_j^b}}& ={\displaystyle \frac{\partial q_i}{\partial q_j^t}}=0,q_i=0\hfill \end{array},i,j=1,2,3$$

(22)

Finally, the terms ${\displaystyle \frac{\partial R_{ii}}{\partial q_b}}$ and ${\displaystyle \frac{\partial R_{ii}}{\partial q_t}}$ can be derived as:

$$\begin{array}{cc}\hfill {\displaystyle \frac{\partial R_{ii}}{\partial q_j^b}}& =2R_{0_{ji}}^t{q}_i^b,i\ne j\hfill \\ \hfill {\displaystyle \frac{\partial R_{ii}}{\partial q_j^b}}& =2R_{0_{ji}}^t{q}_i^b+2\sum _{j=1}^3{R}_{0_{ji}}^t{q}_j^b,i=j\hfill \\ \hfill {\displaystyle \frac{\partial R_{ii}}{\partial q_j^t}}& =2R_{b_{ji}}{q}_i^t,i\ne j\hfill \\ \hfill {\displaystyle \frac{\partial R_{ii}}{\partial q_j^t}}& =2R_{0_{ji}}^b{q}_i^t+2\sum _{j=1}^3{R}_{0_{ji}}^b{q}_j^t,i=j\hfill \end{array}$$

(23)

7. Cooperative Localization

Consider a network of n vehicles numbered 1 to n. A vehicle i, at sample time $\left(k\right)$, utilizes absolute measurements to propagate its pose using the discrete form of (7):

$${\mathsf{x}}^i(k+1)=g^i\left({\mathsf{x}}^i\left(k\right)\right){\mathsf{u}}^i\left(k\right)+{\gamma }^i\left({\mathsf{x}}^i\left(k\right)\right){\eta }^i\left(k\right)$$

(24)

where ${\mathsf{x}}^i$, ${\mathsf{u}}^i$, and ${\eta }^i$ refer to the state, input, and process noise vectors, respectively, and $g^i\left({\mathsf{x}}^i\right)$ and ${\gamma }^i\left({\mathsf{x}}^i\right)$ are the input and process noise coefficient functions, respectively. Next, we assume that vehicle i in the set $\{1,\cdots ,n\}$ measures the relative pose of vehicle, where $j\ne i$. Subsequently, the measurement vector ${\mathsf{z}}^{ij}$ can be written as:

$${\mathsf{z}}^{ij}(k+1)=h^{ij}({\mathsf{x}}^i\left(k\right),{\mathsf{x}}^j\left(k\right))+{\nu }^i\left(k\right)$$

(25)

where ${\nu }^i\left(k\right)$ represents the measurement noise and $h^{ij}={\left[r^{{ij}^{\top }}{q}^{{ij}^{\top }}\right]}^{\top }$; see (14).

For the EKF procedure, we denote variables at the propagation step with “−” and the updated step with “+”. We also use over “ ˆ ” for estimated state values. Thus, ${\widehat{\mathsf{x}}}^{i-}$ and ${\widehat{\mathsf{x}}}^{i+}$ indicate the predicted and updated state estimates for vehicle i, respectively. The key to success of EKF-based CL is the cross-covariance [14], which is the correlation term between vehicles i and j due to the relative measurements. Given initial conditions ${\mathsf{x}}^i\left(0\right)$, $P^{ij}\left(0\right)=0_{7\times 7}$, and the initial guess of $P^{ii}\left(0\right)=I_7$, the prediction step state estimates, covariance matrix, and cross-covariance matrix of vehicles $i=1,\cdots ,n$ at step $k+1$ are given in terms of step k terms as:

$$\begin{array}{cc}\hfill & {\widehat{\mathsf{x}}}^{i-}(k+1)=g^i\left({\mathsf{x}}^{i+}\left(k\right)\right){\mathsf{u}}^i\hfill \\ \hfill & P^{ii-}(k+1)=J^i{P}^{ii+}{J^i}^{\top }+{\mathsf{\Gamma }}_i{Q}^i{{\mathsf{\Gamma }}_i}^{\top }\hfill \\ \hfill & P^{ij-}(k+1)=J^i{P}^{ii+}{J^j}^{\top }\hfill \end{array}$$

(26)

where $j=1,\cdots ,n,j\ne i$, $J^i={\displaystyle \frac{\partial g^i}{\partial {\mathsf{x}}^i}}\left(k\right)$ is calculated using (9)–(10), ${\mathsf{\Gamma }}^i={\displaystyle \frac{\partial {\gamma }^i}{\partial {\mathsf{x}}^i}}$, and $Q^i>0$ is the covariance of process noise.

At the update step, the state vector and (cross) covariance matrices remain unchanged if there are no relative measurements. Let us now assume that a “base” vehicle b takes relative measurements of a “target” vehicle t. Then, the innovation error between the measurement ${\mathsf{z}}^{bt}$ and the estimated output through propagated states is:

$$\begin{array}{cc}\hfill e^b(k+1)=& {\mathsf{z}}^{bt}-h^{bt}({\widehat{\mathsf{x}}}^{b-}(k+1),{\widehat{\mathsf{x}}}^{t-}(k+1))\\ \hfill \approx & {\mathsf{z}}^{bt}-\left[H^b{H}^t\right]\left[\begin{array}{c}{\widehat{\mathsf{x}}}^{b-}(k+1)\\ {\widehat{\mathsf{x}}}^{t-}(k+1)\end{array}\right]\end{array}$$

(27)

where $\left[H^b{H}^t\right]$ is defined in (17). The cross-covariance terms modifies the innovation covariance matrix as follows:

$$\begin{array}{cc}\hfill S_{bt}(k+1)& =H^b{P}^{bb-}{H^b}^{\top }+H^t{P}^{tt-}{H^t}^{\top }\hfill \\ \hfill & +H^b{P}^{bt-}{H^t}^{\top }+H^t{P}^{tb-}{H^b}^{\top }+R_b\hfill \end{array}$$

(28)

Utilizing (27) and (28), the Kalman gains along with the states and covariance terms for all vehicles $i=1,\cdots ,n$ are updated as follows:

$$\begin{array}{cc}\hfill & K^i(k+1)=[P^{it-}{H^t}^{\top }+P^{ib-}{H^b}^{\top }]{S^{bt}}^{-1}\hfill \\ \hfill & {\widehat{\mathsf{x}}}^{i+}(k+1)={\widehat{\mathsf{x}}}^{i-}+K^i{e}^b\hfill \\ \hfill & P^{ii+}(k+1)=P^{ii-}-K^i{S}^{bt}{K^i}^{\top }\hfill \\ \hfill & P^{ij+}(k+1)=P^{ij-}-K^i{S}^{bt}{K^j}^{\top }\hfill \end{array}$$

(29)

where $j=1,\cdots ,n,j\ne i$. The Kalman gain, due to its semi-definite nature, ensures that a decrease in localization error is achieved by any relative measurement, as long as the measurements are not too noisy and the EKF linearization is accurate. The step-by-step procedure for the centralized CL algorithm can be found in [14].

The structure of the EKF-based CL algorithm is illustrated as a diagram in Figure 3 for better understanding of the procedure.

8. Simulation Results

Given a propagation time step size of $\mathsf{\Delta }t$, the seven state equations of vehicle i in (4) and (5) can be written in discrete form as:

$$\left\{\begin{array}{c}{r}_0^i(k+1)=r_0^i\left(k\right)+R_0^i\left(k\right)\left(v^i\left(k\right)+{\eta }_v^i\right)\mathsf{\Delta }t\hfill \\ {\overline{q}}^i(k+1)={\overline{q}}^i\left(k\right)+\frac{1}{2}{G}^i\left(k\right)\left({\omega }^i\left(k\right)+{\eta }_{\omega }^i\right)\mathsf{\Delta }t\hfill \end{array}\right.$$

(30)

where ${\eta }_v^i$ and ${\eta }_{\omega }^i$ are the corresponding components of ${\eta }^i$ in (24) representing the input velocity process noise. Similarly, $R_0^i\left({\overline{q}}^i\left(k\right)\right)$ and $\frac{1}{2}{G}^i\left({\overline{q}}^i\left(k\right)\right)$ are the components of ${\gamma }^i$ in (24).

The relative measurements obtained by all vehicles are polluted by additive Gaussian measurement noise (AWGN) with a standard deviation of $0.05$ m for the position states (based on distances between vehicles) and $0.02$ for the quaternion states of each vehicle. Vehicles may also be capable of obtaining absolute pose measurements with a standard deviation of $0.1$ m for position and $0.02$ for the quaternion states. As far as process noise is concerned, the standard deviations are $0.1v^i$ for ${\eta }_v^i$ and $1°/\mathrm{s}$ for ${\eta }_{\omega }^i$ across all vehicles. Finally, since there is additive noise in both measurement and process steps, after several trial runs, we concluded that about 100 runs in each simulation scenario are required to remove any significant effect stemming from outliers.

All simulations are run for 200 s and use the same input velocities and process and measurement noise properties in order to remove any input bias from the assessments. Input velocities are applied to each vehicle with varying magnitudes, as listed in

Table 2. Input linear (m/s) and angular (°/s) velocities of vehicles for all simulations.

	$[{\mathit{v}}_{\mathit{x}},{\mathit{v}}_{\mathit{y}},{\mathit{v}}_{\mathit{z}}]$	$[{\mathit{\omega }}_{\mathit{x}},{\mathit{\omega }}_{\mathit{y}},{\mathit{\omega }}_{\mathit{z}}]$
	Linear	Angular
$V^1$	−0.30, 0.00, 0.00	0.28, 0.55, 0.55
$V^2$	−0.35, 0.00, 0.00	0.35, 0.70, 0.70
$V^3$	0.40, 0.00, 0.00	0.43, 0.85, 0.85
$V^4$	0.35, 0.00, 0.00	0.50, 1.00, 1.00
$V^5$	−0.30, 0.00, 0.00	0.58, 1.15, 1.15
$V^6$	−0.35, 0.00, 0.00	0.65, 1.30, 1.30

. Since vehicles normally have a “forward” velocity input, 3D movement is attained through orientation changes, as a result of input angular velocities. Therefore, linear velocity is only applied in the x-direction. We selected $\mathsf{\Delta }t=0.02$ s, which is sufficiently small for accurate propagation without any noise, given the magnitude of input velocities.

To provide a comprehensive assessment, we initially present a test case with a changing measurement scenario. Next, we evaluate the effect of sequentially adding measurements that do not directly involve a given vehicle to show how CL can improve its localization and the localization of the entire network. We then assess the effect of redundant measurements when two or more vehicles simultaneously measure each other’s poses. Next, we assess the effectiveness of CL when only relative positions are measured, followed by an investigation of how a single vehicle’s absolute measurement can affect localization in this scenario.

8.1. A Test Case

We initially simulate a test case to demonstrate the effectiveness of CL using the first four vehicles listed in Table 2. In the test case, there are no relative measurements for the first 40 s, and therefore, each state is estimated using propagation only. Then, some vehicles fall within the line of sight of other vehicles such that vehicle 1 measures the relative pose of vehicle 2, vehicle 2 measures vehicle 3, and vehicle 3 measures vehicle 4 from $t=40$ s until $t=160$ s. These measurements all happen simultaneously. For the final 40 s, all vehicles lose sight of each other, such that there are no relative measurements. However, vehicles 1 and 4 take absolute measurements of their own pose until $t=200$ s possibly due to the availability of GPS or SLAM for those vehicles.

The actual paths of the four vehicles along with those estimated with and without CL are shown in Figure 4 where significant improvement in the estimation of the path is observed after 40 s when CL is applied. We denote the norm of error in position vector $r_0^v$ as $\mathsf{\Delta }r$ and the norm of error in orientation vector $q^v$ as $\mathsf{\Delta }q$. Figure 5 and Figure 6 compares the time history of $\mathsf{\Delta }r$ and $\mathsf{\Delta }q$ for the four vehicles with and without CL, along with their $2\sigma$ plots. It is clear that there is a significant improvement in the estimation of the pose of all vehicles during CL (from 40 s to 160 s), including vehicle 1, whose relative pose is never measured. In addition, the absolute measurements of vehicles 1 and 4 not only greatly reduce localization errors for those vehicles but also for vehicles 2 and 3 due to their cross-covariance matrices with vehicles 1 and 4. Another observation is that there are discontinuities in state estimations when measurement scenarios are changed, caused by abrupt changes in the covariance matrix, as observed in the $2\sigma$ plots in Figure 5 and Figure 6. These discontinuities, however, are not of any concern as long as the algorithm remains numerically stable.

8.2. Simultaneous Relative Pose Measurement Assessment

For CL, the number of relative measurements plays a significant role in lowering uncertainty. Therefore, we investigate how the error for a given vehicle (and across all vehicles) is affected by sequentially adding simultaneous measurements by other vehicles. We also assess what happens when vehicles make redundant measurements, such as two vehicles simultaneously taking relative measurements and exchanging information with each other. Hence, we consider two networks, one with all six vehicles, $n=6$, for assessment of additional simultaneous measurements (Scenario 1), and another with only the first three vehicles, $n=3$, for the assessment of redundant measurements (Scenario 2). In each scenario, we start with one vehicle taking the relative measurements of another vehicle. In subsequent cases, we continue to add more relative measurements to assess their contribution.

8.2.1. Scenario 1—Effect of Other Vehicle Measurements on Single-Vehicle Localization

The idea for Scenario 1 is to see how other vehicles taking additional simultaneous relative measurements affects the localization of a single vehicle and, consequently, all vehicles in the network.

Table 3. Simultaneous measurement descriptions for Scenario 1.

	Simultaneous Measurements ($\mathit{b}\to \mathit{t}$)
Case 1	1 → 2
Case 2	1 → 2, 2 → 3
Case 3	1 → 2, 2 → 3, 3 → 4
Case 4	1 → 2, 2 → 3, 3 → 4, 4 → 5
Case 5	1 → 2, 2 → 3, 3 → 4, 4 → 5, 5 → 6

shows five cases that were simulated, concentrating on the localization of vehicle 1. In case 1, relative measurements of vehicle 2 are taken by vehicle 1, while in case 2, vehicle 2 simultaneously takes relative measurements of vehicle 3 and so on. In each case, errors are averaged across the 100 simulations.

The mean of the position error norm ($\mathsf{\Delta }r$) and the orientation error norm ($\mathsf{\Delta }q$) for vehicle 1 in all 100 simulations are plotted in Figure 7 and Figure 8, respectively, for propagation without measurements and the five CL cases. Both $\mathsf{\Delta }r$ and $\mathsf{\Delta }q$ show that the performance increases as more relative measurements are added, and all CL cases show significant improvement over the propagation. An interesting observation is the significant error reduction in both $\mathsf{\Delta }r$ and $\mathsf{\Delta }q$ of vehicle 1 from Case 1 to Case 2 when vehicle 2 measures the relative pose of vehicle 3. Hence, the overall error for vehicle 1 is reduced even though it is not involved in the additional measurement. When subsequent measurements are added in cases 3 through 5, the localization errors continue to decrease but are relatively marginal. This demonstrates that the beneficial effects of CL may not outweigh its additional computational cost as the number of simultaneous measurements is increased. It should also be noted that the temporary dip in quaternion propagation error in Figure 8 is due to the complete rotation of the vehicles, bringing some components of the quaternion closer to their actual values.

Table 4. Position (m) and quaternion error statistics of vehicle 1 across 100 simulations for Scenario 1.

	Position				Orientation
	${\mathit{e}}_{{\mathit{r}}_{\mathit{m}\mathit{e}\mathit{a}\mathit{n}}}$	${\mathit{e}}_{{\mathit{r}}_{\mathit{s}\mathit{t}\mathit{d}}}$	${\mathit{e}}_{{\mathit{r}}_{\mathit{m}\mathit{i}\mathit{n}}}$	${\mathit{e}}_{{\mathit{r}}_{\mathit{m}\mathit{a}\mathit{x}}}$	${\mathit{e}}_{{\mathit{q}}_{\mathit{m}\mathit{e}\mathit{a}\mathit{n}}}$	${\mathit{e}}_{{\mathit{q}}_{\mathit{s}\mathit{t}\mathit{d}}}$	${\mathit{e}}_{{\mathit{q}}_{\mathit{m}\mathit{i}\mathit{n}}}$	${\mathit{e}}_{{\mathit{q}}_{\mathit{m}\mathit{a}\mathit{x}}}$
Propagate	2.87	0.01	2.84	2.91	0.05	0.00	0.05	0.05
Case 1	1.99	0.82	0.70	4.48	0.05	0.01	0.02	0.09
Case 2	1.48	0.85	0.38	4.56	0.04	0.02	0.01	0.09
Case 3	1.50	0.76	0.39	4.34	0.03	0.01	0.01	0.08
Case 4	1.33	0.69	0.39	4.34	0.03	0.01	0.01	0.07
Case 5	1.23	0.58	0.24	3.03	0.03	0.01	0.01	0.05
Case 5 [14]	1.27	0.01	1.16	1.19	0.04	0.00	0.03	0.04

lists the error norm statistics across the 100 simulations for vehicle 1, which includes statistical terms such as mean and standard deviation for position r and orientation q. To calculate these terms, we determine the norms of $\mathsf{\Delta }r$ and $\mathsf{\Delta }q$ at each time step of each simulation, and their root means squared are derived over time. Finally, the mean, standard deviation, min, and max are derived across all simulations. In CL, the cross-covariances contribute to lower overall error for vehicles when additional measurements are performed. The statistics again illustrate that the error reduction beyond Case 2 is relatively marginal. However, it also demonstrates that the variability is significantly increased in CL (see the standard deviation of CL cases compared to propagation).

We also compared the same statistics across all vehicles. In this case, $\mathsf{\Delta }r$ and $\mathsf{\Delta }q$ are again calculated at each time step of each simulation, and the root mean squared of the means is derived across all time steps and then averaged for all vehicles. Finally, the mean, standard deviation, min, and max are derived across all simulations. The results are listed in

Table 5. Position (m) and quaternion error statistics across 100 simulations of all vehicles for Scenario 1.

	Position				Orientation
	${\mathit{e}}_{{\mathit{r}}_{\mathit{m}\mathit{e}\mathit{a}\mathit{n}}}$	${\mathit{e}}_{{\mathit{r}}_{\mathit{s}\mathit{t}\mathit{d}}}$	${\mathit{e}}_{{\mathit{r}}_{\mathit{m}\mathit{i}\mathit{n}}}$	${\mathit{e}}_{{\mathit{r}}_{\mathit{m}\mathit{a}\mathit{x}}}$	${\mathit{e}}_{{\mathit{q}}_{\mathit{m}\mathit{e}\mathit{a}\mathit{n}}}$	${\mathit{e}}_{{\mathit{q}}_{\mathit{s}\mathit{t}\mathit{d}}}$	${\mathit{e}}_{{\mathit{q}}_{\mathit{m}\mathit{i}\mathit{n}}}$	${\mathit{e}}_{{\mathit{q}}_{\mathit{m}\mathit{a}\mathit{x}}}$
Propagate	14.97	0.02	14.91	15.03	0.43	0.00	0.43	0.44
Case 1	13.30	1.61	10.80	18.37	0.42	0.03	0.37	0.49
Case 2	10.58	2.58	7.16	19.59	0.36	0.04	0.28	0.50
Case 3	9.33	2.77	5.40	19.83	0.30	0.05	0.21	0.44
Case 4	7.82	2.91	3.50	20.95	0.23	0.05	0.13	0.40
Case 5	6.15	2.59	1.67	14.13	0.15	0.06	0.05	0.33
Case 5 [14]	8.20	0.05	8.07	8.32	0.21	0.00	0.20	0.21

. The data show a similar trend to Table 4, where there is a reduction in the error estimates of both $\mathsf{\Delta }r$ and $\mathsf{\Delta }q$ as simultaneous measurements are added. However, as expected, the reduction in overall localization errors continues to be significant as the number of vehicles that rely solely on propagation decreases. The same increase in variability is also observed in the location of all vehicles.

To demonstrate the importance of cross-covariances, we repeated the simulations in case 5 but without maintaining the cross-covariance updates as presented in [14]. The error norm statistics in the 100 simulations for vehicle 1 and all vehicles are listed as the last rows of Table 4 and Table 5. It is clear that our method performs better for the overall network and the localization of individual vehicles. In addition, we plotted the time history of the error norms $\mathsf{\Delta }r$ and $\mathsf{\Delta }q$ for this method in Figure 7 and Figure 8, respectively. It is observed that while initially the method in [14] performs better, the errors grow at much faster rate.

8.2.2. Scenario 2—Effect of Redundant Pose Measurements

Scenario 2 is added in order to assess simultaneous redundant pose measurements and communications. In this scenario, we only consider the first three vehicles, $n=3$, while presenting the localization errors across all vehicles. We again assume vehicle 1 measures the relative pose of vehicle 2 as Case 1 and then add measurements until all vehicles simultaneously measure the relative pose of all other vehicles in the network, as listed in

Table 6. Simultaneous measurement descriptions for Scenario 2.

	Simultaneous Measurements ($\mathit{b}\to \mathit{t}$)
Case 1	1 → 2
Case 2	1 → 2, 2 → 3
Case 3	1 → 2, 2 → 3, 3 → 1
Case 4	1 → 2, 2 → 3, 3 → 1, 2 → 1
Case 5	1 → 2, 2 → 3, 3 → 1, 2 → 1, 3 → 2
Case 6	1 → 2, 2 → 3, 3 → 1, 2 → 1, 3 → 2,1 → 3

.

Table 7. Position (m) and quaternion error statistics across 100 simulations of all vehicles for Scenario 2.

	Position				Orientation
	${\mathit{e}}_{{\mathit{r}}_{\mathit{m}\mathit{e}\mathit{a}\mathit{n}}}$	${\mathit{e}}_{{\mathit{r}}_{\mathit{s}\mathit{t}\mathit{d}}}$	${\mathit{e}}_{{\mathit{r}}_{\mathit{m}\mathit{i}\mathit{n}}}$	${\mathit{e}}_{{\mathit{r}}_{\mathit{m}\mathit{a}\mathit{x}}}$	${\mathit{e}}_{{\mathit{q}}_{\mathit{m}\mathit{e}\mathit{a}\mathit{n}}}$	${\mathit{e}}_{{\mathit{q}}_{\mathit{s}\mathit{t}\mathit{d}}}$	${\mathit{e}}_{{\mathit{q}}_{\mathit{m}\mathit{i}\mathit{n}}}$	${\mathit{e}}_{{\mathit{q}}_{\mathit{m}\mathit{a}\mathit{x}}}$
Propagate	18.08	0.04	17.98	18.16	0.36	0.00	0.36	0.36
Case 1	14.20	2.93	9.95	23.07	0.33	0.05	0.24	0.47
Case 2	10.28	4.76	3.69	24.40	0.23	0.08	0.09	0.50
Case 3	8.44	3.66	2.61	20.94	0.18	0.07	0.06	0.37
Case 4	8.23	4.60	2.56	23.84	0.17	0.07	0.06	0.33
Case 5	10.73	4.57	3.47	23.46	0.20	0.07	0.05	0.39
Case 6	11.33	4.13	4.66	24.18	0.21	0.06	0.10	0.40

summarizes the mean, standard deviation, minimum, and maximum error statistics for position and orientation estimates across all vehicle. As expected, the initial errors are reduced as measurements are added. However, there is no improvement beyond Case 3. In fact, there seems to be an increase in localization errors beyond Case 4 attributed mainly to computational inaccuracies. Therefore, redundant measurements (i.e., two vehicles simultaneous measuring each other’s relative poses and communicating their estimations and statistical properties) can negatively affect CL.

8.3. Relative Position Only Measurement Assessment

In this section, we assume each vehicle can reliably obtain its own orientation using an IMU and/or a gyroscope and does not measure the relative orientation of other vehicles. Therefore, the only means of CL is through relative position measurements. We use Scenario 2 for the simulations, but only relative position measurements are considered.

Table 8. Mean position error (m) statistics across 100 simulations of all vehicles for Scenario 2 with only relative position measurements.

	${\mathit{e}}_{{\mathit{r}}_{\mathit{m}\mathit{e}\mathit{a}\mathit{n}}}$	${\mathit{e}}_{{\mathit{r}}_{\mathit{s}\mathit{t}\mathit{d}}}$	${\mathit{e}}_{{\mathit{r}}_{\mathit{m}\mathit{i}\mathit{n}}}$	${\mathit{e}}_{{\mathit{r}}_{\mathit{m}\mathit{a}\mathit{x}}}$
Propagate	1.35	0.01	1.32	1.39
Case 1	1.34	0.01	1.31	1.38
Case 2	1.30	0.10	1.25	2.28
Case 3	1.28	0.02	1.24	1.32
Case 4	1.28	0.02	1.25	1.32
Case 5	1.28	0.01	1.25	1.32
Case 6	1.27	0.01	1.25	1.31

lists the error statistics for only $\mathsf{\Delta }r$ across the 100 simulations. Our first observation is that the inclusion of absolute orientation measurements drastically reduces the overall error across vehicles, since IMUs can provide relatively accurate orientation information. Our second observation is that CL does not achieve statistically significant improvement in any of the cases. Therefore, the additional computational cost of CL does not justify its use in conjunction with the kinematic model presented in this and other works. There were no advantages in applying CL to this set because the equations of motion (4) and (5) and consequently the state equations (7) and (30) do not depend on position variables. We can further conclude that the improvements observed in Section 8.2 are only due to relative orientation measurements. These conclusions have not been previously reported and are valid even when applying CL to 2D vehicle models employed by other researchers [13,14].

8.4. Effect of Absolute Position Measurements in CL

It has previously been reported that measuring the absolute position of one vehicle can reduce the localization errors of other vehicles in CL [14]. Therefore, CL with only relative position measurements may still be useful if a vehicle is able to attain the absolute position measurement via GPS, SLAM, etc. The three-vehicle system of Scenario 2 was utilized to illustrate the effect of absolute measurements. We consider two cases. In one case, the relative positions of vehicles 2 and 3 are measured by vehicles 1 and 2, respectively, for the first 100 s (CL). Afterwards, vehicle 1 measures its own absolute position for a short period of 1 s in addition to the relative measurements. Afterwards, there are no subsequent measurements by any of the vehicles. In the other case, there are no measurements for the first 100 s (no CL), after which vehicle 1 measures its own absolute position. There are no other measurements involved.

The mean error norm $\mathsf{\Delta }r$ across all vehicles and simulations was calculated to be $0.85\pm 0.04$ for the CL case and $1.05\pm 0.01$ for the no CL case. This clearly indicates that the absolute measurement reduces the overall localization errors much more significantly when there has been CL with prior relative position measurements.

Additional details can be found in Figure 9 where the comparisons between the time histories of the two cases for the three vehicles are plotted. It is clear that CL does not make a significant contribution during the first 100 s. After the first 100 s vehicle 1 measures its own position and its localization error drops to near zero in both cases. However, despite the lack of relative position measurements from 100 s to 200 s, the localization errors for the “CL” case are significantly lower than the “no CL” case for vehicles 2 and 3. This is due to the cross-covariance evolution in the CL algorithm during the first 100 s of the CL case. In conclusion, we propose that when dependable absolute orientation measurements are accessible, CL with relative position measurement does not improve localization errors using the kinematic model. Nevertheless, if any vehicle can measure its absolute position at some point during the experiment, CL can still greatly reduce the position errors of all vehicles.

9. Experimental Results

We conduct two experiments with three vehicles to verify and evaluate the CL algorithm. The first experiment utilizes relative pose measurements, while the second one only takes relative position measurements. The duration for both experiments is 80 s, and each vehicle follows a predefined path. The actual path data are processed through the CL EKF algorithm as measurements obtained by a vehicle. Each vehicle propagates its own states at a rate of $\mathsf{\Delta }t=0.02$ s. However, relative measurements are obtained every $0.2$ s.

Figure 10 shows an overview of one of the vehicles. All three vehicles have the same generic configuration and possess similar capabilities. The vehicles are differential-drive mobile robots with two motors controlled via a dual motor control and a PI controller implemented on an Arduino Duo board using motor encoder measurements. A Raspberry Pi computer performs most of the computation required to navigate the robot autonomously. Relative positions are calculated via LiDAR measurements of distance and bearing angle. However, the relative orientations are measured using a motion capture system that tracks markers, which are otherwise unused. The $0.2$ s measurement rate is the time required for the LiDAR to rotate fully and the required data processing.

9.1. Relative Pose Measurement Experiment

In these experiments, we investigated the effect of CL when relative pose measurements are only available for a very short period. For the first case (no CL), each vehicle propagates its own states for 40 s. Starting at 40 s, vehicle 3 measures its own pose for 1 s, after which there are no other measurements. In the second case (CL), the same scenario is repeated except that between 40 s and 41 s, vehicle 3 simultaneously measures the relative pose of vehicle 2 and 1 in addition to its own pose.

Figure 11 compares the time history of the position error norms of the two cases in this experiment for all three vehicles. As expected the performances for all three vehicles are identical for the first 40 s and vehicle 3’s localization is similar throughout. However, despite only 1 s of relative measurements (i.e., five samples), vehicle 1’s position estimation is dramatically improved. Vehicle 2, on the other hand, does not initially benefit from CL. However, as time passes, the localization errors start decreasing for CL while increasing for the “no CL” case.

9.2. Relative Position Measurement Experiment

Next, we performed experiments with only relative position measurements, similar to the simulations in Section 8.4, where the absolute orientation measurements are available throughout the experiments for all vehicles. In the first experiment (no CL), the three vehicles propagate their position throughout while vehicle 1 measures its absolute position at 40 s for only 1 s. The second experiment (CL) is similar to the first except that vehicles 1 and 2 measure the relative positions of vehicles 2 and 3, respectively, for the first 40 s. Note that vehicle simply propagate their positions for the last 39 s in both cases.

Figure 12 shows the comparisons between the time history of the position error norms of the two experiments for the three vehicles. For the first 40 s, it is evident that CL has a mixed performance, where vehicles 2 and 3 seem to benefit from it while vehicle 1 localization deteriorates. However, after 40 s when vehicle 1 measures its own position for only 1 s, there is a significant drop in estimation errors of vehicles 2 and 3 when compared to the “no CL” case. This experiment confirms that even with intermittent absolute orientation measurements, CL can be very effective in improving localization errors.

10. Conclusions

In this work, we implemented a centralized EKF-based CL algorithm to several networks of 3D vehicles. We build upon a previous publication [46] by adding additional cases, experimental results, and explicit limitations of the proposed CL algorithm. We derived state and relative measurement equations in terms of quaternions and also produced the singularity-free linearized matrices required for EKF. We utilize quaternions to avoid singularities linked to Euler angles. Simulation results demonstrated that CL considerably reduces state estimation errors when relative orientation measurements are available, though the performance does not proportionally improve with additional measurements. We further showed that CL does not provide any notable advantages when redundant measurements are applied. When utilizing CL with only relative position measurements, no improvement is observed, which is attributed to the absence of position variables in the state equations. However, CL with only position measurements can still provide significant improvement in state estimation if absolute measurements by a vehicle in the network becomes available any time during simulation. Furthermore, these conclusions are verified with experimental results that demonstrate the effect that absolute position and pose measurements have on CL. These experimental findings provide additional justification and credibility towards the CL algorithm presented in this paper.

The main limitation of the current algorithm is the use of a simplistic kinematic model that is not realistic for real-world applications. This limitation also leads to the lack of effective localization when only relative positions are employed. Future work involves updating the algorithm to utilize a fully nonlinear dynamic model and applying closed-loop control. There will also be closed-loop control experiments to further evaluate the effect CL has on a vehicle’s state estimation.