# Unscented Transformation-Based Multi-Robot Collaborative Self-Localization and Distributed Target Tracking

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Problem Formulation

#### 2.1. Models

#### 2.2. Motivation and Objective

## 3. UT-Based CLAT

#### 3.1. Propagation

#### 3.2. Update

#### 3.2.1. Private Update

#### 3.2.2. Target Measurement Update

#### 3.3. Neighbor Measurement Update and Target Information Fusion

## 4. Simulation

#### 4.1. Scenario 1

#### 4.2. Scenario 2

## 5. Experimental Validation of Quadrotors

#### 5.1. Robot and Target Dynamics Model

#### 5.2. Measurement Model

#### 5.3. Experiment Results and Analysis

## 6. Conclusions

## Author Contributions

## Funding

## Conflicts of Interest

## References

- Ren, W.; Beard, R.W.; Atkins, E.M. Information consensus in multivehicle cooperative control. IEEE Control Syst.
**2007**, 27, 71–82. [Google Scholar] - Alonso-Mora, J.; Baker, S.; Rus, D. Multi-robot formation control and object transport in dynamic environments via constrained optimization. Int. J. Robot. Res.
**2017**, 36, 1000–1021. [Google Scholar] [CrossRef] [Green Version] - Liu, Y.; Nejat, G. Multirobot cooperative learning for semiautonomous control in urban search and rescue applications. J. Field Robot.
**2016**, 33, 512–536. [Google Scholar] [CrossRef] - La, H.M.; Sheng, W. Distributed sensor fusion for scalar field mapping using mobile sensor networks. IEEE Trans. Cybern.
**2013**, 43, 766–778. [Google Scholar] [PubMed] - Panzieri, S.; Pascucci, F.; Setola, R. Multirobot localisation using interlaced extended Kalman filter. In Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing, China, 9–15 October 2006; pp. 2816–2821. [Google Scholar]
- Chen, L.; Arambel, P.O.; Mehra, R.K. Fusion under unknown correlation-covariance intersection as a special case. In Proceedings of the Fifth International Conference on Information Fusion, Annapolis, MD, USA, 8–11 July 2002; Volume 2, pp. 905–912. [Google Scholar]
- Li, H.; Nashashibi, F. Cooperative multi-vehicle localization using split covariance intersection filter. IEEE Intell. Transp. Syst. Mag.
**2013**, 5, 33–44. [Google Scholar] [CrossRef] - Li, H.; Nashashibi, F.; Yang, M. Split covariance intersection filter: Theory and its application to vehicle localization. IEEE Trans. Intell. Transp. Syst.
**2013**, 14, 1860–1871. [Google Scholar] [CrossRef] - Luft, L.; Schubert, T.; Roumeliotis, S.I.; Burgard, W. Recursive decentralized localization for multi-robot systems with asynchronous pairwise communication. Int. J. Robot. Res.
**2018**, 37. [Google Scholar] [CrossRef] - Hu, J.; Xie, L.; Zhang, C. Diffusion Kalman filtering based on covariance intersection. IEEE Trans. Signal Process.
**2012**, 60, 891–902. [Google Scholar] [CrossRef] - Wang, S.; Ren, W. On the convergence conditions of distributed dynamic state estimation using sensor networks: A unified framework. IEEE Trans. Control Syst. Technol.
**2018**, 26, 1300–1316. [Google Scholar] [CrossRef] - Wang, S.; Ren, W.; Chen, J. Fully distributed state estimation with multiple model approach. In Proceedings of the 2016 IEEE 55th Conference on Decision and Control (CDC), Las Vegas, NV, USA, 12–14 December 2016; pp. 2920–2925. [Google Scholar]
- Wang, S.; Lyu, Y.; Ren, W. Unscented-Transformation-Based Distributed Nonlinear State Estimation: Algorithm, Analysis, and Experiments. IEEE Trans. Control Syst. Technol.
**2018**, 99, 1–14. [Google Scholar] [CrossRef] - Carrillo-Arce, L.C.; Nerurkar, E.D.; Gordillo, J.L.; Roumeliotis, S.I. Decentralized multi-robot cooperative localization using covariance intersection. In Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Tokyo, Japan, 3–7 November 2013; pp. 1412–1417. [Google Scholar]
- Cunningham, A.; Paluri, M.; Dellaert, F. DDF-SAM: Fully distributed slam using constrained factor graphs. In Proceedings of the 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Taipei, Taiwan, 18–22 October 2010; pp. 3025–3030. [Google Scholar]
- Ren, W.; Beard, R.W.; Kingston, D.B. Multi-agent Kalman consensus with relative uncertainty. In Proceedings of the American Control Conference, Portland, OR, USA, 8–10 June 2005; pp. 1865–1870. [Google Scholar]
- Farahmand, S.; Roumeliotis, S.I.; Giannakis, G.B. Set-membership constrained particle filter: Distributed adaptation for sensor networks. IEEE Trans. Signal Process.
**2011**, 59, 4122–4138. [Google Scholar] [CrossRef] - Vaghefi, R.M.; Buehrer, R.M.; Wymeersch, H. Collaborative Sensor Network Localization: Algorithms and Practical Issues. Proc. IEEE
**2018**, PP, 1–26. [Google Scholar] - Chang, T.K.; Mehta, A. Optimal Scheduling for Resource-Constrained Multirobot Cooperative Localization. IEEE Robot. Autom. Lett.
**2018**, 3, 1552–1559. [Google Scholar] [CrossRef] - Kia, S.S.; Rounds, S.; Martinez, S. Cooperative Localization for Mobile Agents: A Recursive Decentralized Algorithm Based on Kalman-Filter Decoupling. IEEE Control Syst.
**2016**, 36, 86–101. [Google Scholar] [Green Version] - Kia, S.S.; Hechtbauer, J.; Gogokhiya, D.; Martínez, S. Server-Assisted Distributed Cooperative Localization Over Unreliable Communication Links. IEEE Trans. Robot.
**2017**, 99, 1–8. [Google Scholar] [CrossRef] - Chong, C.Y.; Chang, K.C.; Mori, S. A Review of Forty Years of Distributed Estimation. In Proceedings of the 2018 21st International Conference on Information Fusion (FUSION), Bonn, Germany, 10–12 October 2018; pp. 1–8. [Google Scholar]
- Battistelli, G.; Chisci, L.; Selvi, D. A distributed Kalman filter with event-triggered communication and guaranteed stability. Automatica
**2018**, 93, 75–82. [Google Scholar] [CrossRef] - Battistelli, G.; Chisci, L.; Mugnai, G.; Farina, A.; Graziano, A. Consensus-based linear and nonlinear filtering. IEEE Trans. Autom. Control
**2015**, 60, 1410–1415. [Google Scholar] [CrossRef] - Battistelli, G.; Chisci, L. Stability of consensus extended Kalman filter for distributed state estimation. Automatica
**2016**, 68, 169–178. [Google Scholar] [CrossRef] - Li, W.; Wei, G.; Han, F.; Liu, Y. Weighted average consensus-based unscented Kalman filtering. IEEE Trans. Cybern.
**2016**, 46, 558–567. [Google Scholar] [CrossRef] [PubMed] - Ajgl, J.; Straka, O. Covariance Intersection in Track-to-Track Fusion: Comparison of Fusion Configurations. IEEE Trans. Ind. Inform.
**2018**, 14, 1127–1136. [Google Scholar] [CrossRef] - Ahmad, A.; Tipaldi, G.D.; Lima, P.; Burgard, W. Cooperative robot localization and target tracking based on least squares minimization. In Proceedings of the 2013 IEEE International Conference on Robotics and Automation (ICRA), Karlsruhe, Germany, 6–10 May 2013; pp. 5696–5701. [Google Scholar]
- Huang, G.; Truax, R.; Kaess, M.; Leonard, J.J. Unscented iSAM: A consistent incremental solution to cooperative localization and target tracking. In Proceedings of the 2013 European Conference on Mobile Robots (ECMR), Barcelona, Spain, 25–27 September 2013; pp. 248–254. [Google Scholar]
- Morbidi, F.; Mariottini, G.L. Active target tracking and cooperative localization for teams of aerial vehicles. IEEE Trans. Control Syst. Technol.
**2013**, 21, 1694–1707. [Google Scholar] [CrossRef] - Meyer, F.; Hlinka, O.; Wymeersch, H.; Riegler, E.; Hlawatsch, F. Distributed Localization and Tracking of Mobile Networks Including Noncooperative Objects. IEEE Trans. Signal Inf. Process. Netw.
**2016**, 2, 57–71. [Google Scholar] [CrossRef] - Zhou, B.; Chen, Q.; Xiao, P. The error propagation analysis of the received signal strength-based simultaneous localization and tracking in wireless sensor networks. IEEE Trans. Inf. Theory
**2017**, 63, 3983–4007. [Google Scholar] [CrossRef] - Huang, G.P.; Roumeliotis, S.I. An Observability Constrained UKF for Improving SLAM Consistency; Tech. Rep.; University of Minnesota: Minneapolis, MN, USA, 2008. [Google Scholar]
- Zhu, J.; Kia, S.S. Consistent loosely coupled decentralized cooperative navigation for team of mobile agents. In Proceedings of the ION’s International Technical Meeting, Monterey, CA, USA, 30 January–2 February 2017. [Google Scholar]
- Chakraborty, A.; Misra, S.; Sharma, R.; Taylor, C.N. Observability Conditions for Switching Sensing Topology for Cooperative Localization. Unmanned Syst.
**2017**, 5, 141–157. [Google Scholar] [CrossRef] - Henriques, J.F.; Caseiro, R.; Martins, P.; Batista, J. High-speed tracking with kernelized correlation filters. IEEE Trans. Pattern Anal. Mach. Intell.
**2015**, 37, 583–596. [Google Scholar] [CrossRef] [PubMed]

