TOA and FOA based mobile target source position and speed estimation algorithm

Abstract

A TOA and FOA based mobile target source position and speed estimation algorithm. Measurement models based on a series of noisy TOA and FOA measurement values collected by mobile sensors are respectively equivalently transformed into a series of pseudo-linear equations related to the TOA measurement values and the FOA measurement values; on the basis of the weighted least squares criterion, a target positioning problem is represented as a non-convex weighted least squares problem, and is relaxed into a convex semidefinite programming problem by means of the semidefinite relaxation technique; and a globally optimal solution is effectively calculated by means of the interior-point method. In this way, the problem of a large positioning error caused by a local minimum value is avoided; in addition, because the case that the signal propagation speed is unknown is taken into consideration, the influence of a random signal propagation speed is small, and good positioning accuracy is achieved.

Description

Moving target source position and velocity estimation algorithm based on TOA and FOA Technical Field

[0001] The present invention relates to a mobile target source positioning technology, belonging to the field of radar, sonar, and wireless sensor network positioning, and in particular to a mobile target source position and speed estimation algorithm based on TOA (time of arrival) and FOA (frequency of arrival) under unknown clock parameters and signal propagation speed. Background Art

[0002] Localization of mobile targets holds broad application prospects in fields such as radar, sonar, and wireless sensor networks, and has been a research hotspot for the past decade. Generally speaking, accurate target location is primarily achieved by constructing a localization problem based on a series of measurements. The most common of these are time-based measurements, including time of arrival (TOA) and time difference of arrival (TDOA). For TOA localization, clock synchronization between the target and the sensor is a prerequisite. Numerous studies have shown that clock asynchrony between the target and the sensor degrades the performance of TOA localization algorithms, hindering their practical application. Consequently, a growing number of systems are addressing the clock synchronization issue between the target and the sensor while determining the target's location. When the target is mobile, frequency of arrival (FOA) measurements, which include information about the target's velocity, can also be generated. Therefore, for mobile targets, joint TOA and FOA localization is of particular practical significance.

[0003] However, solving the problem of localizing mobile target sources based on TOA and FOA with unknown clock parameters (clock bias and clock drift) is quite challenging. For example, in their work published in Signal Processing, Jiong Shi et al. first converted the TOA and FOA measurement models into distance and range-rate models. They then converted the distance and range-rate models into a series of pseudo-linear equations with the position, velocity, clock bias, and clock drift of the mobile source as position variables, thereby constructing the corresponding weighted least squares (WLS) problem. Finally, they used semi-definite relaxation optimization techniques to approximate the solution to the WLS problem as an estimate of these unknown variables. However, for ease of derivation, this algorithm assumes that the signal propagation velocity is known. However, in reality, this is not the case in most environments. For example, when the signal passes through a solid underground environment, the signal propagation velocity depends heavily on the propagation medium, making the signal propagation velocity unknown.

[0004] Summary of the Invention

[0005] The technical problem to be solved by the present invention is to provide a mobile target source position and velocity estimation algorithm based on TOA and FOA under unknown clock parameters and signal propagation speed, which can locate the position and velocity of the mobile target source with high precision when the mobile target source and sensor clocks are not synchronized and the signal propagation speed is unknown.

[0006] The technical solution adopted by the present invention to solve the above technical problems is: a moving target source position and speed estimation algorithm based on TOA and FOA, characterized by comprising the following steps:

[0007] Step 1: In a two-dimensional environment, establish a rectangular coordinate system as a reference coordinate system and assume that there are N mobile sensors and one mobile target source; where N represents the number of mobile sensors in the two-dimensional environment;

