A Small-Sized Locating Message for Cooperative Localization under Communication Constraint

Abstract

When one agent simultaneously measures multiple teammates in cooperative localization, how to share these relative measurements with low-cost communication is interesting. In this chapter, different communication requirements for sharing absolute measurement and relative measurement are analyzed under extended Kalman filtering, whereby we develop an algorithm to convert multiple relative measurements into one virtual absolute measurement. By this, less number of elements are required to be transmitted across the agents. The utilization of multi-relative measurements is changed to the mode of one round communication from conventional mode where the measurements are processed and broadcast one by one. Simulation results demonstrate that, using the virtual absolute measurement, the almost equal localization performance with real relative measurements is obtained.

1. Introduction

Multi-agent cooperation has great potential on forest monitoring, disaster rescuing, and so on. Accurate localization is a premise for these tasks. As a novel localization strategy, cooperative localization (CL) provides higher positioning accuracy over independent localization (IL) where each agent solves its localization by itself. In some application scenes, where not all agents have accuracy localization due to the environment or hardware level, the advantages of CL are prominent [1–4].

In CL, an agent equipped with the wide-field exteroceptive sensor may simultaneously detect multiple agents moving in its vicinity, especially in mobile robot community. This case is regarded as the multi-relative measurements scene; see Figure 1(a). To share the benefits brought by multi-relative measurements, usually, the relative measurements are processed and disseminated one by one (sequential process and sequential dissemination, SPSD); see Figure 1(b). As a result, if n relative measurements occur in one agent, correspondingly, n communication rounds across the agents are required. It means that many communication resources are required. However, in reality, the communication constraint problem have to be considered in some applications, for example, (i) energy economy: energy consumption is proportional to data transmissions (it must be considered in the small-scale robot); (ii) war situation: stealth requirements in the electronic countermeasures; (iii) task priority: communication resources are saved for higher-level tasks; and (iv) some communication protocols, e.g., time division multiple access (TDMA), limit the length of time slice. In this paper, the effort of changing the SPSD to the mode of one round communication is paid to save the communication resources; see Figure 1(c), where multi-relative measurements are processed as a whole and broadcast at one stroke.

Figure 1.

Comparison of different communication strategies in the context of multi-relative measurements. (a) Example of multi-relative measurements where agent 4 detects agents 1, 2, 3 at the same time. (b) Sequential process and sequential dissemination. (c) One round communication.

In CL, the exteroceptive measurement (e.g., relative distance/bearing, GPS) occurring in single agent will help all agents to refine their position estimates. In order to share the exteroceptive measurement across the agents, they are usually converted into a locating message that contains the useful positioning elements for network flowing. In this paper, we pursue a new form of locating message that, (1) contains less elements; and, (2) has the approximate equal effect on CL as the SPSD.

Here, some special terms used throughout this paper are defined. The agent that implements exteroceptive measurements is called as the measuring agent, and as the non-measuring agent otherwise. The role is varying with time due to ever-changing geographical position. A measuring agent and an observed agent constitute one measurement pair.

1.1. Related works

Many effective algorithms for CL have been developed. These algorithms can be largely divided into two categories: the centralized CL (CCL) [5] and the distributed CL (DCL) [6, 7]. Considering computational cost, reliability and survivability (especially in war situation), the DCL is more attractive than the CCL. Corresponding to the CCL and the DCL, many estimation techniques have also been investigated, for example, filtering [8–13], parameter estimation [14] and optimization techniques [15].

When communication consumption is primarily considered, neither the parameter estimation nor the optimization technique is suitable due to the infinite iterations and communication [4, 14]; therefore, in this paper, the extended Kalman filtering (EKF) is employed, where the state estimate and its uncertainty are maintained together. We focus on pursuing a new form of locating message under the EKF.

Most recently, the communication constraint has been studied in some literatures [16–18]. Some typical algorithms such as IQKF (iteration quantized KF) [19], SoI-KF (sign of innovations KF) [20], have been designed to minimize communication bytes. These methods are designed originally for the multi-sensor community like target tracking problem (target-localization). It has been extended to the CL problem (self-localization). In ref.[21], with less quantized information, each agent executes the same algorithm for CL; however, besides the quantized measurement, all agents have to exchange their states.

In ref.[17], an optimal strategy is employed to minimize the exteroceptive measurement frequency; correspondingly, the communication frequency for the measurement sharing are also reduced. In ref.[22], the egocentric KF is proposed where the state error correlation ¹ , measured by the covariance, is ignored. As a result, the element types for exchanging are reduced. However, due to ignoring the state error correlation, the state estimator will diverge. To address this drawback, the covariance intersection filter (CIF) technique as well as the improved split CIF is developed for CL [12, 13]. By this technique, the covariance is not necessary to be always maintained, and it can be conservatively calculated whenever it is needed.

Under the condition of full network connectivity (i.e., each agent can communicate freely), it has been proved that the covariance can be well maintained in the distributed manner [8]. By the continual decomposition and combination of covariance matrix, the covariance can be independently propagated by each agent and the centralized-equivalent localization performance is achieved.

In the above methods, the communication requirements are lowered in term of the communication quantization, the communication frequency, or the communicated element types; but few work is studied on the communication strategy. For the case of multi-relative measurements, the common strategy is the SPSD where many communication resources are required. This strategy can also be tracked in refs.[6, 12, 23].

1.2. Contributions

In this paper, the DCL proposed in ref.[8] is employed as the fundamental framework of our algorithm. Based on their efforts, pursuing a small-sized locating message form is our focus. In this process, three contributions are devoted as follows.

• According to the relative measurements to different observed agents, a series of position estimates on the measuring agent are obtained. Regarding these estimates as the virtual observations on the measuring agent, a virtual observation equation group is constructed and the statistical characteristic of the virtual noise is derived.

• Relative measurements (one or more) are converted into one set of ‘pseudo-absolute locating message’ for network sharing, whereby less data need to be transmitted. In the conversion, the measurement update model of EKF is re-derived to avoid the overconfident use of virtual absolute measurement.

• Multi-relative measurements are processed as a whole and then broadcast at one stroke (no matter how many relative measurements). Consequently, the communication constraint is mitigated.

Compared with the common method where the locating message is directly produced from the real relative measurements, the new algorithm can reduce the number of elements in the locating message by 33% in the case of one relative measurement.

1.3. Outline

The rest is organized as follows. In Section 2, the related models are formulated. In Section 3, how to convert the relative measurements into the pseudo-absolute locating message is described. In Section 4, the simulation results are presented to demonstrate the application of the proposed algorithm. In Section 5, the conclusions are stated and an outlook on the future work is pointed out.

