CN109460539A - A kind of object localization method based on simplified volume particle filter - Google Patents
A kind of object localization method based on simplified volume particle filter Download PDFInfo
- Publication number
- CN109460539A CN109460539A CN201811196808.6A CN201811196808A CN109460539A CN 109460539 A CN109460539 A CN 109460539A CN 201811196808 A CN201811196808 A CN 201811196808A CN 109460539 A CN109460539 A CN 109460539A
- Authority
- CN
- China
- Prior art keywords
- particle
- resampling
- moment
- weight
- state
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
Abstract
The invention discloses a kind of based on the object localization method for simplifying volume particle filter, which comprises step 1) establishes the state-space model of the target movement at k moment；K moment dbjective state is predicted to obtain first time filter result using state-space modelAnd covariance matrixStep 2) noteFor generate particle assembly " seed ", withCentered on,The particle collection at k moment is generated for radius, and each particle concentrated for particle calculates weight and normalizes weight；Step 3) is concentrated from the particle of step 2) and chooses " high-quality particle ", and the weight of " high-quality particle " is normalized；These " high-quality particles " form new particle collection；Step 4) carries out resampling to the new particle collection that step 3) obtains, and obtains resampling particle collection；Step 5) calculate resampling particle concentrate particle mean value be WithIt is the output of the dbjective state vector at k moment.
Description
Technical field
The present invention relates to target positioning fields, and in particular to a kind of based on the target positioning side for simplifying volume particle filter
Method.
Background technique
The filtering algorithm of numerous classics has been widely used in target following navigation and immediately positioning and map structuring
In systems such as (simultaneous localization and mapping, SLAM), such as Kalman filtering (KF), extension
Kalman filtering (EKF), Unscented kalman filtering (UKF), volume Kalman filtering (CKF), particle filter (PF) scheduling algorithm.Needle
To the shortcoming of above-mentioned each algorithm, a series of correction algorithms are suggested successively.Classical filtering algorithm can be divided into two major classes:
Filtering algorithm based on KF frame and based on PF frame.The former is to be directed to multiprecision arithmetic under the conditions of Gaussian noise, and the latter is
Optimal solution under the conditions of non-gaussian.The precision of particle filter depends greatly on the number and importance of particle
The selection of function.When the number of particle is more, the sharp increase of calculation amount is that system brings " dimension disaster ".In addition, if weight
The selection of the property wanted function is bad, can frequently result in filtering divergence or very poor filter effect.Therefore, importance function is for particle
The design of filter is vital.That very last standard wanted for judging importance function quality is exactly to see its energy
It is no farthest to utilize newest measuring value, enable sampling particle to embody newest measurement value information.Standard particle filtering
One latent defect of algorithm is exactly: based on one group can not definitely describe really be posterior probability density function tail portion, this
One phenomenon becomes apparent when measuring value comes across except estimation function.Main cause is exactly using determining hybrid estimation letter
The true Posterior distrbutionp to estimate time-varying is counted, i.e. the importance density function is chosen undesirable.
In the optimization of PF, finding suitable suggestion density fonction is a more feasible thinking, also always
It is the research hotspot of scholars.If optimal suggestion density fonction can be found, and resampling is guided to do correct sampling
Distribution, so that it may guarantee the validity of sample set, can also guarantee the estimation performance of PF.In recent years, some scholars proposition will
The thinking that two schemes integrate improves PF with the filtering algorithm based on KF, i.e., generates suggestion Density Distribution with the former, instructs
" sampling " during subsequent PF.More representational algorithm has extended particle filter (EPF) algorithm of EKF in conjunction with PF,
UKF with PF combine obtain without mark particle filter (UPF) algorithm and compared with new volume particle filter (CPF) algorithm-CKF with
PF combines gained.Since the filtering accuracy of CKF is better than EKF and UKF, the precision of CPF is also superior to EPF and UPF.With PF,
EPF, UPF algorithm are compared, and CPF significantly improves filtering accuracy, are one of highest algorithms of current filtering accuracy.But due to
PF calculation amount itself is bigger, and is easy to appear sample degeneracy phenomenon, increases each grain of generation again after introducing CKF algorithm
The calculation amount of son, makes calculation amount be multiplied, " dimension disaster " is more acute.Therefore, the target location algorithm based on CPF algorithm
It is very high to the hardware requirement of positioning system although higher estimated accuracy can be obtained, position is carried out to target each time
The process of update requires to pay expensive calculating cost.For the positioning system of underwater operation, charging, replacement
Battery is extremely difficult even not possible with thing, so the power consumption of system directly determines its working life.Use tens times
The power consumption of even hundreds of times goes to exchange for several percentage points in precision of rising, for underwater positioning system, especially positioning in real time
System, it is clear that be not a very wise selection.In short, huge calculation amount is the object locating system based on CPF algorithm
Disadvantage.To sum up, the filtering algorithm of positioning system how is improved, the compromise for obtaining precision and efficiency is of great significance.
Summary of the invention
It is an object of the invention to overcome above-mentioned technological deficiency, propose a kind of based on the target for simplifying volume particle filter
Localization method can significantly reduce operand.
To achieve the goals above, the technical solution of the present invention is as follows:
A kind of object localization method based on simplified volume particle filter, which comprises
Step 1) establishes the state-space model of the target movement at k moment；Using state-space model to k moment target-like
State is predicted to obtain first time filter resultAnd covariance matrix
Step 2) noteFor generate particle assembly " seed ", withCentered on,The grain at k moment is generated for radius
Subset, and each particle concentrated for particle calculates weight and normalizes weight；
Step 3) is concentrated from the particle of step 2) and chooses " high-quality particle ", and the weight of " high-quality particle " is carried out normalizing
Change；These " high-quality particles " form new particle collection；
Step 4) carries out resampling to the new particle collection that step 3) obtains, and obtains resampling particle collection；
Step 5) calculate resampling particle concentrate particle mean value be WithIt is the dbjective state vector at k moment
Output.
As a kind of improvement of the above method, the step 1) is specifically included:
Step 1-1) movement of target that is described using following state-space model:
Xk=f (Xk-1)+wk, (3)
Zk=h (Xk)+vk, (4)
Wherein, k is integer, and the dbjective state of moment k is Xk∈Rn；ZkFor the collected measurement information of sensor；wk∈Rn
For the white noise of input, vk∈RmFor observation noise；Formula (5) is state equation, and formula (6) is observational equation, and f () turns for state
Function is moved, h () is observation information function, and the initial value X of dbjective state is given in conjunction with priori knowledge0；
Step 1-2) bonding state spatial model, it is predicted using state of the volume Kalman filtering to k moment target,
Obtain first time filter resultAnd its corresponding covariance matrix
As a kind of improvement of the above method, the step 2) is specifically included:
Step 2-1) noteFor generate particle assembly " seed ", withCentered on,The k moment is generated for radius
Particle collection includes N number of particle
Wherein,Indicate withFor mean value,For the Gaussian Profile of variance；
Step 2-2) it is that particle concentrates each particle to calculate weight
Wherein,Indicate given prior density function andUnder conditions of, obtain ZkProbability；
Step 2-3) it is rightNormalized weight is obtained after being normalized
As a kind of improvement of the above method, the step 3) specifically:
Give a weight threshold ωth, weight is less than ωthParticle all give up, it is remaining to be denoted as " high-quality grain
Son ", total M；By " high-quality particle ", being numbered in order from 1 again is denoted asωiFor the power after normalization
Value.
As a kind of improvement of the above method, the weight threshold ωth=1/N.
As a kind of improvement of the above method, the resampling again are as follows: random resampling, multinomial resampling, system are adopted
Sample or residual error resampling.
As a kind of improvement of the above method, the step 5) specifically:
Resampling particle collection in the step 4) are as follows:ωj'=1/M；The mean value of resampling particle collectionAre as follows:
Export the dbjective state vector at k moment: the mean value of resampling particle collectionWith the covariance matrix of step 1)
Present invention has an advantage that
1, method of the invention maintains the high advantage of CPF method for tracking and positioning estimation performance；But its operand declines
About an order of magnitude；
2, method of the invention can also reach very high positioning accuracy to the motion model of strong nonlinearity, non-gaussian.
Detailed description of the invention
Fig. 1 is the flow chart based on the object localization method for simplifying volume particle filter at k moment of the invention；
Fig. 2 is the continuous object localization method flow chart based on simplified volume particle filter of the invention；
Fig. 3 is standard CPF and simplifies the estimation track comparison diagram (population 100) of CPF；
Position error curve graph (the particle that Fig. 4 is CKF, PF, standard CPF, ICPF proposed by the present invention are once filtered at certain
Number 100)；
Fig. 5 is CPF figure (population 500) compared with the position error curve that ICPF is once filtered；
Fig. 6 is the updated particle collection comparison diagram (population 100) of certain moment resampling；
Fig. 7 is the updated particle collection comparison diagram (population 200) of certain moment resampling；
Fig. 8 is the updated particle collection comparison diagram (population 500) of certain moment resampling；
Fig. 9 is CKF, PF, the position error RMSE of standard CPF, ICPF of the invention compare figure (Monte Carlo Experiment 10
It is secondary)；
Figure 10 is the comparison figure (Monte Carlo Experiment 10 of the runing time of CKF, PF, standard CPF, ICPF of the invention
It is secondary)；
Figure 11 is the estimation track comparison diagram (population 100) of CKF, PF, standard CPF, ICPF of the invention；
Figure 12 is certain position error curve graph (population once filtered of CKF, PF, standard CPF, ICPF of the invention
100)；
Figure 13 is the updated particle collection comparison diagram (population 100) of certain moment resampling；
Figure 14 is the updated particle collection comparison diagram (population 200) of certain moment resampling；
Figure 15 is the updated particle collection comparison diagram (population 500) of certain moment resampling；
Figure 16 is CKF, PF, the position error RMSE of standard CPF, ICPF of the invention compare figure；
Figure 17 is the comparison figure of the runing time of CKF, PF, standard CPF, ICPF of the invention.
Specific embodiment
The present invention will be described in detail with reference to the accompanying drawing.
The present invention proposes a kind of improved filters solutions, referred to as simplifies volume particle filter (Improved Cubature
Particle Filter, ICPF) object localization method.The method that the present invention is mentioned not only is avoided that sample degeneracy occur shows
As also having the advantages that easy to operate, calculation amount is low (calculation amount declines an order of magnitude).The estimated accuracy of this method does not have simultaneously
There is reduction.It is believed that this more succinct new algorithm will push the further application of nonlinear filtering, and greatly reduce
The difficulty applied in real time.Emulation and Lake trial result demonstrate the superiority of new method.
1, status system model
Consider the dynamical system described with following state-space model
Xk=f (Xk-1)+wk, (5)
Yk=h (Xk)+vk, (6)
In formula, k is discrete time, and system is X in the state of moment kk∈Rn；Yk∈RmFor the observation signal of corresponding states,
wk∈RnFor the white noise of input, vk∈RmFor observation noise.
Wherein, formula (5) is state equation, and formula (6) is observational equation, and f is state transition function, and h is observation information function.
2, orthobaric volume particle filter (CPF)
It is with the core that volume Kalman filtering CKF improves particle filter PF algorithm: in sample phase, is calculated using CKF
Method is that each particle calculates mean value and covariance, then utilizes the mean value and covariance, then " is referred to using the mean value and variance
Lead " sampling.Because the function of approximate posteriority filtering density is utilized, i.e., during calculating mean value and variance with CKF algorithm
Newest observation information Z " is received "k, under the frame of particle filter, CKF algorithm meets Gauss for the generation of each particle and builds
Discuss Density Distribution.In other words, CKF algorithm and newest observation information Z are utilized at the k momentkTo calculate the equal of i-th of particle
Value and variance, and sampled using the mean value and update the particle.
Standard CPF algorithm flow is as follows.
(1) it initializes, k=0.Primary value X is given in conjunction with priori knowledge0。
(2) For k=1:K,
A) the importance sampling stage.
(3) For i=1:N,
Each particle is estimated with CKF algorithm, obtains the mean value of each particleAnd covariance
Particle assembly is updated,It is expressed as
What each particle generated meets the distribution that Gauss suggests density,Be withFor mean value,For the Gauss of variance
Distribution.
(4) weight is calculated.
For i=1:N,
Weight and normalized weight are recalculated for particle each in particle assembly.
P () is posterior probability Density Distribution.
B) choice phase (resampling).
N number of random sample set is generated according to APPROXIMATE DISTRIBUTION, and calculates weight, is normalized, particle assembly is carried out
It replicates and eliminates, obtain updated particle collection.
C) it exports: calculating the mean value X of particle collectionkOutput as algorithm.
CPF algorithm and the maximum difference of PF algorithm are exactly to generate the process of particle by means of CKF algorithm, other steps are almost
It is identical.
The essence of this method is that the problem of improving suggestion Density Distribution, the particle assembly of prior distribution is transferred to seemingly
Right region, cost are to have done system Gauss hypothesis.It is known that elementary particle filtering be not by linear Gauss model about
Beam, then CPF receives the constraint of Gauss model.This is also the shortcoming of algorithm.
Standard CPF algorithm introduces CKF algorithm and carrys out " guidance " sampling process, its most prominent advantage is estimated accuracy height,
Meanwhile it is there is also following disadvantage, results in the limitation in practical application.
1, the process complexity for generating new particle is directly proportional to number of particles.(according to standard CPF algorithm steps, upper one
Each particle after process will be handled with CKF)
2, the resampling stage may cause the new polarization of particle weight and particle diversity scarcity, particle caused to move back
Change phenomenon.Particle has the particle of many weight very littles (error is very big) that can may also be adopted again to the new error of introducing.
If 3, number of particles is bigger, resampling stage computation complexity can be very high.
3. simplifying volume particle filter (ICPF)
Simplification volume particle filter ICPF of the invention not only remains the high advantage of standard CPF estimated accuracy, also solves
Particle filter maximum two denounce: operand is big and sample degeneracy phenomenon.
The specific steps of ICPF are given below.
(1) it initializes, k=0.Initial value X is given in conjunction with priori knowledge0。
(2) For k=1:K,
First step forecast updating: prediction result is updated with CKF algorithm, obtains first time filter resultAnd covariance matrixNoteFor " seed " for generating particle assembly.
(3) second step forecast updating:
A) the importance sampling stage.
" seed " and newest covariance matrix are utilized, new particle assembly is generated,1≤i≤N。
Weight and normalized weight are calculated for each particle.
In formulaIndicate given prior density function andUnder conditions of, obtain ZkProbability.
B) choice phase (resampling).
Give a weight threshold ωth, weight is less than ωthParticle all give up, it is remaining to be denoted as " high-quality grain
Son ", by the weight ω of " high-quality particle "iIt is normalized.That is, the weight ω of each particleiIt is updated to" high-quality particle "
And its corresponding weight constitutes new particle collectionSelect a kind of method for resampling (random resampling, multinomial
Resampling, system resampling, residual error resampling etc.), particle collection is updated again；Updated particle collection is denoted asωi'=1/M.
C) it exports
The mean value of resampling particle collectionAre as follows:
WithIt is the output of k moment ICPF algorithm.
As can be seen that the algorithm in the present invention has a characteristic that
1. the process for generating new particle is simple and quick.
2. sample degeneracy phenomenon is avoided the occurrence of, because recursive process all regenerates particle each time.
3. the resampling stage has the particle of many weight very littles to be directly rejected, only retain valuable " high-quality " particle.
4. even if resampling stage computation complexity will not be very high number of particles is bigger.
5. maintaining the high advantage of CPF algorithm estimation performance.
As shown in Figure 1, the step of based on the object localization method for simplifying volume particle filter (ICPF), is as follows:
1, setup parameter P, R, Q, T, N, f (), ωth, and the initial value X for combining priori knowledge to give coordinates of targets0.One
As in the case of, it is proposed that take ωth=1/N.
2, sensor acquires target information, such as azimuth of the target relative to sensor, distance, and collected information claims
Measurement information for measurement information Z, k moment is denoted as Zk。
3, measurement equation h () and weighting function ω are determined according to the property of measurement informationiForm of calculation.
4, forecast updating: prediction result is updated with CKF algorithm, obtains first time filter resultAnd covariance matrix
NoteFor " seed " for generating particle assembly.
5, particle collection is generated: withCentered on,For radius grain scattering, number of particles is the N set in the first step, is obtained
To the particle collection at k moment.
6, the importance sampling stage: calculating weight and normalized weight for each particle, choose " high-quality particle ", and will
The weight ω of " high-quality particle "iIt is normalized.These " high-quality particles " form new particle collectionM is " excellent
The number of plasmid ".
7, the resampling stage: resampling is carried out to new particle collection obtained in the previous step: particle being replicated and is eliminated.
8, mean value is calculated to the result that resampling obtains, be denoted as WithIt is output of the tracking system at the k moment.
If the tracking to target is not over, repeatedly step 2-8, continue processing downwards；Otherwise output is as a result, entire
Tracking phase terminates；As shown in Figure 2.
4. performance evaluation
It carries out Meng Te-Carlow experiment below to analyze the performance of the proposed ICPF algorithm of the present invention, and is compared with former algorithm
Compared with.The state vector for considering the k moment first is Xk=[x, y, vx, vy, ax, ay]T, state equation is established according to formula (5) and formula (6)
It is as follows with observational equation:
Xk=Φ Xk-1+wk (7)
Yk=h (Xk, vk)=dist (Xk, { A, B, C }) and+vk (8)
Note T is the sampling interval, the state-transition matrix in formula (3) are as follows:
wkAnd vkIt is mean value be respectively zero variance is Q and the white Gaussian noise of R；dist(Xk, { A, B, C }) and indicate XkIt determines
Distance of the coordinate from node { A, B, C }.
Target from origin be first uniform rectilinear (CV) movement, speed vx=5m/s, vyAccelerate after=3m/s, 5s
Degree is ax=-0.1m/s2, ay=0.1m/s2And continueing to 55s, 55s to 105s acceleration is ay=-0.1m/s2,
105s to 110s does CV movement, speed be on last stage at the end of (105s) target speed.The coordinate point of given three nodes
It Wei not [0m, 500m], [250m, 100m], [300m, 400m].Measurement information is that three nodes measure target with a distance from oneself,
Measurement information covariance is taken as R=diag [5,5,5].Uniformly accelerated motion (CA) model is selected, giving initial state vector is X0
=[0,0,0,0,0,0]T, using standard CPF algorithm and ICPF algorithm proposed by the present invention respectively to the position of the passive target
Information is tracked and is estimated.Assuming that process-noise variance is Q=diag [1,1,0.1,0.1,0.01,0.01], initial association side
Poor matrix is unit matrix, i.e.,Wherein n is the dimension of state vector, herein n=6.
Fig. 3 gives number of particles when being 100, CKF, PF, standard CPF, ICPF these types method proposed by the present invention
The comparison of track estimated result and real trace.From the figure, it can be seen that the geometric locus of standard CPF and ICPF are almost overlapped
, illustrate that the estimated result of two methods is almost consistent.Position error curve of the these types of method in each sampled point in Fig. 4
Also above-mentioned conclusion has been confirmed.Fig. 5 gives the error curve of standard CPF and ICPF when population is 500, it can be seen that grain
After subnumber increases, the error curve of the two is more nearly.When Fig. 6 to 8 provides number of particles and is respectively 100,200,500, with
The updated particle collection comparison diagram of certain sampling instant resampling of machine interception.From this several comparison diagrams, we can be clearly
See, the particle collection that the particle collection and standard CPF that ICPF of the invention is generated generate is very approximately, with number of particles
Increase, two set have coincidence trend.These results suggest that with the increase of number of particles, standard CPF and ICPF two methods
Effect it is more and more approximate.
This trend is understood that.With the increase of number of particles, the particle of generation covers with becoming better and better
The likelihood region of true value.In addition, since the method for screening particle is the identical (formula and resampling methods of calculating weight
It is constant), therefore the particle stayed after screening is all collected on the particle around true value.With the increase of number of particles, mark
The region that the particle collection that quasi- CPF is generated is covered can also be covered by the particle collection of ICPF, therefore the knot of two kinds of algorithms estimation
Fruit is also very similar.
Using same measurement information and estimation parameter (initial estimate, initial covariance matrix, noise variance matrix
Deng) in the case where, the result of CKF, PF, CPF method is all approach true value.And by actual conditions it is found that only using CKF
Or PF method can also be estimated as a result, only performance ratio CPF is weaker.Requiring processing in real time or other requirements processing
In the system of speed, CKF and PF method be can yet be regarded as the good alternative of CPF.And when number of particles infinity, PF
It is equivalent with CPF.Therefore, for method of the invention, we can so understand: filter when being a CKF to system
After processing, PRELIMINARY RESULTS is obtainedAnd covariance matrix With true value XkBetween there is a certain error.It can be
Regard the standard or " seed " that particle is generated in PF as, in conjunction with another parameterTo generate new particle collection Ω.
As long asBe it is believable, then XkIt should fall in the region that Ω is covered.Here it is the core concepts of ICPF.It is calculated with standard PF
Method is compared, and ICPF algorithm is utilized newest observed quantity when generating particle collection, therefore the particle generated is closer to true value；With
Standard CPF algorithm is compared, and ICPF algorithm goes each particle in simulation CPF to pass through with the multiple repairing weld for the result that early period, CKF was predicted
The particle collection obtained after CKF filtering.
Under the conditions of Fig. 9 gives above-mentioned parameter, the error correlation curve of several method is acquired after Monte Carlo simulation 50 times.
The operation time of note standard CPF method and ICPF method of the invention is respectively Tcpf、Ticpf.Figure 10 gives two methods
Runing time, standard CPF method and the new multiple T for proposing ICPF method operation timecpf/TicpfWith populated variation
Curve.We can be clearly seen that, ICPF method is almost equal, but operation with the estimated accuracy of standard CPF method
Amount has dropped at least one order of magnitude.The Computer model for running program is association 90D4CT01WW, and processor is AMD A10
PR0-7800B R7,4CPUs, 3.5GHz, memory 4GB, operating system is Windows 7, and MATLAB version is 2015a.
In order to further verify process proposed herein, we have been carried out under water on March 22nd, 2018 in Thousand-Island Lake
Positioning experiment.Runing time by comparing above two method is how many and tracking result to target location coordinate, it was demonstrated that
Method of the invention can obtain good compromise between high-precision and high efficiency.In test, target is to be tightly attached to small hull bottom
One transmitting transducer in portion, it is believed that the track of target and canoe is to be overlapped.The real trace of canoe is provided by differential GPS.
Three node (nodename is denoted as A, B, C) cloth that this experiment uses are placed on the lake water region of about one kilometer of square.Each section
Point is divided into two parts: the transmitting-receiving for receiving signal under water, which is closed, sets transducer module and the buoy for being connected to GPS signal receiver
Module.Underwater acoustic transducer in node is located at underwater 5 meters of depths, and the buoy at the top of node is bubbled through the water column.Target is only considered in test
Two-dimensional coordinate information (i.e. east orientation and north orientation), the reference point of coordinate system is chosen for the position that C node is carved at the beginning.GPS is every
Location information is sent every 1s, subaqueous sound ranging sigtnal interval 2s is sent and acquisition.
Similar with the model in emulation, we still select CA model to track above-mentioned target.Consider the state at k moment
Vector is Xk=[x, y, vx, vy, ax, ay]T, dimension n=6；Assuming that process-noise variance be Q=diag [0.15,0.15,0.01,
0.01,0.01,0.01], measurement noise variance isM=3；Initial covariance matrix is P0=diag [1,
1,0.01,0.01,0.01,0.01].State-transition matrix Φ is constant.
Significantly, since experiment condition is more severe, our three nodes are not that can all adopt always
Collect information, in order to improve the utilization rate of measurement information and the precision of positioning as far as possible, we are more than or equal to 2 to measurement information
Situation is filtered, i.e. the dimension of measurement information desirable 2 or 3.Target tracking algorism used in experiment and phase in emulation experiment
Together.Emulated in the processing result of experimental data and previous trifle the result is that similar.It is 100 that Figure 11, which gives number of particles,
When, the track estimated result of two kinds of algorithms and the comparison of real trace.As can see from Figure 11, standard CPF and ICPF curve
It is almost overlapped, illustrates that the estimated result of two methods is almost consistent.These types of method is in each sampled point in Figure 12
Position error curve has also confirmed above-mentioned conclusion.It is random to cut when Figure 13 to Fig. 5 provides number of particles and is respectively 100,200,500
The updated particle collection comparison diagram of certain sampling instant resampling taken.Identical as simulation result, ICPF is produced in this several comparison diagrams
The particle collection that raw particle collection and standard CPF generates is very approximately that, with the increase of number of particles, two set have coincidence
Trend.
In addition, the mentioned new method of the present invention is while guaranteeing estimated accuracy, operation time is significantly compared to standard method
It reduces, greatly improves operation efficiency.Under the conditions of Figure 16 gives above-mentioned parameter, concentration is acquired after Monte Carlo simulation 50 times and is calculated
The error correlation curve of method.The RMSE of standard CPF algorithm and ICPF algorithm differs very little, about 0.2m, it is believed that two kinds of algorithms
Performance in terms of the estimation of target position is consistent.Figure 17 gives the runing time of two methods, standard CPF algorithm and new
It is proposed the multiple T of ICPF algorithm operation timecpf/TicpfWith populated change curve.Compared to CPF, the operation of ICPF
Amount has dropped about an order of magnitude.In addition, PF algorithm is easily dissipated when number of particles is not very big, this is because
The variance parameter setting for generating particle collection is not reasonable.In this test, due to there is very serious " jump point " situation, letter is measured
The case where breath often fails, it is not continuously distributed for leading to the orientable point in some regions, and between being possible to
Every very big distance, this distance has exceeded the range of a subset-cover, therefore PF algorithm is easy failure.In such case
Under, the main reason for causing PF performance bad is the imperfect of data, if blindly tuning up the region for generating particle collection, and can be made
The particle collection that algorithm is obtained in the complete stage generation of metric data is excessively sparse, and the particle fallen in valid interval is very few, reduces
Estimated accuracy.
It should be noted last that the above examples are only used to illustrate the technical scheme of the present invention and are not limiting.Although ginseng
It is described the invention in detail according to embodiment, those skilled in the art should understand that, to technical side of the invention
Case is modified or replaced equivalently, and without departure from the spirit and scope of technical solution of the present invention, should all be covered in the present invention
Scope of the claims in.
Claims (7)
1. a kind of based on the object localization method for simplifying volume particle filter, which comprises
Step 1) establishes the state-space model of the target movement at k moment；Using state-space model to k moment dbjective state into
Row prediction obtains first time filter resultAnd covariance matrix
Step 2) noteFor generate particle assembly " seed ", withCentered on,The particle collection at k moment is generated for radius,
And each particle concentrated for particle calculates weight and normalizes weight；
Step 3) is concentrated from the particle of step 2) and chooses " high-quality particle ", and the weight of " high-quality particle " is normalized；This
" high-quality particle " forms new particle collection a bit；
Step 4) carries out resampling to the new particle collection that step 3) obtains, and obtains resampling particle collection；
Step 5) calculate resampling particle concentrate particle mean value be WithIt is the defeated of the dbjective state vector at k moment
Out.
2. according to claim 1 based on the object localization method for simplifying volume particle filter, which is characterized in that the step
It is rapid 1) to specifically include:
Step 1-1) movement of target that is described using following state-space model:
Xk=f (Xk-1)+wk, (1)
Zk=h (Xk)+vk, (2)
Wherein, k is integer, and the dbjective state of moment k is Xk∈Rn；ZkFor the collected measurement information of sensor；wk∈RnIt is defeated
The white noise entered, vk∈RmFor observation noise；Formula (5) is state equation, and formula (6) is observational equation, and f () is that state shifts letter
Number, h () are observation information function, and the initial value X of dbjective state is given in conjunction with priori knowledge0；
Step 1-2) bonding state spatial model, it is predicted, is obtained using state of the volume Kalman filtering to k moment target
First time filter resultAnd its corresponding covariance matrix
3. according to claim 2 based on the object localization method for simplifying volume particle filter, which is characterized in that the step
It is rapid 2) to specifically include:
Step 2-1) noteFor generate particle assembly " seed ", withCentered on,The particle at k moment is generated for radius
Collection includes N number of particle
Wherein,Indicate withFor mean value,For the Gaussian Profile of variance；
Step 2-2) it is that particle concentrates each particle to calculate weight
Wherein,Indicate given prior density function andUnder conditions of, obtain ZkProbability；
Step 2-3) it is rightNormalized weight is obtained after being normalized
4. according to claim 3 based on the object localization method for simplifying volume particle filter, which is characterized in that the step
It is rapid 3) specifically:
Give a weight threshold ωth, weight is less than ω thParticle all give up, it is remaining to be denoted as " high-quality particle ", total M
It is a；By " high-quality particle ", being numbered in order from 1 again is denoted asωiFor the weight after normalization.
5. according to claim 4 based on the object localization method for simplifying volume particle filter, which is characterized in that the power
Weight threshold value ωth=1/N.
6. according to claim 5 based on the object localization method for simplifying volume particle filter, which is characterized in that described heavy
Sampling are as follows: random resampling, multinomial resampling, system resampling or residual error resampling.
7. according to claim 6 based on the object localization method for simplifying volume particle filter, which is characterized in that the step
It is rapid 5) specifically:
Resampling particle collection in the step 4) are as follows:ωj'=1/M；The mean value of resampling particle collectionAre as follows:
Export the dbjective state vector at k moment: the mean value of resampling particle collectionWith the covariance matrix of step 1)
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811196808.6A CN109460539B (en) | 2018-10-15 | 2018-10-15 | Target positioning method based on simplified volume particle filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811196808.6A CN109460539B (en) | 2018-10-15 | 2018-10-15 | Target positioning method based on simplified volume particle filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109460539A true CN109460539A (en) | 2019-03-12 |
| CN109460539B CN109460539B (en) | 2020-05-26 |
Family
ID=65607606
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811196808.6A Active CN109460539B (en) | 2018-10-15 | 2018-10-15 | Target positioning method based on simplified volume particle filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109460539B (en) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110361744A (en) * | 2019-07-09 | 2019-10-22 | 哈尔滨工程大学 | RBMCDA underwater multi-target tracking based on Density Clustering |
| CN110702093A (en) * | 2019-09-27 | 2020-01-17 | 五邑大学 | Particle filter-based positioning method and device, storage medium and robot |
| CN110765897A (en) * | 2019-10-08 | 2020-02-07 | 哈尔滨工程大学 | Underwater target tracking method based on particle filtering |
| CN112631430A (en) * | 2020-12-30 | 2021-04-09 | 安徽鸿程光电有限公司 | Gesture motion trajectory processing method, device, equipment and medium |
| CN113051529A (en) * | 2021-03-17 | 2021-06-29 | 哈尔滨工程大学 | Particle filter data assimilation method based on statistical observation and localized average weight |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110084871A1 (en) * | 2009-10-13 | 2011-04-14 | Mcmaster University | Cognitive tracking radar |
| CN103389094A (en) * | 2013-07-15 | 2013-11-13 | 哈尔滨工程大学 | Improved particle filter method |
| CN108375374A (en) * | 2018-02-26 | 2018-08-07 | 重庆邮电大学 | Monte carlo localization algorithm based on adaptive iteration volume particle filter |
-
2018
- 2018-10-15 CN CN201811196808.6A patent/CN109460539B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110084871A1 (en) * | 2009-10-13 | 2011-04-14 | Mcmaster University | Cognitive tracking radar |
| CN103389094A (en) * | 2013-07-15 | 2013-11-13 | 哈尔滨工程大学 | Improved particle filter method |
| CN108375374A (en) * | 2018-02-26 | 2018-08-07 | 重庆邮电大学 | Monte carlo localization algorithm based on adaptive iteration volume particle filter |
Non-Patent Citations (3)
| Title |
|---|
| KUMAR PAKKI BHARANI CHANDRA ET AL.: "Square Root Cubature Information Filter", 《IEEE SENSORS JOURNAL》 * |
| 穆静等: "容积粒子滤波算法及其应用", 《西安交通大学学报》 * |
| 鹿传国等: "马尔可夫链蒙特卡罗容积粒子滤波器", 《电子科技大学学报》 * |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110361744A (en) * | 2019-07-09 | 2019-10-22 | 哈尔滨工程大学 | RBMCDA underwater multi-target tracking based on Density Clustering |
| CN110361744B (en) * | 2019-07-09 | 2022-11-01 | 哈尔滨工程大学 | RBMCDA underwater multi-target tracking method based on density clustering |
| CN110702093A (en) * | 2019-09-27 | 2020-01-17 | 五邑大学 | Particle filter-based positioning method and device, storage medium and robot |
| CN110765897A (en) * | 2019-10-08 | 2020-02-07 | 哈尔滨工程大学 | Underwater target tracking method based on particle filtering |
| CN112631430A (en) * | 2020-12-30 | 2021-04-09 | 安徽鸿程光电有限公司 | Gesture motion trajectory processing method, device, equipment and medium |
| CN113051529A (en) * | 2021-03-17 | 2021-06-29 | 哈尔滨工程大学 | Particle filter data assimilation method based on statistical observation and localized average weight |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109460539B (en) | 2020-05-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109460539A (en) | A kind of object localization method based on simplified volume particle filter | |
| US9562965B2 (en) | Two-stage filtering based method for multiple target tracking | |
| CN107193009A (en) | A kind of many UUV cooperative systems underwater target tracking algorithms of many interaction models of fuzzy self-adaption | |
| CN107832575A (en) | Band feedback maneuvering target Asynchronous Track Fusion based on pseudo-measurement | |
| CN108802707A (en) | The improved kalman filter method for target following | |
| Li et al. | An information theoretic approach to interacting multiple model estimation | |
| CN107635204A (en) | A kind of indoor fusion and positioning method and device of motor behavior auxiliary, storage medium | |
| CN106599368A (en) | FastSLAM method based on particle proposal distribution improvement and adaptive particle resampling | |
| CN109581353A (en) | A kind of multi-object tracking method and system based on car radar | |
| CN105954743B (en) | A kind of variable structure multi-model maneuvering target tracking method improving weights | |
| Jwo et al. | A novel design for the ultra-tightly coupled GPS/INS navigation system | |
| CN102279399B (en) | Dim target frequency spectrum tracking method based on dynamic programming | |
| CN111307143B (en) | Bionic navigation algorithm for multi-target evolution search based on geomagnetic gradient assistance | |
| CN106289281B (en) | A kind of double mode map-matching method based on three evidence DS theories | |
| CN103487056A (en) | Gravity matching method based on artificial bee colony algorithm and average Hausdorff distance | |
| CN107133555A (en) | Recognize 8 word movement locus mesh calibration methods | |
| CN115932913B (en) | Satellite positioning pseudo-range correction method and device | |
| CN113156368B (en) | Error parameter identification co-location method based on factor graph | |
| CN109655057A (en) | A kind of six push away the filtering optimization method and its system of unmanned plane accelerator measured value | |
| Zhao et al. | Adaptive double-resampling particle filter algorithm for target tracking | |
| Song et al. | Box-particle cardinality balanced multi-target multi-Bernoulli filter | |
| Yang et al. | Particle swarm optimization algorithm for passive multi-target tracking | |
| CN107590509B (en) | Cherenov fusion method based on maximum expectation approximation | |
| Bogomolov | Test Results of the Long Baseline Navigation Solutions under a Large a Priori Position Uncertainty | |
| Zhang et al. | The research for a kind of information fusion model based on BP neural network with multi position sources and big data selection |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20201217 Address after: 311800 Room 101, 1st floor, 22 Zhongjie building, 78 Zhancheng Avenue, Taozhu street, Zhuji City, Shaoxing City, Zhejiang Province Patentee after: Zhejiang wanghaichao Technology Co.,Ltd. Address before: 100190, No. 21 West Fourth Ring Road, Beijing, Haidian District Patentee before: INSTITUTE OF ACOUSTICS, CHINESE ACADEMY OF SCIENCES |