[0008] Step 2: The target source moves in a two-dimensional environment to transmit a measurement signal. The mobile sensor receives the measurement signal and obtains the TOA and FOA measurement values. The measurement model of the TOA measurement value is described as: The measurement model of FOA measurement value is described as: Among them, τ i represents the TOA measurement value obtained by the i-th mobile sensor, f i represents the FOA measurement value obtained by the i-th mobile sensor, u o Represents the true value of the position coordinate of the moving target source in the reference coordinate system, s i represents the position coordinates of the i-th sensor in the reference coordinate system, c o It represents the true value of the signal propagation speed when the measurement signal is transmitted from the mobile target source to the i-th mobile sensor. represents the true value of the clock offset between the mobile target source and the mobile sensor, Δτ i represents the TOA measurement noise in the TOA measurement value obtained by the i-th mobile sensor, Δτ i Obey Gaussian distribution Q τ Denotes Δτ i The covariance matrix of c represents the carrier frequency when the measurement signal is transmitted from the mobile target source to the i-th mobile sensor, represents the true value of the clock drift between the mobile target source and the mobile sensor, Δf i represents the FOA measurement noise present in the FOA measurement value obtained by the i-th mobile sensor, Δf i Obey Gaussian distribution Q f represents Δf i The covariance matrix of Represents the true value of the velocity coordinate of the moving target source in the reference coordinate system, represents the velocity coordinate of the i-th sensor in the reference coordinate system. The symbol “|| ||” is the symbol for finding the Euclidean norm, and the superscript “T” is the transpose symbol.

[0009] Step 3: c o Expressed as the nominal signal propagation speed and the true value of the velocity error term The sum of Substitute them into the measurement models of TOA and FOA respectively. Then, under high signal-to-noise ratio conditions, process the measurement models of TOA and FOA respectively to obtain the pseudo-linear equation expression for TOA, which is described as:

[0010] And the pseudo linear equation expression of FOA measurement value is obtained, which is described as:

[0011] Step 4: Based on the pseudo-linear equation expression of TOA measurement value, and introducing the auxiliary unknown variable vector true value y o , the pseudo linear equation expression about TOA measurement value is stacked into vector form, which is described as: b1-A1y o ≈BΔτ; Similarly, according to the pseudo-linear equation expression about the FOA measurement value, and introducing the auxiliary unknown variable vector true value y o , the pseudo linear equation expression about FOA measurement value is stacked into vector form, which is described as: b2-A2y o ≈C1Δτ+C2Δf; where the i-th element of vector b1 is represented as b1(i), The i-th row vector of matrix A1 is represented as A1(i,:), 0 1×2 Represents a 2D row vector of all 0s, 0 1×3 represents a 3D row vector of all 0s, matrix I N represents the N-dimensional identity matrix, diag{} represents the diagonal matrix, and Δτ represents the TOA measurement noise vector stacked by the TOA measurement noise existing in the TOA measurement values ​​obtained by all mobile sensors, that is, Δτ=[Δτ1,Δτ2,...,Δτ N ] T , the i-th element of vector b2 is represented as b2(i), The i-th row vector of matrix A2 is represented by A2(i,:) ， matrix matrix Δf represents the FOA measurement noise vector stacked by the FOA measurement noise existing in the FOA measurement values ​​obtained by all mobile sensors, that is, Δf = [Δf1, Δf2, ..., Δf N ] T ;

[0012] Step 5: Integrate the vector form of the pseudo-linear equation expression for TOA measurement value and the vector form of the pseudo-linear equation expression for FOA measurement value into a vector form, which is described as: b-Ay o ≈DΔm; where the vector matrix matrix O N represents an N-dimensional all-zero matrix, Δm=[Δτ T ,Δf T ] T ;

[0013] Step 6: In b-Ay o ≈DΔm, combined with the weighted least squares criterion, and according to the true value of the unknown variable vector y o Based on the definition of , we can obtain the non-convex weighted least squares problem for solving the position coordinates and velocity coordinates of the mobile target source, the clock bias and clock drift between the mobile target source and the mobile sensor, and the velocity error term of the signal propagation speed, which can be described as:

[0014] Among them, min represents the minimum function, y represents the unknown variable vector, and y (*) Represents the *th element of y, y (1:2) Represents the vector consisting of the first and second elements of y, y (3:4) represents the vector consisting of the third and fourth elements of y, the matrix W2=(Ddiag{Q τ ,Q f}D T ) -1 ;

[0015] Step 7: Introduce the auxiliary variable Y = yy in the description of the non-convex weighted least squares problem T , and using the semi-definite relaxation technique, we get the convex semi-definite programming problem for solving y and Y, which can be described as:

