Abstract

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.

1. 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 [35]. 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 [68]. 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 [1012].

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 [1315] studied the optimal formation of MLBL system. Accordingly, the optimal range between the USV and the target is studied in [16]. References [1719] 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) [2628]. 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 well-known matching condition does not hold [33]. Yong et al. presented an exponentially stable filter for linear discrete-time 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.

2. Problem Formulation

The notations used throughout the paper are as follows. denotes the -dimensional Euclidean space and is the identity matrix of size . For matrix , , and are its transpose and inverse, respectively. We use to denote the rank of . For random variable , the expectation is denoted by . We use and to indicate the prediction and estimation of . Some basic notions from estimation theory are defined as follows.

Definition 1. (see [37]). Let denotes a statistic, one say is an unbiased estimator of if .

Definition 2. (see [37]). The estimator is the minimum variance unbiased estimator (MVUE) of , if is unbiased, and if the variance of , , is less than or equal to the variance of every other unbiased estimator of .

Consider an earth fixed reference frame with on the water surface and the -axis pointing downward from the water surface. Suppose there are USVs to locate the target. The coordinate of the target at stamp is , . The kinematics models of the target is described as [38] where is the sampling period at stamp , and is the velocity of the target. Due to the USVs are on the water surface, we have . Hence, the coordinate of USVi at stamp is . Define 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 range (2), we formulate this system in a mixed linear/nonlinear, time-variant, discrete-time system. with where is the state at stamp , is the unknown input. is the measurement. and are the noises. According to the error model of range measurement [39, 40], we have where is the measurement error of range measurement , is a Gaussian stochastic process with , and is the parameter for the range-dependent error component. Simultaneously, we define where is a Gaussian stochastic process with .

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 as the range between the USVj and the target, we have

Combining (2) and (8), we have with

It follows that where is the measurement and is the zero mean white noise with covariance . and are known matrices with

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), is not of full column rank and the coefficient of is zero. Hence, by using the SISE algorithm for linear system (3) and (11), we could only estimate and . 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 . The noises and are uncorrelated white Gaussian noise with known covariances and . According to the measurement (11) and (4), we rewrite the measurement equation as where is the same as , , and . is the array of ones. is the zero mean white noise with covariance . , , , and are known matrices and vector with

Note that, in (18), and are the position predictions of target in local level plane. Hence, in measurement (16), 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 and to estimate the input and predict the state . where and represent the estimations of and at stamp , respectively. is the predictions of at stamp . and are the gain matrices that will be designed.

Step 2. According to the predicted state , is calculated. For the system in (14) and (16), we design the gain matrix to estimate the input and predict the state . where and represent the estimations of and , respectively. is the prediction of . is the gain matrix that will be determined. According to the definition of , we have . Hence, combining the estimated inputs and , we get the estimated input .

Step 3. For the system in (3) and (17), the state is estimated from the results of two previous steps. where is the prediction of . 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.

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 ;

3. 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 . We divide the unknown input into two components, namely, the velocity in the local level plane and the velocity in the heave direction . The estimation errors of the input and state are expressed as where .

3.1. Velocity Estimate in Local Level Plane

By minimizing the covariance matrix of the estimation error , we obtain the unbiased velocity estimate in the local level plane .

Substituting the state estimate (19) and the system (13) and (15) into the estimation error of , we obtain

Since and are uncorrelated white Gaussian noises, then we have

In MLBL, is not equal to zero, and it changes with time. Hence, for the system in (13) and (15), is an unbiased estimator of if and only if the following conditions are satisfied.

Theorem 1. Let is an unbiased estimator and is given by where , then (19) is the MVUE of .

Proof. Under the unbiasedness (27), the estimation error of is simplified as Substituting (29) into the covariance matrix s, then we have For the minimum variance estimation, the problem is equivalent to finding the gain matrix which minimizes the trace of (30) subject to (27). The Lagrangian is [41] where is the matrix of Lagrange multipliers. The minimum value of is reached when the following equation is satisfied. Substituting (27) into (32), we have Define , then we have . It is obvious that is positive definite and reversible. Combining (32) and (33), we obtain the gain matrix (28).

