# Multi-Robot Relative Pose Estimation in SE(2) with Observability Analysis: A Comparison of Extended Kalman Filtering and Robust Pose Graph Optimization

arXiv: https://arxiv.org/abs/2401.15313 · HTML: https://mlsanity.com/paper/2401.15313/

## Key facts

- No citing papers in the map yet (data through 14 Oct 2026).
- Builds on 28 of its 129 references that are in the map.
- First posted on arXiv 27 Jan 2024.

## Builds on (most cited first, 28 of 28)

- [A General and Adaptive Robust Loss Function](https://mlsanity.com/paper/1701.03077/) (2017)
- [Quaternion kinematics for the error-state Kalman filter](https://mlsanity.com/paper/1711.02508/) (2017)
- [DOOR-SLAM: Distributed, Online, and Outlier Resilient SLAM for Robotic Teams](https://mlsanity.com/paper/1909.12198/) (2019)
- [NTU VIRAL: A Visual-Inertial-Ranging-Lidar Dataset, From an Aerial Vehicle Viewpoint](https://mlsanity.com/paper/2202.00379/) (2022)
- [Estimating Uncertain Spatial Relationships in Robotics](https://mlsanity.com/paper/1304.3111/) (2013)
- [Certifiable Relative Pose Estimation](https://mlsanity.com/paper/2003.13732/) (2020)
- [Observability Analysis of Aided INS with Heterogeneous Features of Points, Lines and Planes](https://mlsanity.com/paper/1805.05876/) (2018)
- [VIRAL SLAM: Tightly Coupled Camera-IMU-UWB-Lidar SLAM](https://mlsanity.com/paper/2105.03296/) (2021)
- [Certifiably Correct Range-Aided SLAM](https://mlsanity.com/paper/2302.11614/) (2023)
- [SCORE: A Second-Order Conic Initialization for Range-Aided SLAM](https://mlsanity.com/paper/2210.03177/) (2022)
- [Advancing Mixture Models for Least Squares Optimization](https://mlsanity.com/paper/2103.02472/) (2021)
- [On Semidefinite Relaxations for Matrix-Weighted State-Estimation Problems in Robotics](https://mlsanity.com/paper/2308.07275/) (2023)
- [Nonlinear Unknown Input Observability: The Analytic Solution in the case of a Single Unknown Input](https://mlsanity.com/paper/1501.02968/) (2015)
- [Ultra-Wideband Aided Fast Localization and Mapping System](https://mlsanity.com/paper/1710.00156/) (2017)
- [UWB/LiDAR Fusion For Cooperative Range-Only SLAM](https://mlsanity.com/paper/1811.02854/) (2018)
- [A Code for Unscented Kalman Filtering on Manifolds (UKF-M)](https://mlsanity.com/paper/2002.00878/) (2020)
- [Toward Globally Optimal State Estimation Using Automatically Tightened Semidefinite Relaxations](https://mlsanity.com/paper/2308.05783/) (2023)
- [Expectation-Maximization for Adaptive Mixture Models in Graph Optimization](https://mlsanity.com/paper/1811.04748/) (2018)
- [AEROS: Adaptive RObust least-Squares for Graph-Based SLAM](https://mlsanity.com/paper/2110.02018/) (2021)
- [Observability Analysis and Keyframe-Based Filtering for Visual Inertial Odometry with Full Self-Calibration](https://mlsanity.com/paper/2201.04989/) (2022)
- [Nonlinear Unknown Input Observability and Unknown Input Reconstruction: The General Analytical Solution](https://mlsanity.com/paper/2201.07610/) (2022)
- [Robust Incremental Smoothing and Mapping (riSAM)](https://mlsanity.com/paper/2209.14359/) (2022)
- [UVIO: An UWB-Aided Visual-Inertial Odometry Framework with Bias-Compensated Anchors Initialization](https://mlsanity.com/paper/2308.00513/) (2023)
- [Distributed Ranging SLAM for Multiple Robots with Ultra-WideBand and Odometry Measurements](https://mlsanity.com/paper/2207.03700/) (2022)
- [FEJ-VIRO: A Consistent First-Estimate Jacobian Visual-Inertial-Ranging Odometry](https://mlsanity.com/paper/2207.08214/) (2022)
- [MOTLEE: Distributed Mobile Multi-Object Tracking with Localization Error Elimination](https://mlsanity.com/paper/2304.12175/) (2023)
- [Optimal Initialization Strategies for Range-Only Trajectory Estimation](https://mlsanity.com/paper/2309.09011/) (2023)
- [Range-Visual-Inertial Sensor Fusion for Micro Aerial Vehicle Localization and Navigation](https://mlsanity.com/paper/2311.09056/) (2023)

## Abstract (arXiv)

In this study, we address multi-robot localization issues, with a specific focus on cooperative localization and observability analysis of relative pose estimation. Cooperative localization involves enhancing each robot's information through a communication network and message passing. If odometry data from a target robot can be transmitted to the ego robot, observability of their relative pose estimation can be achieved through range-only or bearing-only measurements, provided both robots have non-zero linear velocities. In cases where odometry data from a target robot are not directly transmitted but estimated by the ego robot, both range and bearing measurements are necessary to ensure observability of relative pose estimation. For ROS/Gazebo simulations, we explore four sensing and communication structures. We compare extended Kalman filtering (EKF) and pose graph optimization (PGO) estimation using different robust loss functions (filtering and smoothing with varying batch sizes of sliding windows) in terms of estimation accuracy. In hardware experiments, two Turtlebot3 equipped with UWB modules are used for real-world inter-robot relative pose estimation, applying both EKF and PGO and comparing their performance.