[0016] Among them, Tr() means finding the trace of the matrix, Representation matrix is a positive semidefinite matrix, Y (1:2,1:2) represents the matrix consisting of all elements from the 1st row to the 2nd row and the 1st column to the 2nd column of Y,(1:2,3:4) represents the matrix consisting of all elements from the 1st to the 2nd row and the 3rd to the 4th column of Y, (g,h) represents the element in the gth row and hth column of Y;

[0017] Step 8: Solve the convex semi-definite programming problem of y and Y by the interior point method to obtain the global optimal solution of y As the estimated value of the position coordinates of the moving target source in the reference coordinate system, As the estimated value of the velocity coordinate of the moving target source in the reference coordinate system, As the clock offset estimate of the mobile target source and the mobile sensor, As an estimate of the clock drift between the mobile target source and the mobile sensor, The velocity error term is an estimate of the signal propagation speed when the measurement signal is transmitted from the mobile target source to the i-th mobile sensor.

[0018] Compared with the prior art, the advantages of the present invention are:

[0019] The method of the present invention equivalently converts the TOA measurement model and the FOA measurement model under unknown clock bias, unknown clock drift and unknown signal propagation speed into pseudo-linear equations, then constructs a non-convex positioning problem for solving the position, speed, clock bias, clock drift and signal propagation speed error terms of a moving target source based on the weighted least squares criterion, and combines the semi-positive definite relaxation technology to relax the description of the original problem into the description of a semi-positive definite programming problem, thereby ensuring the convergence of the problem and providing good positioning accuracy under random clock bias, clock drift and signal propagation speed, thereby more accurately estimating the position coordinates and velocity coordinates of the moving target source, and the performance remains stable under the condition of large measurement value noise power. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] FIG1 is a block diagram of the overall implementation of the method of the present invention;

[0021] Figure 2 is a schematic diagram of the deployment of a mobile target source and nine mobile sensors in a reference coordinate system in a two-dimensional environment;

[0022] FIG3 a is a schematic diagram showing a curve of the mean square error (MSE) of the position coordinate estimation value of the mobile target source obtained by the method of the present invention and the existing semi-positive definite relaxation method versus the distance measurement noise power;

[0023] FIG3 b is a schematic diagram showing a curve of the square deviation of the estimated position coordinates of the moving target source obtained by the method of the present invention and the existing semi-definite relaxation method versus the distance measurement noise power;

[0024] FIG4 a is a schematic diagram showing a curve of the mean square error (MSE) of the velocity coordinate estimation value of the moving target source obtained by the method of the present invention and the existing semi-positive definite relaxation method versus the distance measurement noise power;

[0025] FIG4 b is a schematic diagram of curves showing how the square deviation of the estimated velocity coordinates of the moving target source obtained by the method of the present invention and the existing semi-definite relaxation method varies with the distance measurement noise power. DETAILED DESCRIPTION

[0026] The present invention will be described in further detail below with reference to the accompanying drawings and embodiments.

[0027] The present invention proposes a moving target source position and velocity estimation algorithm based on TOA and FOA, the overall implementation block diagram of which is shown in Figure 1, which includes the following steps:

[0028] Step 1: In a 2D environment, establish a rectangular coordinate system as a reference coordinate system and assume the presence of N mobile sensors and one mobile target source. N represents the number of mobile sensors in the 2D environment. Figure 2 shows the deployment of the mobile target source and nine mobile sensors in the reference coordinate system.

[0029] Step 2: The target source moves in a two-dimensional environment to transmit a measurement signal. The mobile sensor receives the measurement signal and obtains the TOA and FOA measurement values. The measurement model of the TOA measurement value is described as: The measurement model of FOA measurement value is described as: Among them, τ i represents the TOA measurement value obtained by the i-th mobile sensor, f i represents the FOA measurement value obtained by the i-th mobile sensor, u o Represents the true value of the position coordinate of the moving target source in the reference coordinate system, s i represents the position coordinates of the i-th sensor in the reference coordinate system, c o It represents the true value of the signal propagation speed when the measurement signal is transmitted from the mobile target source to the i-th mobile sensor. represents the true value of the clock offset between the mobile target source and the mobile sensor, Δτ i represents the TOA measurement noise in the TOA measurement value obtained by the i-th mobile sensor, Δτ i Obey Gaussian distribution Q τ Denotes Δτ i The covariance matrix of c represents the carrier frequency when the measurement signal is transmitted from the mobile target source to the i-th mobile sensor, represents the true value of the clock drift between the mobile target source and the mobile sensor, Δf i represents the FOA measurement noise present in the FOA measurement value obtained by the i-th mobile sensor, Δf i Obey Gaussian distribution Q f represents Δf i The covariance matrix of Represents the true value of the velocity coordinate of the moving target source in the reference coordinate system, represents the velocity coordinate of the i-th sensor in the reference coordinate system. The symbol “|| ||” is the symbol for finding the Euclidean norm. The superscript “T” is the transpose symbol. i is a positive integer, 1≤i≤N.

