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.


Introduction
Since electromagnetic signal decays quickly in the water, the well-known GPS cannot be used [1,2].The acoustic positioning systems play an important role for underwater positioning [3][4][5].These systems are widely applied in many underwater tasks, including salvage operations, minehunting, animal tracking, marine archaeology, oceanographic survey, and military activities.Classical underwater acoustic positioning systems include long baseline (LBL) system, short baseline (SBL) system, and ultrashort baseline (USBL) system [6][7][8].Among these systems, LBL system has the best positioning accuracy [9].However, it has several drawbacks, for example, difficult to obtain the positions of the seabed transponders, fixed and limited positioning regions, and hard to place and recover the transponders [10][11][12].
Moving long baseline (MLBL) system is a generalization of LBL system by replacing the precalibrated arrays of static transponders with unmanned surface vessels (USVs).Figure 1 shows the schematic of multi-USV positioning system.It overcomes the shortcomings of LBL system described above.The recent researches of MLBL system are concentrated on the positioning algorithms, the optimal formation, and formation control.References [13][14][15] studied the optimal formation of MLBL system.Accordingly, the optimal range between the USV and the target is studied in [16].References [17][18][19] provided some resource-reducing data transmission approaches in the sensor network.References [20,21] studied the formation control of the underwater vehicles.In past few years, many positioning algorithms based on the range measurements have been proposed in the literature, such as least squares (LS) [22], Kalman filtering (KF) [23,24], particle filtering (PF) [25], and maximum likelihood estimation (MLE) [26][27][28].In LS and MLE algorithms, the target position is estimated by the current range measurements and unrelated with the velocity of the target.In KF and PF algorithms, the target position is estimated by the current range measurements, the previous position, and the velocity of the target.Among all these positioning algorithms, the KF and PF algorithms have better positioning accuracies.In both algorithms, more information, such as the velocity of the target, is used to estimate the position of the target.In some underwater tasks, such as salvage operations and marine archaeology, the velocity of the underwater vehicle can be measured by doppler velocity log (DVL) fitted to it, and the velocity of the target is the key to predict the position of the target in advance.However, in other tasks, such as animal tracking, the velocity of the target is hard to be measured.Hence, in this paper, we propose a method to simultaneously estimate the velocity and position of the target based on simultaneous input and state estimation (SISE).
In recent years, the unbiased minimum variance SISE for linear systems has been extensively studied.Li et al. presented extensive reviews for state filtering with unknown inputs [29].Kitanidis proposed an unbiased recursive filter to estimate the state of linear systems without prior information about the unknown input [30].Gillijns and De Moor proposed the unbiased minimum variance SISE for linear discrete-time systems with/without direct feedthrough [31,32].Floquet and Barbot designed an input and state delayed estimator for discrete-time linear systems even if some wellknown matching condition does not hold [33].Yong et al. presented an exponentially stable filter for linear discretetime stochastic systems that simultaneously estimates the state and unknown input [34].Su et al. investigate the properties of the Kalman filter for linear stochastic time-varying systems with partially observed inputs [35].Fang et al. analysed the stability conditions of SISE algorithms for linear discrete-time systems with/without direct feedthrough [36].Among all these SISE algorithms, the input is obtained by least square estimation and the state estimation problem is transformed into a KF problem.The main objective of this paper is to design a simultaneous velocity and position estimation method for range-based multi-USV positioning system.The main contributions of this paper are mainly three-fold.First, we formulate the positioning system in a mixed linear/nonlinear discrete-time system.In this system, the velocity and position of the target are seen as the input and state, respectively.Second, a three-step minimum variance unbiased SISE algorithm is proposed by converting the nonlinear measurement equation into two linear measurement equations.Finally, we analyse the estimation conditions for this system.
The remainder of this paper is organized as follows.In Section 2, we formulate the velocity and position estimation for the multi-USV positioning system.The unbiased minimum variance velocity and position estimation algorithms are designed in Sections 3 and 4, respectively.Section 5 derives the unbiased SISE conditions for this system.Section 6 illustrates simulation results to verify the effectiveness of problem formulation and demonstrate the performance of the proposed algorithm.In Section 7, we conclude with a brief discussion of ongoing and future work.

