CN105898865A - Cooperative location method based on EKF (Extended Kalman Filter) and PF (Particle Filter) under nonlinear and non-Gaussian condition - Google Patents

Abstract

The invention discloses a cooperative location method based on an EKF (Extended Kalman Filter) and a PF (Particle Filter) under a nonlinear and non-Gaussian condition. The method comprises the following steps of obtaining TOA (Time of Arrival) original data of a target node and each base station and computing to obtain a distance between the target node and each base station; utilizing a Wylie identification method to judge whether an NLOS (Non Line of Sight) error exists; obtaining a difference of the TOA value to obtain a TDOA (Time Difference of Arrival) value and reconstructing a distance different rm1 corresponding to the TDOA value; respectively utilizing an EKF algorithm and a PF algorithm to estimate position coordinates of the target node at the moment tk; carrying out residual weighting to obtain a final estimated value at the moment tk; and carrying out weighted smoothing on the position coordinates at all moments to obtain a final positioning result. In comparison with the EKF, the method is more suitable for the nonlinear and non-Gaussian positioning environment; in comparison with the PF, the use of incorrect data is effectively avoided and the calculated amount is reduced. According to the method, the influence of the NLOS error is effectively reduced, the advantages of the EKF and the F are combined, the defects of the EKF and the F are overcome, and the more precise positioning is realized.

Description

Colocated method based on EKF and PF under the conditions of nonlinear and non-Gaussian

Technical field

The present invention relates to indoor positioning tracking technique field, be specially under the conditions of a kind of nonlinear and non-Gaussian based on EKF and The colocated method of PF.

Background technology

Along with the fast development of microelectric technique, communication technology and computer technology, wireless location is as Sensor Network and thing The important application of networking, is increasingly paid close attention to by people.Currently, localization method based on range finding mainly has RSSI (to arrive letter Number intensity), AOA (arriving signal angle), TOA (time of arrival (toa)), TDOA (signal step-out time) etc..Wherein, RSSI is The most more satisfactory a kind of algorithm, is converted into distance by propagation loss, but in actual location environment, electromagnetic wave propagation There is dynamic characteristic, range measurement is caused severe jamming, bigger range error can be produced；Location side based on AOA range finding Hardware system equipment required for formula is complicated, relatively costly；TOA and TDOA positioning precision is higher, and positioning time is short, and system realizes Simply, there is certain ability of anti-multipath, so having preferable application prospect.Wherein, the method for estimation of TDOA typically has two Kind: a kind of is that the difference of the time of arrival (toa) TOA asking two base stations is to obtain TDOA value；Another kind is to use cross-correlation technique, The signal that the signal received one base station and another base station receive carries out computing cross-correlation and calculates TDOA value.

Indoor environment is the most complex, and the signal between destination node and base station is propagated and is likely that there are NLOS error (non-line-of-sight propagation).Use TOA and TDOA technology time destination node is carried out location estimation, TOA value can produce one positive Additional excessive delay, TDOA value also corresponding can produce an error component.Should by this TOA or the TDOA value with bigger error For the location estimation of destination node, necessarily cause being remarkably decreased of location algorithm performance, make estimation position that bigger error to occur. Therefore, how to differentiate to become the key factor improving positioning precision with restraining NLOS error.To this, Wylie differential method Being a kind of relatively effective method, first it determine whether line-of-sight propagation, if not regarding from the variance measuring parameter error Away from propagation, then reconstruct line-of-sight propagation value, and the sighting distance value of reconstruct is filtered, then estimate mobile station with sighting distance location algorithm Position.

Kalman filtering is to utilize minimum mean square error criterion to carry out target dynamic estimation in the case of linear Gauss Excellent filtering method.In the case of but for nonlinear and non-Gaussian, in addition it is also necessary to make improvements and extend, and spreading kalman filter Ripple (EKF, Extended Kalman Filter) is a kind of relatively effective improved method.Particle filter (PF, Particle Filter) being a kind of non-linear, non-Gaussian filtering filtering method based on Monte Carlo thought, it breaches Kalman's filter completely Ripple theoretical frame, process noise and measurement noise to system do not have any restriction, are processing non-linear, non-gaussian time-varying system Parameter estimation and state filtering problem aspect have uniqueness advantage and wide prospect.

In actual application, EKF is a kind of the more commonly used non-linear filtering method, but it is only applicable to filtering Error and the least situation of forecast error, otherwise may result in filtering instability and even dissipate；It is the most linear that PF breaks away from understanding During filtering problem, random quantity must is fulfilled for the restriction condition of Gauss distribution, but this algorithm there is also some problems, and it relies on Substantial amounts of sample data could the posterior probability density of preferably approximation system, amount of calculation is bigger.

In summary, EKF and PF has respective advantage, but is also respectively arranged with deficiency.Therefore, the present invention proposes a kind of non-linear non- Colocated method based on EKF and PF under Gauss conditions.

Summary of the invention

It is an object of the invention to for the deficiencies in the prior art, it is provided that under the conditions of a kind of nonlinear and non-Gaussian based on EKF and The colocated method of PF.

Step (1), utilize nanoLOC Development kit 3.0 development kit dispose in located space target joint Point and base station, read destination node respectively with the TOA initial data of each base station, be calculated between destination node and each base station Distance, wherein M is the total number in base station；

Step (2), Wylie differential method is utilized to judge r_mWhether there is NLOS error；

Step (3), TOA value is done difference obtain TDOA value, and the range difference r to all TDOA values correspondence_m1It is reconstructed；