[0030] Step 3: The true value c of the signal propagation speed when the measurement signal is transmitted from the mobile target source to the i-th mobile sensor o Expressed as the nominal signal propagation speed and the true value of the velocity error term The sum of Substitute them into the measurement model of TOA measurement value and the measurement model of FOA measurement value respectively. Then, under the condition of high signal-to-noise ratio, the measurement model of TOA measurement value and the measurement model of FOA measurement value are processed respectively to obtain the pseudo-linear equation expression of TOA measurement value, which is described as:

[0031] And the pseudo linear equation expression of FOA measurement value is obtained, which is described as:

[0032] Step 4: Based on the pseudo-linear equation expression of TOA measurement value, and introducing the auxiliary unknown variable vector true value y o , the pseudo linear equation expression about TOA measurement value is stacked into vector form, which is described as: b1-A1y o ≈BΔτ; Similarly, according to the pseudo-linear equation expression about the FOA measurement value, and introducing the auxiliary unknown variable vector true value y o , the pseudo linear equation expression about FOA measurement value is stacked into vector form, which is described as: b2-A2y o ≈C1Δτ+C2Δf; where the i-th element of vector b1 is represented as b1(i), The i-th row vector of matrix A1 is represented as A1(i,:), The symbol “[]” is a vector representation symbol, 0 1×2 Represents a 2D row vector of all 0s, 0 1×3 Represents a 3D row vector of all 0s, the true value of the unknown variable vector The symbol “≈” means approximately equal to, the matrix I N Represents the N-dimensional identity matrix, diag{} represents the diagonal matrix, that is, diag{τ1,τ2,...,τ N} is represented as a diagonal matrix with diagonal elements [τ1,τ2,...,τ N ],τ1,τ2,...,τ N represents the TOA measurement value obtained by the first mobile sensor, the TOA measurement value obtained by the second mobile sensor, ..., the TOA measurement value obtained by the Nth mobile sensor, and Δτ represents the TOA measurement noise vector formed by stacking the TOA measurement noise existing in the TOA measurement values ​​obtained by all mobile sensors, that is, Δτ = [Δτ1, Δτ2, ..., Δτ N ] T ,Δτ1,Δτ2,...,Δτ N The TOA measurement noise present in the TOA measurement value obtained by the first mobile sensor, the TOA measurement noise present in the TOA measurement value obtained by the second mobile sensor, ..., the TOA measurement noise present in the TOA measurement value obtained by the Nth mobile sensor, the i-th element of the vector b2 is denoted as b2(i), The i-th row vector of matrix A2 is represented by A2(i,:), matrix It is represented as a diagonal matrix with diagonal elements f1,f2,…,f N The matrix represents the FOA measurement value obtained by the first mobile sensor, the FOA measurement value obtained by the second mobile sensor, ..., the FOA measurement value obtained by the Nth mobile sensor. Δf represents the FOA measurement noise vector stacked by the FOA measurement noise existing in the FOA measurement values ​​obtained by all mobile sensors, that is, Δf = [Δf1, Δf2, ..., Δf N ] T ,Δf1,Δf2,...,Δf N Correspondingly represents the FOA measurement noise existing in the FOA measurement value obtained by the first motion sensor, the FOA measurement noise existing in the FOA measurement value obtained by the second motion sensor, ..., the FOA measurement noise existing in the FOA measurement value obtained by the Nth motion sensor.