Problem Formulation
The notations used throughout the paper are as follows.ℝ n denotes the n-dimensional Euclidean space and I n is the identity matrix of size n.For matrix X, X T , and X −1 are its transpose and inverse, respectively.We use rank X to denote the rank of X.For random variable d, the expectation is denoted by E d .We use d and d to indicate the prediction and estimation of d.Some basic notions from estimation theory are defined as follows.
Consider an earth fixed reference frame O ≔ x 0 , y 0 , z 0 with z = 0 on the water surface and the z-axis pointing downward from the water surface.Suppose there are m USVs to locate the target.The coordinate of the target at stamp k is x k , y k , z k , i = 1, 2, … , m.The kinematics models of the target is described as [38] where t k is the sampling period at stamp k, and v x k , v y k , v z k is the velocity of the target.Due to the USVs are on the water surface, we have z i,k = 0. Hence, the coordinate of USVi at stamp k is x i,k , y i,k , 0 .Define r i,k as the range between the USVi and the target, we have In order to simultaneously estimate the velocity and position of the target, we transform the multi-USV positioning system into a time-variant discrete-time system.The kinematics model of the target and the range measurement are regarded as the process equation and measurement equation, respectively.Combining the kinematics model (1) and the 2 Complexity range (2), we formulate this system in a mixed linear/nonlinear, time-variant, discrete-time system. with where T ∈ ℝ m are the noises.According to the error model of range measurement r i,k [39,40], we have where ε i,k is the measurement error of range measurement r i,k , ε is a Gaussian stochastic process with ε ∼ N 0, σ 2 , and η is the parameter for the range-dependent error component.Simultaneously, we define where ε r is a Gaussian stochastic process with ε r ∼ N 0, σ r 2 .Since system in (3) and ( 4) is a mixed linear/nonlinear time-variant discrete-time system, it is hard to simultaneously estimate the state and input.Hence, we design a three-step unbiased minimum variance SISE algorithm to solve this problem.
Define r j,k as the range between the USVj and the target, we have Combining ( 2) and (8), we have Based on the above analysis, the mixed linear/nonlinear system in ( 3) and ( 4) is transformed into a linear system in (3) and (11).In measurement (11), B k is not of full column rank and the coefficient of z k is zero.Hence, by using the SISE algorithm for linear system (3) and ( 11), we could only estimate x k and y k .Hence, we redefine the system equation and design a three-step unbiased minimum variance SISE algorithm.We divide the process (3) into two parts: the kinematics model in the local level plane and the kinematics model in the heave direction, where and According to the measurement ( 11) and ( 4), we rewrite the measurement equation as where , and Y 2,k are known matrices and vector with 3 Complexity Note that, in ( 18), x k and y k are the position predictions of target in local level plane.Hence, in measurement (16), z k is the only unknown variable to be estimated.The details about them will be explained in the algorithm.As shown in Figure 2, the unbiased minimum variance SISE algorithm for the multi-USV positioning system is divided into three steps.
Step 1.For the system in ( 13) and ( 15), we design the gain matrices M 1,k and K 1,k to estimate the input V 1,k−1 and predict the state X 1,k .
and K 1,k are the gain matrices that will be designed.
Step 2. According to the predicted state X 1,k , Y 2,k is calculated.For the system in ( 14) and ( 16), we design the gain matrix M 2,k to estimate the input v z k−1 and predict the state z k .
where vz k−1 and ẑk−1 represent the estimations of v z k−1 and z k−1 , respectively.z k is the prediction of z k .M 2,k is the gain matrix that will be determined.According to the definition of the estimated inputs V1,k−1 and vz k−1 , we get the estimated input Vk−1.
Step 3.For the system in ( 3) and ( 17), the state X k is estimated from the results of two previous steps. where is the prediction of X k .K k is the gain matrix that will be designed.Note that the order of the algorithm cannot be changed.The framework of the three-step minimum variance unbiased simultaneous velocity and position estimation algorithm is illustrated in Algorithm 1.In the next two sections, we will discuss the details of this algorithm.

Velocity Estimate
Based on the above analysis, the velocity of the target is seen as the input.In this section, we establish the estimation of the unknown input V k−1 .We divide the unknown input into two components, namely, the velocity in the local level plane V 1,k−1 and the velocity in the heave direction v z k−1 .The estimation errors of the input and state are expressed as

