Keywords

1 Introduction

In recent years, the multi-unmanned platform cooperative task execution mode has attracted more and more international attention (Kurazume et al. 1996). Multi-platform cooperation can expand surveillance, search and combat scope, and has the advantages of multiple tasks, high overall efficiency, increased system reliability, and strong stability (Fallon et al. 2010). Collaborative task execution by multiple unmanned platforms is an important trend in developing unmanned platform technology (Hirose et al. 1995; Han et al. 2020).

Multi-unmanned collaborative localization is a localization method to improve the system’s localization accuracy by observing each other in movement and using the constraint relationship between platforms (Bahr et al. 2009; Hua et al. 2011). According to the data fusion structure, it can be divided into centralized, distributed, and decentralized. Compared with the other two data fusion methods, decentralized data fusion has the advantage of not relying on a separate processing center to solve the problem of unmanned platform collaborative localization (Kia et al. 2014). In this way, the system stability and reliability can be improved effectively by avoiding a single platform’s failure, leading to the collaborative system collapse (Chen et al. 2020; Han et al. 2020).

Bahr et al. (2009) proposed a suboptimal algorithm in which each robot retains a set of extended Kalman filters. Only the robot obtaining the relative measured value can update its state. Each filter in the algorithm is updated only if its state is not related to the current update equation’s state. This algorithm’s main disadvantages are high computational complexity, large memory requirements, and more information required for each update. Roumeliotis et al. (2015) proposed a distributed computing decentralized algorithm equivalent to the centralized Kalman filter. The core of the algorithm is to disperse each covariance item of the covariance matrix on each platform. In the algorithm, each platform estimates the state covariance between its state and other platforms. A data fusion center is still needed in this calculation mode, and decentralized data fusion is not achieved. Kia et al. (2014) targeted the above problems and took the robot carrying out relative measurement as the centralized processing center. The centralized processing center computes and broadcasts intermediate variables that other robots use to update their estimates to match the centralized extended Kalman filtering forecast for collaborative localization. However, when the robot’s status update frequency is high, the whole collaborative system’s communication frequency will increase, increasing the communication burden.

In this paper, a decentralized data fusion algorithm based on covariance propagation is proposed, and a corresponding communication strategy is designed according to the proposed algorithm. The proposed algorithm’s accuracy is equivalent to that of the centralized collaborative location algorithm through simulation experiments.

2 Recursion Law of State Vector and Covariance Between Synergies

In order to analyze the covariance transfer relationship between synergies, it is assumed that there are two platforms.

Assume that for each unmanned platform, its discrete mathematical model is as follows:

