Optimal position and velocity estimation for multi-USV positioning systems with range measurements

This paper investigates the problem on simultaneously estimating the velocity and position of the target for range-based multi-USV positioning systems. According to the range measurement and kinematics model of the target, we formulate this problem in a mixed linear/nonlinear discrete-time system. In this system, the input and state represent the velocity and position of the target, respectively. We divide the system into two components and propose a three-step minimum variance unbiased simultaneous input and state estimation (SISE) algorithm. First, we estimate the velocity in the local level plane and predict the corresponding position. Then, we estimate the velocity in the heave direction. Finally, we estimate the 3-dimensional (3D) velocity and position. We establish the unbiased conditions of the input and state estimation for the MLBL system. Simulation results illustrate the effectiveness of the problem formulation and demonstrate the performance of the proposed algorithm.