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

Authors: Wei Chen; Ruisheng Sun; Weisheng Yan
Journal: Complexity
 (2018)

---

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


---

## Body

## 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 [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 [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–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). Figure1 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 [16]. 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) [22], Kalman filtering (KF) [23, 24], particle filtering (PF) [25], 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).Figure 1
MLBL system consists of four USVs [16].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 Section2, 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.ℝn denotes the n-dimensional Euclidean space and In is the identity matrix of size n. For matrix X, XT, and X−1 are its transpose and inverse, respectively. We use rankX to denote the rank of X. For random variable d, the expectation is denoted by Ed. We use d¯ and d^ to indicate the prediction and estimation of d. 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 Eθ−θ^=0.Definition 2.
(see [37]). The estimator θ^ is the minimum variance unbiased estimator (MVUE) of θ, if θ^ is unbiased, and if the variance of θ^, varθ^≔Eθ−θ^θ−θ^T, is less than or equal to the variance of every other unbiased estimator of θ.Consider an earth fixed reference frameO≔x0,y0,z0 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 xk,yk,zk, i=1,2,…,m. The kinematics models of the target is described as [38]
(1)xk+1=xk+vkxtk,yk+1=yk+vkytk,zk+1=zk+vkztk,where tk is the sampling period at stamp k, and vkx,vky,vkz is the velocity of the target. Due to the USVs are on the water surface, we have zi,k=0. Hence, the coordinate of USVi at stamp k is xi,k,yi,k,0. Define ri,k as the range between the USVi and the target, we have
(2)ri,k2=xk−xi,k2+yk−yi,k2+zk2.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.
(3)Xk+1=Xk+TkVk+wk,(4)rk=fXk+ξr,k,with
(5)Tk≔diagtk,tk,tk,fXk≔f1,f2,…,fmT,fi≔xk−xi,k2+yk−yi,k2+zk2,where Xk≔xk,yk,zkT∈ℝ3 is the state at stamp k, Vk≔vkx,vky,vkzT∈ℝ3 is the unknown input. rk≔r1,k2,…,rm,k2T∈ℝm is the measurement. wk∈ℝ3 and ξr,k≔ξr1,k,…,ξrm,kT∈ℝm are the noises. According to the error model of range measurement ri,k [39, 40], we have
(6)εi,k=1+ηri,kε,where εi,k is the measurement error of range measurement ri,k, ε is a Gaussian stochastic process with ε∼N0,σ2, and η is the parameter for the range-dependent error component. Simultaneously, we define
(7)ξri,k=1+ηri,k2εr,where εr is a Gaussian stochastic process with εr∼N0,σr2.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.Definerj,k as the range between the USVj and the target, we have
(8)rj,k2=xk−xj,k2+yk−yj,k2+zk2.Combining (2) and (8), we have
(9)γki,j=αki,jxk+βki,jyk,with
(10)γki,j≔ri,k2−rj,k2−xi,k2−yi,k2+xj,k2+yj,k2,αki,j≔−2xi,k−xj,k,βki,j≔−2yi,k−yj,k.It follows that(11)γk=BkXk+ξ1,k,where γk≔γk1,2,γk2,3,…,γkm−1,mT∈ℝm−1 is the measurement and ξ1,k≔ξr1,k−ξr2,k,…,ξrm−1,k−ξrm,kT∈ℝm is the zero mean white noise with covariance R1,k. Bk and R1,k are known matrices with
(12)Bk≔αk1,2βk1,20αk2,3βk2,30⋮⋮⋮αkm−1,mβkm−1,m0,R1,k≔σr1,22−σr22000−σr22σr2,32−σr32000−σr32σr3,42⋱000⋱⋱−σrm−12000−σrm−12σrm−1,m2,σri2≔1+ηri,k4σr2,σri,j2≔1+ηri,k4σr2+1+ηrj,k4σr2.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), Bk is not of full column rank and the coefficient of zk is zero. Hence, by using the SISE algorithm for linear system (3) and (11), we could only estimate xk and yk. 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,
(13)X1,k=X1,k−1+T1,k−1V1,k−1+w1,k−1,(14)zk=zk−1+tk−1vkz−1+w2,k−1,where X1,k−1≔xk−1,yk−1T∈ℝ2, V1,k−1≔vkx−1,vky−1T∈ℝ2, and T1,k−1≔diagtk−1,tk−1. The noises w1,k−1∈ℝ2 and w2,k−1∈ℝ are uncorrelated white Gaussian noise with known covariances Q1,k−1 and Q2,k−1. According to the measurement (11) and (4), we rewrite the measurement equation as
(15)Y1,k=C1,kX1,k+ξ1,k,(16)Y2,k=Im×1zk+ξ2,k,(17)Yk=CkXk+ξk,where Y1,k is the same as γk, Yk≔Y1,kT,Y2,kTT∈ℝ2m−1, and ξk≔ξ1,kT,ξ2,kTT∈ℝ2m−1. Im×1 is the m×1 array of ones. ξ2,k is the zero mean white noise with covariance R2,k. C1,k, Ck, R2,k, and Y2,k are known matrices and vector with
(18)C1,k≔αk1,2βk1,2αk2,3βk2,3⋮⋮αkm−1,mβkm−1,m,Ck≔C1,k00Im×1,R2,k≔diagσ12,σ22,…,σm2,,σi2≔1+ηri,k2σ2,Y2,k≔φ1,k,φ2,k,…,φm,kT,φi,k≔ri,k2−x¯k−xi,k2−y¯k−yi,k21/2.Note that, in (18), x¯k and y¯k are the position predictions of target in local level plane. Hence, in measurement (16), zk 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 M1,k and K1,k to estimate the input V1,k−1 and predict the state X1,k.
(19)V^1,k−1=M1,kY1,k−C1,kX^1,k−1,(20)X¯1,k=X^1,k−1+T1,k−1V^1,k−1+K1,kY1,k−1−C1,k−1X^1,k−1,where V^1,k−1 and X^1,k−1 represent the estimations of V1,k−1 and Xk−1 at stamp k−1, respectively. X¯1,k≔x¯ky¯k is the predictions of X1,k at stamp k. M1,k and K1,k are the gain matrices that will be designed.Step 2.
According to the predicted stateX¯1,k, Y2,k is calculated. For the system in (14) and (16), we design the gain matrix M2,k to estimate the input vk−1z and predict the state zk.
(21)v^k−1z=M2,kY2,k−Im×1z^k−1,(22)z¯k=z^k−1+tk−1v^k−1z,where v^k−1z and z^k−1 represent the estimations of vk−1z and zk−1, respectively. z¯k is the prediction of zk. M2,k is the gain matrix that will be determined. According to the definition of V1,k−1, we have Vk−1=V1,k−1T,vk−1zTT. Hence, combining the estimated inputs V^1,k−1 and v^k−1z, we get the estimated input V^k−1.Step 3.
For the system in (3) and (17), the state Xk is estimated from the results of two previous steps.
(23)X^k=X¯k+KkYk−CkX¯k,where X¯k≔X¯1,kTz¯kTT is the prediction of Xk. Kk is the gain matrix that will be designed.Figure 2
Block diagram of three-step SISE algorithm for multi-USV positioning system.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 Algorithm1. In the next two sections, we will discuss the details of this algorithm.Algorithm 1: Framework of three-step SISE algorithm.
Input:
Positions of USVsxi,k,yi,k,zi,k, i=1,2,…,m;
Position estimationX^k−1 and variance Pk−1X;
MeasurementsY1,k−1 and Y1,k;
Output:
Position estimationX^k and velocity estimation V^1,k−1;
Step 1: Estimate velocity V^1,k−1 and predict position X¯1,k
1:L1,k=C1,kPk−1X1+Q1,k−1C1,kT+R1,k;
2:M1,k=T1,k−1TC1,kTL1,k−1C1,kT1,k−1−1T1,k−1TC1,kTL1,k−1;
3:V^1,k−1=M1,kY1,k−C1,kX^1,k−1;
4:Sk=I2−T1,k−1M1,kC1,kPk−1X1C1,k−1T;
5:Fk=C1,k−1Pk−1X1C1,k−1T+R1,k−1;
6:K1,k=SkFk−1;
7:X¯1,k=X^1,k−1+T1,k−1V^1,k−1
+K1,kY1,k−1−C1,k−1X^1,k−1;
8:Pk∣k−1X1=I2−T1,k−1M1,kC1,kPk−1X1+Q1,k−1
I2−T1,k−1M1,kC1,kT;
9:PkX1∗=Pk∣k−1X1+T1,k−1M1,kR1,kM1,kTM1,k−1T−SkFk−1SkT;
Step 2: Estimate velocity v^k−1z and predict position z¯k
10:Hk=Im×1Pk−1z+Q2,k−1Im×1T+R2,k;
11:M2,k=tk−1TIm×1THk−1Im×1tk−1−1tk−1TIm×1THk−1;
12:v^k−1z=M2,kY2,k−Im×1Z^k−1;
13:z¯k=z^k−1+tk−1v^k−1z;
14:Pkz∗=I−tk−1M2−kIm×1Pk−1z+Q2k−1
I−tk−1M2,kIm×1T+tk−1M2,kR2,kM2,kTtk−1T;
15:V^k−1=V^1,k−1Tv^k−1xTT;
Step 3: Estimate position X^k
16:Jk=CkPkX∗CkT+Rk,PkX∗=diagPkX1∗,Pkz∗;
17:PkX∗=diagPkX1∗,Pkz∗;
18:Kk=PkX∗CkTJk−1;
19:X^k=X¯k+KkYk−CkX¯k;
20:PkX=PkX∗−PkX∗CkTCkPkX∗CkT+Rk−1CkPkX∗;
Return Position estimation X^k and variance PkX;