Velocity Estimate in Local Level
Plane.By minimizing the covariance matrix of the estimation error V 1,k−1 , we obtain the unbiased velocity estimate in the local level plane V1,k−1.
Substituting the state estimate (19) and the system (13) and (15)   4 Complexity In MLBL, E V 1,k−1 is not equal to zero, and it changes with time.Hence, for the system in ( 13) and ( 15), V1,k−1 is an unbiased estimator of V 1,k−1 if and only if the following conditions are satisfied.E X 1,k−1 = 0, Theorem 1.Let V1,k−1 is an unbiased estimator and M 1,k is given by where Proof.Under the unbiasedness (27), the estimation error of Substituting (29) into the covariance matrix s , then we have For the minimum variance estimation, the problem is equivalent to finding the gain matrix M 1,k which minimizes the trace of (30) subject to (27).The Lagrangian is [41] Input: Positions of USVs Position estimation Xk and velocity estimation V1,k−1 ; Step 1: Estimate velocity V1,k−1 and predict position X 1,k 1: Step 2: Estimate velocity vz k−1 and predict position z k 10: Step 3: Estimate position Xk 16: Algorithm 1: Framework of three-step SISE algorithm.
where λ k−1 is the matrix of Lagrange multipliers.The minimum value of tr P V k−1 λ k−1 is reached when the following equation is satisfied.
From Theorem 1, we get the gain matrix M 1,k and prove that (19)

Velocity
Estimate in Heave Direction.The velocity estimator in the heave direction is shown as (21).We estimate the velocity v z k−1 in two stages.Firstly, according to (20), we predict the position of the target in the local level plane X 1,k .Then, based on the predicted position X 1,k , we estimate the velocity in the heave direction vz k−1 .In this section, we will discuss the unbiased property of vz k−1 and design the gain matrix M 2,k .
Similar to the velocity estimate in local level plane, by minimizing the covariance matrix of the estimation error v z k−1 , we get the unbiased velocity estimate in heave direction.
The following conclusions are obtained.
For the system in ( 14) and ( 16), vz k−1 is an unbiased estimator of v z k−1 if and only if the following conditions are satisfied.
Let vz k−1 is an unbiased estimator and M 2,k is given by where Note that under the unbiasedness (34), ( 21) is simplified as

Position Estimate
In Section 3, we estimated the velocity of the target.In this section, we will estimate the position of the target.As was mentioned above, the position of the target is seen as the state.First, we predict the states X 1,k and z k and then estimate the minimum variance unbiased state Xk .

Position Predict.
The position predictors are shown as (20) and (22).Based on the estimated velocity in the heave direction vz k−1 , we can easily predict the target position in the heave direction.In order to predict the target position in the local level plane, we will design the gain matrix K 1,k in this section.The unbiased properties of z k and X 1,k are also analysed.
Define z * k = z k − z k as the prediction error of z k .From ( 14) and ( 22), we obtain Since E w 2,k−1 = 0, then we have Then, for the system in ( 14), z k is an unbiased estimator of z k if and only if the following conditions are satisfied.
as the prediction error of X 1,k .Substituting the estimator (20) and the system ( 13) and ( 15) into this prediction error, we obtain Since w 1,k−1 and ξ 1,k−1 are uncorrelated white Gaussian noises, then the prediction error of X 1,k is simplified as It is obviously that, for the system in ( 13) and ( 15), X 1,k is an unbiased estimator of X 1,k if and only if the following conditions are satisfied.
k is an unbiased estimator and the gain matrix K 1,k is given by where where Proof.Substituting the estimation error ( 29) into (40), the prediction error X * 1,k is simplified as Then, the covariance matrix P 1,k is written as

47
where P Taking the derivatives with respect to the gain matrix K 1,k equal to zero yields It is obvious that F k is positive definite and reversible.Based on the above analysis, we get (43).Substituting (43) into (47), we obtain the covariance matrix (44).

Position Estimate.
In Section 4.1, we predicted the position of the target.In this section, we will estimate the position of the target by using the estimated velocity and the predicted position.First, we discuss the unbiased property of Xk and then design the gain matrix K k .
Define X * k ≔ X k − X k and X k ≔ X k − Xk as the prediction and estimation errors of X k , respectively.From ( 17) and ( 23), we obtain