Notation: In this paper, k is used to denote the time index. δ is used to denote the time interval from time step k to $k+1$ . $\mathrm{\Theta }(k|k-1)$ denotes the predictive estimate of the variable Θ at time step k, which is based on the available information up to time step $k-1$ . $\mathrm{\Theta }(k|k)$ is the updated estimate based on the available information up to time step k. For simplicity of notation, $\mathrm{\Theta }(k|k-1)$ and $\mathrm{\Theta }(k|k)$ are replaced by $\bar{\mathrm{\Theta }}(k)$ and $\widehat{\mathrm{\Theta }}(k)$ respectively. ${\mathrm{\Theta }}^{\mathrm{T}}$ denotes the matrix transpose operator. 0 denotes a matrix of zeros and I is an identity matrix, both of appropriate dimensions.

2. System Models

We will derive the equations with 3 agents constituting a group (the same steps describe the derivation for more agents). Each agent has onboard proprioceptive and exteroceptive sensors. The proprioceptive sensor measures the self-motion (linear/rotational velocity). The exteroceptive sensor measures the relative pose to the others or obtains the absolute measurement.

2.1. State model

Assume that all the agents move in 2-dimensions plane. In a local fixed reference frame, each agent's pose is denoted as $x_i(k)=$ ${\left[x_i(k)\mathrm{,}{y}_i(k)\mathrm{,}{\varphi }_i(k)\right]}^{\mathrm{T}}$ ; ${\left[x_i(k)\mathrm{,}{y}_i(k)\right]}^{\mathrm{T}}$ and ${\varphi }_i(k)$ (i=1,2,3) denote the position coordinate and the heading, respectively. The motion of each agent is driven by the linear velocity V_i and the rotational velocity ω_i from the proprioceptive sensors. The kinematic model is written as:

$\left[\begin{array}{l}{x}_i(k+1)\\ y_i(k+1)\\ {\varphi }_i(k+1)\end{array}\right]=\left[\begin{array}{l}{x}_i(k)\\ y_i(k)\\ {\varphi }_i(k)\end{array}\right]+\left[\begin{array}{cc}\delta \text{cos}({\varphi }_i(k))& 0\\ \delta \text{sin}({\varphi }_i(k))& 0\\ 0& \delta \end{array}\right]\left[\begin{array}{l}{V}_i(k)\\ {\omega }_i(k)\end{array}\right].$

(1)

It can be rewritten compactly as:

$x_i(k+1)=x_i(k)+B_i({\varphi }_i(k))u_i(V_i(k)\mathrm{,}{\omega }_i(k))$

(2)

where $B_i$ and u_i denote the control matrix and the control input vector, respectively. Using the measured linear velocity $V_{mi}$ and the rotational velocity ${\omega }_{mi}$ , the state propagation equation is expressed as:

$x_i(k+1)=x_i(k)+B_i({\varphi }_i(k))u_i(V_{mi}(k)\mathrm{,}{\omega }_{mi}(k))\mathrm{.}$

(3)

where, taking into consideration the measurement noise, the proprioceptive measurements $V_{mi}$ and ${\omega }_{mi}$ are modeled as:

$V_{mi}=V_i+{\nu }_i$

(4)

${\omega }_{mi}={\omega }_i+w_i\mathrm{.}$

(5)

$V_i\mathrm{,}{\omega }_i$ are the real values. ${\nu }_i\mathrm{,}{w}_i$ are white, Gaussian noise with constant covariance ${\sigma }_{{\nu }_i}^2\mathrm{,}{\sigma }_{w_i}^2$ . The system noise covariance matrix is expressed as:

$Q_i=diag\left\{{\sigma }_{{\nu }_i}^2\mathrm{,}{\sigma }_{w_i}^2\right\}$

By linearizing (3), the linearized error-state propagation equation is expressed as:

${\tilde{x}}_i(k+1)={\Phi }_i(k){\tilde{x}}_i(k)+G_i(k)W_i(k)$

(6)

where ${\tilde{x}}_i$ is the error-state vector, i.e., ${\tilde{x}}_i=x_i-{\bar{x}}_i$ ; $W_i(k)=$ $[{\nu }_i;w_i]$ . ${\Phi }_i$ is the state transition matrix, $G_i$ is the system noise input matrix, i.e.,

${\Phi }_i(k)=\left[\begin{array}{ccc}1& 0& -V_{mi}(k)sin({\widehat{\varphi }}_i(k))\delta \\ 0& 1& V_{mi}(k)cos({\widehat{\varphi }}_i(k))\delta \\ 0& 0& 1\end{array}\right],G_i(k)=\left[\begin{array}{cc}\text{cos}{\widehat{\varphi }}_i(k)\delta & 0\\ \text{sin}{\widehat{\varphi }}_i(k)\delta & 0\\ 0& \delta \end{array}\right].$

2.2. Measurement model

Two kinds of exteroceptive measurements are considered: i) absolute exteroceptive measurement $z_i$ ; and ii) relative measurement $z_{ij}$ including distance measurement $z_{ij}^{\rho }$ , bearing measurement $z_{ij}^{\theta }$ and orientation measurement $z_{ij}^{\varphi }$ ( $i\mathrm{,}j=\mathrm{1,2,3,}i\ne j$ ). They are represented by

$z_i(k)=h_i(x_i(k))+{\epsilon }_i(k)\mathrm{,}$

(7)

$z_{ij}(k)=h_{ij}(x_i(k)\mathrm{,}{x}_j(k))+{\epsilon }_{ij}(k)$

(8)

where $h(\cdot )$ is a known representation; ε_i and ${\epsilon }_{ij}$ is the white, zero-mean, gaussian measurement noise and

$\begin{array}{l}{R}_i=E\{{\epsilon }_i(k){\left({\epsilon }_i(k)\right)}^{\mathrm{T}}\}\mathrm{,}{R}_{ij}=E\{{\epsilon }_{ij}(k){\left({\epsilon }_{ij}(k)\right)}^{\mathrm{T}}\}\mathrm{.}\hfill \end{array}$

By linearizing (7), and (8), the measurement error models are derived as follows:

${\tilde{z}}_i(k)=H_i(k){\tilde{x}}_i(k)+{\epsilon }_i(k)$

(9)

${\tilde{z}}_{ij}(k)=H_{ij}(k){\left[{\tilde{x}}_i^{\mathrm{T}}(k)\mathrm{,}{\tilde{x}}_j^{\mathrm{T}}(k)\right]}^{\mathrm{T}}+{\epsilon }_{ij}(k)$

(10)

where the measurement matrix $H_i$ ( $H_{ij}$ ) is the Jacobian matrix of $h(\cdot )$ at the point ${\bar{x}}_i(k)$ ( $[{\bar{x}}_i(k)\mathrm{,}{\bar{x}}_j(k)]$ ). In the case of the absolute exteroceptive measurement,

$\begin{array}{l}{H}_i(k)=\frac{\partial z_i}{\partial x_i^{\mathrm{T}}}{|}_{x_i={\bar{x}}_i(k)};\hfill \end{array}$