## 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 inputVk−1. We divide the unknown input into two components, namely, the velocity in the local level plane V1,k−1 and the velocity in the heave direction vk−1z. The estimation errors of the input and state are expressed as
(24)V~k−1≔Vk−1−V^k−1,X~k−1≔Xk−1−X^k−1,where V~k−1≔V~1,k−1T,v~k−1zTT,X~k−1≔X~1,k−1T,z~k−1TT.


### 3.1. Velocity Estimate in Local Level Plane

By minimizing the covariance matrix of the estimation errorV~1,k−1, we obtain the unbiased velocity estimate in the local level plane V^1,k−1.Substituting the state estimate (19) and the system (13) and (15) into the estimation error of V1,k−1, we obtain
(25)V~1,k−1=V1,k−1−M1,kY1,k−C1,kX^1,k−1=I2−M1,kC1,kT1,k−1V1,k−1−M1,kC1,kX~1,k−1+w1,k−1+ξ1,k.Sincew1,k−1 and ξ1,k are uncorrelated white Gaussian noises, then we have
(26)EV~1,k−1=I2−M1,kC1,kT1,k−1EV1,k−1−M1,kC1,kEX~1,k−1.In MLBL,EV1,k−1 is not equal to zero, and it changes with time. Hence, for the system in (13) and (15), V^1,k−1 is an unbiased estimator of V1,k−1 if and only if the following conditions are satisfied.
(27)EX~1,k−1=0,I2=M1,kC1,kT1,k−1.Theorem 1.
LetV^1,k−1 is an unbiased estimator and M1,k is given by
(28)M1,k=T1,k−1TC1,kTL1,k−1C1,kT1,k−1−1T1,k−1TC1,kTL1,k−1,where L1,k=C1,kPk−1X1+Q1,k−1C1,kT+R1,k, then (19) is the MVUE of V1,k−1.Proof.
Under the unbiasedness (27), the estimation error of V1,k−1 is simplified as
(29)V~1,k−1=−M1,kC1,kX~1,k−1+w1,k−1+ξ1,k.
Substituting (29) into the covariance matrix s, then we have
(30)Pk−1V1=M1,kC1,kPk−1X1+Q1,k−1C1,kT+R1,kM1,kT.
For the minimum variance estimation, the problem is equivalent to finding the gain matrixM1,k which minimizes the trace of (30) subject to (27). The Lagrangian is [41]
(31)trPk−1V1λk−1=trM1,kC1,kPk−1X1+Q1,k−1C1,kT+R1,kM1,kT−2trI2−M1,kC1,kT1,k−1λk−1T,where λk−1 is the matrix of Lagrange multipliers. The minimum value of trPk−1Vλk−1 is reached when the following equation is satisfied.
(32)2C1,kPk−1X+Q1,k−1C1,kT+R1,kM1,kT−2C1,kT1,k−1λk−1T=0.
Substituting (27) into (32), we have
(33)λk−1=T1,k−1TC1,kTL1,k−1C1,kT1,k−1−1.
Definel1,k=C1,kX~1,k−1+w1,k−1+ξ1,k, then we have L1,k=El1,kl1,kT. It is obvious that L1,k is positive definite and reversible. Combining (32) and (33), we obtain the gain matrix (28).From Theorem1, we get the gain matrix M1,k and prove that (19) is the MVUE of V1,k−1.