[0033] Step 5: Integrate the vector form of the pseudo-linear equation expression for TOA measurement value and the vector form of the pseudo-linear equation expression for FOA measurement value into a vector form, which is described as: b-Ay o ≈DΔm; where the vector matrix matrix O N represents an N-dimensional all-zero matrix, Δm represents the stacking of the TOA measurement noise vector Δτ and the FOA measurement noise vector Δf, that is, Δm=[Δτ T ,Δf T ] T .

[0034] Step 6: In b-Ay o ≈DΔm, combined with the weighted least squares criterion, and according to the true value of the unknown variable vector y o Based on the definition of , we can obtain the non-convex weighted least squares problem for solving the position coordinates and velocity coordinates of the mobile target source, the clock bias and clock drift between the mobile target source and the mobile sensor, and the velocity error term of the signal propagation speed, which can be described as:

[0035] Among them, min represents the minimum function, y represents the unknown variable vector, and y (*) Represents the *th element of y, that is, y (5) ,y (6) ,…,y (14) Corresponding to the 5th element of y, the 6th element of y, ..., the 14th element of y, y (1:2) Represents the vector consisting of the first and second elements of y, y (3:4) Represents the vector composed of the third and fourth elements of y, the matrix W2=( D d i ag{Q τ ,Q f}D T ) -1 ,diag{Q τ ,Q f} indicates that Q τ and Q f is a diagonal matrix of diagonal blocks, and “st” means “subject to…”.

[0036] Step 7: Introduce the auxiliary variable Y = yy in the description of the non-convex weighted least squares problem T , and using the semi-definite relaxation technique, we get the convex semi-definite programming problem for solving y and Y, which can be described as:

[0037] Among them, Tr() means finding the trace of the matrix, Representation matrix is a positive semidefinite matrix, Y (1:2,1:2) represents the matrix consisting of all elements from the 1st row to the 2nd row and the 1st column to the 2nd column of Y, (1:2,3:4)represents the matrix consisting of all elements from the 1st to the 2nd row and the 3rd to the 4th column of Y, (g,h) Represents the element at row g and column h of Y.

[0038] Step 8: Solve the convex semi-definite programming problem of y and Y by the interior point method to obtain the global optimal solution of y As the estimated value of the position coordinates of the moving target source in the reference coordinate system, As the estimated value of the velocity coordinate of the moving target source in the reference coordinate system, As the clock offset estimate of the mobile target source and the mobile sensor, As an estimate of the clock drift between the mobile target source and the mobile sensor, The velocity error term is an estimate of the signal propagation speed when the measurement signal is transmitted from the mobile target source to the i-th mobile sensor.

[0039] The feasibility, effectiveness and positioning performance of the proposed method are verified by simulation experiments. Assume that there are N = 9 mobile sensors and a mobile target source in a two-dimensional environment, where the position coordinates of the N = 9 mobile sensors are s1 = [-300, -300] T , s2=[300,-300] T , s3=[300,300] T , s4=[-300,300] T , s5=[0,0] T , s6=[-150,-150] T , s7=[150,-150] T , s8=[150,150] T , s9=[-150,150] T , the velocity coordinates of N=9 mobile sensors are The true value of the position coordinate of the moving target source is u o =[-300,800] T , the true value of the velocity coordinate of the moving target source is Assume that the carrier frequency of the measurement signal transmitted from the mobile target source to all mobile sensors is the same, that is, f c =10kHz.

[0040] Assuming the clock bias is true From the interval [0,150 / c o ]m randomly selected, assuming the clock drift is the true value From the interval [0,15 / c o]m / s. Assume that the true value of the signal propagation speed when the measurement signal is transmitted from the mobile target source to all mobile sensors is the same, and is randomly selected from the interval [1450,1550]m / s. Assume that the TOA measurement noise power in the TOA measurement value obtained by any mobile sensor is the same, and its covariance matrix is ​​expressed as in, The noise power is measured for a given distance; assuming that the FOA measurement noise power present in the FOA measurement values ​​obtained by any mobile sensor is the same, its covariance matrix is ​​expressed as Q f =10 4 Q τ .

[0041] The performance of the method of the present invention was tested as the distance measurement noise power increased.

