Optimal Spatial-Temporal Triangulation for Bearing-Only Cooperative Motion Estimation

TL;DR

The paper tackles bearing-only cooperative motion estimation for multi-robot teams by moving beyond DKF toward a distributed recursive least squares framework. It introduces spatial-temporal triangulation (STT), which fuses information across spatially separated observers and temporal history via a carefully designed objective function, enabling a recursive, numerically stable estimator. The authors prove exponential convergence of STT despite rank-deficient measurement matrices and demonstrate its superiority over state-of-the-art DKFs in accuracy and convergence speed, with performance close to a centralized Kalman filter. A real-world vision-based autonomous pursuit system with three pursuers and one target validates STT under practical conditions, highlighting its potential for real-time, bandwidth-efficient bearing-only estimation in cooperative robotics. The work advances bearing-only estimation by integrating triangulation geometry, DRLS optimization, and rigorous stability guarantees, offering practical impact for autonomous target pursuit and related multi-robot sensing tasks.

Abstract

Vision-based cooperative motion estimation is an important problem for many multi-robot systems such as cooperative aerial target pursuit. This problem can be formulated as bearing-only cooperative motion estimation, where the visual measurement is modeled as a bearing vector pointing from the camera to the target. The conventional approaches for bearing-only cooperative estimation are mainly based on the framework distributed Kalman filtering (DKF). In this paper, we propose a new optimal bearing-only cooperative estimation algorithm, named spatial-temporal triangulation, based on the method of distributed recursive least squares, which provides a more flexible framework for designing distributed estimators than DKF. The design of the algorithm fully incorporates all the available information and the specific triangulation geometric constraint. As a result, the algorithm has superior estimation performance than the state-of-the-art DKF algorithms in terms of both accuracy and convergence speed as verified by numerical simulation. We rigorously prove the exponential convergence of the proposed algorithm. Moreover, to verify the effectiveness of the proposed algorithm under practical challenging conditions, we develop a vision-based cooperative aerial target pursuit system, which is the first of such fully autonomous systems so far to the best of our knowledge.

Figures (5)

• Figure 1: Estimation results of CMKF, CIKF, HCMCI-KF, GTSAM, and STT algorithms in a simulation trial. The target moves along a circle. There are 10 observers, and each of them can obtain information from the three closest neighboring observers. The topology of the network is shown in the up-leftmost subfigure.
• Figure 2: The performance of different algorithms in estimating the target's state with circle motion and square motion, respectively.
• Figure 3: The influence of measure noises on the estimation of different algorithms.
• Figure 4: The MAV platforms and flight experimental scenario.
• Figure 5: Flight experimental results.

Theorems & Definitions (14)

• Lemma 1: Expression of $\mathbb{E}[\boldsymbol{\eta}_{i,k}]$
• Proof 1
• Lemma 2: Expression of $\sum_{t =1}^k\lambda_{t}^{(k)}\mathbf{S}_{i,t}^{(k)}$
• Proof 2
• Lemma 3: Value of $\sigma_{\min}(\mathbf{P}_{i,t} +\mathbf{P}_{j,t})$
• Proof 3
• Lemma 4: Lower bound of $\sigma_{\min}(\bar{ \mathbf{P}}_{i,t})$
• Proof 4
• Lemma 5: Lower bound of $\sigma_{\min}\left(\sum_{t =1}^k\lambda_t^{(k)}\mathbf{F}_t^{(k)}\right)$
• Proof 5