CN101644758A - Target localization and tracking system and method - Google Patents

Abstract

The invention relates to a target localization and tracking system and a method. The system comprises a plurality of cluster localization modules and command control modules, each cluster localizationmodule comprises a plurality of sensor nodes and cluster head nodes, and each cluster head node comprises an initialization module, a particle filtration module, a particle weight calculation module,a re-sampling determination module and an estimation target state module. The method which combines Kalman and particle filtration is adopted for realizing the passive localization of a target, and the computation speed is far lower than the particle filtration method on the basis of realizing high-precision localization; the localization and the track of the target is finally realized by adopting multiple sensors and being based on the observation of the direction of the target, thereby being capable of overcoming the constraint that the traditional machine-mounted or ship-mounted single station system must carry out motorization during the observation period, having no need of observing the motorization of a platform, improving the flexibility of target localization, greatly increasingthe area of a region of target monitoring localization, avoiding the shortcoming of existence of localization blind regions and having very high effectiveness, accuracy and feasibility.

Description

A kind of target localization and tracking system and method

Technical field

The present invention relates to areas of information technology, particularly a kind of target localization and tracking system and method.

Background technology

Because modern information technologies, network technology and development of wireless communication devices have promoted developing rapidly of wireless sensor network technology.The location of moving target and tracking technique have wide application background, relate to the military and civilian field.In traditional target passive positioning technology, the target passive positioning technology that adopt based on pure measurement of bearing, its problem that mainly solves are how to utilize the target azimuth information of observation to come the kinematic parameter of estimating target and then the location of realization target to follow the tracks of more.System configuration mostly is airborne or carrier-borne single station measuring system, this system configuration is owing to be subjected to the restriction of moving target observability principle, realize location to target, observation platform need carry out motor-driven in the observation phase, can be sometimes because observation platform can't be motor-driven, the near and location that can not realize target; Wireless sensor network is formed by a plurality of sensor nodes of disposing, the network system of the self-organization of a multi-hop that forms by communication.Its objective is the information of perceptive object in perception collaboratively, collection and the processing network's coverage area.

From technical elements, based on being a non-linear Bayes filtering problem in the pure orientation target motion analysis question essence.Owing to nonlinear reason, its exact solution is not resolved usually, so the suboptimal solutions that adopt based on EKF (EKF) on the engineering, it is to utilize Taylor series expansion more, get its first approximation and make the model linearization, utilize Kalman filtering (KF) algorithm to find the solution again.And the precision that EKF filtering is estimated when the non-linear strong and non-Gauss of system noise of model seriously reduces, and may cause dispersing of wave filter.And tasteless Kalman (UKF) adopts UT (Unscented Transformation) to change, though than EKF better filter effect is arranged, all is under the condition based on model linearization and Gauss hypothesis.Therefore people are applicable to that in searching non-linear non-Gauss Bayes filtering method improves estimated accuracy always.Obtained the achievement that attracts people's attention in the research of this direction in the world in recent years, wherein be subjected to paying close attention to more widely with particle filter (PF) especially, so occurred realizing the pure measurement of bearing location technology of target in recent years again based on particle filter method, though this method is than the method bearing accuracy height based on KF, but this method needs more population to the accurate estimation that realizes posterior probability, cause calculated amount big, computing time is long, is unfavorable for real-time application.

Summary of the invention

In order to overcome unobservable property and slow these two deficiencies of particle filter computing velocity to single station recording geometry, the invention provides a kind of target localization and tracking system and method, this system and method is based on the above-mentioned analysis foundation to traditional position location techniques, adopt a kind of new target localization technology to realize the analysis of moving target, the measurement of single station can be effectively overcome and motor-driven deficiency must be carried out, and adopt a kind of improved particle filter method to realize the location, its computing time is well below adopting particle filter to realize the method for location.

In order to achieve the above object, a kind of target localization and tracking system provided by the invention comprises: a plurality of bunches of locating modules and are accused module.

Described bunch of locating module comprises: a plurality of sensor nodes and a leader cluster node.

Described sensor node is used for its place bunch inner region is monitored, and occurs as target, then target direction is measured, and this measured value and self coordinate information are sent to leader cluster node in this bunch.

Described leader cluster node is used for the measured value and the coordinate information thereof that send according to the sensor node in this bunch, finishes dbjective state and estimates, realizes that the location of target is followed the tracks of, and will locate tracking results and send to the charge module.

Described charge module is used to collect and show the location tracking results of each leader cluster node.

Described leader cluster node further comprises:

One initialization module is used to set up target travel and observation equation, the initial state distribution function of target and the estimation variance of original state, and produces N particle at random.

One particle filter module is used to utilize state equation that each particle state is carried out prediction, renewal based on EKF.

One particle weights computing module is used to calculate the particle weights.

One resampling judge module is used for according to effective population N _EffJudge whether and to resample, if effective population N _EffLess than preset value, then resample, according to N particle of importance density function resampling and distribute weights.And

One estimating target block of state is used for calculating the estimating target state according to the weights of particle.

Wherein, described initialization module further comprises: one sets up target travel and observation model module, is set up object initialization state and an estimation variance module and a random particles generation module.

