Observability based Path Planning for Multi-Agent Systems to aid Relative Pose Estimation

When multiple coordinating vehicles are involved in missions like landing, docking, refueling, etc., knowledge of relative pose between vehicles is crucial. The absence of GPS in such scenarios is detrimental to the mission. Although vision-based techniques can be used to identify and localize one vehicle for the other, they can be computationally intensive and fail in dark or featureless environments. In this paper, a range-based relative navigation framework is proposed for missions like landing and refueling, where localization accuracy is enhanced in the presence of additional vehicles that help by communicating their sensor measurements. Satisfying the observability criteria of the system is crucial for localization accuracy. Therefore, a trajectory optimization technique coupled with Model Predictive Control is proposed to solve for trajectories of the additional vehicles to maximize the system’s observability. Using simulations, it is shown that having an individual controller for each additional vehicle is as effective as having a centralized control at a fraction of computation cost. Simulation results also show that it is efficient for each additional vehicle to maximize observability with respect to the "ego-vehicle" when all vehicles are in sensing range of each other.