CN100520442C - Arrival time difference positioning method by total least square equalization algorithms - Google Patents

Abstract

In the method positioning is carried on by applying prebalance to error matrix and combining with at least square method according to feature of balanced error matrix covariance matrix being approximately to unit array in order to raise accuracy of positioning; the processed error model includes both of array element position error and estimation error of distance difference which is not the same to each other so it is more is accordance with actual situation comparing to existed positioning method.

Description

Adopt the step-out time localization method of total least square and equalization algorithm

Technical field

Adopt the step-out time localization method of total least square and equalization algorithm to belong to the extraterrestrial target field of locating technology.

Background technology

The bearing accuracy of target also depends on the evaluated error of positional parameter and the measuring error of element position except outside the Pass having with respect to each space geometry position that receives array element with target.In actual applications, be subjected to the restriction of influence on signal-to-noise ratio (SNR) and concrete element position measuring technique (as GPS), these errors are difficult to eliminate, and error variance also is not quite similar.If the location algorithm that adopts can not be taken into account these error components, then the actual location precision will be had a strong impact on.

The TDOA location technology has stronger engineering realizability and high orientation precision, is therefore extensively applied in radar, sonar and the radiation source positioning system.

The principle of TDOA localization method:

1, Chang Gui TDOA localization method: as shown in Figure 1, set M the signal that distributing arbitrarily in the three dimensions and receive array element, be without loss of generality, suppose that array element 1 is in true origin, the position of each array element is respectively s _i=[x _iy _iz _i] ^T(i=1 ..., M), the position of target is u=[x y z] ^T, [] here ^TThe representing matrix transposition.Each array element is R to the distance of initial point _i=‖ s _i‖, ‖ ‖ represents the Euclidean norm.Like this, array element i range-to-go is exactly r _i=‖ s _i-u ‖, array element i, j range-to-go poor (RD) are exactly:

r _i，j＝r _i-r _j＝‖s _i-u‖-‖s _j-u‖，(i，j＝1，...，M) (1)

Because range difference is exactly the mistiming that echo signal arrives two array elements divided by signal velocity, therefore RD might as well be set up observation equation as observed quantity.Make j=1, (1) formula can be transformed to:

x _ix+y _iy+z _iz+r _i，1r ₁＝-0.5(r _i，1 ²-R _i ²)(i＝2，...，M) (2)

(2) formula is expressed as the form of matrix, obtains:

A ₁u ₁＝b ₁ (3)

Wherein

$A_1=-\left[\begin{array}{cccc}{x}_2& y_2& z_2& {\mathrm{<figure-callout\; id="2"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">r</figure-callout>}}_{\mathrm{2,1}}\\ x_3& y_3& z_3& {\mathrm{<figure-callout\; id="1"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">r</figure-callout>}}_{\mathrm{3,1}}\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ x_M& y_M& z_M& r_{M,1}\end{array}\right]$ $b_1=0.5\left[\begin{array}{c}{{\mathrm{<figure-callout\; id="2"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">\; <figure-callout\; id="1"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">r</figure-callout>\; </figure-callout>}}_{\mathrm{2,1}}}^2-{{\mathrm{<figure-callout\; id="2"\; label="R"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">R</figure-callout>}}_2}^2\\ {{\mathrm{<figure-callout\; id="1"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">r</figure-callout>}}_{\mathrm{3,1}}}^2-{R_3}^2\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {r_{M,1}}^2-{R_M}^2\end{array}\right]$ $u_1=\left[\begin{array}{c}x\\ \mathrm{<figure-callout\; id="1"\; label="y\; z\; r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">y</figure-callout>}& \\ z\\ r_{}{}_1\end{array}\right]$

Work as coefficient matrices A ₁Order equal the unknown number number n, during and M 〉=n, can obtain the estimated position of target according to the least-squares algorithm of classics:

${\hat{u}}_1={\left({A_1}^T{A}_1\right)}^{-1}{A_1}^T{b}_1---\left(4\right)$

When having only observation vector b ₁Contain error, and each error component is separate, variance is identical, then least-squares estimation is best on the meaning of variance minimum.But coefficient matrices A in actual applications, ₁Disturbance is often also arranged, and the covariance matrix of error is for unit matrix (error variance difference or error component have correlativity), so the practicality of least-squares estimation method is very limited.

Another kind of conventional method is to open up formula (Taylor-series) with Taylor non-linear observation equation ((1) formula) is converted into linear equation, estimates unknown vector by the mode of iteration again.The major defect of this method is to need to set initial value, and because iteration makes that the real-time of method is relatively poor.

2, existing TDOA localization method mainly contains two kinds: method one is to estimate unknown vector with total least square (TLS), has so not only considered the disturbance of observation vector, has also considered the disturbance of matrix of coefficients, makes bearing accuracy be improved.The general characteristic value decomposition (SVD) that adopts is found the solution unknown vector in this method, thereby has higher numerical stability again.Covariance matrix at error component is under the situation of unit matrix, and TLS has best estimation effect.But in the reality, the matrix of coefficients element often with the various measured values of the different dimensions of observation vector, so this condition is difficult to satisfy.Method two is improved weighted least-squares method, the method is when the instrument error matrix, not only considered the disturbance that RD causes observation vector, also considered the disturbance that RD causes matrix of coefficients simultaneously, and the covariance matrix of error matrix is inverted as weighting matrix.Moreover, this method is also considered unknown vector u ₁In the relation of each element, i.e. x ²+ y ²+ z ²=r ₁ ², and utilize this intrinsic relation further to improve bearing accuracy.The disadvantage of method two is not consider the element position measuring error, because be difficult to construct weighting matrix when considering this error.The estimation that a large amount of situations about existing are not only RD in the actual location system has error, and the measurement of element position also has error, and the variance of error also is different (being caused by the not equal factor of signal to noise ratio (S/N ratio)).