otherwise,

$\begin{array}{l}{H}_{ij}(k)=[\nabla h_{ij}^i(k)\mathrm{,}\nabla h_{ij}^j(k)]\hfill \end{array}$

where

$\nabla h_{ij}^i(k)=\frac{\partial z_{ij}}{\partial x_i^{\mathrm{T}}}{|}_{x_i={\bar{x}}_i(k)\mathrm{,}{x}_j={\bar{x}}_j(k)}\mathrm{,}\nabla h_{ij}^j(k)=\frac{\partial z_{ij}}{\partial x_j^{\mathrm{T}}}{|}_{x_i={\bar{x}}_i(k)\mathrm{,}{x}_j={\bar{x}}_j(k)}\mathrm{.}$

2.3. Cl model based on ekf

For the DCL, each agent carries out two operations alternately: self–estimation and exteroceptive measurement update.

2.3.1. Self–estimation

During no exteroceptive measurement occurring in any agent, at each time step k, the state of each agent can be independently propagated using its proprioceptive measurements as (1). Initially, assume that the whole error covariance matrix is $\bar{P}=diag$ $\left[{\bar{P}}_{11}(0)\mathrm{,}{\bar{P}}_{22}(0)\mathrm{,}{\bar{P}}_{33}(0)\right]$ , where the block diagonal terms denote the uncertainty of the initial states. Along with the relative measurement being fused, the error covariance matrix will not be block diagonal. Each block element ${\bar{P}}_{ij}$ ( $i\mathrm{,}j=\mathrm{1,2,3}$ ) satisfies