[0042] FIG3 a shows a schematic diagram of a curve showing the mean square error (MSE) of the position coordinate estimation value of the mobile target source obtained by the method of the present invention and the existing semi-positive definite relaxation method as a function of the distance measurement noise power, and FIG3 b shows a schematic diagram of a curve showing the square deviation of the position coordinate estimation value of the mobile target source obtained by the method of the present invention and the existing semi-positive definite relaxation method as a function of the distance measurement noise power. As can be observed from FIG3 a , the positioning performance of the existing semi-positive definite relaxation method is poor, that is, the existing semi-positive definite relaxation method has a large mean square error, which indicates that the uncertainty of the signal propagation speed has a great influence on the positioning accuracy. In addition, the mean square error of the method of the present invention is less than that of the existing semi-positive definite relaxation method, and fits the Cramer-Rao bound (CRLB) within the range of the distance measurement noise power considered. As can be observed from FIG3 b , compared with the square deviation of the position coordinate estimation value of the mobile target source obtained by the existing semi-positive definite relaxation method, the square deviation of the position coordinate estimation value of the mobile target source obtained by the method of the present invention is significantly reduced, which illustrates the superior performance of the method of the present invention in terms of positioning accuracy.

[0043] FIG4a shows a schematic diagram of the curves of the mean square error (MSE) of the velocity coordinate estimation value of the mobile target source obtained by the method of the present invention and the existing semi-positive definite relaxation method as the distance measurement noise power changes, and FIG4b shows a schematic diagram of the curves of the mean square error (MSE) and the square deviation of the velocity coordinate estimation value of the mobile target source obtained by the method of the present invention and the existing semi-positive definite relaxation method as the distance measurement noise power changes. It can be observed from FIG4a that the method of the present invention has a smaller mean square error than the existing semi-positive definite relaxation method in various ranges of distance measurement noise power changes considered, and is closer to the Cramer-Rao lower bound. It can be observed from FIG4b that the square deviation of the position coordinate estimation value of the mobile target source obtained by the method of the present invention is significantly reduced compared to the square deviation of the existing semi-positive definite relaxation method. In addition, the positioning performance of the method of the present invention is also more stable under random signal propagation speeds.

[0044] The existing semi-definite relaxation method in Figures 3a to 4b is the Moving Source Localization Using TOA and FOA Measurements With Imperfect Synchronization published by Jiong Shi et al. in Signal Processing, which is referred to as the semi-definite relaxation method.

[0045] It can be seen from the above simulation results that the method of the present invention has good performance, can well meet the requirements of high-precision positioning, and is less affected by the propagation speed of random signals.

Claims