Summary of the invention

The objective of the invention is to overcome the weak point of prior art, at more near the practical position SYSTEM ERROR MODEL, propose a kind of adopt error matrix is carried out preequalization and in conjunction with the localization method of TLS algorithm, utilize the characteristic of the covariance matrix of the error matrix after the equilibrium, improve bearing accuracy for unit matrix.

When TDOA estimates and element position measurement when having error, by (3) formula as can be known, coefficient matrices A ₁And observation vector b ₁All there is disturbance.Therefore, augmented matrix [A ₁b ₁] can be expressed as the matrix A of forming by true value ₀With error matrix E sum, i.e. [A ₁b ₁]=[A ₁ ⁰b ₁ ⁰]+[e ' e]=A ₀+ E.If r _{I, 1}=r _{I, 1} ⁰+ Δ r _{I, 1}, s _i=s _i ⁰+ Δ s _i=[x _i ⁰y _i ⁰z _i ⁰] ^T+ [Δ x _iΔ y _iΔ z _i] ^T(i, j=2 ..., M), and neglect second order error terms, then error matrix E is:

$E=\left[\begin{array}{ccccc}{-\mathrm{\&Delta;x}}_2& {-\mathrm{\&Delta;y}}_2& -{\mathrm{\&Delta;z}}_2& {-\mathrm{\&Delta;<figure-callout\; id="2"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">r</figure-callout>}}_{\mathrm{2,1}}& {{\mathrm{<figure-callout\; id="2"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">\; <figure-callout\; id="1"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">r</figure-callout>\; </figure-callout>}}_{\mathrm{2,1}}}^0{\mathrm{\&Delta;<figure-callout\; id="2"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">r</figure-callout>}}_{\mathrm{2,1}}\\ {-\mathrm{\&Delta;x}}_3& -{\mathrm{\&Delta;y}}_3& {-\mathrm{\&Delta;z}}_3& {-\mathrm{\&Delta;r}}_{\mathrm{3,1}}& {{\mathrm{<figure-callout\; id="1"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">r</figure-callout>}}_{\mathrm{3,1}}}^0{\mathrm{\&Delta;<figure-callout\; id="1"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">r</figure-callout>}}_{\mathrm{3,1}}\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ {-\mathrm{\&Delta;x}}_M& -{\mathrm{\&Delta;y}}_M& -{\mathrm{\&Delta;z}}_M& -{\mathrm{\&Delta;r}}_{M,1}& {r_{M,1}}^0{\mathrm{\&Delta;r}}_{M,1}\end{array}\right]---\left(5\right)$

R in the formula _{I, 1} ⁰And s _i ⁰The exact value of representing RD and element position respectively, Δ r _{I, 1}[Δ x _iΔ y _iΔ z _i] ^TRepresent the error of RD and element position respectively, and be assumed to separate zero-mean white Gaussian noise stochastic variable.If error matrix do not satisfy independent same distribution structure (IID), for example the variance of each element does not wait or element between not separate etc., then existing TLS algorithm just can not reach the optimum estimate effect.Therefore, traditional TLS algorithm is replaced by finds the solution unknown vector and make it satisfied:

minimize‖DET‖ _F (6)

subject?to?b ₁+e∈Range(A ₁+e′)

Weighting matrix is taken advantage of, ‖ ‖ in D, T have been respectively the effect of balancing error matrix in the formula the premultiplication weighting matrix and the right side _FExpression Frobenius norm.Premultiplication weighting matrix D is that the Localization Parameter Estimation error to different array elements is weighted equilibrium treatment; The right side takes advantage of weighting matrix T that different errors (as element position measuring error, range difference evaluated error) are weighted equilibrium treatment.This shows that the method has greater flexibility than WLS and TLS.

The employing total least square that the present invention proposes and the step-out time localization method of equalization algorithm, as shown in Figure 2, it is characterized in that: it carries out following steps successively with computing machine:

After location Calculation began, computing machine was transferred to step 2b, 2c and the 2d of executed in parallel simultaneously from step 2a.Wherein step 2b is observed quantity (range difference) r according to front end TDOA estimates and the element position measurement provides respectively _{I, 1}(i=2 ..., M) with element position vector s _i(i=1 ..., M) construct coefficient matrices A ₁And observation vector b ₁, suppose that here array element number is M.Step 2c estimates root-mean-square error σ according to range difference _I1Calculating is used for the premultiplication weighting matrix D of balancing error matrix:

$D=\left[\begin{array}{cccc}1/{\mathrm{\&sigma;}}_{21}& 0& \&CenterDot;\&CenterDot;\&CenterDot;& 0\\ 0& 1/{\mathrm{\&sigma;}}_{31}& \&CenterDot;\&CenterDot;\&CenterDot;& 0\\ \&CenterDot;& \&CenterDot;& & \&CenterDot;\\ \&CenterDot;& \&CenterDot;& & \&CenterDot;\\ \&CenterDot;& \&CenterDot;& & \&CenterDot;\\ 0& 0& \&CenterDot;\&CenterDot;\&CenterDot;& 1/{\mathrm{\&sigma;}}_{M1}\end{array}\right]---\left(7\right)$

Matrix D is that the element position measuring error that relates to different array elements, range difference evaluated error are weighted equilibrium treatment.Step 2d utilizes range difference estimation variance σ _I1 ², element position measuring error variances sigma ²With range difference observed quantity r _{I, 1}, calculating the right side that is used for the balancing error matrix and take advantage of weighting matrix T, this matrix is that different errors (as element position measuring error, range difference evaluated error) are weighted equilibrium treatment.In this step, the covariance matrix that at first obtains the row vector of matrix E '=DE is:

$d_i=\left[\begin{array}{ccccc}{\mathrm{\&sigma;}}^2/{{\mathrm{\&sigma;}}_{i1}}^2& 0& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}^2/{{\mathrm{\&sigma;}}_{i1}}^2& 0& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}^2/{{\mathrm{\&sigma;}}_{i1}}^2& 0& 0\\ 0& 0& 0& 1& -{r_{i,1}}^0\\ 0& 0& 0& -{r_{i,1}}^0& {\left({r_{i,1}}^0\right)}^2\end{array}\right](i=2,...,M)---\left(8\right)$