### 3.2. Velocity Estimate in Heave Direction

The velocity estimator in the heave direction is shown as (21). We estimate the velocity vk−1z 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 v^k−1z. In this section, we will discuss the unbiased property of v^k−1z and design the gain matrix M2,k.Similar to the velocity estimate in local level plane, by minimizing the covariance matrix of the estimation errorv~k−1z, we get the unbiased velocity estimate in heave direction. The following conclusions are obtained.For the system in (14) and (16), v^k−1z is an unbiased estimator of vk−1z if and only if the following conditions are satisfied.
(34)Ez~k−1=0,I1=M2,kIm×1tk−1.Letv^k−1z is an unbiased estimator and M2,k is given by
(35)M2,k=tk−1TIm×1THk−1Im×1tk−1−1tk−1TIm×1THk−1,where Hk=Im×1Pk−1z+Q2,k−1Im×1T+R2,k, then (21) is the MVUE of vk−1z.Note that under the unbiasedness (34), (21) is simplified as
(36)v~k−1z=−M2,kIm×1z~k−1+w2,k−1+ξ2,k.


## 3.1. Velocity Estimate in Local Level Plane

By minimizing the covariance matrix of the estimation errorV~1,k−1, we obtain the unbiased velocity estimate in the local level plane V^1,k−1.Substituting the state estimate (19) and the system (13) and (15) into the estimation error of V1,k−1, we obtain
(25)V~1,k−1=V1,k−1−M1,kY1,k−C1,kX^1,k−1=I2−M1,kC1,kT1,k−1V1,k−1−M1,kC1,kX~1,k−1+w1,k−1+ξ1,k.Sincew1,k−1 and ξ1,k are uncorrelated white Gaussian noises, then we have
(26)EV~1,k−1=I2−M1,kC1,kT1,k−1EV1,k−1−M1,kC1,kEX~1,k−1.In MLBL,EV1,k−1 is not equal to zero, and it changes with time. Hence, for the system in (13) and (15), V^1,k−1 is an unbiased estimator of V1,k−1 if and only if the following conditions are satisfied.
(27)EX~1,k−1=0,I2=M1,kC1,kT1,k−1.Theorem 1.
LetV^1,k−1 is an unbiased estimator and M1,k is given by
(28)M1,k=T1,k−1TC1,kTL1,k−1C1,kT1,k−1−1T1,k−1TC1,kTL1,k−1,where L1,k=C1,kPk−1X1+Q1,k−1C1,kT+R1,k, then (19) is the MVUE of V1,k−1.Proof.
Under the unbiasedness (27), the estimation error of V1,k−1 is simplified as
(29)V~1,k−1=−M1,kC1,kX~1,k−1+w1,k−1+ξ1,k.
Substituting (29) into the covariance matrix s, then we have
(30)Pk−1V1=M1,kC1,kPk−1X1+Q1,k−1C1,kT+R1,kM1,kT.
For the minimum variance estimation, the problem is equivalent to finding the gain matrixM1,k which minimizes the trace of (30) subject to (27). The Lagrangian is [41]
(31)trPk−1V1λk−1=trM1,kC1,kPk−1X1+Q1,k−1C1,kT+R1,kM1,kT−2trI2−M1,kC1,kT1,k−1λk−1T,where λk−1 is the matrix of Lagrange multipliers. The minimum value of trPk−1Vλk−1 is reached when the following equation is satisfied.
(32)2C1,kPk−1X+Q1,k−1C1,kT+R1,kM1,kT−2C1,kT1,k−1λk−1T=0.
Substituting (27) into (32), we have
(33)λk−1=T1,k−1TC1,kTL1,k−1C1,kT1,k−1−1.
Definel1,k=C1,kX~1,k−1+w1,k−1+ξ1,k, then we have L1,k=El1,kl1,kT. It is obvious that L1,k is positive definite and reversible. Combining (32) and (33), we obtain the gain matrix (28).From Theorem1, we get the gain matrix M1,k and prove that (19) is the MVUE of V1,k−1.