$$ \left\{ \begin{gathered} {\varvec{X}}_{k} ={\varvec{\varPhi}}_{k/k - 1} {\varvec{X}}_{k - 1} +{\varvec{\varGamma}}_{k - 1} {\varvec{W}}_{k - 1} \hfill \\ {\varvec{Z}}_{k} = {\varvec{H}}_{k} {\varvec{X}}_{k} + {\varvec{V}}_{k} \hfill \\ \end{gathered} \right. $$
(1)

The basic assumption about noise is as follows:

$$ \begin{gathered} \left\{ \begin{gathered} E[{\varvec{W}}_{k} ] = {\mathbf{0}},\,E[{\varvec{W}}_{k} {\varvec{W}}_{j}^{{\text{T}}} ] = {\varvec{Q}}_{k} \delta_{kj} \hfill \\ \,E[{\varvec{V}}_{k} ] = {\mathbf{0}},\,\,E[{\varvec{V}}_{k} {\varvec{V}}_{j}^{{\text{T}}} ] = {\varvec{R}}_{k} \delta_{kj} \hfill \\ E[{\varvec{W}}_{k} {\varvec{V}}_{j}^{{\text{T}}} ] = {\mathbf{0}} \hfill \\ \end{gathered} \right. \hfill \\ {\varvec{Q}}_{k} \ge 0,\quad {\varvec{R}}_{k} > 0 \hfill \\ \end{gathered} $$
(2)

Suppose that the state quantity at the initial moment is \(\hat{\user2{X}}_{k - 1}\), and the covariance matrix is \({\varvec{P}}_{k - 1}\). After state prediction, we can get:

$$ \begin{gathered} \hat{\user2{X}}_{k/k - 1} ={\varvec{\varPhi}}_{k/k - 1} \hat{\user2{X}}_{k - 1} \hfill \\ {\varvec{P}}_{k/k - 1} ={\varvec{\varPhi}}_{k/k - 1} {\varvec{P}}_{k - 1}{\varvec{\varPhi}}_{k/k - 1}^{{\text{T}}} +{\varvec{\varGamma}}_{k - 1} {\varvec{Q}}_{k - 1}{\varvec{\varGamma}}_{k - 1}^{{\text{T}}} \hfill \\ \end{gathered} $$
(3)

After measurement and update, we get:

$$ \begin{gathered} \hat{\user2{X}}_{k} = \hat{\user2{X}}_{k/k - 1} + {\varvec{K}}_{k} \tilde{\user2{Z}}_{k} = \hat{\user2{X}}_{k/k - 1} + {\varvec{K}}_{k} ({\varvec{Z}}_{k} - {\varvec{H}}_{k} \hat{\user2{X}}_{k/k - 1} ) \\ = ({\varvec{I}} - {\varvec{K}}_{k} {\varvec{H}}_{k} )\hat{\user2{X}}_{k/k - 1} + {\varvec{K}}_{k} {\varvec{Z}}_{k} \\ = ({\varvec{I}} - {\varvec{K}}_{k} {\varvec{H}}_{k} ){\varvec{\varPhi}}_{k/k - 1} \hat{\user2{X}}_{k - 1} + {\varvec{K}}_{k} {\varvec{Z}}_{k} \\ \end{gathered} $$
(4)
$$ \begin{gathered} {\varvec{P}}_{k} = ({\varvec{I}} - {\varvec{K}}_{k} {\varvec{H}}_{k} ){\varvec{P}}_{k/k - 1} ({\varvec{I}} - {\varvec{K}}_{k} {\varvec{H}}_{k} )^{{\text{T}}} + {\varvec{K}}_{k} {\varvec{R}}_{k} {\varvec{K}}_{k}^{{\text{T}}} \\ = \,\,({\varvec{I}} - {\varvec{K}}_{k} {\varvec{H}}_{k} ){\varvec{\varPhi}}_{k/k - 1} {\varvec{P}}_{k - 1}{\varvec{\varPhi}}_{k/k - 1}^{{\text{T}}} ({\varvec{I}} - {\varvec{K}}_{k} {\varvec{H}}_{k} )^{{\text{T}}} \\ + ({\varvec{I}} - {\varvec{K}}_{k} {\varvec{H}}_{k} ){\varvec{\varGamma}}_{k - 1} {\varvec{Q}}_{k - 1}{\varvec{\varGamma}}_{k - 1}^{{\text{T}}} ({\varvec{I}} - {\varvec{K}}_{k} {\varvec{H}}_{k} )^{{\text{T}}} + {\varvec{K}}_{k} {\varvec{R}}_{k} {\varvec{K}}_{k}^{{\text{T}}} \\ \end{gathered} $$
(5)

For the convenience of simplification, let \({\varvec{\xi}}_{k} = ({\varvec{I}} - {\varvec{K}}_{k} {\varvec{H}}_{k} ){\varvec{\varPhi}}_{k/k - 1}\) and \({\varvec{\zeta}}_{k} = ({\varvec{I}} - {\varvec{K}}_{k} {\varvec{H}}_{k} ){\varvec{\varGamma}}_{k - 1}\), then:

$$ \hat{\user2{X}}_{k} \,\, = \,\,\,{\varvec{\xi}}_{k} \hat{\user2{X}}_{k - 1} + {\varvec{K}}_{k} {\varvec{Z}}_{k} $$
(6)
$$ {\varvec{P}}_{k} \,\,\rm{ = }\,\,{\varvec{\xi}}_{k} {\varvec{P}}_{k - 1} {\varvec{\xi}}_{k}^{{\text{T}}} + {\varvec{\zeta}}_{k} {\varvec{Q}}_{k - 1} {\varvec{\zeta}}_{k}^{{\text{T}}} + {\varvec{K}}_{k} {\varvec{R}}_{k} {\varvec{K}}_{k}^{{\text{T}}} $$
(7)

Assume that the position covariance matrix of the two platforms \(A,\,B\) at the time \(k - 1\) is:

$$ {\varvec{P}}_{{_{{\left[ {\begin{array}{*{20}c} A \\ B \\ \end{array} } \right]\left( {k - 1} \right)}} }} = \left[ {\begin{array}{*{20}c} {{\varvec{P}}_{{_{{A\left( {k - 1} \right)}} }} } & {{\varvec{P}}_{{_{{AB\left( {k - 1} \right)}} }} } \\ {{\varvec{P}}_{{_{{BA\left( {k - 1} \right)}} }} } & {{\varvec{P}}_{{_{{B\left( {k - 1} \right)}} }} } \\ \end{array} } \right] $$
(8)

Since the process noise and measurement noise of the platform are not related to the state quantity of other platforms, the position covariance matrix of the two platforms \(A,\,B\) at the time \(k\) is:

(9)

It can be seen that the change of the covariance term of the two platforms \(A,\,B\) at the time \(k\) is only related to \({\varvec{\xi}}_{A(k)}\) and \({\varvec{\xi}}_{B(k)}\), that is, exclusively related to the coefficient matrix in front of the recursive state Eq. (6).

Similarly, assuming that the time of two synergies is time \(k\) and time \(k + n\) respectively, the corresponding recursive relation of covariance term can be expressed as follows:

$$ {\varvec{P}}_{AB(k + n)} \,\,\rm{ = }\,\,{\varvec{\xi}}_{A(k + n)} {\varvec{\xi}}_{A(k + n - 1)} \ldots {\varvec{\xi}}_{A(k + 2)} {\varvec{\xi}}_{A(k + 1)} {\varvec{P}}_{AB(k)} \left( {{\varvec{\xi}}_{B(k + 1)} {\varvec{\xi}}_{B(k + 2)} \ldots {\varvec{\xi}}_{B(k + n - 1)} {\varvec{\xi}}_{B(k + n)} } \right)^{{\text{T}}} $$
(10)

As a result, in a collaborative localization, each platform preset a state and position vector of the same dimension recursive coefficient matrix. Its initial value for the unit matrix, updated every update or time measurement, the quantity of state before the coefficient matrix of left by recursive coefficient matrix, so that when the next collaborative localization can according to the recursive coefficient matrix of each platform and collaborative covariance item after the last time to calculate the covariance of the current time.

3 Design of Decentralized Collaborative Localization System

3.1 Problem Hypothesis

Assume that the entire collaborative localization system meets the following assumptions:

  • The whole collaborative system has a total of M independent unmanned platforms moving in n-dimensional space. Each unmanned platform can realize dead reckoning according to the sensors carried by itself.

  • Through communication network connection between platforms, each platform can broadcast its state information and sensor data at corresponding time nodes, and the communication delay can be ignored.

  • Each platform is equipped with the corresponding ranging sensor, measuring the platform’s distance information and other platforms.

3.2 Collaborative Localization System Equation

In this paper, each member’s position information and relative distance carry out collaborative localization. Instead of building the state equation of co-positioning separately, the prediction value of the state quantity involved in the measurement equation and the prediction covariance matrix is directly used to obtain each platform’s current state chain information reckoning the navigation position.

Assuming at the moment \(k\), the position error vector of the ith member is \(\delta {\varvec{X}}_{i} (k)\), and the covariance matrix is \({\varvec{P}}_{ii} (k)\). The relative distance between this member and other \(N - 1\) members is observed, and the corresponding position error vector is \(\delta {\varvec{X}}_{j} (k)\left( {j = 1,2, \ldots ,N,j \ne i} \right)\). The position covariance is \({\varvec{P}}_{jj} (k)\left( {j = 1,2, \ldots ,N,j \ne i} \right)\), then the state vector of collaborative localization is:

$$ {\varvec{X}}(k) = \left[ {\begin{array}{*{20}c} {\delta {\varvec{X}}_{i} (k)} & {\delta {\varvec{X}}_{1} (k)} & {\delta {\varvec{X}}_{2} (k)} & \ldots & {\delta {\varvec{X}}_{N} (k)} \\ \end{array} } \right] $$

Considering the position correlation among all members, the state covariance matrix is:

$$ {\varvec{P}}(k) = \left[ {\begin{array}{*{20}c} {{\varvec{P}}_{ii} (k)} & {{\varvec{P}}_{i1} (k)} & {{\varvec{P}}_{i2} (k)} & \ldots & {{\varvec{P}}_{iN} (k)} \\ {{\varvec{P}}_{1i} (k)} & {{\varvec{P}}_{11} (k)} & {{\varvec{P}}_{12} (k)} & \ldots & {{\varvec{P}}_{1N} (k)} \\ {{\varvec{P}}_{2i} (k)} & {{\varvec{P}}_{21} (k)} & {{\varvec{P}}_{22} (k)} & \ldots & {{\varvec{P}}_{2N} (k)} \\ \ldots & \ldots & \ldots & \ldots & \ldots \\ {{\varvec{P}}_{Ni} (k)} & {{\varvec{P}}_{N1} (k)} & {{\varvec{P}}_{N2} (k)} & \ldots & {{\varvec{P}}_{NN} (k)} \\ \end{array} } \right] $$

In the formula, \(\delta {\varvec{X}}_{i} (k)\) and \({\varvec{P}}_{ii} (k)\) are obtained from its state chain. \(\delta {\varvec{X}}_{j} (k)\) and \({\varvec{P}}_{jj} (k)\left( {j = 1,2, \ldots ,N,j \ne i} \right)\) are received from the state information broadcast by data link and \({\varvec{P}}_{ij} (k)\left( {i \ne j} \right)\) is given by the calculation method in the previous section. The observation distance between this member and the jth member is \(d_{ij}\), and the relationship between the distance and coordinates is as follows:

$$ d_{ij} (k) = h_{ij} \left( {{\varvec{X}}_{i} (k),{\varvec{X}}_{j} (k)} \right) + \zeta_{ij} (k) $$
(11)

The function expression of \(h_{ij} \left( {{\varvec{X}}_{i} (k),\,{\varvec{X}}_{j} (k)} \right)\) is:

$$ h_{ij} \left( {{\varvec{X}}_{i} (k),{\varvec{X}}_{j} (k)} \right) = \sqrt {\left( {x_{i} (k) - x_{j} (k)} \right)^{2} + \left( {y_{i} (k) - y_{j} (k)} \right)^{2} + \left( {z_{i} (k) - z_{j} (k)} \right)^{2} } $$

In the formula \(h_{ij}\) is the observation function of this member and the jth member. \({\varvec{X}}_{i} (k)\) and \({\varvec{X}}_{j} (k)\) are the position vector of \(i\) and \(j\) member, and \(\zeta_{ij} (k)\) is the ranging error.

The Equation is linearized to obtain:

$$ \begin{gathered} d_{ij} (k) = h_{ij} \left( {\hat{\user2{X}}_{i} (k),\hat{\user2{X}}_{j} (k)} \right) + \nabla h_{ij}^{i} \left( {{\varvec{X}}_{i} (k) - \hat{\user2{X}}_{i} (k)} \right){ + }\nabla h_{ij}^{j} \left( {{\varvec{X}}_{j} (k) - \hat{\user2{X}}_{j} (k)} \right) + \zeta_{ij} (k) \\ = \hat{d}_{ij} (k) + \nabla h_{ij}^{i} \left( {{\varvec{X}}_{i} (k) - \hat{\user2{X}}_{i} (k)} \right){ + }\nabla h_{ij}^{j} \left( {{\varvec{X}}_{j} (k) - \hat{\user2{X}}_{j} (k)} \right) + \zeta_{ij} (k) \\ = \,\,\hat{d}_{ij} (k) + \nabla h_{ij}^{i} \delta {\varvec{X}}_{i} (k){ + }\nabla h_{ij}^{j} \delta {\varvec{X}}_{j} (k) + \zeta_{ij} (k) \\ \end{gathered} $$
(12)

Among them:

$$ \nabla h_{ij}^{i} = \left. {\frac{{\partial h_{ij}^{i} }}{{\partial {\varvec{X}}_{i} (k)}}} \right|{}_{{{\varvec{X}}_{i} (k) = \hat{\user2{X}}_{i} (k)}},\,\,\,\nabla h_{ij}^{j} = \left. {\frac{{\partial h_{ij}^{j} }}{{\partial {\varvec{X}}_{j} (k)}}} \right|{}_{{{\varvec{X}}_{j} (k) = \hat{\user2{X}}_{j} (k)}} $$

The corresponding observation equation is as follows:

$$ {\varvec{Z}}_{ij} (k) = {\varvec{H}}_{ij} (k){\varvec{X}}(k) + \zeta_{ij} (k) $$
(13)

In the formula:

$$ \begin{gathered} {\varvec{Z}}_{ij} (k) = d_{ij} (k) - \hat{d}_{ij} (k) \\ {\varvec{H}}_{ij} (k) = \left[ {\begin{array}{*{20}c} {\nabla h_{ij}^{i} } & \ldots & 0 & \ldots & {\nabla h_{ij}^{j} } & \ldots & 0 \\ \end{array} } \right] \\ \end{gathered} $$

According to Eq. (13), the corresponding observation \(N - 1\) equations can be written to form a complete observation equation.

3.3 System Design

According to the covariance calculation formula derived in the previous section, this paper designs a decentralized filtering algorithm equivalent to the centralized filtering algorithm. Two filters are designed for each platform. A filter is used to recursion its state chain, which can be different according to different sensors. The other filter is used for collaborative localization, which contains each platform’s position state quantity in the current collaborative localization system and their covariance information, so this filter on each platform is the same. The algorithm is described after the first measurement update of collaborative localization is completed to facilitate description.

Step 1 state recursion and recursion coefficient matrix recursion

According to the state estimate and state covariance obtained after collaborative localization, each platform takes the initial value as the time update and measurement update according to its different sensors. For each time update or measurement update, the coefficient matrix in front of the state quantity is left multiplied by the recursive coefficient matrix. The recursive coefficient matrix is updated accordingly.

Step 2 Ranging

Each platform of the collaborative system shall conduct ranging between platforms following the agreed frequency and range rules.

Step 3 Data broadcasting

The data broadcast stage is divided into two parts:

  1. 1.

    The ranging information (including the length of the distance and the platforms’ names at both ends of the distance) is broadcast outward by the platform that conducts ranging at the agreed time point so that each platform gets the same distance information.

  2. 2.

    At the time node of ranging, each platform in the collaborative localization system broadcasts its current state chain information, including the estimated value of state quantity, state covariance matrix, and a recursive coefficient matrix.

Step 4 Collaborative localization filter setting

The state vector and covariance matrix are set as follows:

  • The state vector of collaborative localization is composed of combining the position state information broadcast by each platform. The dimension and value of the state vector of the collaborative localization filter for each platform are equal.

  • The state vector covariance matrix is composed of two parts. The covariance matrix’s diagonal block, namely the covariance item of each platform’s position, is obtained from the broadcast state covariance matrix. The covariance matrix’s non-diagonal block, namely the covariance item of the position state quantity between each platform, is calculated by the transmitted recursive coefficient matrix and the locally saved covariance item after the last collaborative localization according to Eq. (10).

Step 5 Collaborative localization measurement update

Based on the state vector and covariance matrix obtained in step 4, the corresponding measurement equation is constructed according to the transmitted distance information, and the measurement is updated.

After the measurement update, the obtained self-state estimation and covariance information are brought back to the platform’s state chain. In contrast, the covariance items with other platforms are saved locally to use the new covariance items before the next collaborative localization.

It should be noted that, in the first collaborative localization, the non-diagonal blocks of the initial covariance matrix are all zero because there is no correlation between platforms.

4 Experimental Results and Performance Analysis

Digital simulation analysis is carried out to verify the effectiveness of the proposed algorithm. In the simulation, a total of three mobile land platforms were simulated for collaborative localization. Each platform carried out dead reckoning through inertial navigation and odometer, as shown in Table 1 for specific parameters. The simulation trajectory is shown in Fig. 1. The simulation time is 380 s in total. When comparing the accuracy of dead reckoning and collaborative localization, the ranging frequency is set at 1 Hz. Distance measurement was carried out at 1 S, 100 S, 200 S, and 300 S, respectively, when the collaborative localization algorithms’ accuracy differences were compared to observe covariance’s influence on the collaborative localization results.

Table 1. Platform sensor parameters
Full size table
Fig. 1.
figure 1

Platform trajectory diagram

Full size image

Figure 2 respectively show the positioning error and heading Angle error of the platform in the co-positioning mode without covariance transmission and dead reckoning mode. It can be seen that both the positioning error and heading Angle error are significantly suppressed in the collaborative localization mode. In terms of localization error, each platform’s last localization error is 6–12 m in the single dead reckoning mode. The last localization error is up to 3 m in the collaborative localization mode, and the error is reduced by 50%–75%. In terms of heading Angle error, each platform’s final heading Angle error reached 0.5° in the single dead reckoning mode. The last heading Angle error was less than a 0.23° in the collaborative localization mode, which reduced by 54%.

Fig. 2.
figure 2

Comparison diagram of dead reckoning and collaborative localization

Full size image

Figure 3 is a covariance passes centralized collaborative localization algorithm and distributed collaborative localization algorithm of location error and the heading Angle error contrast figure. It can be found by comparing the location precision of the two quite, can draw based on covariance propagation of distributed collaborative localization accuracy with the centralized location on the precision are equivalent.

Fig. 3.
figure 3

Comparison diagram of centralized and distributed

Full size image

Figure 4 shows the comparison of location errors and heading Angle errors under two different collaborative localization modes of covariance transfer and non-covariance transfer. It can be seen that under the covariance transfer mode, the effect of collaborative localization is further improved. As can be seen from the figure, after four collaborative localization times, the positioning error can be reduced by 49.3% at most, 10.3% at least, and 24.1% on average. The heading Angle is reduced by 13.6% at most, 7.1% at least, and 11.9% on average. As can be seen from the figure, when measuring collaborative localization with distance each time, the correlation between locations is taken into account in the collaborative localization mode of covariance transmission, which reduces the influence of over-correction of distance. It can be seen that with the increase of the number of collaborative localization, the accuracy of this collaborative localization mode is improved more obviously.

Fig. 4.
figure 4

Comparison diagram of platform location error

Full size image

5 Conclusions

In this paper, based on the Kalman filter, the transfer equation of covariance between two collaborative localization methods is derived. A decentralized collaborative localization algorithm equivalent to the centralized filtering method is designed. The simulation results show that the collaborative localization algorithm based on covariance transfer can reduce the over-correction of the collaborative localization position by distance information according to the correlation of position errors between various platforms and improve the location accuracy of the collaborative localization. The improvement effect is more obvious with the increase of time. The algorithm has a certain reference value to the design of a multi-platform decentralized collaborative positioning system.