Step (4), use EKF TDOA algorithm estimation t_kThe position coordinates of moment destination node also judges；

Step (5), use PF TDOA algorithm estimation t_kThe position coordinates of moment destination node also judges；

Step (6), to step (4), two position coordinateses of (5) judge, residual weighted obtains t_kMoment final Estimated value；

Step (7), all elements of a fix data obtaining step (6) are weighted smoothing, and obtain destination node Estimate position coordinates eventually(x, y) compares, the location of method used herein with the true coordinate of destination node Precision is better than the positioning precision of the localization method that nanoLOC development kit carries, also superior to being used alone EKF algorithm or PF algorithm Positioning precision.

The method specifically comprises the following steps that

CSS (Chirp Spread Spectrum, linear frequency modulation spreads) is to open based on IEEE802.15.4a agreement The wireless communication technology sent out.The positioning experiment platform of the present invention uses Nanotron company nanoLOC Development kit 3.0 development kit, this development kit, based on nanoLOC TRX radio frequency chip, can be used for developing communication based on CSS technology, survey The wireless application such as away from, location.The present invention carries out building of alignment system by using this set development kit.

The alignment system of the present invention utilizes nanoLOC Development kit 3.0 development kit in the middle part of located space Administration's destination node and base station, use one destination node of M architecture；Wherein, M base station is ordered respectively according to sequence counter-clockwise Entitled A₁, A₂..., A_m..., A_M, the named Tag of destination node, measured obtain actual coordinate value for (x, y).In conjunction with Fig. 1 The step that is embodied as of the present invention is described:

Step one: first read the TOA initial data of destination node and base station, is calculated destination node and M base station Between distance r_m(m=1,2 ..., M).

Step 2: utilize Wylie differential method to judge distance r between destination node and base station_mWhether there is NLOS error.

Wylie differential method idiographic flow is as follows:

The location moment is designated as t_k=0, t₁,…,t_K, wherein t_KFor total positioning time, T is the interval of two adjacent moment Time, it is assumed that destination node is smoothed by fitting of a polynomial with the range measurements of each base station, i.e.

$r_m\left(t_k\right)=\underset{j=0}{\overset{J-1}{\Σ}}{a}_m\left(j\right)t_k^j,k=1,2,...,K---\left(1\right)$

Wherein, the exponent number of J-1 representative polynomial；r_m(t_k) represent that destination node is at t_kMoment and the distance of m-th base station.

Method of least square is utilized to solve unknowm coefficientMeasured value after can being smoothed is

$S_m\left(t_k\right)=\underset{j=0}{\overset{J-1}{\Σ}}{\hat{a}}_m\left(j\right)t_k^j,k=1,2,...,K---\left(2\right)$

With S_m(t_k) as actual distance reference value, calculate r_m(t_k) distance measure criteria deviation, be represented by

${\widehat{\mathrm{\σ}}}_m=\sqrt{\frac{1}{K}\underset{k=0}{\overset{K-1}{\Σ}}{\left(S_m\right(t_k)-r_m(t_k\left)\right)}^2}---\left(3\right)$

Assume that the distance measure criteria deviation under LOS environment is σ_m, in actual applications, σ_mCan be previously according to experimental field Measure and obtain.

Under NLOS environment, owing to NLOS error exists with the measurement error under LOS environment simultaneously, following knot can be obtained Really:

1. under LOS environment, it is known that

The most in a nlos environment, it is known that

Gained standard deviationThe biggest, show to be affected by NLOS the biggest.

Step 3: the signal between destination node and all base stations can be identified by step 2 and whether propagate by NLOS shadow Ring.The distance measure criteria deviation that will recordSort from small to large, select all r_mIn by NLOS affected minimum value, by it It is set to r₁, respective base station is as reference base station, and coordinate is (x₁,y₁)；Distance between destination node and remaining base station is according to base station Distribution sequence counterclockwise be set to r₂,…,r_M, respective base station coordinate is set to (x₂,y₂),…,(x_M,y_M)。

The conversion of TOA value is generated TDOA value, and the range difference obtaining correspondence is:

r_m1=r_m-r₁,2≤m≤M (4)

If being measured statistics by reality it follows that destination node and base station are LOS propagation, then the distance that average delay is corresponding For η_LOS.Propagate if destination node and base station are NLOS, then the distance that average delay is corresponding is η_NLOS.Corresponding to all TDOA values Range difference r_m1Value be reconstructed, method is as follows:

If 1. r_mAnd r₁It is LOS to propagate or NLOS propagation, then r_m1=r_m-r₁；

If 2. r_mPropagate for NLOS, r₁Propagate for LOS, then r_m1=(r_m-η_NLOS)-(r₁-η_LOS)；

Note: because all r_mValue has utilized Wylie differential method judge and sort, so r₁Distance measure criteria poor One definite proportion r_mLittle, therefore there is not r_mThe r for LOS propagates₁Situation about propagating for NLOS.

Step 4: the range difference r corresponding by TDOA value_m1After carrying out sighting distance reconstruct, calculated by Kalman TDOA algorithm To destination node at t_kThe theoretical coordinate value in momentThen, first threshold δ is set₁, according to inequality group:

$|\sqrt{{(x_m-{\tilde{x}}_{tk})}^2+{(y_m-{\tilde{y}}_{tk})}^2}-\sqrt{{(x_1-{\tilde{x}}_{tk})}^2+{(y_1-{\tilde{y}}_{tk})}^2}-r_{m1}|<{\mathrm{\&delta;}}_1---\left(5\right)$