## 3.2. Velocity Estimate in Heave Direction

The velocity estimator in the heave direction is shown as (21). We estimate the velocity vk−1z 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 v^k−1z. In this section, we will discuss the unbiased property of v^k−1z and design the gain matrix M2,k.Similar to the velocity estimate in local level plane, by minimizing the covariance matrix of the estimation errorv~k−1z, we get the unbiased velocity estimate in heave direction. The following conclusions are obtained.For the system in (14) and (16), v^k−1z is an unbiased estimator of vk−1z if and only if the following conditions are satisfied.
(34)Ez~k−1=0,I1=M2,kIm×1tk−1.Letv^k−1z is an unbiased estimator and M2,k is given by
(35)M2,k=tk−1TIm×1THk−1Im×1tk−1−1tk−1TIm×1THk−1,where Hk=Im×1Pk−1z+Q2,k−1Im×1T+R2,k, then (21) is the MVUE of vk−1z.Note that under the unbiasedness (34), (21) is simplified as
(36)v~k−1z=−M2,kIm×1z~k−1+w2,k−1+ξ2,k.


## 4. Position Estimate

In Section3, 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 X^k.


### 4.1. Position Predict

The position predictors are shown as (20) and (22). Based on the estimated velocity in the heave direction v^k−1z, 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 K1,k in this section. The unbiased properties of z¯k and X¯1,k are also analysed.Definez~k∗=zk−z¯k as the prediction error of zk. From (14) and (22), we obtain
(37)z~k∗=z~k−1+tk−1v~k−1z+w2,k−1.SinceEw2,k−1=0, then we have
(38)Ez~k∗=Ez~k−1+tk−1Ev~k−1z.Then, for the system in (14), z¯k is an unbiased estimator of zk if and only if the following conditions are satisfied.
(39)Ez~k−1=0,Ev~k−1z=0.DefineX~1,k∗=X1,k−X¯1,k as the prediction error of X1,k. Substituting the estimator (20) and the system (13) and (15) into this prediction error, we obtain
(40)X~∗1,k=X1,k−X^1,k−1−T1,k−1V^1,k−1−K1,kY1,k−1−C1,k−1X^1,k−1=I2−K1,kC1,kX~1,k−1+T1,k−1V~1,k−1+w1,k−1−K1,kξ1,k−1.Sincew1,k−1 and ξ1,k−1 are uncorrelated white Gaussian noises, then the prediction error of X1,k is simplified as
(41)EX~1,k∗=I2−K1,kC1,kEX~1,k−1+T1,k−1EV~1,k−1.It is obviously that, for the system in (13) and (15), X¯1,k is an unbiased estimator of X1,k if and only if the following conditions are satisfied.
(42)EX~1,k−1=0,EV~1,k−1=0.Theorem 2.
LetX¯1,k is an unbiased estimator and the gain matrix K1,k is given by
(43)K1,k=SkFk−1,where Sk=I2−T1,k−1M1,kC1,kPk−1X1C1,k−1T and Fk=C1,k−1Pk−1X1C1,k−1T+R1,k−1, then (20) is the MVUE of X1,k. The covariance matrix of the prediction error X~1,k∗ is
(44)PkX1∗=Pkk−1X1+T1,k−1M1,kR1,kM1,kTT1,k−1T−SkFk−1SkT,where
(45)Pkk−1X1=I2−T1,k−1M1,kC1,kPk−1X1+Q1,k−1I2−T1,k−1M1,kC1,kT.Proof.
Substituting the estimation error (29) into (40), the prediction error X~1,k∗ is simplified as
(46)X~1,k∗=I2−T1,k−1M1,kC1,kX~1,k−1+w1,k−1−K1,kC1,k−1X~1,k−1−K1,kξ1,k−1−T1,k−1M1,kξ1,k.
Then, the covariance matrixPkX1∗≔EX~1,k∗X~1,k∗T is written as
(47)PkX1∗=K1,kFkK1,kT−K1,kSkT−SkK1,kT+Pkk−1X1+T1,k−1M1,kR1,kM1,kTT1,k−1T,where Pk−1X1≔EX~1,k−1X~1,k−1T. Taking the derivatives with respect to the gain matrix K1,k equal to zero yields
(48)2FkK1,kT−2SkT=0.
Definefk=C1,k−1X~1,k−1+ξ1,k−1, then we have Fk=EfkfkT. It is obvious that Fk 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 Section4.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 X^k and then design the gain matrix Kk.DefineX~k∗≔Xk−X¯k and X~k≔Xk−X^k as the prediction and estimation errors of Xk, respectively. From (17) and (23), we obtain
(49)X~k=Xk−X¯k−KkCkXk+ξk−CkX¯k=I3−KkCkX~k∗−Kkξk,where I3 is the 3 × 3 identity matrix. Since Eξk=0, then we have
(50)EX~k=I3−KkCkEX~k∗.By definition, we have(51)EX~k∗=EX~1,k∗Ez~k∗.It can be seen thatX^k is an unbiased estimator if and only if the following equations are satisfied.
(52)EX~1,k∗=0,Ez~k∗=0.Based on the results in Section3, we get that X^k is an unbiased estimator of Xk if and only if the following conditions are satisfied.
(53)EX~k−1=0,EV~k−1=0.Theorem 3.
LetX^k is an unbiased estimator and the gain matrix Kk is given by
(54)Kk=PkX∗CkTJk−1,where Jk=CkPkX∗CkT+Rk,,PkX∗=diagPkX1∗,Pkz∗, Pkz∗=tk−1M2,kR2,kM2,kTtk−1T+tkPk−1z+Q2,k−1tkT, and tk=I−tk−1M2,kIm×1, then (23) is the MVUE of Xk. The covariance matrix of the estimation error X~k is
(55)PkX=PkX∗−PkX∗CkTCkPkX∗CkT+Rk−1CkPkX∗.Proof.
Substituting (37) and (40) into (49), the estimation error of Xk can be written as
(56)X~k=I3−KkCkX~1,k∗z~k∗−Kkξk,with
(57)X~1,k∗=I2−K1,kC1,kX~1,k−1+T1,k−1V~1,k−1+w1,k−1−K1,kξ1,k−1,(58)z~k∗=z~k−1+tk−1v~k−1z+w2,k−1.
From (56), (57), and (58), ξk is unrelated with X~1,k∗ and z~k∗. Then, the covariance matrix PkX≔EX~kX~kT is written as
(59)PkX=PkX∗−KkCkPkX∗−PkX∗CkTKkT+KkJkKkX,where
(60)PkX∗=EX~1,k∗X~1,k∗TEX~1,k∗z~k∗TEX~1,k∗z~k∗TTEz~k∗z~k∗T≔PkX1∗PkX1∗,z∗PkX1∗,z∗TPkz∗.
Substituting (36) into (37), the prediction error of zk is written as
(61)z~k∗=I−tk−1M2,kIm×1z~k−1−w2,k−1−tk−1M2,kξ2,k.
It follows that(62)Pkz∗=I−tk−1M2,kIm×1Pk−1z+Q2,k−1I−tk−1M2,kIm×1T+tk−1M2,kR2,kM2,kTtk−1T,where Pk−1z≔Ez~k−1z~k−1T. The prediction errors X~1,k∗ and z~k∗ are shown in (46) and (61). It is obvious that X~1,k∗ and z~k∗ are independent, then we have PkX1∗,z∗=0. Taking the derivatives of (59) with respect to the gain matrix Kk equal to zero yields
(63)−2PkX∗CkT+2KkJk=0.
Definejk=CkX~k∗+ξk, we have Jk=EjkjkT. Since Jk is positive definite and reversible, we obtain the gain matrix (54). Substituting (54) into (59), we get the covariance matrix (55).