Described target travel and the observation model module set up is used to set up the single order markov system equation of target and the observation model of target observation:

${\mathrm{\θ}}_i={\tan }^{-1}\left(\frac{y_T-s_y^i}{x_T-s_x^i}\right)+n_i;$

Wherein: X _T=[x _T, v _x, y _T, v _y] expression target motion state, x _T, y _TRepresent the coordinate figure of target respectively, v in X-axis and Y-axis _x, v _yRepresent that respectively target is at the absolute velocity component in X-axis and Y-axis; $S^i=[s_x^i,s_y^i]$ I=1,2 ... N, expression sensor node i is respectively at the coordinate of X-axis and Y-axis, and N represents to observe the sensor node number of target; θ _iThe relative orientation of expression target and sensor node; n _iThe measurement noise of representing i sensor node.

Described object initialization state and the estimation variance module set up is used to set up the original state probability distribution function and the estimation variance of target.

Described original state probability distribution function is: X _K+1=F _kX _k+ G _kA _k

Wherein: A _k=[a _{X, k}, a _{Y, k}] ^TBe the processing noise of system, promptly because the uncertainty of target travel environment causes respectively the acceleration noise that causes in X and Y-axis, A _k～N (0, R _k);

$F_k=\left[\begin{array}{cccc}1& T_s& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T_s\\ 0& 0& 0& 1\end{array}\right]$ $G_k=\left[\begin{array}{cc}{\mathrm{<figure-callout\; id="2"\; label="T\; s"\; filenames="A2009100784740025H1.png"\; state="\{\{state\}\}">T</figure-callout>}}_s^{}/2& 0\\ T_s& 0\\ 0& T_s^2/2\\ 0& T_s\end{array}\right];$

Described random particles generation module is used for the (x from initial state distribution p ₀) in randomly draw N primary { x ₀ ⁱ, i=1,2 ... N}, the covariance matrix corresponding with primary is: { P ₀ ⁱ, i=1,2 ... N}.

Wherein, described particle filter module further comprises: a prediction module and a update module.

Described prediction module is used to utilize state equation to predict the state of k target constantly

And prediction covariance matrix

Wherein: ${\hat{x}}_{k/k-1}^i=F_{k-1}^i{x}_{k-1}^i+G_k^i{A}_k^i;$ ${\hat{P}}_{k/k-1}^i=F_{k-1}^i{P}_{k-1}^i{F_{k-1}^i}^T+Q_{k-1}^i;$ $F_{k-1}^i=F_k.$

Described update module: be used to utilize k measured value constantly that particle is predicted the outcome and the estimate covariance matrix upgrades, obtain k updating value and covariance matrix x constantly _k ⁱAnd P _k ⁱ

Wherein: $x_k^i={\hat{x}}_{k/k-1}^i+K_k^i(y_k-h\left({\hat{x}}_{k/k-1}^i\right)),$ $P_k^i=(I-K_k^i{H}_k^i){\hat{P}}_{k/k-1}^i;$

$H_k^i=\frac{\&PartialD;h}{\&PartialD;x}{|}_{x={\hat{x}}_{k/k-1}^i},$ $K={\hat{P}}_{k/k-1}^i{H_k^i}^T{(H_k^i{\hat{P}}_{k/k-1}^i{H_k^i}^T+R_k)}^{-1}.$

Wherein, described particle weights computing module is according to Bayes and particle filter Theoretical Calculation particle weights:

${\mathrm{\&omega;}}_k^i={\mathrm{\&omega;}}_{k-1}^i\frac{p(z_k/x_k^i)p(x_k^i/x_{k-1}^i)}{q(x_k^i/x_{0:k-1}^i,z_{0:k})};$

Wherein: q (x _k/ x _0:k-1 ⁱ, z _1:k) be the importance density function, p (z _k/ x _k ^j) be the observation likelihood function, p (x _k/ x _K-1) be the transition probability of system's first-order Markov process.

Wherein, described effective population N _EffBe defined as follows:

$N_{\mathrm{eff}}=\frac{1}{\underset{i-1}{\overset{N}{\mathrm{\&Sigma;}}}{\left({\stackrel{\&OverBar;}{\mathrm{\&omega;}}}_k^i\right)}^2}.$

Wherein, described sensor node and leader cluster node are equipped with gps system, and target localization and tracking system adopts the mode of GPS time service to realize the time synchronized of network.

A kind of target localization tracking provided by the invention comprises the steps:

(1) finishes system configuration, comprise sensor node, leader cluster node and charge system.

(2) finish netinit.

(3) sensor node is monitored in the place bunch, has judged whether that according to acquired signal target occurs.

(4) after target occurring, sensor node is measured target direction.

(5) sensor node sends to leader cluster node in this bunch with oneself measurement of bearing value and self coordinate information.

(6) leader cluster node is finished dbjective state and is estimated according to measured value and coordinate information thereof that the sensor node in this bunch sends, realizes that the location of target is followed the tracks of, and will locate tracking results and send to the charge module; Comprise following substep:

(61) initialization module is set up target travel and observation equation, the initial state distribution function of target and the estimation variance of original state, and produces N particle at random.

(62) the particle filter module utilizes state equation that each particle state is carried out prediction, renewal based on EKF.