Judging whether to meet formula (5), if meeting, then retaining this positioning resultContinue next step；Otherwise Abandon next step to calculate, terminate this position fixing process, return step one and re-read original TOA data.

Step 5: through aforementioned four step, it is believed that this time location data used are accurate data；Utilizing should Data, are calculated destination node at t by PF TDOA algorithm_kThe theoretical coordinate value in momentSpecific as follows:

5.1 state equations setting up destination node motion and observational equation are as follows:

X_tk=F X_t(k-1)+S·W_t(k-1) (6)

Z_tk=f (X_tk)+V_tk (7)

In formula, F is state-transition matrix；S is interference transfer matrix；W_t(k-1)And V_tkIt is respectively process noise and observation is made an uproar Sound；Z_tkFor destination node at t_kThe range difference that the TDOA observation of moment and all base stations is converted to；For destination node at moment t_kStatus information, X_t(k-1)For destination node at moment t_k-1State letter Breath, whereinFor t_kThe position coordinates of moment destination node,For t_k(this speed can for the speed of moment destination node Obtained by the acceleration transducer in the equipment of location, direction sensor estimation, or by pre-setting the initial speed of destination node Degree and acceleration calculation obtain), transfer matrix is:

$\begin{array}{cc}F=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]& S=\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right]\end{array}---\left(8\right)$

5.2 state equation moved according to destination node and observational equations, set up likelihood probability density function, see formula (9):

$p\left(Z_{\mathrm{tk}}\right|X_{\mathrm{tk}}^i)=\underset{m=2}{\overset{M}{\mathrm{\&Pi;}}}p\left(Z_{\mathrm{tk}}^m\right|X_{\mathrm{tk}}^i)=\underset{m=2}{\overset{M}{\mathrm{\&Pi;}}}\frac{1}{\sqrt{2\mathrm{\&pi;}}{\mathrm{\&sigma;}}_v}{e}^{-\frac{{({\hat{Z}}_{\mathrm{tk}}^m-Z_{\mathrm{tk}}^m)}^2}{2{\left({\mathrm{\&sigma;}}_v\right)}^2}---\left(9\right)}$

Wherein,For destination node at t_kThe range difference that the TDOA predictive value of moment and m-th base station is converted to, For destination node at t_kThe range difference that the TDOA observation of moment and m-th base station is converted to, σ_vFor observation noise V_tkSide Difference.Represent t_kThe status information of moment i-th particle, i=1,2 ..., N, N are total number of particles；

After 5.3 have obtained state equation and observational equation and likelihood probability density function by 5.1 and 5.2, in conjunction with PF TDOA algorithm filtering, obtains positioning result renewal step as follows:

(1) initialize: t_k=0

By prior distribution p (x₀) produce populationAll particle weights are 1/N.Wherein, p (x₀) by destination node Known initial state information bonding state equation obtains.

(2)for t_k=t₁,…,t_K

1. at t_kMoment, more new particle weights

$w_{tk}^i=w_{t(k-1)}^{i}p\left(Z_{tk}\right|X_{tk}^i),---\left(10\right)$

And normalization

$w_{tk}^i=w_{tk}^i/\underset{i=1}{\overset{N}{\&Sigma;}}{w}_{tk}^i---\left(11\right)$

2. resampling, the problem solving sample degeneracy, i.e. carrying out step by step along with iteration, the weights of a lot of particles can become Obtaining the least, the only particle weights of minority are relatively big, the problem that the number of effective particles in state space reduces.

Utilize effectively sampling yardstick N_effWeigh the degree of degeneration of particle weights

$N_{eff}=\frac{1}{\underset{i=1}{\overset{N}{\&Sigma;}}{\left(w_{tk}^i\right)}^2}---\left(12\right)$

Set an effective sample number N_thresholdAs threshold value, if N_eff＜ N_threshold, then carry out resampling, obtain new PopulationAll particle weights are set to 1/N.

3. state estimation

Destination node is at t_kThe status information in moment is

$X_{tk}=\underset{i=1}{\overset{N}{\&Sigma;}}{w}_{tk}^i{X}_{tk}^i---\left(13\right)$

Obtain destination node at t_kThe theoretical coordinate value in moment

The most more new state information

The t that will obtain in step 6_kThe final elements of a fix value (x of moment destination node_tk,y_tk) be assigned to

end

Step 6: Second Threshold δ is set₂, utilize inequality:

$\sqrt{{({\stackrel{\&OverBar;}{x}}_{tk}-{\tilde{x}}_{tk})}^2+{({\stackrel{\&OverBar;}{y}}_{tk}-{\tilde{y}}_{tk})}^2}<{\mathrm{\&delta;}}_2---\left(14\right)$

Judge step 4, five twice estimated result obtainedWithThe most close, if meeting inequality, Residual weighted formula (15) is then utilized to obtain t_kThe final elements of a fix value (x of moment destination node_tk,y_tk), utilize this coordinate figure Update the status information of destination node previous moment in EKF TDOA algorithm and PF TDOA algorithm respectively；If being unsatisfactory for inequality, Then terminate this position fixing process, return step one and read initial data.

$x_{tk}=\frac{R_{es\_kalman}{\tilde{x}}_{tk}+R_{es\_particle}{\stackrel{\&OverBar;}{x}}_{tk}}{R_{es\_kalman}+R_{es\_particle}}$

$y_{tk}=\frac{R_{es\_kalman}{\tilde{y}}_{tk}+R_{es\_particle}{\stackrel{\&OverBar;}{y}}_{tk}}{R_{es\_kalman}+R_{es\_particle}}---\left(15\right)$