${\bar{P}}_{ij}(k+1)=\{\begin{array}{cc}{\Phi }_i(k){\widehat{P}}_{ij}(k){\Phi }_i^{\mathrm{T}}(k)+G_i(k)Q_i(k)G_i^{\mathrm{T}}(k)& i=j\\ {\Phi }_i(k){\widehat{P}}_{ij}(k){\Phi }_j^{\mathrm{T}}(k)& i\ne j\end{array}$

(11)

It can be found that ${\bar{P}}_{ii}$ can be propagated by agent i without other agents' knowledge; as for the $i\ne j$ case, how to propagate the covariance in the distributed manner has been addressed in ref.[8]. The above method for the position and covariance propagation is called as the self–estimation, and its output is the predictive estimate ${\bar{x}}_i$ . The self–estimation maintains a continuous, rough position estimation.

In CL, the exteroceptive measurement frequency is less than the proprioceptive measurement frequency due to different sensing capacity; therefore, each agent propagates its position by the self–estimation in most of the time. Once an exteroceptive measurement occurs, the predictive estimate will be refined.

2.3.2. Exteroceptive measurement update

When one agent obtains the absolute measurement or the relative measurement, all the states and covariances in the group will be updated as follows [6]:

1. Update caused by the absolute measurement $z_i(k)$

   Assume that only agent 1 obtains an absolute measurement at a certain time. In the EKF framework, the state of each agent is updated as follows:

   $\begin{array}{cc}{\widehat{x}}_n(k)={\bar{x}}_n(k)+{\bar{P}}_{n1}(k)\mathrm{\Lambda }(\mathrm{1,}k)& (n=\mathrm{1,2,3})\mathrm{.}\end{array}$

   (12)

   The covariance term is updated as:

   $\begin{array}{cc}{\widehat{P}}_{nm}(k)={\bar{P}}_{nm}(k)-{\bar{P}}_{n1}(k)\bar{\mathrm{\Lambda }}(\mathrm{1,}k){\bar{P}}_{1m}(k)& (n\mathrm{,}m=\mathrm{1,2,3})\end{array}$

   (13)

   with

   $\mathrm{\Lambda }(\mathrm{1,}k)=\left(H_1{\left(k\right)}^{\mathrm{T}}\right){\left(S_1\left(k\right)\right)}^{-1}{\upsilon }_1(k)\in {ℝ}^{p\times 1}\mathrm{,}$

   (14)
   $\bar{\mathrm{\Lambda }}(\mathrm{1,}k)=\left(H_1{\left(k\right)}^{\mathrm{T}}\right){\left(S_1\left(k\right)\right)}^{-1}{H}_1(k)\in {ℝ}^{p\times p}$

   (15)

   where p denotes the state dimension. ${\upsilon }_1(k)=$ $z_1(k)-$ $h_1({\bar{x}}_1(k))$ denotes the measurement innovation vector and its covariance is represented as $S_1(k)=$ $H_1(k)$ ${\bar{P}}_{11}(k)$ ${(H_1(k))}^{\mathrm{T}}+R_1$ . $\mathrm{\Lambda }(\mathrm{1,}k)$ and $\bar{\mathrm{\Lambda }}(\mathrm{1,}k)$ are called as the real absolute locating message generated by agent 1.

2. Update caused by the relative measurement $z_{ij}(k)$

Assume that only one measurement pair (agents 1 and 2) occurs at a certain time. Similar to case (1), the state of each agent is updated as follows:

$\begin{array}{cc}{\widehat{x}}_n(k)={\bar{x}}_n(k)+\left[{\bar{P}}_{n1}(k)\mathrm{,}{\bar{P}}_{n2}(k)\right]\mathrm{\Lambda }(\mathrm{1,2,}k)& (n=\mathrm{1,2,3})\mathrm{.}\end{array}$

(16)

The covariance term is updated as

$\begin{array}{cc}{\widehat{P}}_{nm}(k)={\bar{P}}_{nm}(k)-\left[{\bar{P}}_{n1}(k)\mathrm{,}{\bar{P}}_{n2}(k)\right]\bar{\mathrm{\Lambda }}(\mathrm{1,2,}k)\left[\begin{array}{l}{\bar{P}}_{1m}(k)\\ {\bar{P}}_{2m}(k)\end{array}\right]& (n\mathrm{,}m=\mathrm{1,2,3})\end{array}$

(17)

with

$\mathrm{\Lambda }(\mathrm{1,2,}k)={\left(H_{12}(k)\right)}^{\mathrm{T}}{\left(S_{12}(k)\right)}^{-1}{\upsilon }_{12}\in {ℝ}^{2p\times 1}\mathrm{,}$

(18)

$\bar{\mathrm{\Lambda }}(\mathrm{1,2,}k)={\left(H_{12}(k)\right)}^{\mathrm{T}}{\left(S_{12}(k)\right)}^{-1}{H}_{12}(k)\in {ℝ}^{2p\times 2p}$

(19)

where ${\upsilon }_{12}(k)=$ $z_{12}(k)-$ $h_{12}\left({\bar{x}}_1(k)\mathrm{,}{\bar{x}}_2(k)\right)$ denotes the innovation vector. The innovation covariance is calculated as $S_{12}(k)=$ $H_{12}(k)\left[\begin{array}{cc}{\bar{P}}_{11}(k)& {\bar{P}}_{12}(k)\\ {\bar{P}}_{21}(k)& {\bar{P}}_{22}(k)\end{array}\right]$ $H_{12}^{\mathrm{T}}(k)+R_{12}$ . $\mathrm{\Lambda }(\mathrm{1,2,}k)$ and $\bar{\mathrm{\Lambda }}(\mathrm{1,2,}k)$ are called as the real relative locating message generated by agent 1.

Comparing $\mathrm{\Lambda }(\mathrm{1,2,}k)$ , and $\bar{\mathrm{\Lambda }}(\mathrm{1,2,}k)$ with $\mathrm{\Lambda }(\mathrm{1,}k)$ , and $\bar{\mathrm{\Lambda }}(\mathrm{1,}k)$ , it is clear that less elements are included by the latter, even the matrix symmetry being considered. The comparison results are enumerated in Table 1. The goal is converting the relative measurement (one or more) to a virtual absolute measurement with the same form as $\mathrm{\Lambda }(\mathrm{1,}k)$ , and $\bar{\mathrm{\Lambda }}(\mathrm{1,}k)$ , and this process is called as the ‘similarity conversion’.

Table 1.

Comparison of two locating messages in size

Measurement type	absolute measurement	relative measurement
Λ	p	2p
$\bar{\mathrm{\Lambda }}$	(p2+p)/2	2p2+p

In this paper, the strategy proposed in ref.[8], especially in the covariance maintaining, is employed for the DCL; however, what is communicated across the agents and in what form are our distinctions. Firstly, a general communication round for the DCL is given as follows. Three communication stages are divided according to the communication contexts. (1) Handshaking communication between the measuring agent and the observed agent: the measuring agent sends a request and the observed agent sends its state to the measuring agent. (2) Covariance maintaining communication among all agents: all the agents communicate with each other to obtain the current covariance terms. (3) Locating message sharing communication between the measuring agent and the non-measuring agents: the measuring agent sends the processed locating message to the others. In this paper, what is communicated in the first two communications is the same as the usual method. By the first two communications, the raw data for the locating message are ready. Then the locating message is generated by the measuring agent and then spread to the others.

3. Similarity Conversion for Locating Message

In order to generate the small-sized locating message, two stages of the similarity conversion are studied: the relative measurement converting into one virtual absolute measurement and, the virtual absolute measurement converting into the pseudo-absolute locating message.

3.1. Partner estimate on measuring agent

For any measurement pair, the true states of them are constrained by ² :

$x_i=x_j-f(x_j\mathrm{,}{h}_{ij})(i\mathrm{,}j=\mathrm{1,2,3,}i\ne j)$

(20)

where $h_{ij}=$ ${\left[r_{ij}^{\rho }\mathrm{,}{r}_{ij}^{\theta }\mathrm{,}{r}_{ij}^{\varphi }\right]}^{\mathrm{T}}$ and $r_{ij}^{\rho }\mathrm{,}{r}_{ij}^{\theta }\mathrm{,}{r}_{ij}^{\varphi }$ denote distance, bearing and orientation, respectively. They are the true relative pose of agent j in the coordinate system attached to agent i and satisfy:

$f(x_j\mathrm{,}{h}_{ij})={\left[r_{ij}^{\rho }\text{cos}(r_{ij}^{\alpha })\mathrm{,}{r}_{ij}^{\rho }\text{sin}(r_{ij}^{\alpha })\mathrm{,}{r}_{ij}^{\varphi }\right]}^{\mathrm{T}}$

(21)

where $r_{ij}^{\alpha }={\varphi }_j-r_{ij}^{\varphi }+r_{ij}^{\theta }$ .

Based on the relative measurement and the observed agent's state, there exist an opinion (state estimate) on the measuring agent. For instance, assuming that agent 1 and agent 2 constitute a measurement pair and agent 1 is the measuring agent, then on the conditions of the known relative measurement $z_{12}$ and the predictive estimate of agent 2, the state of agent 1 can be obtained as follows [24]:

${\bar{x}}_1^{\left\{2\right\}}=\left[\begin{array}{l}{\bar{x}}_1^{\left\{2\right\}}\\ {\bar{y}}_1^{\left\{2\right\}}\\ {\bar{\varphi }}_1^{\left\{2\right\}}\end{array}\right]=\left[\begin{array}{l}{\bar{x}}_2\\ {\bar{y}}_2\\ {\bar{\varphi }}_2\end{array}\right]-\left[\begin{array}{l}{z}_{12}^{\rho }\text{cos}({\bar{\varphi }}_2-z_{12}^{\varphi }+z_{12}^{\theta })\\ z_{12}^{\rho }sin({\bar{\varphi }}_2-z_{12}^{\varphi }+z_{12}^{\theta })\\ z_{12}^{\varphi }\end{array}\right]$

(22)

where the superscript {2} denotes that the state estimate is from the view of agent 2. Instead, this kind of estimate is called as the partner estimate, drawing a contrast with the predictive estimate ${\bar{x}}_1$ . Based on the error propagation principle, the uncertainty of the partner estimate ${\bar{x}}_1^{\left\{2\right\}}$ is given by:

${\bar{P}}_{11}^{\left\{2\right\}}=H_{12}^{\left\{2\right\}}\left[\begin{array}{cc}{\bar{P}}_{22}& 0_{3\times 3}\\ 0_{3\times 3}& R_{12}\end{array}\right]{\left(H_{12}^{\left\{2\right\}}\right)}^T$

(23)

where $H_{12}^{\left\{2\right\}}$ is the Jacobian matrix of $x_1^{\left\{2\right\}}$ at the point ( ${\bar{x}}_2\mathrm{,}{z}_{12}$ ). When more than one agents are measured by the same measuring agent, without loss of generality, assume that agent 1 observes agents 2 and 3. According to (22), and (23), an estimate set ${\bar{x}}_1^s$ = $\left\{{\bar{x}}_1^{\left\{2\right\}}\mathrm{,}{\bar{x}}_1^{\left\{3\right\}}\right\}$ about agent 1 as well as the corresponding uncertainty can be yielded.

Regarding the estimate set ${\bar{x}}_1^s$ as a series of virtual observations, a common opinion on the measuring agent can be obtained by some fusion techniques, e.g., weighted least square (WLS). Further, the common opinion is regarded as a virtual absolute measurement and used for the exteroceptive measurement update as the input. Different from the real absolute measurement, two issues have to be considered. (i) Since the state of each agent is interdependent, the various partner estimates are correlated. (ii) The virtual absolute measurement correlates with the predictive estimate. In the following, how to describe the two correlations will be addressed.

3.2. Generation of virtual absolute measurement

It is known that the predictive estimate can be represented as:

${\bar{x}}_i=x_i+{\epsilon }_{xi}(i=\mathrm{1,2,3})$

(24)

where

$COV([{\epsilon }_{x1}\mathrm{,}{\epsilon }_{x2}\mathrm{,}{\epsilon }_{x3}])=\left[\begin{array}{ccc}{\bar{P}}_{11}& {\bar{P}}_{12}& {\bar{P}}_{13}\\ {\bar{P}}_{21}& {\bar{P}}_{22}& {\bar{P}}_{23}\\ {\bar{P}}_{31}& {\bar{P}}_{32}& {\bar{P}}_{33}\end{array}\right]\mathrm{.}$

(25)

Regarding the partner estimate set as a class of observation on the measuring agent, according to (22), a general expression of the observation model is proposed as follows:

$x_1^{\left\{i\right\}}={\bar{x}}_i-f({\bar{x}}_i\mathrm{,}{z}_{1i})(i=\mathrm{2,3})$

(26)

where $x_1^{\left\{i\right\}}$ is variable. Substituting (20), and (24) to (26), we have:

$x_1^{\left\{i\right\}}=x_1+f(x_i\mathrm{,}{h}_{1i})+{\epsilon }_{xi}-f({\bar{x}}_i\mathrm{,}{z}_{1i})(i=\mathrm{2,3})\mathrm{.}$

(27)

Through the Taylor expansion of $f(x_i\mathrm{,}{h}_{1i})$ at the point ( ${\bar{x}}_i\mathrm{,}{z}_{1i}$ ), we have:

$\begin{array}{c}\begin{array}{l}f(x_i\mathrm{,}{h}_{1i})=f({\bar{x}}_i\mathrm{,}{z}_{1i})-\left(\frac{\partial f}{\partial x_i}{|}_{{\bar{x}}_i\mathrm{,}{z}_{1i}}\right){\epsilon }_{xi}-\left(\frac{\partial f}{\partial h_{1i}}{|}_{{\bar{x}}_i\mathrm{,}{z}_{1i}}\right){\epsilon }_{1i}=\\ =f({\bar{x}}_i\mathrm{,}{z}_{1i})-f_k^{x_i}{\epsilon }_{xi}-f_k^{h_{1i}}{\epsilon }_{1i}\end{array}\end{array}$

(28)

where ${\epsilon }_{1i}$ is the relative measurement noise. Substituting (28) to (27), the linear expression of the partner estimate is given by:

$x_1^{\left\{i\right\}}\approx x_1+(I-f_k^{x_i}){\epsilon }_{xi}-f_k^{h_{1i}}{\epsilon }_{1i}\mathrm{.}$

(29)

For the simplification, define that

$h_{1i}^a=(I-f_k^{x_i})=\left[\begin{array}{ccc}1& 0& z_{1i}^{\rho }\text{sin}{\alpha }_{1i}\\ 0& 1& -z_{1i}^{\rho }\text{cos}{\alpha }_{1i}\\ 0& 0& 1\end{array}\right]$

(30)

$h_{1i}^b=f_k^{h_{1i}}=\left[\begin{array}{ccc}\text{cos}{\alpha }_{1i}& -z_{1i}^{\rho }\text{sin}{\alpha }_{1i}& z_{1i}^{\rho }\text{sin}{\alpha }_{1i}\\ \text{sin}{\alpha }_{1i}& z_{1i}^{\rho }\text{cos}{\alpha }_{1i}& -z_{1i}^{\rho }\text{cos}{\alpha }_{1i}\\ 0& 0& 1\end{array}\right]$

(31)

where ${\alpha }_{1i}={\bar{\varphi }}_i-z_{1i}^{\varphi }+z_{1i}^{\theta }$ .

Arraying the vector entities in the partner estimate set ${\bar{x}}_1^s$ , then an observation equation group is constituted as:

$\left[\begin{array}{l}{x}_1^{\left\{2\right\}}\\ x_1^{\left\{3\right\}}\end{array}\right]=\left[\begin{array}{l}I\\ I\end{array}\right]x_1+\left[\begin{array}{l}{h}_{12}^a{\epsilon }_{x2}-h_{12}^b{\epsilon }_{12}\\ h_{13}^a{\epsilon }_{x3}-h_{13}^b{\epsilon }_{13}\end{array}\right].$

(32)

It can be compactly rewritten as

$y_1=L_1{x}_1+v_1$

(33)

where

$COV(v_1)=R_{v_1}=\left[\begin{array}{cc}{h}_{12}^a{\bar{P}}_{22}{\left(h_{12}^a\right)}^{\mathrm{T}}+h_{12}^b{R}_{12}{\left(h_{12}^b\right)}^{\mathrm{T}}& h_{12}^a{\bar{P}}_{23}{\left(h_{13}^a\right)}^{\mathrm{T}}\\ h_{13}^a{\bar{P}}_{32}{\left(h_{12}^a\right)}^{\mathrm{T}}& h_{13}^a{\bar{P}}_{33}{\left(h_{13}^a\right)}^{\mathrm{T}}+h_{13}^b{R}_{13}{\left(h_{13}^b\right)}^{\mathrm{T}}\end{array}\right].$

(34)

$R_{v_1}$ characterizes the error correlation among the partner estimates, and the first issue proposed in Section 3.1 is addressed.

According to the available specific ${\bar{x}}_1^s$ and ${\bar{R}}_{v_1}$ , the solution of (33) can be obtained by many fusion techniques. Herein, the weighted least square (WLS) method is employed, and a common opinion on the measuring agent is obtained as follows:

${\bar{z}}_1^c={\left(L_1^{\mathrm{T}}{\bar{R}}_{v_1}^{-1}{L}_1\right)}^{-1}{L}_1^{\mathrm{T}}{\bar{R}}_{v_1}^{-1}{\bar{y}}_1\mathrm{,}{\bar{P}}_{11}^c={\left(L_1^{\mathrm{T}}{\bar{R}}_{v_1}^{-1}{L}_1\right)}^{-1}$

(35)

where ${\bar{y}}_1=\left[\begin{array}{l}{\bar{x}}_1^{\left\{2\right\}}\\ {\bar{x}}_1^{\left\{3\right\}}\end{array}\right]$ . We call ${\bar{z}}_1^c$ as the virtual absolute measurement. ${\bar{P}}_{11}^c$ denotes the uncertainty of the virtual absolute measurement.

3.3. Virtual absolute measurement to pseudo-absolute locating message

Based on the fact that $COV({\epsilon }_{xi}\mathrm{,}{\epsilon }_{xj})\ne 0$ $(i\mathrm{,}j=\mathrm{1,2,3})$ , inspecting (32) and (35), it can be found that the virtual absolute measurement ${\bar{z}}_1^c$ correlates with the predictive estimate ${\bar{x}}_1$ . Thus, the virtual absolute measurement cannot be directly used for the measurement update model (12)–(15) as the general EKF. The effects from this correlation on the state and covariance update should be considered to avoid the overconfident use of the virtual absolute measurement.

Regarding the solution process of (33) as a virtual measurement on the measuring agent 1, the measurement model can be proposed as:

$z_1^c=W{y}_1$

(36)

where $W={\left(L^{\mathrm{T}}{R}_{v_1}^{-1}L\right)}^{-1}$ $L^{\mathrm{T}}{R}_{v_1}^{-1}\in {ℝ}^{3\times 6}$ . Substituting (33) to (36), then

$z_1^c=x_1+W{v}_1\mathrm{.}$

(37)

With the virtual measurement model (37) and (24), the predictive measurement error can be represented as follows:

${\tilde{z}}_1^c=z_1^c-{\bar{x}}_1=-{\epsilon }_{x1}+W{v}_1\mathrm{.}$

(38)

Its covariance matrix is that:

$\begin{array}{l}{R}_{{\tilde{z}}_1^c}=COV({\tilde{z}}_1^c)={\bar{P}}_{11}+{\bar{P}}_{11}^c-\\ -\left[\begin{array}{cc}{\bar{P}}_{12}{\left(h_{12}^a\right)}^{\mathrm{T}}& {\bar{P}}_{13}{\left(h_{13}^a\right)}^{\mathrm{T}}\end{array}\right]W^{\mathrm{T}}-\\ -W\left[\begin{array}{cc}{h}_{12}^a{\bar{P}}_{21};& h_{13}^a{\bar{P}}_{31}\end{array}\right].\end{array}$

(39)

Compared with the usual measurement update model, the third and fourth terms are added to remove the effect from the correlation between the predictive estimate and the virtual absolute measurement.

The covariance of each predictive estimate error ${\tilde{x}}_i$ and the predictive measurement error ${\tilde{z}}_1^c$ is given by:

$R_{{\tilde{x}}_i{\tilde{z}}_1^c}=COV({\tilde{x}}_i\mathrm{,}{\tilde{z}}_{v_1})={\bar{P}}_{i1}-\left[\begin{array}{cc}{\bar{P}}_{i2}{\left(h_{12}^a\right)}^{\mathrm{T}}& {\bar{P}}_{i3}{\left(h_{13}^a\right)}^{\mathrm{T}}\end{array}\right]W^{\mathrm{T}}(i=\mathrm{1,2,3})\mathrm{.}$

(40)

Let $W=[W_1^2\mathrm{,}{W}_1^3]$ $(W_1^j\in {ℝ}^{3\times 3}\mathrm{,}j=\mathrm{2,3})$ , then (40) can be rewritten as

$R_{{\tilde{x}}_i{\tilde{z}}_{v_1}}={\bar{P}}_{i1}-{\displaystyle \sum _{j=2}^3}{\bar{P}}_{ij}{\kappa }_{(\mathrm{1,}j)}^{\mathrm{T}}$

(41)

where ${\kappa }_{(\mathrm{1,}j)}=W_1^j{h}_{1j}^a\in {ℝ}^{3\times 3}(j=\mathrm{2,3})$ .

Regarding the available ${\upsilon }_c={\bar{z}}_1^c-{\bar{x}}_1$ , $S_c$ = $R_{{\tilde{z}}_{v_1}}$ as the virtual innovation vector and the innovation covariance, respectively, the basic locating message ${\mathrm{\Lambda }}_1^e\mathrm{,}{\bar{\mathrm{\Lambda }}}_1^e$ can be obtained through (14), and (15). Meanwhile, ${\kappa }_{(\mathrm{1,}j)}$ is required due to the correlation between the predictive estimate and the virtual measurement. ${\mathrm{\Lambda }}^e\mathrm{,}{\bar{\mathrm{\Lambda }}}^e$ and ${\kappa }_{(\mathrm{1,}j)}$ constitute the pseudo-absolute locating message. Compared with the real absolute locating message, the elements ${\kappa }_{(\mathrm{1,}j)}\in {ℝ}^{p\times p}$ is added; consequently, when n relative measurements occur, $n{p}^2$ extra elements need to be broadcast. Considering Table 1, the elements in the pseudo-absolute locating message and that in the real relative locating message are $p+(p^2+p)/2+n{p}^2$ and $n(2p+2p^2+p)$ , and the former is less than the latter for any n and p.

The new algorithm can be summarized from the perspectives of sender and receiver. Sender: using all the partner estimates, an observation equation group is constituted, where the measuring agent's state is unknown parameters, to be computed. It can be solved by a common fusion technique and the solution is regarded as a virtual absolute measurement. Then, a set of pseudo-absolute locating message is generated. The complete procedure is provided in Algorithm 3.3. Receiver: the pseudo-absolute locating message including ${\mathrm{\Lambda }}^e\mathrm{,}{\bar{\mathrm{\Lambda }}}^e$ and ${\kappa }_{(\mathrm{1,}j)}$ is broadcast. When it is received by agent j, firstly, ${\bar{P}}_{j1}$ is updated according to (40); then the state and covariance of the receiver agent are updated as (12) and (13).

Algorithm 1 Similarity conversion from the relative measurements to the pseudo-absolute locating message

Require: the state set of the observed agents {2,3, ··· n}; Ensure: pseudo-absolute locating message ${\Lambda }_1^e,{\bar{\Lambda }}_1^e,{\kappa }_{(1,j)}$ 1: for j = 2 to n do 2: Calculate x̅₁^{j} according to (22); 3: Calculate h_1j ^a , h_1j ^b , according to (30), (31); 4: end for 5: Establish the covariance of the partner estimates R̅_v1; 6: Based on the available y̅₁, L₁, R̅_v1, then z̅₁ ^c and P̅₁₁ ^c are derived through (35); 7: Substituting v _c = x̅₁ ^c - Hx̅₁, S _c into (14), (15), then Λ₁ ^e , ${\Lambda }_1^e,{\bar{\Lambda }}_1^e$ are obtained; 8: According to (41), κ_{(1, j)} is obtained.

3.4. Analysis of similarity conversion loss

In order to evaluate the valuable information from the real measurement and the virtual measurement respectively, the determinant of Fisher information matrix(FIM) is employed as scalar measures. The case of one relative measurement $z_{12}$ occurring is analyzed, where the virtual measurements is equal to the parter estimate, i.e., ${\bar{z}}_1^c={\bar{x}}_1^{\left\{2\right\}}$ . For the virtual measurement ${\bar{z}}_1^c$ , according to the uncertainty of the partner estimate (i.e., the block diagonal element in $R_{v_1}$ ), the determinant of FIM on the measuring agent 1 is given by:

$F_a=\text{det}{\left(h_{12}^a{\bar{P}}_{22}{\left(h_{12}^a\right)}^{\mathrm{T}}+h_{12}^b{R}_{12}{\left(h_{12}^b\right)}^{\mathrm{T}}\right)}^{-1}\mathrm{.}$

(42)

For the real relative measurements, the determinant of FIM from the relative measurement $z_{12}$ is given by:

$\begin{array}{l}{F}_r=\text{det}\left[{\left(\frac{\partial z_{12}}{\partial x_1^{\mathrm{T}}}\right)}^{\mathrm{T}}{R}_{12}^{-1}\frac{\partial z_{12}}{\partial x_1^{\mathrm{T}}}\right]=\\ =\text{det}{\left[{\left(\frac{\partial z_{12}}{\partial x_1^{\mathrm{T}}}\right)}^{-1}{R}_{12}{\left(\frac{\partial z_{12}}{\partial x_1^{\mathrm{T}}}\right)}^{-\mathrm{T}}\right]}^{-1}=\\ =\text{det}{\left(h_{12}^b{R}_{12}{\left(h_{12}^b\right)}^{\mathrm{T}}\right)}^{-1}\mathrm{.}\end{array}$

(43)

$h_{12}^a{\bar{P}}_{22}{\left(h_{12}^a\right)}^{\mathrm{T}}$ and $h_{12}^b{R}_{12}{\left(h_{12}^b\right)}^{\mathrm{T}}$ are positive definite; therefore,

$\text{det}\left(h_{12}^a{\bar{P}}_{22}{\left(h_{12}^a\right)}^{\mathrm{T}}+h_{12}^b{R}_{12}{\left(h_{12}^b\right)}^{\mathrm{T}}\right)>\text{det}{h}_{12}^a{\bar{P}}_{22}{\left(h_{12}^a\right)}^{\mathrm{T}}+\text{det}\left(h_{12}^b{R}_{12}{\left(h_{12}^b\right)}^{\mathrm{T}}\right)$

(44)

and

$\text{det}{\left(h_{12}^a{\bar{P}}_{22}{\left(h_{12}^a\right)}^{\mathrm{T}}+h_{12}^b{R}_{12}{\left(h_{12}^b\right)}^{\mathrm{T}}\right)}^{-1}<\text{det}{\left(h_{12}^b{R}_{12}{\left(h_{12}^b\right)}^{\mathrm{T}}\right)}^{-1}\mathrm{.}$

(45)

It means that there exists information loss in the similarity conversion, and it is the uncertainty of the state estimate that determines the loss of the similarity conversion. When the self–estimation runs in most of the time, the uncertainty of the state estimate is mainly determined by the proprioceptive sensor.

Additionally, in the centralized EKF (gold-standard benchmark) or some centralized-equivalent DCL, the linear measurement model is obtained by Taylor expansion at the predictive estimates of the measuring and the observed agent; so the proprioceptive sensors of both affect the accuracy and the algorithm convergence. While in our algorithm, the linear measurement model is not necessary; however, as (27), the taylor expansion is required to linearize the partner observation model at the predictive estimate of the observed agent and the relative measurement value; therefore, the similarity conversion is sensitive to the observed agent's state and the relative measurement noise.

4. Simulation and Analysis

The performance of the proposed algorithm is evaluated with MATLAB simulations. Assume that three agents are approaching to the same object ‘T’, and each agent adjusts its orient dynamically by an ideal controller, ³ . Seeing Figure 2, the black lines denote the ground-truth trajectories. The homogeneous sensors are mounted on each agent. The basic simulation parameters are listed in Table 1. Simulation lasts for 180s. Agent 1 measures its relative poses to agents 2 and, 3 in a fixed interval. At the beginning of the simulation ( $t<20s$ ), the real relative measurements are used for establishing the state–error correlation among the agents instead of the similarity conversion. Three aspects are demonstrated by the simulation results: (1) less communication rounds; (2) less elements in the locating message, and (3) the impact factors on the proposed algorithm.

Figure 2.

Relative layout of three agents and trajectory comparison with different locating messages. The initial positions are marked by ‘⋅’. The encircled object is marked by ‘T’. The black line denotes the true trajectory (benchmark). The green line denotes the trajectory from IL algorithm (no relative measurements are used and no information exchanging among the agents); the blue line denotes the trajectory from the real relative locating message; the red line denotes the trajectory from the pseudo-absolute locating message.

Firstly, different communication rounds under different strategies are compared in Figure 3. The effects from different relative measurements do not act on the state update in a linear addition manner. Figure 3(a) shows that generating the current locating message depends on the updated results from the last locating message. Hence, (1) its generation and broadcast are triggered only when the last one being utilized by all agents is confirmed. (2) Each relative measurement requires a round of covariance information exchanging across the group. In the setting scene, two rounds of communication are required. While in the proposed algorithm, as Figure 3(b), all relative measurements are processed as a whole and one round communication is enough for sharing the locating message. In what follows, what and how many elements are transmitted in the locating message sharing communication are contrasted firstly.

Figure 3.

Comparison of the whole communication rounds with different strategies. (a) The procedure of the proposed strategy. (b) The procedure of the conventional strategy. The dotted frame denotes the non-communication part.

Table 2.

Simulation Parameters Setting

Parameter	Value
Simulation time T	180sec
Simulation period δ	0.1sec
Linear velocity V1,V2,V3	0.065m/s
Rotational velocity of ω1	0rad/s
Rotational velocity of ω2/ω3	−0.005/0.005rad/s
Measurement noise of linear velocity	N(0,(0.005m/s)2)
Measurement noise of rotational velocity	N(0,(0.05rad/s)2)
Measurement noise of relative distance	N(0,(0.02m)2)
Measurement noise of relative bearing/orientation	N(0,(0.002rad)2)

4.1. Elements comparison in different locating message

In the simulation, only one agent can obtain multiple relative measurements. Here, assume that agent 1 measures agents 2 and 3 simultaneously every 1.5 seconds. In order to share the benefits from the measurements, in the common methods, two sets of locating message, i.e., $\mathrm{\Lambda }(\mathrm{1,}i\mathrm{,}k)$ , $\bar{\mathrm{\Lambda }}(\mathrm{1,}i\mathrm{,}k)(i=\mathrm{2,3})$ , will be successively generated by the measuring agent 1. Then, these locating messages are spread to the others one by one. While in our algorithm, through the similarity conversion, two relative measurements are integrated as if an absolute measurement occurred in agent 1. As a result, only one set of pseudo-absolute locating message ${\mathrm{\Lambda }}^e(\mathrm{1,}k)$ , ${\bar{\mathrm{\Lambda }}}^e(\mathrm{1,}k)$ , ${\kappa }_{(\mathrm{1,2})}$ , ${\kappa }_{(\mathrm{1,3})}$ is generated by agent 1 and then spread across the network.

The communicated element items as well as their corresponding sizes are listed in Table 3 (the covariance is maintained as in [8] and the requirement for it is not contrasted). Figure 4 presents a contrast in term of the communicated elements with different numbers of relative measurements. It can be found that, compared with the real locating message, the number of elements in the pseudo-absolute locating message are cut down about 33% (from 27 to 18 numbers) for one relative measurement. This benefit will be more striking when more relative measurements occur.

Figure 4.

Comparison of the number of elements in different locating messages under different relative measurements

Table 3.

Comparison of the elements in different locating messages from different measurements (two relative measurements)

Real relative measurement		Virtual absolute measurement
item	numbers	item	numbers
Λ(1,2,k),Λ(1,3,k)	6+6	${\mathrm{\Lambda }}^e(\mathrm{1,}k)$	3
$\bar{\mathrm{\Lambda }}(\mathrm{1,2,}k)\mathrm{,}\bar{\mathrm{\Lambda }}(\mathrm{1,3,}k)$	21+21	${\bar{\mathrm{\Lambda }}}^e(\mathrm{1,}k)$	6
−	−	${\kappa }_{(\mathrm{1,2})}\mathrm{,}{\kappa }_{(\mathrm{1,3})}$	9+9
Total	54	Total	27

4.2. Localization performance comparison

Although the size of locating message is reduced, Figure 2 shows that, using the pseudo-absolute locating message, the position performance drops relative to that of the real locating message. Hence, the low communication cost is achieved at the expense of CL accuracy. As the statements in Section 3.4, it is caused by the similarity conversion loss.

For the measuring agent and the observed agent, Figures 5, and 6 respectively, present the position y errors between the true trajectory and the trajectory from the virtual absolute measurement. The bounding lines denote the $3\sigma$ confidence region for these errors. The position y errors from different locating message are within the corresponding $3\sigma$ curves. In order to highlight the localization performance brought by the similarity conversion of the relative measurements, no agent obtains the real absolute measurement; consequently, the uncertainty of each agent's state estimate grow continuously ⁴ .

Figure 5.

Position y error for the measuring agent 1. (a) In the case of the real measurement. (b) In the case of the virtual absolute measurement. The two bounding lines determine the 3σ region of confidence for the position y error, and they are calculated based on the covariance of the position y estimate.

Figure 6.

Position y error for the observed agent 2 under the two different measurements. Agent 2 always is the observed agent.

With respect to the measuring agent 1, all the relative measurements are converted into a virtual observation on it and then intensively acted on its state estimate. Consequently, in spite of the similarity conversion loss, the bound uncertainties from the real measurement and the virtual absolute measurement are in the same order as Figure 5. While, for the observed agent, the position uncertainty from the virtual measurement is almost one order of magnitude larger compared with the case of the real measurement; see Figure 6. It means the similarity conversion loss is burden by the observed agent. With the same three agents, if we assume that each agent takes turns at measuring the relative pose to the others (each agent can become the measuring agent in turn), then the similarity conversion loss is averagely burden over a period of time. As a result, for any agent, the localization uncertainty is reduced, and the localization uncertainties from the real measurement and the virtual measurement are in the same order as Figure 7.

Figure 7.

Position y error for agent 2 under the two different measurements. Each agent can become the measuring agent in turn.

4.3. Sensor performance effect on similarity conversion

The effect of the sensor performance on the proposed algorithm is depicted in Figure 8. It can be found that, with the performance of the relative measurement decreasing, the distance error between the true trajectory and the trajectory from the pseudo-absolute locating message grows. It means that the similarity conversion is sensitive to the performance of exteroceptive sensor. If the low precision exteroceptive sensor is adopted, the convergence of the proposed algorithm cannot be assured; of course, under this circumstance, the measurement information is not also significant for other CL algorithm. The effect of the proprioceptive sensor performance is also given in Figure 8(b). With the proprioceptive sensor performance decreasing, the localization performance is degenerating.

Figure 8.

Distance error between the trajectories from the pseudo-absolute locating message and the true trajectories of the observed agent 2. (a) Different exteroceptive sensor. (b) Different proprioceptive sensor. σ_b is a diagonal matrix of the measurement variance of relative distance/bearing/orientation in Table 2. σ_c is a diagonal matrix of the measurement variance of linear/rotational velocity.

The usefulness of our algorithm is also tested by the truth datasets from the Autonomous Space Robotics Lab (ASRL) of University of Toronto. The datasets are intended for studying the problems of cooperative localization. 5 iRobot Create robots move in a 15m × 8m indoor area randomly. The proprioceptive and relative measurement data, accurate landmark and groundtruth position data from a motion capture system are collected. More details can be found through http://asrl.utias.utoronto.ca/datasets/index.html.

A total of nine datasets are provided by the ASRL. The second dataset is employed in our test. Here, only part data in dataset 2 are employed for our test ⁵ . Different from our simulation scene, there exist the real absolute measurements in the second dataset. Using our algorithm, the results are shown as Figure 9. Compared with the unbounded drift trajectory (no cooperation), it can be found that the trajectories estimated by our algorithm are much close to the groundtruth trajectories. It manifests the usefulness of our algorithm.

Figure 9.

The trajectories of five agents by the proposed algorithm. ‘⋅’ denotes the initial position. □ denotes the landmark.

5. Conclusions

In order to reduce the communication requirement of CL, when one or more relative measurements occur in a group, they are converted into the pseudo-absolute locating message by the similarity conversion. In this way, the locating message form for CL are unified and the small-sized locating message is supplied for network flowing. Even so, the basic function of CL, i.e., the local measurement refines all the position estimates, can still be realized. In the context of multiple relative measurements, benefiting from the similarity conversion, one-pass communication mode is naturally employed, and it outperforms the sequential dissemination mode in term of the communication requirement.

In the similarity conversion, firstly, based on the relative measurement to each observed agent, a series of partner estimates on the same measuring agent are obtained. Secondly, all the partner estimates are used to construct a virtual observation equation where the measuring agent's state is an unknown variable, and its solution is regarded as a virtual absolute measurement on the measuring agent. Thirdly, the virtual absolute measurement is used for the EKF measurement update. In the conversion, two issues are addressed. One is that the various partner estimates on the same measuring agent are correlated. The other is that the virtual absolute measurement correlates with the predictive estimate of each observed agent. For the former, a close-form expression is given to characterize the error correlation among the partner estimates. For the latter, the EKF measurement update is re-modeled, and the extra elements corresponding to this correlation are added to the pseudo-absolute locating message packet; even so, the number of elements in the pseudo-absolute locating message still are smaller than that from the real relative measurement. With more relative measurements, the advantages in reducing communication requirement are more prominent.

In the proposed algorithm, the utilization of the real measurements is relatively conservative. The influence factors on the similarity conversion loss are analyzed and verified in the simulation. The proposed algorithm has a good effect when high performance exteroceptive sensors are employed. The proposed algorithm is not sensitive to the performance of proprioceptive sensor on the measuring agent.

The conclusions are obtained on the assumption of only one measuring agent in the group at a time step. In the next work, it should be extended to the case of multiple measuring agents at a time step.