(63) particle weights computing module calculates the particle weights.

(64) the resampling judge module is according to effective population N _EffJudge whether and to resample, if effective population N _EffLess than preset value, then resample, according to N particle of importance density function resampling and distribute weights.

(65) the estimating target block of state calculates the estimating target state according to the weights of particle, with observation increase constantly, returns step (62) and carries out iterative algorithm.

(7) the location tracking results of each leader cluster node is collected and shown to the charge module.

Wherein, described step (2) comprises following substep:

(21) sensor node is finished the numbering to oneself, the identity of unique sign oneself.

(22) sensor node is realized time synchronized by gps system.

(23) sensor node is realized self poisoning by gps system.

(24) communication distance according to sensor node carries out a bunch division to network, selects leader cluster node.

Wherein, described step (61) comprises following substep:

(611) set up target travel and observation model module and set up the single order markov system equation of target and the observation model of target observation:

${\mathrm{\&theta;}}_i={\tan }^{-1}\left(\frac{y_T-s_y^i}{x_T-s_x^i}\right)+n_i;$

Wherein: X _T=[x _T, v _x, y _T, v _y] expression target motion state, x _T, y _TRepresent the coordinate figure of target respectively, v in X-axis and Y-axis _x, v _yRepresent that respectively target is at the absolute velocity component in X-axis and Y-axis; $S^i=[s_x^i,s_y^i]$ I=1,2 ... N, expression sensor node i is respectively at the coordinate of X-axis and Y-axis, and N represents to observe the sensor node number of target; θ _iThe relative orientation of expression target and sensor node; n _iThe measurement noise of representing i sensor node.

(612) set up original state probability distribution function and the estimation variance that object initialization state and estimation variance module are set up target.

Described original state probability distribution function is: X _K+1=F _kX _k+ G _kA _k

Wherein: A _k=[a _{X, k}, a _{Y, k}] ^TBe the processing noise of system, promptly because the uncertainty of target travel environment causes respectively the acceleration noise that causes in X and Y-axis, A _k～N (0, R _k);

$F_k=\left[\begin{array}{cccc}1& T_s& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T_s\\ 0& 0& 0& 1\end{array}\right]$ $G_k=\left[\begin{array}{cc}{T}_s^2/2& 0\\ T_s& 0\\ 0& T_s^2/2\\ 0& T_s\end{array}\right];$

(613) the random particles generation module is from initial state distribution p (x ₀) in randomly draw N primary { x ₀ ⁱ, i=1,2 ... N}, the covariance matrix corresponding with primary is: { P ₀ ⁱ, i=1,2 ... N}.

Wherein, described step (62) comprises following substep:

(621) prediction module is utilized the state of state equation prediction k target constantly

And prediction covariance matrix

Wherein: ${\hat{x}}_{k/k-1}^i=F_{k-1}^i{x}_{k-1}^i+G_k^i{A}_k^i;$ ${\hat{P}}_{k/k-1}^i=F_{k-1}^i{P}_{k-1}^i{F_{k-1}^i}^T+Q_{k-1}^i;$ $F_{k-1}^i=F_k.$

(622) update module utilizes k measured value constantly that particle is predicted the outcome and the estimate covariance matrix upgrades, and obtains k updating value and covariance matrix x constantly _k ⁱAnd P _k ⁱ

Wherein: $x_k^i={\hat{x}}_{k/k-1}^i+K_k^i(y_k-h\left({\hat{x}}_{k/k-1}^i\right)),$ $P_k^i=(I-K_k^i{H}_k^i){\hat{P}}_{k/k-1}^i;$

$H_k^i=\frac{\&PartialD;h}{\&PartialD;x}{|}_{x={\hat{x}}_{k/k-1}^i},$ $K={\hat{P}}_{k/k-1}^i{H_k^i}^T{(H_k^i{\hat{P}}_{k/k-1}^i{H_k^i}^T+R_k)}^{-1}.$

Wherein, in the described step (63), particle weights computing module is according to Bayes and particle filter Theoretical Calculation particle weights:

${\mathrm{\&omega;}}_k^i={\mathrm{\&omega;}}_{k-1}^i\frac{p(z_k/x_k^i)p(x_k^i/x_{k-1}^i)}{q(x_k^i/x_{0:k-1}^i,z_{0:k})};$

Wherein: q (x _k/ x _0:k-1 ⁱ, z _1:k) be the importance density function, p (z _k/ x _k ^j) be the observation likelihood function, p (x _k/ x _K-1) be the transition probability of system's first-order Markov process.

The invention has the advantages that:

1, target localization and tracking system provided by the invention and method adopt a kind of passive positioning in conjunction with Kalman and particle filtering method realization target, it is on the basis of realizing hi-Fix, arithmetic speed has very high validity, accuracy and feasibility well below particle filter method.

2, target localization and tracking system provided by the invention and method adopt the motion analysis of realizing target based on the system configuration of sensor network, have overcome the limitation of the observability principle of single node.

