Relative Pose Estimation using Range-only Measurements with Large Initial Uncertainty

Abstract

In this paper, a relative frame localization problem is addressed for pairs of vehicles with significant initial pose uncertainties. A relative frame Extended Kalman filter (EKF) is developed for the case where vehicles share odometry (body-frame delta position values), and are capable of getting inter-vehicle range measurements. The extended Kalman filter is not robust to large initial errors in this nonlinear system, therefore a Multi-Hypothesis approach is used to accommodate the unknown, up to approximate range, initial relative pose (relative position and orientation). This formulation provides a lightweight and robust relative initialization approach that identifies an accurate relative pose and would be appropriate for relative guidance, initialization of more complex (multi-vehicle) navigation approaches, or shared, synthetic aperture-like measurement processing. Results are presented for both simulated and hardware implementations of the filter.