## 4.1. Position Predict

The position predictors are shown as (20) and (22). Based on the estimated velocity in the heave direction v^k−1z, 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 K1,k in this section. The unbiased properties of z¯k and X¯1,k are also analysed.Definez~k∗=zk−z¯k as the prediction error of zk. From (14) and (22), we obtain
(37)z~k∗=z~k−1+tk−1v~k−1z+w2,k−1.SinceEw2,k−1=0, then we have
(38)Ez~k∗=Ez~k−1+tk−1Ev~k−1z.Then, for the system in (14), z¯k is an unbiased estimator of zk if and only if the following conditions are satisfied.
(39)Ez~k−1=0,Ev~k−1z=0.DefineX~1,k∗=X1,k−X¯1,k as the prediction error of X1,k. Substituting the estimator (20) and the system (13) and (15) into this prediction error, we obtain
(40)X~∗1,k=X1,k−X^1,k−1−T1,k−1V^1,k−1−K1,kY1,k−1−C1,k−1X^1,k−1=I2−K1,kC1,kX~1,k−1+T1,k−1V~1,k−1+w1,k−1−K1,kξ1,k−1.Sincew1,k−1 and ξ1,k−1 are uncorrelated white Gaussian noises, then the prediction error of X1,k is simplified as
(41)EX~1,k∗=I2−K1,kC1,kEX~1,k−1+T1,k−1EV~1,k−1.It is obviously that, for the system in (13) and (15), X¯1,k is an unbiased estimator of X1,k if and only if the following conditions are satisfied.
(42)EX~1,k−1=0,EV~1,k−1=0.Theorem 2.
LetX¯1,k is an unbiased estimator and the gain matrix K1,k is given by
(43)K1,k=SkFk−1,where Sk=I2−T1,k−1M1,kC1,kPk−1X1C1,k−1T and Fk=C1,k−1Pk−1X1C1,k−1T+R1,k−1, then (20) is the MVUE of X1,k. The covariance matrix of the prediction error X~1,k∗ is
(44)PkX1∗=Pkk−1X1+T1,k−1M1,kR1,kM1,kTT1,k−1T−SkFk−1SkT,where
(45)Pkk−1X1=I2−T1,k−1M1,kC1,kPk−1X1+Q1,k−1I2−T1,k−1M1,kC1,kT.Proof.
Substituting the estimation error (29) into (40), the prediction error X~1,k∗ is simplified as
(46)X~1,k∗=I2−T1,k−1M1,kC1,kX~1,k−1+w1,k−1−K1,kC1,k−1X~1,k−1−K1,kξ1,k−1−T1,k−1M1,kξ1,k.
Then, the covariance matrixPkX1∗≔EX~1,k∗X~1,k∗T is written as
(47)PkX1∗=K1,kFkK1,kT−K1,kSkT−SkK1,kT+Pkk−1X1+T1,k−1M1,kR1,kM1,kTT1,k−1T,where Pk−1X1≔EX~1,k−1X~1,k−1T. Taking the derivatives with respect to the gain matrix K1,k equal to zero yields
(48)2FkK1,kT−2SkT=0.
Definefk=C1,k−1X~1,k−1+ξ1,k−1, then we have Fk=EfkfkT. It is obvious that Fk 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 Section4.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 X^k and then design the gain matrix Kk.DefineX~k∗≔Xk−X¯k and X~k≔Xk−X^k as the prediction and estimation errors of Xk, respectively. From (17) and (23), we obtain
(49)X~k=Xk−X¯k−KkCkXk+ξk−CkX¯k=I3−KkCkX~k∗−Kkξk,where I3 is the 3 × 3 identity matrix. Since Eξk=0, then we have
(50)EX~k=I3−KkCkEX~k∗.By definition, we have(51)EX~k∗=EX~1,k∗Ez~k∗.It can be seen thatX^k is an unbiased estimator if and only if the following equations are satisfied.
(52)EX~1,k∗=0,Ez~k∗=0.Based on the results in Section3, we get that X^k is an unbiased estimator of Xk if and only if the following conditions are satisfied.
(53)EX~k−1=0,EV~k−1=0.Theorem 3.
LetX^k is an unbiased estimator and the gain matrix Kk is given by
(54)Kk=PkX∗CkTJk−1,where Jk=CkPkX∗CkT+Rk,,PkX∗=diagPkX1∗,Pkz∗, Pkz∗=tk−1M2,kR2,kM2,kTtk−1T+tkPk−1z+Q2,k−1tkT, and tk=I−tk−1M2,kIm×1, then (23) is the MVUE of Xk. The covariance matrix of the estimation error X~k is
(55)PkX=PkX∗−PkX∗CkTCkPkX∗CkT+Rk−1CkPkX∗.Proof.
Substituting (37) and (40) into (49), the estimation error of Xk can be written as
(56)X~k=I3−KkCkX~1,k∗z~k∗−Kkξk,with
(57)X~1,k∗=I2−K1,kC1,kX~1,k−1+T1,k−1V~1,k−1+w1,k−1−K1,kξ1,k−1,(58)z~k∗=z~k−1+tk−1v~k−1z+w2,k−1.
From (56), (57), and (58), ξk is unrelated with X~1,k∗ and z~k∗. Then, the covariance matrix PkX≔EX~kX~kT is written as
(59)PkX=PkX∗−KkCkPkX∗−PkX∗CkTKkT+KkJkKkX,where
(60)PkX∗=EX~1,k∗X~1,k∗TEX~1,k∗z~k∗TEX~1,k∗z~k∗TTEz~k∗z~k∗T≔PkX1∗PkX1∗,z∗PkX1∗,z∗TPkz∗.
Substituting (36) into (37), the prediction error of zk is written as
(61)z~k∗=I−tk−1M2,kIm×1z~k−1−w2,k−1−tk−1M2,kξ2,k.
It follows that(62)Pkz∗=I−tk−1M2,kIm×1Pk−1z+Q2,k−1I−tk−1M2,kIm×1T+tk−1M2,kR2,kM2,kTtk−1T,where Pk−1z≔Ez~k−1z~k−1T. The prediction errors X~1,k∗ and z~k∗ are shown in (46) and (61). It is obvious that X~1,k∗ and z~k∗ are independent, then we have PkX1∗,z∗=0. Taking the derivatives of (59) with respect to the gain matrix Kk equal to zero yields
(63)−2PkX∗CkT+2KkJk=0.
Definejk=CkX~k∗+ξk, we have Jk=EjkjkT. Since Jk 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 Sections3 and 4, we have derived the necessary and sufficient conditions for unbiased estimators V^1,k−1, v^k−1z, and X^k, respectively. In this section, we will analyse the corresponding conditions in multi-USV positioning system.From the results about unbiased velocity estimates in Sections3.1 and 3.2, we obtain that V^k−1 is an unbiased estimator if and only if the following conditions are satisfied.
(64)EX~k−1=0,(65)I3=MkCkTk−1.Combining it with the unbiased result in Section4.2, we obtain that X^k and V^k−1 are unbiased estimators if (64) and (65) are satisfied. By inductive arguments, for the system in (3) and (17), X^k and V^k−1 are unbiased estimators if and only if the following conditions are satisfied.
(66)EX~0=0,I3=MkCkTk−1,where Mk=diagM1,k,M2,k.From (65), we get that CkTk−1 is a full column rank. The following constraint should be satisfied.
(67)rankCkTk−1=rankCk=3.It follows that(68)rankC1,k=rankxp,k−x1,kyp,k−y1,kxp,k−x2,kyp,k−y2,k⋮⋮xp,k−xm,kyp,k−ym,k=2.Hence, the following equation should be satisfied.(69)xp,k−xq,kxp,k−xr,k≠yp,k−yq,kyp,k−yr,k,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.