From Theorem 1, we get the gain matrix and prove that (19) is the MVUE of .

3.2. Velocity Estimate in Heave Direction

The velocity estimator in the heave direction is shown as (21). We estimate the velocity in two stages. Firstly, according to (20), we predict the position of the target in the local level plane . Then, based on the predicted position , we estimate the velocity in the heave direction . In this section, we will discuss the unbiased property of and design the gain matrix .

Similar to the velocity estimate in local level plane, by minimizing the covariance matrix of the estimation error , we get the unbiased velocity estimate in heave direction. The following conclusions are obtained.

For the system in (14) and (16), is an unbiased estimator of if and only if the following conditions are satisfied.

Let is an unbiased estimator and is given by where , then (21) is the MVUE of .

Note that under the unbiasedness (34), (21) is simplified as

4. 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 and and then estimate the minimum variance unbiased state .

4.1. Position Predict

The position predictors are shown as (20) and (22). Based on the estimated velocity in the heave direction , 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 in this section. The unbiased properties of and are also analysed.

Define as the prediction error of . From (14) and (22), we obtain

Since , then we have

Then, for the system in (14), is an unbiased estimator of if and only if the following conditions are satisfied.

Define as the prediction error of . Substituting the estimator (20) and the system (13) and (15) into this prediction error, we obtain

Since and are uncorrelated white Gaussian noises, then the prediction error of is simplified as

It is obviously that, for the system in (13) and (15), is an unbiased estimator of if and only if the following conditions are satisfied.

Theorem 2. Let is an unbiased estimator and the gain matrix is given by where and , then (20) is the MVUE of . The covariance matrix of the prediction error is where

Proof. Substituting the estimation error (29) into (40), the prediction error is simplified as Then, the covariance matrix is written as where . Taking the derivatives with respect to the gain matrix equal to zero yields Define , then we have . It is obvious that is positive definite and reversible. Based on the above analysis, we get (43). Substituting (43) into (47), we obtain the covariance matrix (44).

4.2. 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 and then design the gain matrix .

Define and as the prediction and estimation errors of , respectively. From (17) and (23), we obtain where is the 3 × 3 identity matrix. Since , then we have

By definition, we have

It can be seen that is an unbiased estimator if and only if the following equations are satisfied.

Based on the results in Section 3, we get that is an unbiased estimator of if and only if the following conditions are satisfied.

Theorem 3. Let is an unbiased estimator and the gain matrix is given by where ,, , and , then (23) is the MVUE of . The covariance matrix of the estimation error is

Proof. Substituting (37) and (40) into (49), the estimation error of can be written as with From (56), (57), and (58), is unrelated with and . Then, the covariance matrix is written as where Substituting (36) into (37), the prediction error of is written as It follows that where . The prediction errors and are shown in (46) and (61). It is obvious that and are independent, then we have . Taking the derivatives of (59) with respect to the gain matrix equal to zero yields Define , we have . Since is positive definite and reversible, we obtain the gain matrix (54). Substituting (54) into (59), we get the covariance matrix (55).

5. Estimation Conditions

In Sections 3 and 4, we have derived the necessary and sufficient conditions for unbiased estimators , , and , 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 is an unbiased estimator if and only if the following conditions are satisfied.

Combining it with the unbiased result in Section 4.2, we obtain that and are unbiased estimators if (64) and (65) are satisfied. By inductive arguments, for the system in (3) and (17), and are unbiased estimators if and only if the following conditions are satisfied. where .

From (65), we get that is a full column rank. The following constraint should be satisfied.

It follows that

Hence, the following equation should be satisfied. where . 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.

6. 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 tk = 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 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.

6.2. Example in General Formation

In this example, the parameters of the target are the same as the parameters in Table 2 and the USVs are in a general formation. The parameters of the USVs are shown in Table 3.

Figures 68 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.

7. 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.

Data Availability

The data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest

The authors declare that they have no conflicts of interest.

Acknowledgments

This work was supported by the National Natural Science Foundation of China under Grants 61733014 and 61703326.