Optimal Position and Velocity Estimation for Multi-USV Positioning Systems with Range Measurements

Algorithm 1

Framework of three-step SISE algorithm.

Input:

  Positions of USVs , ;

  Position estimation and variance ;

  Measurements and ;

Output:

  Position estimation and velocity estimation ;

  Step 1: Estimate velocity and predict position

 1: ;

 2: ;

 3: ;

 4: ;

 5: ;

 6: ;

 7:

       ;

 8:

       ;

 9: ;

  Step 2: Estimate velocity and predict position

10: ;

11: ;

12: ;

13: ;

14:

      ;

15: ;

  Step 3: Estimate position

16: ;

17: ;

18: ;

19: ;

20: ;

  Return Position estimation and variance ;