So can calculate right weighting matrix T by following formula

${\mathrm{TT}}^T=E{[{E\&prime;}^{T}E\&prime;]}^{-1}={\left(\underset{i}{\mathrm{\&Sigma;}}{d}_i\right)}^{-1}---\left(9\right)$

Thereby make the error matrix after the equilibrium satisfy E[(DET) ^TDET]～I, adopt the optimum estimate that just can access unknown vector based on the TLS algorithm of characteristic value decomposition like this, can prove that this estimation is no inclined to one side.By (9) formula as can be known, matrix T can obtain by the Cholesky decomposition easily.Because r _{I, 1} ⁰Be unknown quantity, available observed reading r in the actual computation _{I, 1}Replace.In order further to reduce the robustness of operand and raising algorithm, T can be approximately a diagonal matrix, be about to matrix E[E ' ^TE '] ^-1Off-diagonal element put 0, very little by the above approximate error of bringing by emulation as can be known, can ignore and not remember.

After finishing above step, estimator is transferred to step 2e, and the augmented matrix after the equilibrium is made characteristic value decomposition:

D[A ₁?b ₁]T＝UΩV ^T (10)

In (10) formula, U is (M-1) * (M-1) diagonal matrix, and V is 5 * 5 diagonal matrix, and Ω is (M-1) * 5 diagonal matrix, and its diagonal element is exactly matrix D [A ₁b ₁] eigenwert of T, and by descending sort.With matrix V, T, Ω piecemeal is following form:

$V=\left[\begin{array}{cc}{V}_{11(4\&times;4)}& V_{15(4\&times;1)}\\ V_{51(1\&times;4)}& V_{55(1\&times;1)}\end{array}\right]$ ? $\mathrm{\&Omega;}=\left[\begin{array}{c}{\mathrm{\&Omega;}}_{1(5\&times;5)}\\ 0_{(M-6\&times;5)}\end{array}\right]$ ? $T=\left[\begin{array}{cc}{T}_{1(4\&times;4)}& 0_{(4\&times;1)}\\ 0_{(1\&times;4)}& t_5\end{array}\right]---\left(11\right)$

The subscript of partitioned matrix in the formula (i * j) represent this partitioned matrix by 1～i of original matrix capable and 1～j row institute corresponding element form Ω here ₁=diag (σ ₁σ ₂σ ₄σ ₅), σ _i(i=1...5) be matrix D [A ₁b ₁] eigenwert of T.

Estimator is transferred to step 2f subsequently, relatively Ω ₁In by the size of latter two eigenwert of descending sort, if previous eigenwert σ ₄Greater than minimal eigenvalue σ ₅, then estimator is transferred to step 2g; Otherwise, rotate back into step 2b, 2c, 2d, from newly beginning the location Calculation of a new round.Step 2g utilizes the characteristic value decomposition result to calculate unknown vector u ₁Estimated value , promptly obtain the initial estimate of target location:

${\hat{u}}_1=-\frac{T_1{V}_{15}}{V_{55}{t}_5}---\left(12\right)$

Can also be expressed as the another kind of equivalent form of value:

${\hat{u}}_1={({A_1}^T{A}_1-{{\mathrm{\&sigma;}}_5}^2{I}_4)}^{-1}{A_1}^T{b}_1---\left(13\right)$

I wherein ₄Represent 4 rank unit matrix.

Next estimator is transferred to step 2h, according to u ₁Estimated value constitute observation vector $b_{}$ ${}_2={{\hat{u}}_1}^2,$ And calculating weighting matrix W ₂Obvious vectorial u ₁In 4 elements be not separate, x, y, z, and r ₁Satisfy relational expression

r ₁ ²＝x ²+y ²+z ² (14)

This prior imformation can be used for further improving the estimated accuracy of target location.Set up following observation equation group according to (14) formula:

A ₂u ₂＝b ₂ (15)

Wherein