3, target localization and tracking system provided by the invention and method are in the position application of carrying out target, the configuration of system is used for reference the technical characterstic of wireless sensor network, adopt multisensor based on final realization of the observation of target azimuth followed the tracks of the location of target, this network configuration can overcome the airborne or carrier-borne single station of tradition system must carry out motor-driven constraint during observing, when the target azimuth information that adopts multisensor to measure realizes the target cooperative location, do not need the motor-driven of observation platform, improved the dirigibility of target localization, and adopt this system configuration to realize increasing target monitoring positioned area area greatly, avoid existing the deficiency of locating the blind area.

Description of drawings

Fig. 1 is a target localization and tracking system configuration schematic diagram of the present invention;

Fig. 2 is a sensor node design function module map of the present invention;

Fig. 3 is sensor node function division figure of the present invention;

Fig. 4 is target localization trace flow figure of the present invention;

Fig. 5 is a positioning system initialization flowchart of the present invention;

Fig. 6 is an EKF-PF algorithm flow chart of the present invention;

Fig. 7 is the general illustration that sensor node and target trajectory are followed the tracks of in the embodiment of the invention;

Fig. 8 is that relatively synoptic diagram of result is amplified in the part of target following in the embodiment of the invention;

Fig. 9 is to the estimated result comparison diagram of target X-axis and Y-axis coordinate in the embodiment of the invention;

Figure 10 is to the estimated result comparison diagram of target X-axis and Y direction speed in the embodiment of the invention.

Embodiment

Below in conjunction with accompanying drawing and a specific embodiment a minute invention is elaborated.

Present embodiment hypothesis positions and follows the tracks of an aircraft target under water, and the target localization and tracking system of present embodiment as shown in Figure 1, is made up of a plurality of bunches of locating modules and a charge system; Each bunch locating module comprises two sensor nodes and a leader cluster node.Each leader cluster node further comprises: an initialization module, a particle filter module, a particle weights computing module, a resampling judge module and an estimating target block of state.

Sensor node is static to be disposed, and also can carry out moving among a small circle.Its position coordinates can be finished self-align by the GPS device that the sensor node utilization carries, and also can finish self-align according to certain self-align agreement by the anchor node of several known coordinates; Each sensor node need have the direction finding function, and this function can realize the target direction estimation by adopting the Array Signal Processing means; In order to carry out the mutual and networking function of data, sensor node need have radio communication function; Leader cluster node is responsible for this internal object location, zonule and is followed the tracks of, and it can be treated as the gateway node of this sub-district; Leader cluster node stores the position coordinates of all the sensors node in this zone; And the responsible measured target azimuth information of this bunch inner sensor node of collecting; Utilize improved quick EKF-PF algorithm to realize the location of target is followed the tracks of; The charge system is responsible for collecting leader cluster node location tracking results in each zone, in terminal positioning result is shown, so that allow the location tracking results be used for different application demands.Wherein leader cluster node can have the direction finding function, also can not have this function, but it must have radio communication function.Leader cluster node can stipulate in advance, also can be in one bunch Dynamic Selection.

For the stationary problem of whole positioning system, the mode of employing GPS time service realizes the time synchronized of network.So the native system interior nodes need be equipped with gps system.

For the network interconnection of positioning system, adopt communication, for not having communication link between bunch of inner sensor node, need not link between the sensor node and carry out exchanges data.There are three kinds of link on-link mode (OLM)s in native system, first be exactly sensor node with the leader cluster node of being responsible in this zone between communication link, be responsible for will own measurement azimuth information send to leader cluster node realization EFK-PF and locate track algorithm; The secondth, the communication link between leader cluster node and the charge system, leader cluster node are responsible for that the location tracking results is sent to the charge system and are shown; The 3rd class is the communication link between each regional leader cluster node, motion along with target, it enters different zones, and then the leader cluster node in previous zone sends to final positioning result the initial results of the leader cluster node of this moment as the tracking of this bunch internal object location.

Fig. 2 has provided the sensor node that relates among the present invention and the design module of leader cluster node, mainly contains four modules and forms.Supply module is finished the electric energy of whole node is supplied with; Processing module is the computing unit of node, adopts commercial dsp chip, is responsible for sensor institute image data is analyzed and handled the line data of going forward side by side packing; Communication module sends packing data, receives packet that other node sends simultaneously and finishes the radio communication task of node with communication module.For sensor node, its communication module device comprises the twireless radio-frequency communication system.Sensor assembly is mainly finished the echo signal data is gathered.

Based on the application in positioning system of sensor node, on the basis of network seven layer protocols, the sensor node data processing capacity has been carried out simple division, mainly be divided into following three parts: communication system; Support system; Application system.As shown in Figure 3, for communication system mainly by Physical layer, the data chainning layer, network layer and transport layer are formed, and are responsible for finishing the normal networking function of node.Support system comprises node synchronously with self-align, and sensor node can be finished the time synchronized of sensor node and the location technology of node self by the GPS device.Application system mainly is responsible for finishing the direction finding function of sensor node on the basis based on the sensor acquisition data, the correlation technique that can use for reference in the Array Signal Processing is finished.

Utilize above-mentioned target localization and tracking system that target is positioned the method for tracking, as shown in Figure 4, may further comprise the steps:

(1) finishes system configuration, comprise sensor node, leader cluster node and charge system.