Wherein,Corresponding residual sum of squares (RSS) is

$R_{es\_kalman}=\underset{m=2}{\overset{M}{\&Sigma;}}{\left(\sqrt{{(x_m-{\tilde{x}}_{tk})}^2+{(y_m-{\tilde{y}}_{tk})}^2}-\sqrt{{(x_1-{\tilde{x}}_{tk})}^2+{(y_1-{\tilde{y}}_{tk})}^2}-r_{m1}\right)}^2---\left(16\right)$

Corresponding residual sum of squares (RSS) is

$R_{es\_particle}=\underset{m=2}{\overset{M}{\&Sigma;}}{\left(\sqrt{{(x_m-{\stackrel{\&OverBar;}{x}}_{tk})}^2+{(y_m-{\stackrel{\&OverBar;}{y}}_{tk})}^2}-\sqrt{{(x_1-{\stackrel{\&OverBar;}{x}}_{tk})}^2+{(y_1-{\stackrel{\&OverBar;}{y}}_{tk})}^2}-r_{m1}\right)}^2---\left(17\right)$

Threshold value δ₁、δ₂Choose and mainly initially select according to the theoretical precision of location equipment, then through actual reality Test examination is finely adjusted, and realizes filtering the data affected by NLOS error with this, improves positioning precision.

Step 7, pass through step 6, it will obtain α (α≤K) individual elements of a fix value (x_tk,y_tk), give each data and add Weight coefficient, is weighted smooth obtaining final estimated resultWherein, the weight coefficient of each data with get this number According to time relevant, data acquisition must be got over early, and weight coefficient is the least, obtains the most late, and weight coefficient is the biggest.

For the evaluation of localization method performance, positioning precision is one of important indicator.In the present invention, positioning precision is by managing Opinion estimates position coordinatesWith true location coordinate (x, y) between degree of closeness weigh:

$err=\sqrt{{(\hat{x}-x)}^2+{(\hat{y}-y)}^2}---\left(18\right)$

According to the explanation of equipment service manual, under LOS environment under, nanoLOC Development kit 3.0 develops External member can reach indoor 2 meters, the positioning precision of outdoor 1 meter.The colocated method that the present invention proposes is applied to this exploitation set In part, it is possible to obtain positioning precision better than it, also will be better than being used alone existing EKF or PF algorithm.

The invention has the beneficial effects as follows: utilize Wylie differential method be identified NLOS error and suppress, preliminary eliminating is not Just data；Consider the positioning performance of two kinds of TDOA algorithm for estimating of EKF and PF, under the conditions of proposing a kind of nonlinear and non-Gaussian simultaneously Colocated method based on EKF and PF, the method compared with being more suitable for the localizing environment of nonlinear and non-Gaussian for EKF, relatively PF For preferably avoid the use to incorrect data, decrease amount of calculation.This colocated method effectively reduces NLOS by mistake The impact of difference, in conjunction with the advantage of EKF and PF, overcomes both deficiencies, it is achieved more accurate location simultaneously.

Accompanying drawing explanation

Fig. 1 is the positioning flow figure of the present invention；

Fig. 2 is the sensing node TDOA location model figure of the present invention.

Detailed description of the invention

The present invention relates to indoor positioning tracking technique field, be specially under the conditions of a kind of nonlinear and non-Gaussian based on EKF and The colocated method of PF, decreases NLOS error, in conjunction with the advantage of EKF and PF, overcomes and is used alone one of which The deficiency of algorithm.

Below in conjunction with accompanying drawing, the present invention will be further described.

CSS (Chirp Spread Spectrum, linear frequency modulation spreads) is to open based on IEEE802.15.4a agreement The wireless communication technology sent out.The positioning experiment platform of the present invention uses Nanotron company nanoLOC Development kit 3.0 development kit, this development kit, based on nanoLOC TRX radio frequency chip, can be used for developing communication based on CSS technology, survey The wireless application such as away from, location.The present invention carries out building of alignment system by using this set development kit.

The alignment system of the present invention utilizes nanoLOC Development kit 3.0 development kit in the middle part of located space Administration's destination node and base station, use one destination node of four architectures, as shown in Figure 2；Wherein, four base stations are according to the inverse time Pin order is respectively designated as A₁, A₂, A₃, A₄, the named Tag of destination node, measured obtain actual coordinate value for (x, y).Knot Close Fig. 1 and the step that is embodied as of the present invention be described:

Step one: first read the TOA initial data of destination node and base station, be calculated destination node and four base stations Between distance r_m(m=1,2,3,4).

Step 2: utilize Wylie differential method to judgeWhether value has NLOS error.

Wylie differential method idiographic flow is as follows:

The location moment is designated as t_k=0, t₁,…,t_K, wherein t_KFor total positioning time, T is the interval of two adjacent moment Time, it is assumed that the range measurements between each base station and destination node is smoothed by fitting of a polynomial, i.e.

$r_m\left(t_k\right)=\underset{j=0}{\overset{J-1}{\&Sigma;}}{a}_m\left(j\right)t_k^j,k=1,2,...,K---\left(1\right)$

Wherein, the exponent number of J-1 representative polynomial；r_m(t_k) represent that destination node is at t_kMoment and the distance of m-th base station.

Method of least square is utilized to solve unknowm coefficientMeasured value after can being smoothed is

$S_m\left(t_k\right)=\underset{j=0}{\overset{J-1}{\&Sigma;}}{\hat{a}}_m\left(j\right)t_k^j,k=1,2,...,K---\left(2\right)$