1. A moving target source position and velocity estimation algorithm based on TOA and FOA, characterized in that The following steps are involved: Step 1: In a two-dimensional environment, establish a rectangular coordinate system as a reference coordinate system, and assume that there are N mobile sensors and one mobile target source; where N represents the number of mobile sensors in the two-dimensional environment; Step 2: In a two-dimensional environment, the mobile target source transmits a measurement signal. After receiving the measurement signal, the mobile sensor obtains the TOA measurement value and the FOA measurement value. The measurement model of the TOA measurement value is described as: The measurement model of FOA measurements is described as: Among them, τ i represents the TOA measurement value obtained by the i-th mobile sensor, f i represents the FOA measurement value obtained by the i-th mobile sensor, u o Represents the true value of the position coordinate of the moving target source in the reference coordinate system, s i represents the position coordinates of the i-th sensor in the reference coordinate system, c o It represents the true value of the signal propagation speed when the measurement signal is transmitted from the mobile target source to the i-th mobile sensor. represents the real value of the clock offset between the mobile target source and the mobile sensor, Δτ i represents the TOA measurement noise in the TOA measurement value obtained by the i-th mobile sensor, Δτ i Obey Gaussian distribution Q τ Denotes Δτ i The covariance matrix of c represents the carrier frequency when the measurement signal is transmitted from the mobile target source to the i-th mobile sensor, represents the real value of the clock drift between the mobile target source and the mobile sensor, Δf i represents the FOA measurement noise present in the FOA measurement value obtained by the i-th mobile sensor, Δf i Obey Gaussian distribution Q f Denotes Δf i The covariance matrix of Represents the true value of the velocity coordinate of the moving target source in the reference coordinate system, represents the velocity coordinate of the i-th sensor in the reference coordinate system, the symbol "||||" is the symbol for finding the Euclidean norm, and the superscript "T" is the transposition symbol; Step 3: c o Expressed as the nominal signal propagation speed The true value of the speed error term The sum of Substitute them into the measurement model of TOA measurement value and the measurement model of FOA measurement value respectively, and then process the measurement model of TOA measurement value and the measurement model of FOA measurement value respectively under the condition of high signal-to-noise ratio. The pseudo linear equation expression about TOA measurement value is obtained, which is described as: And the pseudo linear equation expression about the FOA measurement value is obtained, which is described as: Step 4: Based on the pseudo-linear equation expression of TOA measurement value, and introducing the auxiliary unknown variable vector true value y o , the pseudo linear equation expression about TOA measurement value is stacked into vector form, which is described as: b1-A1y o ≈BΔτ; Similarly, according to the pseudo-linear equation expression about the FOA measurement value, and introducing the auxiliary unknown variable vector true value y o , the pseudo linear equation expression about the FOA measurement value is stacked into a vector form, described as: b2-A2y o ≈C1Δτ+C2Δf; where the i-th element of vector b1 is denoted as b1(i), The i-th row vector of matrix A1 is represented by A1(i,:), 0 1×2 represents a 2D row vector of all zeros, 0 1×3 represents a 3D row vector of all zeros, matrix I N represents the N-dimensional unit matrix, diag{} represents the diagonal matrix, Δτ represents the TOA measurement noise vector stacked by the TOA measurement noise existing in the TOA measurement values ​​obtained by all mobile sensors, that is, Δτ=[Δτ1,Δτ2,...,Δτ N ] T , the i-th element of vector b2 is denoted as b2(i), The i-th row vector of matrix A2 is represented by A2(i,:), matrix matrix Δf represents the FOA measurement noise vector stacked by the FOA measurement noise existing in the FOA measurement values ​​obtained by all mobile sensors, that is, Δf = [Δf1, Δf2, ..., Δf N ] T ; Step 5: Integrate the vector form of the pseudo-linear equation expression for TOA measurement value and the vector form of the pseudo-linear equation expression for FOA measurement value into a vector form, which is described as: b-Ay o ≈DΔm; where vector matrix matrix O N represents an N-dimensional all-zero matrix, Δm=[Δτ T ,Δf T ] T ; Step 6: In b-Ay o ≈DΔm, combined with the weighted least squares criterion, and according to the true value of the unknown variable vector y o Based on the definition of , we can obtain the non-convex weighted least squares problem for solving the position coordinates and velocity coordinates of the mobile target source, the clock bias and clock drift between the mobile target source and the mobile sensor, and the velocity error term of the signal propagation speed, which is described as: Among them, min represents the minimum function, y represents the unknown variable vector, and y (*) Represents the *th element of y, y (1:2) represents the vector consisting of the first and second elements of y, y (3:4) represents the vector composed of the third and fourth elements of y, the matrix W2 = (Ddiag{Q τ ,Q f }D T ) -1 ; Step 7: Introduce auxiliary variables Y = yy in the description of the non-convex weighted least squares problem T , and using the semi-positive definite relaxation technique, we get the convex semi-positive definite programming problem for solving y and Y, which can be described as: Among them, Tr() means finding the trace of the matrix, Representation Matrix is a positive semidefinite matrix, Y (1:2,1:2) represents the matrix consisting of all elements from row 1 to row 2 and column 1 to column 2 of Y, Y (1:2,3:4) represents the matrix consisting of all elements from the 1st to the 2nd row and the 3rd to the 4th column of Y, Y (g,h) represents the element in the gth row and hth column of Y; Step 8: Solve the convex semi-positive definite programming problem of y and Y by the interior point method to obtain the global optimal solution of y As the estimated value of the position coordinates of the moving target source in the reference coordinate system, As the estimated value of the velocity coordinate of the moving target source in the reference coordinate system, As the estimated value of the clock offset between the mobile target source and the mobile sensor, do is the estimated value of the clock drift between the mobile target source and the mobile sensor, The velocity error term is an estimate of the signal propagation speed when the measurement signal is transmitted from the mobile target source to the i-th mobile sensor.

Images