(2) finish netinit, as shown in Figure 5, comprise following substep:

(21) sensor node is finished the numbering to oneself, the identity of unique sign oneself.

(22) sensor node is realized time synchronized by gps system.

(23) sensor node is realized self poisoning by gps system.

(24) communication distance according to sensor node carries out a bunch division to network, selects leader cluster node.This step can artificially be specified a bunch head, also can be at random bunch in select bunch head.

(3) sensor node is monitored in the place bunch, has judged whether that according to acquired signal target occurs.

(4) after target occurring, sensor node is measured target direction.

(5) sensor node sends to leader cluster node in this bunch with oneself measurement of bearing value and self coordinate information.

(6) leader cluster node is finished dbjective state and is estimated according to measured value and coordinate information thereof that the sensor node in this bunch sends, realizes that the location of target is followed the tracks of, and will locate tracking results and send to the charge module by radio communication; As shown in Figure 6, comprise following substep:

(61) initialization module is set up target travel and observation equation, the initial state distribution function of target and the estimation variance of original state, and produces N particle at random.

(611) set up target travel and observation model module and set up the single order markov system equation of target and the observation model of target observation:

${\mathrm{\&theta;}}_i={\tan }^{-1}\left(\frac{y_T-s_y^i}{x_T-s_x^i}\right)+n_i$

Wherein: X _T=[x _T, v _x, y _T, v _y] expression target motion state, x _T, y _TRepresent the coordinate figure of target respectively, v in X-axis and Y-axis _x, v _yRepresent that respectively target is at the absolute velocity component in X-axis and Y-axis; Hypothetical target is with node is on same plane under water, and node remains static, $S^i=[s_x^i,s_y^i]$ I=1,2...N represent sensor node i respectively at the coordinate of X-axis and Y-axis, and N represents to observe the sensor node number of target; θ _iThe relative orientation of expression target and sensor node; n _iThe measurement noise of representing i sensor node.System can represent with matrix form at k observation model constantly:

${\stackrel{\&RightArrow;}{Z}}_K=\stackrel{\&RightArrow;}{h}({\stackrel{\&RightArrow;}{S}}_K,X_K^T)+{\stackrel{\&RightArrow;}{V}}_K$

Wherein: ${\stackrel{\&RightArrow;}{Z}}_K={[{\mathrm{\&theta;}}_1,{\mathrm{\&theta;}}_2...{\mathrm{\&theta;}}_N]}^T,$

$\stackrel{\&RightArrow;}{h}({\stackrel{\&RightArrow;}{S}}_k,X_k^T)={[h({\mathrm{<figure-callout\; id="1"\; label="S"\; filenames="A2009100784740025H1.png"\; state="\{\{state\}\}">S</figure-callout>}}_1,X_k^T),h({\mathrm{<figure-callout\; id="2"\; label="S"\; filenames="A2009100784740025H1.png"\; state="\{\{state\}\}">S</figure-callout>}}_2,X_k^T),...h(S_N,X_k^T),]}^T,$

$h(S_i,X_k^T)={\tan }^{-1}(y_T-s_y^i/x_T-s_x^i),$

${\stackrel{\&RightArrow;}{V}}_K={[{\mathrm{<figure-callout\; id="1"\; label="n"\; filenames="A2009100784740025H1.png"\; state="\{\{state\}\}">n</figure-callout>}}_1,{\mathrm{<figure-callout\; id="2"\; label="n"\; filenames="A2009100784740025H1.png"\; state="\{\{state\}\}">n</figure-callout>}}_2,...n_N]}^T.$

Suppose that noise is the zero-mean gaussian random noise, then Q _kBe noise covariance matrix, and $Q_k=\mathrm{diag}({\mathrm{\&sigma;}}_1^2,{\mathrm{\&sigma;}}_2^2...{\mathrm{\&sigma;}}_N^2).$

(612) set up original state probability distribution function and the estimation variance that object initialization state and estimation variance module are set up target.

(White Noise Acceleration WNA) describes the weak maneuverability of target to adopt white noise acceleration model.Then described original state probability distribution function is: X _K+1=F _kX _k+ G _kA _k

Wherein: A _k=[a _{X, k}, a _{Y, k}] ^TBe the processing noise of system, promptly because the uncertainty of target travel environment causes respectively the acceleration noise that causes in X and Y-axis, A _k～N (0, R _k);

$F_k=\left[\begin{array}{cccc}1& T_s& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T_s\\ 0& 0& 0& 1\end{array}\right]$ $G_k=\left[\begin{array}{cc}{T}_s^2/2& 0\\ T_s& 0\\ 0& T_s^2/2\\ 0& T_s\end{array}\right].$

(613) the random particles generation module is from initial state distribution p (x ₀) in randomly draw N primary { x ₀ ⁱ, i=1,2 ... N}, the covariance matrix corresponding with primary is: { P ₀ ⁱ, i=1,2 ... N}.

By above-mentioned measurement equation and state equation as can be seen, be linear based on the state equation of multisensor orientation TMA problem under water, be non-linear and measure equation, and become when being, the therefore actual nonlinear filtering problem that becomes when being.