With s_m(t_k) as actual distance reference value, calculate r_m(t_k) distance measure criteria deviation, be represented by

${\widehat{\mathrm{\&sigma;}}}_m=\sqrt{\frac{1}{K}\underset{k=0}{\overset{K-1}{\&Sigma;}}{\left(S_m\right(t_k)-r_m(t_k\left)\right)}^2}---\left(3\right)$

Assume that the distance measure criteria deviation under LOS environment is σ_m, in actual applications, σ_mCan be previously according to experimental field Measure and obtain.

Under NLOS environment, owing to NLOS error exists with the measurement error under LOS environment simultaneously, following knot can be obtained Really:

1. under LOS environment, it is known that

The most in a nlos environment, it is known that

Gained standard deviationThe biggest, show to be affected by NLOS the biggest.

Step 3: can identify whether the propagation between destination node and all base stations is affected by NLOS by step 2. The distance measure criteria deviation that will recordSort from small to large, select all r_mIn by NLOS affected minimum value, set For r₁, respective base station is as reference base station, and coordinate is (x₁,y₁)；Distance between destination node and remaining base station is according to base station Distribution sequence is set to r counterclockwise₂,r₃,r₄, respective base station coordinate is set to (x₂,y₂),(x₃,y₃),(x₄,y₄)。

The conversion of TOA value is generated TDOA value, and the range difference obtaining correspondence is:

r_m1=r_m-r₁,2≤m≤4 (4)

If being measured statistics by reality it follows that destination node and base station are LOS propagation, then the distance that average delay is corresponding For η_LOS.Propagate if destination node and base station are NLOS, then the distance that average delay is corresponding is η_NLOS.Corresponding to all TDOA values Range difference r_m1Value be reconstructed, method is as follows:

If 1. r_mAnd r₁It is LOS to propagate or NLOS propagation, then r_m1=r_m-r₁；

If 2. r_mPropagate for NLOS, r₁Propagate for LOS, then r_m1=(r_m-η_NLOS)-(r₁-η_LOS)；

Note: because all r_mValue has utilized Wylie differential method judge and sort, so r₁Distance measure criteria poor One definite proportion r_mLittle, therefore there is not r_mThe r for LOS propagates₁Situation about propagating for NLOS.

Step 4: the range difference r corresponding by TDOA value_m1After carrying out sighting distance reconstruct, calculated by Kalman TDOA algorithm To destination node at t_kThe theoretical coordinate value in momentThen, first threshold δ is set₁, according to inequality group:

$|\sqrt{{(x_m-{\tilde{x}}_{tk})}^2+{(y_m-{\tilde{y}}_{tk})}^2}-\sqrt{{(x_1-{\tilde{x}}_{tk})}^2+{(y_1-{\tilde{y}}_{tk})}^2}-r_{m1}|<{\mathrm{\&delta;}}_1---\left(5\right)$

Judging whether to meet formula (5), if meeting, then retaining this positioning resultContinue next step；Otherwise Abandon next step to calculate, terminate this position fixing process, return step one and re-read original TOA data.

Step 5: through aforementioned four step, it is believed that this time location data used are accurate data；Utilizing should Data, are calculated destination node at t by PF TDOA algorithm_kThe theoretical coordinate value in momentSpecific as follows:

5.1 state equations setting up destination node motion and observational equation are as follows:

X_tk=F X_t(k-1)+S·W_t(k-1) (6)

Z_tk=f (X_tk)+V_tk (7)

In formula, F is state-transition matrix；S is interference transfer matrix；W_t(k-1)And V_tkIt is respectively process noise and observation is made an uproar Sound；Z_tkFor destination node at t_kThe range difference that the TDOA observation of moment and all base stations is converted to；For destination node at moment t_kStatus information, X_t(k-1)For destination node at moment t_k-1State letter Breath, whereinFor t_kThe position coordinates of moment destination node,For t_k(this speed can for the speed of moment destination node Obtained by the acceleration transducer in the equipment of location, direction sensor estimation, or by pre-setting the initial speed of destination node Degree and acceleration calculation obtain), transfer matrix is:

$\begin{array}{cc}F=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]& S=\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right]\end{array}---\left(8\right)$

5.2 state equation moved according to destination node and observational equations, set up likelihood probability density function, see formula (9):

$p\left(Z_{\mathrm{tk}}\right|X_{\mathrm{tk}}^i)=\underset{m=2}{\overset{M}{\mathrm{\&Pi;}}}p\left(Z_{\mathrm{tk}}^m\right|X_{\mathrm{tk}}^i)=\underset{m=2}{\overset{M}{\mathrm{\&Pi;}}}\frac{1}{\sqrt{2\mathrm{\&pi;}}{\mathrm{\&sigma;}}_v}{e}^{-\frac{{({\hat{Z}}_{\mathrm{tk}}^m-Z_{\mathrm{tk}}^m)}^2}{2{\left({\mathrm{\&sigma;}}_v\right)}^2}---\left(9\right)}$

Wherein,For destination node at t_kThe range difference that the TDOA predictive value of moment and m-th base station is converted to, For destination node at t_kThe range difference that the TDOA observation of moment and m-th base station is converted to, σ_vFor observation noise V_tkSide Difference.Represent t_kThe status information of moment i-th particle, i=1,2 ..., N, N are total number of particles；

After 5.3 have obtained state equation and observational equation and likelihood probability density function by 5.1 and 5.2, in conjunction with PF TDOA algorithm filtering, obtains positioning result renewal step as follows:

(3) initialize: t_k=0

By prior distribution p (x₀) produce populationAll particle weights are 1/N.Wherein, p (x₀) by destination node Known initial state information bonding state equation obtains.

(4)for t_k=t₁,…,t_K

1. at t_kMoment, more new particle weights

$w_{tk}^i=w_{t(k-1)}^{i}p\left(Z_{tk}\right|X_{tk}^i),---\left(10\right)$

And normalization

$w_{tk}^i=w_{tk}^i/\underset{i=1}{\overset{N}{\&Sigma;}}{w}_{tk}^i---\left(11\right)$

2. resampling, the problem solving sample degeneracy, i.e. carrying out step by step along with iteration, the weights of a lot of particles can become Obtaining the least, the only particle weights of minority are relatively big, the problem that the number of effective particles in state space reduces.

Utilize effectively sampling yardstick N_effWeigh the degree of degeneration of particle weights

$N_{eff}=\frac{1}{\underset{i=1}{\overset{N}{\&Sigma;}}{\left(w_{tk}^i\right)}^2}---\left(12\right)$

Set an effective sample number N_thresholdAs threshold value, if N_eff＜ N_threshold, then carry out resampling, obtain new PopulationAll particle weights are set to 1/N.

3. state estimation

Destination node is at t_kThe status information in moment is

$X_{tk}=\underset{i=1}{\overset{N}{\&Sigma;}}{w}_{tk}^i{X}_{tk}^i---\left(13\right)$

Obtain destination node at t_kThe theoretical coordinate value in moment

The most more new state information

The t that will obtain in step 6_kThe final elements of a fix value (x of moment destination node_tk,y_tk) be assigned to

end

Step 6: Second Threshold δ is set₂, utilize inequality:

$\sqrt{{({\stackrel{\&OverBar;}{x}}_{tk}-{\tilde{x}}_{tk})}^2+{({\stackrel{\&OverBar;}{y}}_{tk}-{\tilde{y}}_{tk})}^2}<{\mathrm{\&delta;}}_2---\left(14\right)$

Judge step 4, five twice estimated result obtainedWithThe most close, if meeting inequality, Residual weighted formula (15) is then utilized to obtain t_kThe final elements of a fix value (x of moment destination node_tk,y_tk), utilize this coordinate figure Update the status information of destination node previous moment in EKF TDOA algorithm and PF TDOA algorithm respectively；If being unsatisfactory for inequality, Then terminate this position fixing process, return step one and read initial data.

$x_{tk}=\frac{R_{es\_kalman}{\tilde{x}}_{tk}+R_{es\_particle}{\stackrel{\&OverBar;}{x}}_{tk}}{R_{es\_kalman}+R_{es\_particle}}$

$y_{tk}=\frac{R_{es\_kalman}{\tilde{y}}_{tk}+R_{es\_particle}{\stackrel{\&OverBar;}{y}}_{tk}}{R_{es\_kalman}+R_{es\_particle}}---\left(15\right)$

Wherein,Corresponding residual sum of squares (RSS) is

$R_{es\_kalman}=\underset{m=2}{\overset{M}{\&Sigma;}}{\left(\sqrt{{(x_m-{\tilde{x}}_{tk})}^2+{(y_m-{\tilde{y}}_{tk})}^2}-\sqrt{{(x_1-{\tilde{x}}_{tk})}^2+{(y_1-{\tilde{y}}_{tk})}^2}-r_{m1}\right)}^2---\left(16\right)$

Corresponding residual sum of squares (RSS) is

$R_{es\_particle}=\underset{m=2}{\overset{M}{\&Sigma;}}{\left(\sqrt{{(x_m-{\stackrel{\&OverBar;}{x}}_{tk})}^2+{(y_m-{\stackrel{\&OverBar;}{y}}_{tk})}^2}-\sqrt{{(x_1-{\stackrel{\&OverBar;}{x}}_{tk})}^2+{(y_1-{\stackrel{\&OverBar;}{y}}_{tk})}^2}-r_{m1}\right)}^2---\left(17\right)$

Threshold value δ₁、δ₂Choose and mainly initially select according to the theoretical precision of location equipment, then through actual reality Test examination is finely adjusted, and realizes filtering the data affected by NLOS error with this, improves positioning precision.

Step 7 passes through step 6, it will obtain α (α≤K) individual elements of a fix value (x_tk,y_tk), give each data weighting Coefficient, is weighted smooth obtaining final estimated resultWherein, the weight coefficient of each data with get this data Time relevant, data acquisition must be got over early, and weight coefficient is the least, obtains the most late, and weight coefficient is the biggest.

For the evaluation of localization method performance, positioning precision is one of important indicator.In the present invention, positioning precision is by managing Opinion estimates position coordinatesWith true location coordinate (x, y) between degree of closeness weigh:

$err=\sqrt{{(\hat{x}-x)}^2+{(\hat{y}-y)}^2}---\left(18\right)$

According to the explanation of equipment service manual, under LOS environment, nanoLOC Development kit 3.0 develops set Part can reach indoor 2 meters, the positioning precision of outdoor 1 meter.The colocated method that the present invention proposes is applied to this development kit In, it is possible to obtain positioning precision better than it, also will be better than being used alone existing EKF or PF algorithm.

In a word, it is contemplated that reduce the impact on TDOA value of the NLOS error, EKF and PF is be combined with each other, makes full use of Respective advantage, overcomes the deficiency being used alone a kind of algorithm so that localization method is more suitable for nonlinear and non-Gaussian simultaneously Situation, reduces algorithm amount of calculation while improving positioning precision.