$A_2=\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 1& 1& 1\end{array}\right)$ ? $b_2=\left(\begin{array}{c}{\hat{u}}_1{\left(1\right)}^2\\ {\hat{u}}_1{\left(2\right)}^2\\ {\hat{u}}_1{\left(3\right)}^2\\ {\hat{u}}_1{\left(4\right)}^2\end{array}\right)$ ? $u_2=\left(\begin{array}{c}{x}^2\\ y^2\\ {\mathrm{<figure-callout\; id="2"\; label="z"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">z</figure-callout>}}^2\end{array}\right)$

Because coefficient matrices A ₂Be the constant coefficient matrix, so error matrix ε is by estimated value Each error component form.Relation according to error matrix and weighting matrix $W_{}$ ${}_2=E{\left({\mathrm{\&epsiv;\&epsiv;}}^T\right)}^{-1}{|}_{{\hat{u}}_1={u_1}^0}$ (u ₁ ⁰Be vectorial u ₁True value), and ignore second order error terms, can obtain weighting matrix W ₂Expression formula:

${\mathrm{<figure-callout\; id="2"\; label="W"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">W</figure-callout>}}_2={\left(\mathrm{Ccov}\left({\hat{u}}_1\right)C^T\right)}^{-1}---\left(16\right)$

Wherein

C＝diag(x ⁰?y ⁰?z ⁰?r ₁ ⁰)

Here, [x ⁰y ⁰z ⁰] expression target actual position.And in (17) formula

Covariance matrix can obtain by (13) formula:

$\mathrm{cov}\left({\hat{u}}_1\right)={({A_1}^T{A}_1-{{\mathrm{\&sigma;}}_5}^2{I}_4)}^{-1}{A_1}^T\mathrm{cov}\left(b_1\right){\left({({A_1}^T{A}_1-{{\mathrm{\&sigma;}}_5}^2{I}_4)}^{-1}{A_1}^T\right)}^T---\left(17\right)$

Neglect high-order error term equally, observation vector b ₁Covariance matrix be:

$\mathrm{cov}\left(b_1\right)=4\mathrm{diag}({\left({{\mathrm{<figure-callout\; id="2"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">\; <figure-callout\; id="1"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">r</figure-callout>\; </figure-callout>}}_{\mathrm{2,1}}}^0\right)}^2{{\mathrm{\&sigma;}}_{\mathrm{2,1}}}^2{\left({{\mathrm{<figure-callout\; id="1"\; label="r"\; filenames="C200310121707E00191.png"\; state="\{\{state\}\}">r</figure-callout>}}_{\mathrm{3,1}}}^0\right)}^2{{\mathrm{\&sigma;}}_{\mathrm{3,1}}}^2\&CenterDot;\&CenterDot;\&CenterDot;{\left({r_{M,1}}^0\right)}^2{{\mathrm{\&sigma;}}_{M,1}}^2)---\left(18\right)$

In the actual computation, the actual position [x of target ⁰y ⁰z ⁰] can by

In corresponding element replace and matrix cov (b ₁) in r _{I, 1} ⁰(i=2 ..., M) can be by observed reading r _{I, 1}Replace.

After finishing above calculating, estimator is transferred to step 2i, uses weighted least-squares and calculates estimated value

:

${\hat{u}}_2={\left({A_2}^T{W}_2{A}_2\right)}^{-1}{A_2}^T{W}_2{b}_2---\left(19\right)$

Then estimator forwards step 2j to, the output of back is carried out extracting operation and is multiplied by sign matrix G, thereby obtain the estimated value of target location

:

$\hat{u}=G[{\hat{u}}_2{\left(1\right)}^{0.5},{\hat{u}}_2{\left(2\right)}^{0.5},{\hat{u}}_2{\left(3\right)}^{0.5}]---\left(20\right)$

Wherein G is used to determine In the symbol of each element, can be by initial estimate In the symbol of respective element learn, promptly

$G=\mathrm{diag}(\mathrm{sign}\left({\hat{u}}_1\left(1\right)\right),\mathrm{sign}\left({\hat{u}}_1\left(2\right)\right),\mathrm{sign}\left({\hat{u}}_1\left(3\right)\right))---\left(21\right)$

Sign represents sign function (independent variable gets-1 for officiallying enroll 1 for negative) in the formula.Last estimator is transferred to step 2k, and location Calculation finishes.

Feature of the present invention also is: it can be finished by a circuit of being made up of some multipliers, and comprises following steps successively:

(1), the following initial parameter value of measuring of input:

Element position s _i=[x _iy _iz _i] ^T, i=1 ..., M, M are array number, wherein the 1st array element is located at the initial point of X-Y-Z coordinate system,

Observed quantity r _{I, 1}, i=2 ..., M, r _{I, 1}For array element i, 1 range-to-go are poor,

Array element i, the error variance σ of 1 range-to-go difference _I1 ²,

Element position measuring error variances sigma ²

(2), by after the computing circuit computing separately, its result is sent to the relevant register group following array:

Array r _{I, 1} ², σ _{I, 1} ²After the multiplier computing, obtain cov (b ₁), be sent to cov (b ₁) registers group,

s _iObtain R through the quadratic sum circuit _i ², again and r _{I, 1} ²Obtain b through subtracter ₁, be sent to b ₁Registers group,

S _i, r _{I, 1}Be sent to A ₁Registers group,

σ _{I, 1}Obtain 1/ σ through reciprocal circuit _{I, 1}, be sent to the D registers group,

σ _I1 ², σ ²Obtain σ through division circuit ²/ σ _I1 ², again σ ²/ σ _I1 ²And r _{I, 1}Be sent to d together _iRegisters group obtains d _i, then to d _iSummation through inverting, forming T behind the decomposition circuit, is sent to the T registers group;

(3), the output A of above-mentioned each registers group ₁, b ₁, D, T be sent to the matrix multiplication circuit and obtain D[A ₁b ₁] T, again D[A ₁b ₁] T obtains U, V, Ω through well-known features value decomposition circuit, and be sent to U, V, Ω registers group;

(4), V in U, V, the Ω registers group and the T in the T registers group,, obtain through the matrix multiplication circuit

And C, and be sent to

Registers group;

(5), the cov (b that obtains from the relevant register group ₁), A ₁, σ ₅Be sent to the matrix multiplication circuit with C, obtain W ₂, and be sent to W ₂Registers group;

(6), make

Obtain b through squaring circuit ₂, b ₂, W ₂Be sent to the matrix multiplication circuit and obtain u ₂, again

Obtain through the evolution circuit

As an example, set array number M=9, each element position lays respectively at [0 0 0], [50 80 20], [40 60-30], [20 40 40], [60 30 30], [70 50-20], [20 50 10], [40 20-40], [30 30-10], radiation source positions is positioned at [3,002,400 200], presses RD and estimates and the element position measuring accuracy variances sigma of establishing RD respectively _I1 ²(i=2 ..., 9) equal 10 ^-5[149 16 25 36 49 64], element position measuring error variances sigma ²Equal [10 ^-410 ^-3.7510 ^-2.2510 ^-2].Algorithm is programmed with MATLAB, and moves on the X08-34930 of Tsing Hua Tong Fang type computing machine.By 10 ⁵Inferior Monte-Carlo Simulation obtains position root-mean-square error (RMSE) simulation result (as shown in table 1) of three kinds of methods, and these three kinds of methods are respectively improved WLS localization methods, the localization method that TLS localization method and the present invention propose.

The position root-mean-square error simulation result that table 1 changes with element position measuring error variance

As seen, the localization method of the present invention's proposition has the highest bearing accuracy in three kinds of methods.Effect of the present invention is, utilize weighting matrix that error matrix is made equilibrium treatment, make the covariance matrix of the error matrix after the equilibrium be approximately unit matrix, and obtain the estimated value of target location, thereby increased substantially the bearing accuracy of target by characteristic value decomposition method with high value stability.Handled error model had both included the element position measuring error, comprised mutual unequal range difference error again, thereby more realistic applicable cases.Compare in existing method one, when the range difference evaluated error was unequal, this method had higher bearing accuracy; Compare in existing method two, when having the element position measuring error, this method has better positioning performance.

Description of drawings

Fig. 1 is that positioning system target and element position concern synoptic diagram

Fig. 2 is software realization flow figure of the present invention.

Fig. 3 is that the hardware of location estimation device is designed according to this invention realized block scheme.

Fig. 4 is for using the object locating system block diagram of the inventive method.

Embodiment

Explain the present invention in further detail with two embodiment with reference to the accompanying drawings below:

Embodiment one: present embodiment adopts software to realize the localization method that the present invention proposes, as shown in Figure 2.If the element position number is 9, each element position lays respectively at [0 0 0], [50 80 20], [40 60-30], [20 40 40], [60 30 30], [70 50-20], [20 50 10], [40 20-40] and [30 30-10], radiation source positions are [300 2,400 200], press RD and estimate and the element position measuring accuracy variances sigma of establishing RD _I1 ²(i=2 ..., 9) equal 10 ^-5[1 49 16 25 36 49 64], element position measuring error variances sigma ²Equal 10 ^-3After location Calculation began, estimator was transferred to step 2b, 2c and the 2d of executed in parallel simultaneously from step 2a.Wherein step 2b is the observed quantity r according to front end TDOA estimates and the element position measurement provides respectively _{I, 1}(i=2 ..., 9) and element position vector s _i(i=1 ..., 9) construct coefficient matrices A ₁And observation vector b ₁:

A ₁＝ b ₁＝

49.9809 -79.9710 -19.9427 73.7883 1.0e+003 ^*

-39.9848 -59.9946 29.9165 61.3206

19.9417 -40.0340 -40.0427 39.9638 -1.9276

-60.0169 -29.9716 -30.0322 38.7308 -1.1699

69.9681 -50.0313 19.9082 37.8466 -1.0014

-19.9884 -50.0261 -9.9769 52.6999 -1.9500

40.0178 -20.0079 39.9515 10.8141 -3.1838

-30.0209 -29.9921 10.0189 32.3748 -0.1114

-1.7415

-0.4259

Step 2c is according to observed quantity estimation variance σ _I1Calculate the premultiplication weighting matrix D be used for the balancing error matrix with (7) formula, this matrix is that the Localization Parameter Estimation error to different array elements is weighted equilibrium treatment, and its value is:

D＝

316.2278 0 0 0 0 0 0 0

0 158.1139 0 0 0 0 0 0

0 0 105.4093 0 0 0 0 0

0 0 0 79.0569 0 0 0 0

0 0 0 0 63.2456 0 0 0

0 0 0 0 0 52.7046 0 0

0 0 0 0 0 0 45.1754 0

0 0 0 0 0 0 0 39.5285

Step 2d utilizes observed quantity estimation variance σ _I1 ², element position measuring error variances sigma ²With observed quantity r _{I, 1}, calculating the right side that is used for the balancing error matrix with (8), (9) formula and take advantage of weighting matrix T, this matrix is that different errors (as element position measuring error, observed quantity evaluated error) are weighted equilibrium treatment, its value is:

T＝

0.0809 0 0 0 0

0 0.0809 0 0 0

0 0 0.0809 0 0

0 0 0 0.9256 0

0 0 0 0 0.0197

After finishing above step, estimator is transferred to step 2e, and the multiplying of realization matrix also and then is finished the characteristic value decomposition D[A of product matrix ₁b ₁] T=U Ω V ^T, obtain:

U＝

-0.8964 0.0051 -0.3776 -0.0135 0.2301 -0.0161 0.0223 -0.0081

-0.3470 -0.3794 0.7437 -0.3207 -0.1550 -0.1958 -0.0491 -0.1219

-0.1602 -0.0218 -0.1130 0.4711 -0.8099 -0.2688 0.1035 0.0152

-0.1423 0.3549 0.5092 0.6879 0.3141 0.1270 0.0728 -0.0353

-0.1402 0.7028 0.1214 -0.3189 -0.3189 0.1991 -0.4735 0.0676

-0.0833 -0.3417 0.0132 0.0863 -0.2269 0.9040 -0.0054 -0.0069

-0.0417 0.3333 0.0869 -0.3045 -0.1200 0.1251 0.8699 0.0140

-0.0431 -0.0889 0.0990 0.0044 0.0284 -0.0247 0.0152 0.9893

Ω＝

1.0e+004 ^*

2.7700 0 0 0 0

0 0.3402 0 0 0

0 0 0.1137 0 0

0 0 0 0.0534 0

0 0 0 0 0.0001

0 0 0 0 0

0 0 0 0 0

0 0 0 0 0

V＝

-0.0356 0.1172 -0.9081 -0.3812 0.1228

0.0810 0.0286 0.0818 0.1237 0.9852

0.0140 -0.0225 0.3921 -0.9159 0.0820

-0.8699 -0.4851 -0.0165 -0.0006 0.0870

0.4849 -0.8658 -0.1212 -0.0233 -0.0017

Wherein Ω includes the eigenwert of product matrix, and presses descending sort on diagonal line.Estimator is transferred to step 2f then, relatively by the size of latter two eigenwert of descending sort.By the value of Ω σ as can be known ₄=0.0534, σ ₅=0.0001, obviously satisfy σ ₄σ ₅, so estimator is transferred to step 2g.Step 2g utilizes (12) formula to calculate unknown vector u ₁Estimated value

Promptly obtain the initial estimate of target location:

${\hat{u}}_1=$

$1.0e+003*$

$0.2915$

$2.3385$

$0.1946$

$2.3637$

Subsequently, estimator is transferred to step 2h, according to u ₁Estimated value constitute observation vector $b_{}$ ${}_2={{\hat{u}}_1}^2,$ And calculate weighting matrix W by (16), (17) and (18) formula ₂:

W ₂＝

0.0528 -0.0030 0.0010 0.0020

-0.0030 0.0009 0.0010 -0.0008

0.0010 0.0010 0.0614 -0.0014

0.0020 -0.0008 -0.0014 0.0008

After finishing this step, estimator is transferred to step 2i, and (19) formula of using is calculated estimated value

${\hat{u}}_2=$

$1.0e+006*$

$0.0910$

$5.8435$

$0.0406$

Then estimator forwards step 2j to, the output of back is carried out extracting operation and is multiplied by sign matrix G, thereby obtain the estimated value of target location

$\hat{u}=$

$1.0e+003*$

$0.3016$

$2.4173$

$0.2015$

Here G is used to determine

In the symbol of each element, can be by initial estimate The symbol of middle respective element is learnt.Last estimator is transferred to step 2k, and location Calculation finishes.

Embodiment two: present embodiment is realized the localization method that the present invention proposes for the method that adopts hardware.Still be 9 with the element position number, observed quantity estimation error variance σ _I1 ²(i=2 ..., 9) unequal mutually, and element position measures that also to have the situation of error be example, and the position fixing process of this hardware circuit is described.As shown in Figure 3, by the 1st to the 8 road estimated parameter input 31 that front end TDOA estimates and the element position measurement provides, process simple calculations circuit 32～39 (among the figure || ²Expression quadratic sum circuit, () ^-1Reciprocal circuit is asked in expression), obtain matrix of coefficients 313 (A ₁), observing matrix 314 (b ₁), each component of left weighting matrix 315 (D) and right weighting matrix 316 (T).The square value r of the range difference of each road input wherein _{I, 1} ²With range difference error variance σ _{I, 1} ²Product be stored in the registers group, be that the back level provides the covariance matrix 312 (cov (b of observation vector ₁)); Each element position vector s _i(i=1 ..., 9) and range difference r _{I, 1}Be stored in a registers group, for the back level provides matrix of coefficients 313; Obtain the distance R of each array element to initial point _i ²And subtract each other with the square value of range difference, thereby obtain each component value of observation vector 314, and be stored in the registers group; Range difference error variance σ _{I, 1}By after being got inverse, be stored in the registers group as the diagonal element of left weighting matrix; The range difference r of each road input _{I, 1} ²And error variance σ _I1 ², and element position measuring error variances sigma ²Constitute matrix d separately _iElement, and by 310 circuit---comprise the matrix addition, invert and the Cholesky resolver, obtain right weighting matrix 316, and be stored in the registers group.More than the matrix that provides of 5 registers group 311 be sent in the matrix multiplication circuit 317, obtain product matrix 318 (D[A ₁b ₁] T) and value.Subsequently this matrix is carried out characteristic value decomposition 319, and the diagonal matrix 320 of unitary matrix U, V and eigenwert composition is stored in the registers group 321, wherein the partitioned matrix V of matrix 322 (V) ₁₅And V ₅₅In back level matrix multiplication circuit 324 with the partitioned matrix T of right weighting matrix 316 ₁And t ₅Do multiplying, obtain element position initial estimate 327 ( ) and the matrix 328 (C) formed by its respective element.Wherein characteristic value decomposition circuit 319 is by the Systolic array realization (" a Systolic array system design that realizes state-space method ", communication journal, Huang bath etc., Vol.16, No.6, pp.:25～32,1995) of people such as Huang bath proposition.Matrix 328 and the covariance matrix 312 that obtains previously, matrix of coefficients 313 and minimal eigenvalue σ ₅Be sent to together in the matrix multiplication circuit 325, calculate the weighting matrix 332 (W that are used for the weighted least-squares computing ₂).Initial estimate 327 obtains observing matrix 330 (b through squaring circuit 329 ₂), and be sent to matrix multiplication circuit 333 with weighting matrix 332, obtain estimated value

Pass through square root circuit 335 export target location estimation values again

From top embodiment as seen, this localization method is compared with two kinds of existing localization methods that its front was introduced, and has higher bearing accuracy and stronger practicality, and its hardware is realized also also uncomplicated.By an example effect of the present invention in the radiation source positioning system is described again below.

With reference to Fig. 4, the object locating system of employing total least square and equalization algorithm comprises the antenna array 41 of received signal, receiving front-end 42, analog to digital conversion (A/D), TDOA (or RD) estimation 47, element position measurement 410 and location estimation device 49 shown in Figure 3.In this example, each road signal 42 that bay receives is sent to receiving front-end 43 separately respectively, and is converted into intermediate-freuqncy signal 44.Analog to digital conversion 45 becomes digital signal streams 46 with analog if signal, is defeated by next stage TDOA and estimates 47.TDOA estimated for 47 relative delays that calculate thus between each road signal, was used for location estimation thereby obtain observed quantity 48.Element position is measured 410 each element position 411 of output, is input to location estimation device 49 with observed quantity 48, obtains target estimated position 412.

Claims (2)

1, adopt the step-out time localization method of total least square and equalization algorithm, comprise the location Calculation step with balanced matrix and total least square estimating target position, it is characterized in that: it carries out following steps successively with computing machine:

(1), initialization, import following parameter value to computing machine:

Element position s _i=[x _iy _iz _i] ^T, i=1 ..., M, M are array number, wherein the 1st array element is located at the initial point of X-Y-Z coordinate system,

Observed quantity r _{I, 1}, i=2 ..., M, r _{I, 1}For array element i, 1 range-to-go are poor,

Array element i, the error variance σ of 1 range-to-go difference _I1 ²,

Element position measuring error variances sigma ²

(2), computing machine is carried out following steps simultaneously:

(2.1), the range difference observed quantity r that obtains according to the input parameter value of computing machine _{I, 1}, i=2 ..., M, element position s _i, i=1 ..., M, and each array element is to the distance R of initial point _i, i=2 ..., M constructs coefficient matrices A ₁With observation vector b ₁, wherein,

$A_1=-\left[\begin{array}{cccc}{x}_2& y_2& z_2& r_{\mathrm{2,1}}\\ x_3& y_3& z_3& r_{\mathrm{3,1}}\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;& \&CenterDot;\\ x_M& y_M& z_M& r_{M,1}\end{array}\right],$ $b_1=0.5\left[\begin{array}{c}{r_{\mathrm{2,1}}}^2-{R_2}^2\\ {r_{\mathrm{3,1}}}^2-{R_3}^2\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {r_{M,1}}^2-{R_M}^2\end{array}\right];$

(2.2), computing machine is according to the root-mean-square error σ of range difference _I1, i=2 ..., M is calculated as follows the premultiplication weighting matrix D that the balancing error matrix is used,

Wherein,

$D=\left[\begin{array}{cccc}1/{\mathrm{\&sigma;}}_{21}& 0& \&CenterDot;\&CenterDot;\&CenterDot;& 0\\ 0& 1/{\mathrm{\&sigma;}}_{31}& \&CenterDot;\&CenterDot;\&CenterDot;& 0\\ \&CenterDot;& \&CenterDot;& & \&CenterDot;\\ \&CenterDot;& \&CenterDot;& & \&CenterDot;\\ \&CenterDot;& \&CenterDot;& & \&CenterDot;\\ 0& 0& \&CenterDot;\&CenterDot;\&CenterDot;& 1/{\mathrm{\&sigma;}}_{M1}\end{array}\right];$

(2.3), computing machine utilizes range difference error variance σ _I1 ², element position measuring error variances sigma ²With range difference observed quantity r _{I, 1}, calculate the right side that is used for the balancing error matrix and take advantage of weighting matrix T,

Wherein,

${\mathrm{TT}}^T={\left(\underset{i}{\mathrm{\&Sigma;}}{d}_i\right)}^{-1},$

$d_i=\left[\begin{array}{ccccc}{\mathrm{\&sigma;}}^2/{{\mathrm{\&sigma;}}_{i1}}^2& 0& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}^2/{{\mathrm{\&sigma;}}_{i1}}^2& 0& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}^2/{{\mathrm{\&sigma;}}_{i1}}^2& 0& 0\\ 0& 0& 0& 1& -{r_{i,1}}^0\\ 0& 0& 0& -{r_{i,1}}^0& {\left({r_{i,1}}^0\right)}^2\end{array}\right],$

i＝2，...，M；

In the following formula, r _{I, 1} ⁰Be the range difference exact value, available observed reading r _{I, 1}Replace;

(3), computing machine is to the augmented matrix D[A through equilibrium treatment ₁b ₁] T makes characteristic value decomposition:

D[A ₁?b ₁]T＝UΩV ^T，

Wherein, U is (M1) * (M-1) diagonal matrix,

V is 5 * 5 diagonal matrix,

Ω is (M-1) * 5 diagonal matrix, and its diagonal element is matrix D [A ₁b ₁] eigenwert of T, will obtain behind matrix V, T, the Ω piecemeal:

$V=\left[\begin{array}{cc}{V}_{11(4\&times;4)}& V_{15(4\&times;1)}\\ V_{51(1\&times;4)}& V_{55(1\&times;1)}\end{array}\right],$ $\mathrm{\&Omega;}=\left[\begin{array}{c}{\mathrm{\&Omega;}}_{1(5\&times;5)}\\ 0_{(M-6\&times;5)}\end{array}\right],$ $T=\left[\begin{array}{cc}{T}_{1(4\&times;4)}& 0_{(4\&times;1)}\\ 0_{(1\&times;4)}& t_5\end{array}\right],$

Here, Ω ₁=diag (σ ₁σ ₂... σ ₄σ ₅), σ _i(i=1...5) be matrix D [A ₁b ₁] eigenwert of T;

(4), differentiate σ ₄σ ₅

If σ ₄σ ₅, then carry out next step,

If σ ₄σ ₅Be false, then return step (2);

(5), computing machine is pressed total least square to vectorial u ₁=[x y z r ₁] ^TCarry out initial estimation, ask

${\hat{u}}_1=-\frac{T_1{V}_{15}}{V_{55}{t}_5},$

R wherein ₁Be the 1st array element range-to-go;

(6), computing machine is according to estimated value

Calculating observation vector b ₂, to constitute observation equation group A ₂u ₂=b ₂, and calculate weighting matrix W ₂:

Wherein,

$b_2=\left(\begin{array}{c}{\hat{u}}_1{\left(1\right)}^2\\ {\hat{u}}_1{\left(2\right)}^2\\ {\hat{u}}_1{\left(3\right)}^2\\ {\hat{u}}_1{\left(4\right)}^2\end{array}\right),$ $A_2=\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 1& 1& 1\end{array}\right),$ $u_2=\left(\begin{array}{c}{x}^2\\ y^2\\ z^2\end{array}\right),$

W ₂By following various drawing:

$W_2={\left(\mathrm{Ccov}\left({\hat{u}}_1\right)C^T\right)}^{-1},$

In the following formula,

C＝diag(x ⁰y ⁰z ⁰r ₁ ⁰)，

$\mathrm{cov}\left({\hat{u}}_1\right)={({A_1}^T{A}_1-{{\mathrm{\&sigma;}}_5}^2{I}_4)}^{-1}{A_1}^T\mathrm{cov}\left(b_1\right){\left({({A_1}^T{A}_1-{{\mathrm{\&sigma;}}_5}^2{I}_4)}^{-1}{A_1}^T\right)}^T,$

cov(b ₁)＝4diag((r _2，1 ⁰) ²σ _2，1 ²(r _3，1 ⁰) ²σ _3，1 ²…(r _M-1，1 ⁰) ²σ _M-1，1 ²)，

Here, I ₄Represent 4 rank unit matrix, [x ⁰y ⁰z ⁰] expression target actual position, r ₁ ⁰Expression array element 1 is to the accurate distance of target, they can be respectively by

Middle element

With

Replace, and matrix cov (b ₁) in r _{I, 1} ⁰, i=2 ..., M can be by observed reading r _{I, 1}Replace;

(8), computing machine is pressed weighted least-squares to vectorial u ₂Estimate, ask

${\hat{u}}_2={\left({A_2}^T{W}_2{A}_2\right)}^{-1}{A_2}^T{W}_2{b}_2;$

(9), computing machine is pressed following formula export target position u=[xyz] ^TEstimated value

$\hat{u}=G[{\hat{u}}_2{\left(1\right)}^{0.5},{\hat{u}}_2{\left(2\right)}^{0.5},{\hat{u}}_2{\left(3\right)}^{0.5}],$

Wherein,

$G=\mathrm{diag}(\mathrm{sign}\left({\hat{u}}_1\left(1\right)\right),\mathrm{sign}\left({\hat{u}}_1\left(2\right)\right),\mathrm{sign}\left({\hat{u}}_1\left(3\right)\right)).$

2, adopt the step-out time localization method of total least square and equalization algorithm, comprise location Calculation step, it is characterized in that it contains following steps successively with balanced matrix and total least square estimating target position:

(1), the following initial parameter value of measuring of input:

Element position s _i=[x _iy _iz _i] ^T, i=1 ..., M, M are array number, wherein the 1st array element is located at the initial point of X-Y-Z coordinate system,

Observed quantity r _{I, 1}, i=2 ..., M, r _{I, 1}For array element i, 1 range-to-go are poor,

Array element i, the error variance σ of 1 range-to-go difference _I1 ²,

Element position measuring error variances sigma ²

(2), by after the computing circuit computing separately, the operation result of computing circuit is sent to relevant register group: array r following array _{I, 1} ², σ _{I, 1} ²After the multiplier computing, obtain observation vector b ₁Covariance matrix cov (b ₁), be sent to cov (b ₁) registers group,

s _iObtain R through the quadratic sum circuit _i ², again and r _{I, 1} ²Obtain observation vector b through subtracter ₁, be sent to b ₁Registers group,

s _i, r _{I, 1}Be sent to coefficient matrices A ₁Registers group,

σ _{I, 1}Obtain 1/ σ through reciprocal circuit _{I, 1}, be sent to premultiplication weighting matrix D registers group,

σ _I1 ², σ ²Obtain σ through division circuit ²/ σ _I1 ², again σ ²/ σ _I1 ²And r _{I, 1}Be sent to d together _iRegisters group obtains d _i,

$d_i=\left[\begin{array}{ccccc}{\mathrm{\&sigma;}}^2/{{\mathrm{\&sigma;}}_{i1}}^2& 0& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}^2/{{\mathrm{\&sigma;}}_{i1}}^2& 0& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}^2/{{\mathrm{\&sigma;}}_{i1}}^2& 0& 0\\ 0& 0& 0& 1& -{r_{i,1}}^0\\ 0& 0& 0& -{r_{i,1}}^0& {\left({r_{i,1}}^0\right)}^2\end{array}\right],$

i＝2，...，M；

Then to d _iSummation through inverting, forming the right side behind the decomposition circuit and take advantage of weighting matrix T, is sent to the T registers group;

(3), the output A of above-mentioned each registers group ₁, b ₁, D, T be sent to the matrix multiplication circuit and obtain D[A ₁b ₁] T, again D[A ₁b ₁] T obtains each diagonal matrix U, V, Ω through well-known features value decomposition circuit, and be sent to described each U, V, Ω registers group;

(4), V in described each U, V, the Ω registers group and the T in the T registers group,, obtain the initial estimate of target location through the matrix multiplication circuit

With diagonal matrix C, and be sent to Registers group;

(5), the described cov (b that obtains from the relevant register group ₁), A ₁, σ ₅Be sent to matrix multiplication circuit, wherein σ with C ₅Be diagonal matrix Ω ₁=diag (σ ₁σ ₂σ ₄σ ₅) the 5th the row, 5 column elements, obtain weighting matrix W then ₂, and be sent to W ₂Registers group;

(6), make Obtain b through squaring circuit ₂, b ₂, W ₂Be sent to the matrix multiplication circuit and obtain estimated value Again

Obtain the estimated value of target location through the evolution circuit

Images