**Figure 4.**Estimated trajectories of robots and the target in different colors (black dashed lines indicate the ground truth).

**Figure 7.**Self-localization estimation position root-mean-square errors (PRMSEs) of the unscented transformation-based collaborative self-localization and target tracking scheme (UT-CLAT) vs. the extended Kalman filter-based CLAT (EKF-CLAT) for robots 1–4.

**Figure 9.**Intel Aero RTF quadrotors (equipped with an onboard navigation system, ultra-wide bandwidth (UWB) transmitters, and downward cameras) and the TurtleBot ground robot.

**Figure 10.**Snapshot of the CLAT experiment setup (three quadrotors and corresponding captured image).

**Figure 11.**Trajectories of three quadrotors’ self-localization results and target tracking results with different colors. Ground truth is indicated by black lines.

Absolute Self-Localization Error (min/max) | Absolute Target Tracking Error (min/max) | |||||
---|---|---|---|---|---|---|

x(m) | y(m) | $\mathit{\theta}$(rad) | x(m) | y(m) | $\mathit{\theta}$(rad) | |

robot 1 | 0.05/0.35 | 0.03/0.41 | 0/0.08 | 0.03/0.5 | 0.03/0.66 | 0.01/0.65 |

robot 2 | 0.04/0.94 | 0.1/1.13 | 0/0.11 | 0.04/0.54 | 0.05/0.54 | 0.01/0.71 |