At traditional particle filter algorithm finding the solution to the problems referred to above, the key factor that influences the particle filter estimation effect is the selection of importance density function, and be posterior probability density for optimum importance density function, so expect naturally the EKF method is combined with particle filter method, after k moment priori particle upgrades by EKF filtering, its value approaches posterior probability more, so compare with PF, will be less than the PF algorithm when reaching the required population of same performance, and this algorithm also there is certain inhibiting effect to particle tcam-exhaustion among the PF.

(62) the particle filter module utilizes state equation that each particle state is carried out prediction, renewal based on EKF.

(621) prediction module is utilized the state of state equation prediction k target constantly And prediction covariance matrix

Wherein: ${\hat{x}}_{k/k-1}^i=F_{k-1}^i{x}_{k-1}^i+G_k^i{A}_k^i;$ ${\hat{P}}_{k/k-1}^i=F_{k-1}^i{P}_{k-1}^i{F_{k-1}^i}^T+Q_{k-1}^i;$ $F_{k-1}^i=F_k.$

(622) update module utilizes k measured value constantly that particle is predicted the outcome and the estimate covariance matrix upgrades, and obtains k updating value and covariance matrix x constantly _k ⁱAnd P _k ⁱ

Wherein: $x_k^i={\hat{x}}_{k/k-1}^i+K_k^i(y_k-h\left({\hat{x}}_{k/k-1}^i\right)),$ $P_k^i=(I-K_k^i{H}_k^i){\hat{P}}_{k/k-1}^i;$

$H_k^i=\frac{\&PartialD;h}{\&PartialD;x}{|}_{x={\hat{x}}_{k/k-1}^i},$ $K={\hat{P}}_{k/k-1}^i{H_k^i}^T{(H_k^i{\hat{P}}_{k/k-1}^i{H_k^i}^T+R_k)}^{-1}.$

(63) particle weights computing module calculates the particle weights.

Particle weights computing module is according to Bayes and particle filter Theoretical Calculation particle weights:

${\mathrm{\&omega;}}_k^i={\mathrm{\&omega;}}_{k-1}^i\frac{p(z_k/x_k^i)p(x_k^i/x_{k-1}^i)}{q(x_k^i/x_{0:k-1}^i,z_{0:k})};$

Wherein: q (x _k/ x _0:k-1 ⁱ, z _1:k) be the importance density function, p (z _k/ x _k ^j) be the observation likelihood function, p (x _k/ x _K-1) be the transition probability of system's first-order Markov process.

(64) the resampling judge module is according to effective population N _EffJudge whether and to resample, if effective population N _EffLess than preset value, then resample, according to N particle of importance density function resampling and distribute weights.

(65) the estimating target block of state calculates the estimating target state according to the weights of particle, with observation increase constantly, returns step (62) and carries out iterative algorithm.

(7) the location tracking results of each leader cluster node is collected and shown to the charge module.

Fig. 7 to Figure 10 adopts the EKF-PF algorithm to realize the computer artificial result of target localization in the present embodiment, for the present invention compares with traditional E KF and PF method, simulation process has also carried out l-G simulation test to these two kinds of localization methods.Fig. 7 and Fig. 8 are the results who follows the tracks of, and Fig. 8 is the method to Fig. 7 part, finds out the effect of target following so more intuitively.As can be seen from the figure, tracking effect of the present invention is better than EKF method, and to compare its effect suitable with the PF method, but be less than the PF method its computing time far away.Fig. 9 provided target respectively at the estimated value of X-axis and Y-axis coordinate and the comparing result of actual value, and connects the graph of errors that has provided under three kinds of distinct methods; Figure 10 then is that following table has been listed the working time of the inventive method to the estimated value of target in X-axis and Y-axis speed, is reaching under the situation of same bearing accuracy as can be seen, and the method applied in the present invention is well below the PF method of same precision.By the simulation result of above embodiment, validity of the inventive method and accuracy illustrate the feasibility of this method in practical application as can be seen.

Localization method	??PF	??EKF-PF
			Computing (second) consuming time	??0.0822	??0.0346

Claims (12)

1, a kind of target localization and tracking system comprises: a plurality of bunches of locating modules and are accused module;

Described bunch of locating module comprises: a plurality of sensor nodes and a leader cluster node;

Described sensor node is used for its place bunch inner region is monitored, and occurs as target, then target direction is measured, and this measured value and self coordinate information are sent to leader cluster node in this bunch;

Described leader cluster node is used for the measured value and the coordinate information thereof that send according to the sensor node in this bunch, finishes dbjective state and estimates, realizes that the location of target is followed the tracks of, and will locate tracking results and send to the charge module;

Described charge module is used to collect and show the location tracking results of each leader cluster node;

It is characterized in that,

Described leader cluster node further comprises:

One initialization module is used to set up target travel and observation equation, the initial state distribution function of target and the estimation variance of original state, and produces N particle at random;

One particle filter module is used to utilize state equation that each particle state is carried out prediction, renewal based on EKF;

One particle weights computing module is used to calculate the particle weights;

One resampling judge module is used for according to effective population N _EffJudge whether and to resample, if effective population N _EffLess than preset value, then resample, according to N particle of importance density function resampling and distribute weights; And

One estimating target block of state is used for calculating the estimating target state according to the weights of particle.