## 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 oftk = 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 εxi,k,εyi,k follows standardized normal distribution. Table 1 shows the parameters used in the algorithm.Table 1
Simulation parameters.
X0
P0
Q1
Q2
(0,0,300)
diag103,103,103
diag10,10
10The matrices of the time-variant, discrete-time system are described by(70)Tk=diag20,20,20,C1,k≔αk1,2βk1,2αk2,3βk2,3αk3,4βk3,4,Ck≔C1,k00I4×1,Y1,k=γk1,2,γk2,3,γk3,4T,Y2,k≔φ1,k,φ2,k,φ3,k,φ4,kT.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 Table2. 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.Table 2
Parameters of USVs and target in optimal formation.
Vehicle
Initial position (m)
Forward speed (knots)
Heading (°)
Pitch (°)
Target
(0,0,300)
2.0003
90–0.5 t
1
USV1
(400,0,0)
2
90–0.5 t
0
USV2
(0,400,0)
2
90–0.5 t
0
USV3
(−400,0,0)
2
90–0.5 t
0
USV3
(0,−400,0)
2
90–0.5 t
0Figure 3
True and estimated trajectories of the target in optimal formation.Figure 4
Comparison of the true and estimated values in optimal formation. (a) True and estimated position. (b) True and estimated velocity.
(a)
(b)Figure 5
Estimation error in optimal formation. (a) Estimation error of position. (b) Estimation error of velocity.
(a)
(b)