robot 3 | 0.01/1.31 | 0.12/0.96 | 0/0.07 | 0.02/0.61 | 0.05/0.47 | 0/0.72 |

robot 4 | 0.02/1.08 | 0.04/1.24 | 0/0.12 | 0.04/0.44 | 0.03/0.55 | 0/0.75 |

Item | Quantity |
---|---|

robot setup | circular center (in meters): |

${c}_{1}={[-5,-5,15]}^{\top}$, ${c}_{2}={[5,-5,16.5]}^{\top}$ | |

${c}_{3}={[-5,5,18]}^{\top}$ | |

control input: | |

${v}_{i}=0.35$ m/s, ${\omega}_{i}=0.035$ rad/s | |

${Q}_{i}^{v}=0.05{}^{2}$, ${Q}_{i}^{w}={(\pi /180)}^{2}$, ${Q}_{i}^{z}=0.1{}^{2}$ | |

initial state: | |

${\mathbf{P}}_{i,0}=\mathrm{diag}\left([{1}^{2},{1}^{2},0.5{}^{2},{(10\pi /180)}^{2}]\right)$ | |

${\mathbf{x}}_{1,0}={[-8.5,11.5,15,2.35]}^{\top}$, | |

${\mathbf{x}}_{2,0}={[-10,-11,16.5,-0.53]}^{\top}$, | |

${\mathbf{x}}_{3,0}={[-1,1,18,0.78]}^{\top}$ | |

target setup | control input: |

${v}_{i}=0.2$ m/s, ${\omega}_{i}=0$ rad/s | |

${Q}_{t}^{v}=0.02{}^{2}$, ${Q}_{t}^{\omega}={(0.5\pi /180)}^{2}$, ${Q}_{t}^{z}=0.05{}^{2}$ | |

initial state: | |

${\mathbf{P}}_{{t}_{i},0}=\mathrm{diag}\left([0.1{}^{2},0.1{}^{2},0.5{}^{2},{(\pi /180)}^{2}]\right)$, | |

${\mathbf{x}}_{{t}_{i},0}={[-15,-15,0,0.785]}^{\top}$ | |

measurement setup | ${\mathbf{R}}^{a}=\mathrm{diag}\left([0.2{}^{2},0.2{}^{2},0.1{}^{2}]\right)$, |

${\mathbf{R}}^{c}=0.1{}^{2}$, ${\mathbf{R}}^{t}=\mathrm{diag}\left([{5}^{2},{5}^{2}]\right)$ |

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

## Share and Cite

**MDPI and ACS Style**

Lyu, Y.; Pan, Q.; Lv, J.
Unscented Transformation-Based Multi-Robot Collaborative Self-Localization and Distributed Target Tracking. *Appl. Sci.* **2019**, *9*, 903.
https://doi.org/10.3390/app9050903

**AMA Style**

Lyu Y, Pan Q, Lv J.
Unscented Transformation-Based Multi-Robot Collaborative Self-Localization and Distributed Target Tracking. *Applied Sciences*. 2019; 9(5):903.
https://doi.org/10.3390/app9050903

**Chicago/Turabian Style**

Lyu, Yang, Quan Pan, and Jian Lv.
2019. "Unscented Transformation-Based Multi-Robot Collaborative Self-Localization and Distributed Target Tracking" *Applied Sciences* 9, no. 5: 903.
https://doi.org/10.3390/app9050903