2, target localization and tracking system according to claim 1, it is characterized in that described initialization module further comprises: one sets up target travel and observation model module, is set up object initialization state and an estimation variance module and a random particles generation module;

Described target travel and the observation model module set up is used to set up the single order markov system equation of target and the observation model of target observation:

${\mathrm{\&theta;}}_i={\tan }^{-1}\left(\frac{y_T-s_y^i}{x_T-s_x^i}\right)+n_i;$

Wherein: X _T=[x _T, v _x, y _T, v _y] expression target motion state, x _T, y _TRepresent the coordinate figure of target respectively, v in X-axis and Y-axis _x, v _yRepresent that respectively target is at the absolute velocity component in X-axis and Y-axis; $S^i=[s_x^i,s_y^i]$ I=1,2 ... N, expression sensor node i is respectively at the coordinate of X-axis and Y-axis, and N represents to observe the sensor node number of target; θ _iThe relative orientation of expression target and sensor node; n _iThe measurement noise of representing i sensor node;

Described object initialization state and the estimation variance module set up is used to set up the original state probability distribution function and the estimation variance of target;

Described original state probability distribution function is: X _K+1=F _kX _k+ G _kA _k

Wherein: A _k=[a _{X, k}, a _{Y, k}] ^TBe the processing noise of system, promptly because the uncertainty of target travel environment causes respectively the acceleration noise that causes in X and Y-axis, A _k～N (0, R _k);

$F_k=\left[\begin{array}{cccc}1& T_s& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T_s\\ 0& 0& 0& 1\end{array}\right]$ $G_k=\left[\begin{array}{cc}{T}_s^2/2& 0\\ T_s& 0\\ 0& T_s^2/2\\ 0& T_s\end{array}\right];$

Described random particles generation module is used for the (x from initial state distribution p ₀) in randomly draw N primary { x ₀ ⁱ, i=1,2 ... N}, the covariance matrix corresponding with primary is: { P ₀ ⁱ, i=1,2 ... N}.

3, target localization and tracking system according to claim 1 is characterized in that, described particle filter module further comprises: a prediction module and a update module;

Described prediction module is used to utilize state equation to predict the state of k target constantly

And prediction covariance matrix

Wherein: ${\hat{x}}_{k/k-1}^i=F_{k-1}^i{x}_{k-1}^i+G_k^i{A}_k^i;$ ${\hat{P}}_{k/k-1}^i=F_{k-1}^i{P}_{k-1}^i{F_{k-1}^i}^T+Q_{k-1}^i;$ $F_{k-1}^i=F_k;$

Described update module: be used to utilize k measured value constantly that particle is predicted the outcome and the estimate covariance matrix upgrades, obtain k updating value and covariance matrix x constantly _k ⁱAnd P _k ⁱ

Wherein: $x_k^i={\hat{x}}_{k/k-1}^i+K_k^i(y_k-h\left({\hat{x}}_{k/k-1}^i\right)),$ $P_k^i=(I-K_k^i{H}_k^i){\hat{P}}_{k/k-1}^i;$

$H_k^i=\frac{\&PartialD;h}{\&PartialD;x}{|}_{x={\hat{x}}_{k/k-1}^i},$ $K={\hat{P}}_{k/k-1}^i{H_k^i}^T{(H_k^i{\hat{P}}_{k/k-1}^i{H_k^i}^T+R_k)}^{-1}.$

4, target localization and tracking system according to claim 1 is characterized in that, described particle weights computing module is according to Bayes and particle filter Theoretical Calculation particle weights:

${\mathrm{\&omega;}}_k^i={\mathrm{\&omega;}}_{k-1}^i\frac{p(z_k./x_k^i)p(x_k^i/x_{k-1}^i)}{q(x_k^i/x_{0:k-1}^i,z_{0:k})};$

Wherein: q (x _k/ x _0:k-1 ⁱ, z _1:k) be the importance density function, p (z _k/ x _k ^j) be the observation likelihood function, p (x _k/ x _K-1) be the transition probability of system's first-order Markov process.

5, target localization and tracking system according to claim 1 is characterized in that, described effective population N _EffBe defined as follows:

$N_{\mathrm{eff}}=\frac{1}{\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left({\stackrel{\&OverBar;}{\mathrm{\&omega;}}}_k^i\right)}^2}.$

6, target localization and tracking system according to claim 1 is characterized in that, described sensor node and leader cluster node are equipped with gps system, and target localization and tracking system adopts the mode of GPS time service to realize the time synchronized of network.

7, a kind of target localization tracking comprises the steps:

(1) finishes system configuration, comprise sensor node, leader cluster node and charge system;

(2) finish netinit;

(3) sensor node is monitored in the place bunch, has judged whether that according to acquired signal target occurs;

(4) after target occurring, sensor node is measured target direction;

(5) sensor node sends to leader cluster node in this bunch with oneself measurement of bearing value and self coordinate information;

(6) leader cluster node is finished dbjective state and is estimated according to measured value and coordinate information thereof that the sensor node in this bunch sends, realizes that the location of target is followed the tracks of, and will locate tracking results and send to the charge module; Comprise following substep:

(61) initialization module is set up target travel and observation equation, the initial state distribution function of target and the estimation variance of original state, and produces N particle at random;

(62) the particle filter module utilizes state equation that each particle state is carried out prediction, renewal based on EKF;

(63) particle weights computing module calculates the particle weights;

(64) the resampling judge module is according to effective population N _EffJudge whether and to resample, if effective population N _EffLess than preset value, then resample, according to N particle of importance density function resampling and distribute weights;

(65) the estimating target block of state calculates the estimating target state according to the weights of particle, with observation increase constantly, returns step (62) and carries out iterative algorithm;

(7) the location tracking results of each leader cluster node is collected and shown to the charge module.

8, target localization tracking according to claim 7 is characterized in that, described step (2) comprises following substep:

(21) sensor node is finished the numbering to oneself, the identity of unique sign oneself;

(22) sensor node is realized time synchronized by gps system;

(23) sensor node is realized self poisoning by gps system;

(24) communication distance according to sensor node carries out a bunch division to network, selects leader cluster node.

9, target localization tracking according to claim 7 is characterized in that, described step (61) comprises following substep:

(611) set up target travel and observation model module and set up the single order markov system equation of target and the observation model of target observation:

${\mathrm{\&theta;}}_i={\tan }^{-1}\left(\frac{y_T-s_y^i}{x_T-s_x^i}\right)+n_i;$

Wherein: X _T=[x _T, v _x, y _T, v _y] expression target motion state, x _T, y _TRepresent the coordinate figure of target respectively, v in X-axis and Y-axis _x, v _yRepresent that respectively target is at the absolute velocity component in X-axis and Y-axis; $S^i=[s_x^i,s_y^i]$ I=1,2 ... N, expression sensor node i is respectively at the coordinate of X-axis and Y-axis, and N represents to observe the sensor node number of target; θ _iThe relative orientation of expression target and sensor node; n _iThe measurement noise of representing i sensor node;

(612) set up original state probability distribution function and the estimation variance that object initialization state and estimation variance module are set up target;

Described original state probability distribution function is: x _K+1=F _kX _k+ G _kA _k

Wherein: A _k=[a _{X, k}, a _{Y, k}] ^TBe the processing noise of system, promptly because the uncertainty of target travel environment causes respectively the acceleration noise that causes in X and Y-axis, A _k～N (0, R _k);

$F_k=\left[\begin{array}{cccc}1& T_s& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T_s\\ 0& 0& 0& 1\end{array}\right]$ $G_k=\left[\begin{array}{cc}{T}_s^2/2& 0\\ T_s& 0\\ 0& T_s^2/2\\ 0& T_s\end{array}\right];$

(613) the random particles generation module is from initial state distribution p (x ₀) in randomly draw N primary { x ₀ ⁱ, i=1,2 ... N}, the covariance matrix corresponding with primary is: { P ₀ ⁱ, i=1,2 ... N}.

10, target localization tracking according to claim 7 is characterized in that, described step (62) comprises following substep:

(621) prediction module is utilized the state of state equation prediction k target constantly

And prediction covariance matrix

Wherein: ${\hat{x}}_{k/k-1}^i=F_{k-1}^i{x}_{k-1}^i+G_k^i{A}_k^i;$ ${\hat{P}}_{k/k-1}^i=F_{k-1}^i{P}_{k-1}^i{F_{k-1}^i}^T+Q_{k-1}^i;$ $F_{k-1}^i=F_k;$

(622) update module utilizes k measured value constantly that particle is predicted the outcome and the estimate covariance matrix upgrades, and obtains k updating value and covariance matrix x constantly _k ⁱAnd P _k ⁱ

Wherein: $x_k^i={\hat{x}}_{k/k-1}^i+K_k^i(y_k-h\left({\hat{x}}_{k/k-1}^i\right)),$ $P_k^i=(I-K_k^i{H}_k^i){\hat{P}}_{k/k-1}^i;$

$H_k^i=\frac{\&PartialD;h}{\&PartialD;x}{|}_{x={\hat{x}}_{k/k-1}^i},$ $K={\hat{P}}_{k/k-1}^i{H_k^i}^T{(H_k^i{\hat{P}}_{k/k-1}^i{H_k^i}^T+R_k)}^{-1}.$

11, target localization tracking according to claim 7 is characterized in that, in the described step (63), particle weights computing module is according to Bayes and particle filter Theoretical Calculation particle weights:

${\mathrm{\&omega;}}_k^i={\mathrm{\&omega;}}_{k-1}^i\frac{p(z_k./x_k^i)p(x_k^i/x_{k-1}^i)}{q(x_k^i/x_{0:k-1}^i,z_{0:k})};$

Wherein: q (x _k/ x _0:k-1 ⁱ, z _1:k) be the importance density function, p (x _k/ x _k ^j) be the observation likelihood function, p (x _k/ x _K-1) be the transition probability of system's first-order Markov process.

12, target localization tracking according to claim 7 is characterized in that, described effective population N _EffBe defined as follows:

$N_{\mathrm{eff}}=\frac{1}{\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left({\stackrel{\&OverBar;}{\mathrm{\&omega;}}}_k^i\right)}^2}.$

Images