Bio-Inspired Learning and Adaptation for Optimization and Control of Complex SystemsView this Special Issue
Research Article | Open Access
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.
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–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–8]. Among these systems, LBL system has the best positioning accuracy . 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–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–15] studied the optimal formation of MLBL system. Accordingly, the optimal range between the USV and the target is studied in . References [17–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) , Kalman filtering (KF) [23, 24], particle filtering (PF) , and maximum likelihood estimation (MLE) [26–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 . Kitanidis proposed an unbiased recursive filter to estimate the state of linear systems without prior information about the unknown input . 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 . Yong et al. presented an exponentially stable filter for linear discrete-time stochastic systems that simultaneously estimates the state and unknown input . Su et al. investigate the properties of the Kalman filter for linear stochastic time-varying systems with partially observed inputs . Fang et al. analysed the stability conditions of SISE algorithms for linear discrete-time systems with/without direct feedthrough . 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 ). Let denotes a statistic, one say is an unbiased estimator of if .
Definition 2. (see ). 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  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
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 .
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.
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 .
Since and are uncorrelated white Gaussian noises, then we have
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  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).
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.
Let is an unbiased estimator and is given by where , then (21) is the MVUE of .
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.
Since , then we have
Then, for the system in (14), is an unbiased estimator of if and only if the following conditions are satisfied.
Since and are uncorrelated white Gaussian noises, then the prediction error of is simplified as
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 .
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