Above by with reference to accompanying drawing, the present invention is done special displaying and explanation, it will be apparent to those skilled in the art that Should be understood that and make various modifications and changes in the form and details, all under without departing substantially from the thought of the present invention and scope It will be the infringement to patent of the present invention.Therefore the present invention real thought to be protected and scope are limited by appending claims Fixed.

Claims (6)

1. colocated method based on EKF and PF under the conditions of nonlinear and non-Gaussian, it is characterised in that the method includes following step Rapid:

Step (1), obtain destination node respectively with the TOA initial data of each base station, be calculated destination node and each base station it Between distance r_m(m=1,2 ..., M), wherein M is the total number in base station；

Step (2), Wylie differential method is utilized to determine whether NLOS error；

Step (3), TOA value is done difference obtain TDOA value, and the range difference r to all TDOA values correspondence_m1It is reconstructed；

Step (4), use EKF TDOA algorithm estimation t_kThe position coordinates of moment destination node also judges；

Step (5), use PF TDOA algorithm estimation t_kThe position coordinates of moment destination node also judges；

Step (6), two position coordinateses to step (4), (5) judge, residual weighted obtains t_kThe final estimation in moment Value；

Step (7), all elements of a fix data obtaining step (6) are weighted smoothing, and obtain finally estimating of destination node Meter position coordinates

2. colocated method based on EKF and PF under the conditions of nonlinear and non-Gaussian as claimed in claim 1, it is characterised in that Step (2) is specific as follows:

The location moment is designated as t_k=0, t₁,…,t_K, wherein t_KFor total positioning time, T is the interval time of two adjacent moment, Assume that destination node is smoothed by fitting of a polynomial with the range measurements of each base station, i.e.

$r_m\left(t_k\right)=\underset{j=0}{\overset{J-1}{\&Sigma;}}{a}_m\left(j\right)t_k^j,k=1,2,...,K---\left(1\right)$

Wherein, the exponent number of J-1 representative polynomial；r_m(t_k) represent that destination node is at t_kMoment and the distance of m-th base station；

Method of least square is utilized to solve unknowm coefficientMeasured value after can being smoothed is

$S_m\left(t_k\right)=\underset{j=0}{\overset{J-1}{\&Sigma;}}{\hat{a}}_m\left(j\right)t_k^j,k=1,2,...,K---\left(2\right)$

With S_m(t_k) as actual distance reference value, calculate r_m(t_k) distance measure criteria deviation, be represented by

${\widehat{\mathrm{\&sigma;}}}_m=\sqrt{\frac{1}{K}\underset{k=0}{\overset{K-1}{\&Sigma;}}{\left(S_m\right(t_k)-r_m(t_k\left)\right)}^2}---\left(3\right)$

Assume that the distance measure criteria deviation under LOS environment is σ_m, can measure previously according to experimental field and obtain；

IfThen judge between destination node and base station it is that LOS propagates；IfThen judge destination node and base station Between be NLOS propagate.

3. colocated method based on EKF and PF under the conditions of nonlinear and non-Gaussian as claimed in claim 2, it is characterised in that Step (3) is specific as follows:

If there is NLOS to propagate, by distance measure criteria deviationDestination node corresponding to middle minima and respective base station away from From being set to r₁, this base station is as reference base station, and coordinate is (x₁,y₁)；Distance between destination node and remaining base station is set to r₂,…,r_M, respective base station coordinate is set to (x₂,y₂),…,(x_M,y_M)；

The conversion of TOA value is generated TDOA value, and the range difference r corresponding to all TDOA values_m1Value be reconstructed:

If r_mAnd r₁It is LOS to propagate or NLOS propagation, then r_m1=r_m-r₁；

If r_mPropagate for NLOS, r₁Propagate for LOS, then r_m1=(r_m-η_NLOS)-(r₁-η_LOS)；

Wherein η_LOSAverage delay respective distances during for propagating for LOS between destination node and base station；η_NLOSFor destination node and base Average delay respective distances when propagating for NLOS between standing.

4. colocated method based on EKF and PF under the conditions of nonlinear and non-Gaussian as claimed in claim 3, it is characterised in that Step (4) is specific as follows:

It is calculated destination node at t by Kalman TDOA algorithm_kThe theoretical coordinate value in momentThen according to public affairs Formula (5) judges；If meeting, then retain this positioning resultContinue next step；Otherwise abandon next step to calculate, Terminate this position fixing process, return step (1) and re-read original TOA data；

$|\sqrt{{(x_m-{\tilde{x}}_{tk})}^2+{(y_m-{\tilde{y}}_{tk})}^2}-\sqrt{{(x_1-{\tilde{x}}_{tk})}^2+{(y_1-{\tilde{y}}_{tk})}^2}-r_{m1}|<{\mathrm{\&delta;}}_1---\left(5\right)$

Wherein δ₁For threshold value, artificially can select according to the theoretical precision of location equipment and actual experiment.

5. colocated method based on EKF and PF under the conditions of nonlinear and non-Gaussian as claimed in claim 4, it is characterised in that Step (5) is calculated destination node at t by PF TDOA algorithm_kThe theoretical coordinate value in momentSpecific as follows:

5.1 state equations setting up destination node motion and observational equation are as follows:

X_tk=F X_t(k-1)+S·W_t(k-1) (6)

Z_tk=f (X_tk)+V_tk (7)