### 6.2. Example in General Formation

In this example, the parameters of the target are the same as the parameters in Table2 and the USVs are in a general formation. The parameters of the USVs are shown in Table 3.Table 3
Parameters of USVs and target in general formation.
Vehicle
Initial position (m)
Forward speed (knots)
Heading (°)
Pitch (°)
USV1
(400,0,0)
3
90–0.25 t
0
USV2
(0,400,0)
1.5
90
0
USV3
(−400,0,0)
1
90-2 t
0
USV4
(0,−400,0)
2
90–0.5 t
0Figures6–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.Figure 6
True and estimated trajectories of the target in general formation.Figure 7
Comparison of the true and estimated values in general formation. (a) True and estimated position. (b) True and estimated velocity.
(a)
(b)Figure 8
Estimation errors in general formation. (a) Position. (b) Velocity.
(a)
(b)Table 4
The max and average estimation errors.
Error
Optimal formation
General formation
Max
Average
Max
Average
x
1.50
0.46
3.04
0.62
y
1.73
0.47
6.14
1.10
z
0.90
0.33
2.03
0.60
vx
0.097
0.033
0.168
0.045
vy
0.102
0.034
0.367
0.078
vz
0.082
0.024
0.199
0.044


## 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 Table2. 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.Table 2
Parameters of USVs and target in optimal formation.
Vehicle
Initial position (m)
Forward speed (knots)
Heading (°)
Pitch (°)
Target
(0,0,300)
2.0003
90–0.5 t
1
USV1
(400,0,0)
2
90–0.5 t
0
USV2
(0,400,0)
2
90–0.5 t
0
USV3
(−400,0,0)
2
90–0.5 t
0
USV3
(0,−400,0)
2
90–0.5 t
0Figure 3
True and estimated trajectories of the target in optimal formation.Figure 4
Comparison of the true and estimated values in optimal formation. (a) True and estimated position. (b) True and estimated velocity.
(a)
(b)Figure 5
Estimation error in optimal formation. (a) Estimation error of position. (b) Estimation error of velocity.
(a)
(b)