49
where I 3 is the 3 × 3 identity matrix.Since E ξ k = 0, then we have By definition, we have It can be seen that Xk is an unbiased estimator if and only if the following equations are satisfied.
Based on the results in Section 3, we get that Xk is an unbiased estimator of X k if and only if the following conditions are satisfied.

Estimation Conditions
In Sections 3 and 4, we have derived the necessary and sufficient conditions for unbiased estimators V1,k−1 , vz k−1 , and Xk , respectively.In this section, we will analyse the corresponding conditions in multi-USV positioning system.
From the results about unbiased velocity estimates in Sections 3.1 and 3.2, we obtain that Vk−1 is an unbiased estimator if and only if the following conditions are satisfied.E X k−1 = 0, 64 Combining it with the unbiased result in Section 4.2, we obtain that Xk and Vk−1 are unbiased estimators if (64) and (65) are satisfied.By inductive arguments, for the system in 8 Complexity (3) and ( 17), Xk and Vk−1 are unbiased estimators if and only if the following conditions are satisfied.

66
where From (65), we get that C k T k−1 is a full column rank.The following constraint should be satisfied.
Hence, the following equation should be satisfied.
where p, q, r ∈ 1, 2, … , m .That is to say, two conditions should be satisfied: at least three USVs are required to locate the target, these three USVs should be noncollinear.

Simulations
In this section, we illustrate the effectiveness of the problem formulation and demonstrate the performance of the proposed algorithm.We assume that the MLBL system consists of four USVs.The system runs at the sampling period of t k = 20 s.We choose the parameters of the range measurement error as η = 0.001 m −1 and σ r = 1 m.The positioning error of USVi ε x i,k , ε y i,k follows standardized normal distribution.Table 1 shows the parameters used in the algorithm.The matrices of the time-variant, discrete-time system are described by Two examples are presented to verify the effectiveness of the proposed algorithm.In the first example, we identify the effect of the proposed algorithm in the optimal formation.In the second example, the validity of this algorithm is demonstrated in a general formation.
6.1.Example in Optimal Formation.In this example, the USVs are in the optimal formation.That is to say, four USVs are on the vertices of the square centered at the target.The parameters of the USVs and the target are shown in Table 2.The true and estimated trajectories of the target are shown in Figure 3.It can be seen that the target is well positioned.Figure 4 shows the comparison of the true and estimated values.The estimation errors of position and velocity are illustrated in Figure 5.These results demonstrate that the proposed algorithm can simultaneously estimate the position and velocity of the target in optimal formation.3. Figures 6-8 show the true and estimated trajectories of the target, comparison of the true and estimated values, and the estimation errors, respectively.The max and average values of absolute estimation errors in optimal and general formations are shown in Table 4. Simulation results show that the target is well positioned and the velocity of the target is effectively estimated.

Conclusion
In this paper, we consider the problem on how to simultaneously estimate the velocity and position of the target for multi-USV positioning system.Firstly, we formulate the MLBL system in a linear discrete-time system without direct feedthrough.In this system, the velocity and position of the target are seen as the input and state, respectively.Then, we propose a three-step minimum variance unbiased SISE algorithm to simultaneously estimate the velocity and position.The unbiased SISE conditions for this system are analysed.Finally, simulation results show the correctness of the problem formulation and the effectiveness of the algorithm.Besides, the existence condition for asymptotic stability and the experimental validation of the proposed algorithm may be explored and demonstrated in the future work.

Figure 2 :
Figure 2: Block diagram of three-step SISE algorithm for multi-USV positioning system.

Figure 3 :
Figure 3: True and estimated trajectories of the target in optimal formation.

Figure 4 :
Figure 4: Comparison of the true and estimated values in optimal formation.(a) True and estimated position.(b) True and estimated velocity.

Figure 5 :Figure 6 :
Figure 5: Estimation error in optimal formation.(a) Estimation error of position.(b) Estimation error of velocity.

6. 2 .
Example in General Formation.In this example, the parameters of the target are the same as the parameters in

Figure 7 :
Figure 7: Comparison of the true and estimated values in general formation.(a) True and estimated position.(b) True and estimated velocity.

25
Since w 1,k−1 and ξ 1,k are uncorrelated white Gaussian noises, then we have

Table 2 :
Parameters of USVs and target in optimal formation.
The prediction errors X *

Table 3 :
Parameters of USVs and target in general formation.

Table 4 :
The max and average estimation errors.