In formula, F is state-transition matrix, sees formula (8)；S is interference transfer matrix, sees formula (8)；W_t(k-1)And V_tkIt is respectively Process noise and observation noise；Z_tkFor destination node at t_kThe distance that the TDOA observation of moment and all base stations is converted to Difference；For destination node at moment t_kStatus information, X_t(k-1)For destination node at moment t_k-1Shape State information, whereinFor t_kThe position coordinates of moment destination node,For t_kThe speed of moment destination node；

$\begin{array}{cc}F=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]& S=\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right]\end{array}---\left(8\right)$

5.2, according to the state equation of destination node motion and observational equations, set up likelihood probability density function, see formula (9):

Likelihood probability density is represented by:

$p\left(Z_{tk}\right|X_{tk}^i)=\underset{m=2}{\overset{M}{\&Pi;}}p\left(Z_{tk}^m\right|x_{tk}^i)=\underset{m=2}{\overset{M}{\&Pi;}}\frac{1}{\sqrt{2\mathrm{\&pi;}}{\mathrm{\&sigma;}}_v}{e}^{-\frac{{({\hat{Z}}_{tk}^m-Z_{tk}^m)}^2}{2{\left({\mathrm{\&sigma;}}_v\right)}^2}}---\left(9\right)$

Wherein,For destination node at t_kThe range difference that the TDOA predictive value of moment and m-th base station is converted to,For mesh Mark node is at t_kThe range difference that the TDOA observation of moment and m-th base station is converted to, σ_vFor observation noise V_tkVariance； Represent t_kThe status information of moment i-th particle, i=1,2 ..., N, N are total number of particles；

After 5.3 have obtained state equation and observational equation and likelihood probability density function by 5.1 and 5.2, in conjunction with PF TDOA algorithm filtering, obtains positioning result renewal step as follows:

A) initialize: t_k=0

By prior distribution p (x₀) produce populationAll particle weights are 1/N；Wherein, p (x₀) known by destination node Initial state information bonding state equation obtain；

b)t_k=t₁,…,t_K

1. at t_kMoment, more new particle weights

$w_{tk}^i=w_{t(k-1)}^{i}p\left(Z_{tk}\right|X_{tk}^i)---\left(10\right)$

And normalization

2. resampling；

Utilize effectively sampling yardstick N_effWeigh the degree of degeneration of particle weights

$N_{eff}=\frac{1}{\underset{i=1}{\overset{N}{\&Sigma;}}{\left(w_{tk}^i\right)}^2}---\left(12\right)$

Set an effective sample number N_thresholdAs threshold value, if N_eff＜ N_threshold, then carry out resampling, obtain new grain SubgroupAll particle weights are set to 1/N；

3. state estimation

Destination node is at t_kThe status information in moment is

$X_{tk}=\underset{i=1}{\overset{N}{\&Sigma;}}{w}_{tk}^i{X}_{tk}^i---\left(13\right)$

Obtain destination node at t_kThe theoretical coordinate value in moment

The most more new state information

The t that will obtain in step 6_kThe final elements of a fix value (x of moment destination node_tk,y_tk) be assigned to

6. colocated method based on EKF and PF under the conditions of nonlinear and non-Gaussian as claimed in claim 5, it is characterised in that Step (6) is specific as follows:

Second Threshold δ is set₂, utilize inequality:

$\sqrt{{({\stackrel{\&OverBar;}{x}}_{tk}-{\tilde{x}}_{tk})}^2+{({\stackrel{\&OverBar;}{y}}_{tk}-{\tilde{y}}_{tk})}^2}<{\mathrm{\&delta;}}_2---\left(14\right)$

Judge twice estimated result that step (4), (5) obtainWithThe most close, if meeting inequality, then Residual weighted formula (15) is utilized to obtain t_kThe final elements of a fix value (x of moment destination node_tk,y_tk), utilize this coordinate figure to divide Geng Xin the status information of destination node previous moment in EKF TDOA algorithm and PF TDOA algorithm；If being unsatisfactory for inequality, then Terminate this position fixing process, return step (1) and read TOA initial data；

$x_{tk}=\frac{R_{es\_kalman}{\tilde{x}}_{tk}+R_{es\_particle}{\stackrel{\&OverBar;}{x}}_{tk}}{R_{es\_kalman}+R_{es\_particle}}$

$y_{tk}=\frac{R_{es\_kalman}{\tilde{y}}_{tk}+R_{es\_particle}{\stackrel{\&OverBar;}{y}}_{tk}}{R_{es\_kalman}+R_{es\_particle}}---\left(15\right)$

Wherein,Corresponding residual sum of squares (RSS) is

$R_{es\_kalman}=\underset{m=2}{\overset{M}{\&Sigma;}}{\left(\sqrt{{(x_m-{\tilde{x}}_{tk})}^2+{(y_m-{\tilde{y}}_{tk})}^2}-\sqrt{{(x_1-{\tilde{x}}_{tk})}^2+{(y_1-{\tilde{y}}_{tk})}^2}-r_{m1}\right)}^2---\left(16\right)$

Corresponding residual sum of squares (RSS) is

$R_{es\_particle}=\underset{m=2}{\overset{M}{\&Sigma;}}{\left(\sqrt{{(x_m-{\stackrel{\&OverBar;}{x}}_{tk})}^2+{(y_m-{\stackrel{\&OverBar;}{y}}_{tk})}^2}-\sqrt{{(x_1-{\stackrel{\&OverBar;}{x}}_{tk})}^2+{(y_1-{\stackrel{\&OverBar;}{y}}_{tk})}^2}-r_{m1}\right)}^2---\left(17\right)$