## 6.2. Example in General Formation

In this example, the parameters of the target are the same as the parameters in Table2 and the USVs are in a general formation. The parameters of the USVs are shown in Table 3.Table 3
Parameters of USVs and target in general formation.
Vehicle
Initial position (m)
Forward speed (knots)
Heading (°)
Pitch (°)
USV1
(400,0,0)
3
90–0.25 t
0
USV2
(0,400,0)
1.5
90
0
USV3
(−400,0,0)
1
90-2 t
0
USV4
(0,−400,0)
2
90–0.5 t
0Figures6–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.Figure 6
True and estimated trajectories of the target in general formation.Figure 7
Comparison of the true and estimated values in general formation. (a) True and estimated position. (b) True and estimated velocity.
(a)
(b)Figure 8
Estimation errors in general formation. (a) Position. (b) Velocity.
(a)
(b)Table 4
The max and average estimation errors.
Error
Optimal formation
General formation
Max
Average
Max
Average
x
1.50
0.46
3.04
0.62
y
1.73
0.47
6.14
1.10
z
0.90
0.33
2.03
0.60
vx
0.097
0.033
0.168
0.045
vy
0.102
0.034
0.367
0.078
vz
0.082
0.024
0.199
0.044


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



---

*Source: 5452723-2018-07-10.xml*