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 [
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
MLBL system consists of four USVs [
In recent years, the unbiased minimum variance SISE for linear systems has been extensively studied. Li et al
The remainder of this paper is organized as follows. In Section
The notations used throughout the paper are as follows.
(see [
(see [
Consider an earth fixed reference frame
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 (
Since system in (
Define
Combining (
It follows that
Based on the above analysis, the mixed linear/nonlinear system in (
Note that, in (
For the system in (
According to the predicted state
For the system in (
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 Algorithm
Positions of USVs Position estimation Measurements Position estimation 1: 2: 3: 4: 5: 6: 7: 8: 9: 10: 11: 12: 13: 14: 15: 16: 17: 18: 19: 20:
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
By minimizing the covariance matrix of the estimation error
Substituting the state estimate (
Since
In MLBL,
Let
Under the unbiasedness (
Substituting (
For the minimum variance estimation, the problem is equivalent to finding the gain matrix
Substituting (
Define
From Theorem
The velocity estimator in the heave direction is shown as (
Similar to the velocity estimate in local level plane, by minimizing the covariance matrix of the estimation error
For the system in (
Let
Note that under the unbiasedness (
In Section
The position predictors are shown as (
Define
Since
Then, for the system in (
Define
Since
It is obviously that, for the system in (
Let
Substituting the estimation error (
Then, the covariance matrix
Define
In Section
Define
By definition, we have
It can be seen that
Based on the results in Section
Let
Substituting (
From (
Substituting (
It follows that
Define
In Sections
From the results about unbiased velocity estimates in Sections
Combining it with the unbiased result in Section
From (
It follows that
Hence, the following equation should be satisfied.
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
Simulation parameters.
(0,0,300) | 10 |
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.
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
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 | 0 |
True and estimated trajectories of the target in optimal formation.
Comparison of the true and estimated values in optimal formation. (a) True and estimated position. (b) True and estimated velocity.
Estimation error in optimal formation. (a) Estimation error of position. (b) Estimation error of velocity.
In this example, the parameters of the target are the same as the parameters in Table
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 | 0 |
Figures
True and estimated trajectories of the target in general formation.
Comparison of the true and estimated values in general formation. (a) True and estimated position. (b) True and estimated velocity.
Estimation errors in general formation. (a) Position. (b) Velocity.
The max and average estimation errors.
Error | Optimal formation | General formation | ||
---|---|---|---|---|
Max | Average | Max | Average | |
1.50 | 0.46 | 3.04 | 0.62 | |
1.73 | 0.47 | 6.14 | 1.10 | |
0.90 | 0.33 | 2.03 | 0.60 | |
0.097 | 0.033 | 0.168 | 0.045 | |
0.102 | 0.034 | 0.367 | 0.078 | |
0.082 | 0.024 | 0.199 | 0.044 |
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.
The data used to support the findings of this study are available from the corresponding author upon request.
The authors declare that they have no conflicts of interest.
This work was supported by the National Natural Science Foundation of China under Grants 61733014 and 61703326.