CN109460539B - Target positioning method based on simplified volume particle filtering - Google Patents

Target positioning method based on simplified volume particle filtering Download PDF

Info

Publication number
CN109460539B
CN109460539B CN201811196808.6A CN201811196808A CN109460539B CN 109460539 B CN109460539 B CN 109460539B CN 201811196808 A CN201811196808 A CN 201811196808A CN 109460539 B CN109460539 B CN 109460539B
Authority
CN
China
Prior art keywords
particles
particle
target
resampling
weight
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.)
Active
Application number
CN201811196808.6A
Other languages
Chinese (zh)
Other versions
CN109460539A (en
Inventor
鄢社锋
徐立军
石桂欣
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang Wanghaichao Technology Co ltd
Original Assignee
Institute of Acoustics CAS
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Institute of Acoustics CAS filed Critical Institute of Acoustics CAS
Priority to CN201811196808.6A priority Critical patent/CN109460539B/en
Publication of CN109460539A publication Critical patent/CN109460539A/en
Application granted granted Critical
Publication of CN109460539B publication Critical patent/CN109460539B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Physics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Biology (AREA)
  • Software Systems (AREA)
  • Computer Hardware Design (AREA)
  • Operations Research (AREA)
  • Probability & Statistics with Applications (AREA)
  • Geometry (AREA)
  • Algebra (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Databases & Information Systems (AREA)
  • Evolutionary Computation (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)
  • Complex Calculations (AREA)

Abstract

The invention discloses a target positioning method based on simplified volume particle filtering, which comprises the following steps: step 1) establishing a state space model of target motion at the moment k; predicting the target state at the k moment by using a state space model to obtain a first filtering result
Figure DDA0001828982250000011
Sum covariance matrix
Figure DDA0001828982250000012
Step 2) recording
Figure DDA0001828982250000013
To generate "seeds" of a collection of particles, to
Figure DDA0001828982250000014
Is used as the center of the device,
Figure DDA0001828982250000015
generating a particle set at the moment k for the radius, and calculating a weight value and normalizing the weight value for each particle in the particle set; step 3) selecting high-quality particles from the particle set in the step 2), and normalizing the weight of the high-quality particles; these "good-quality particles" constitute a new set of particles; step 4) resampling the new particle set obtained in the step 3) to obtain a resampled particle set; step 5) calculating the mean value of the particles in the resampled particle set as
Figure DDA0001828982250000016
Figure DDA0001828982250000017
And
Figure DDA0001828982250000018
is the output of the target state vector at time k.

Description

Target positioning method based on simplified volume particle filtering
Technical Field
The invention relates to the field of target positioning, in particular to a target positioning method based on simplified volume particle filtering.
Background
Many classical filtering algorithms have been widely applied to systems such as target tracking navigation and instant positioning and mapping (SLAM), such as Kalman Filtering (KF), Extended Kalman Filtering (EKF), Unscented Kalman Filtering (UKF), volumetric kalman filtering (CKF), Particle Filtering (PF), and the like. Aiming at the defects of the algorithms, a series of correction algorithms are proposed successively. Classical filtering algorithms can be divided into two broad categories: and filtering algorithms based on a KF framework and a PF framework. The former is a high-precision algorithm under the condition of Gaussian noise, and the latter is an optimal solution under the condition of non-Gaussian. The accuracy of particle filtering depends to a large extent on the number of particles and the choice of the importance function. With a large number of particles, the dramatic increase in computational effort brings a "dimensional disaster" to the system. In addition, if the importance function is not well selected, it often results in filtering divergence or very poor filtering effect. Therefore, the importance function is crucial for the design of the particle filter. A very final criterion for determining the quality of the importance function is to see whether it can utilize the latest measurement value to the maximum extent, so that the sampled particles can represent the latest measurement value information. One potential drawback of the standard particle filter algorithm is that: the group on which this is based does not exactly describe the end of the true a posteriori probability density function, a phenomenon that is more pronounced when the measured values appear outside the estimated function. The main reason is that the determined mixed estimation function is adopted to estimate the time-varying real posterior distribution, namely the selection of the importance density function is not ideal.
In the optimization method of the PF, finding a suitable suggested density distribution function is a feasible idea, and has been a research hotspot of researchers. If the optimal suggested density distribution function can be found and the resampling is guided to make correct sampling distribution, the effectiveness of the sample set can be ensured, and the estimation performance of the PF can also be ensured. In recent years, some researchers have developed the idea of integrating two schemes to improve PFs with KF-based filtering algorithms, i.e., to generate a proposed density profile to guide "sampling" in subsequent PF processes. Typical algorithms include Extended Particle Filter (EPF) algorithm with EKF combined with PF, Unscented Particle Filter (UPF) algorithm with UKF combined with PF, and new volumetric particle filter (CPF) algorithm-CKF combined with PF. Since the filtering accuracy of CKF is better than EKF and UKF, the accuracy of CPF is also better than EPF and UPF. Compared with PF, EPF and UPF algorithms, CPF greatly improves the filtering precision, and is one of the algorithms with the highest filtering precision at present. However, since the calculated amount of the PF itself is relatively large and the particle degradation phenomenon is easily generated, the amount of calculation for generating each particle is increased after introducing the CKF algorithm, so that the calculated amount is multiplied, and the "dimension disaster" is more severe. Therefore, although the target positioning algorithm based on the CPF algorithm can achieve higher estimation accuracy, the hardware requirement on the positioning system is high, and each time the position of the target is updated, expensive calculation cost is required. Especially for a positioning system working underwater, it is very difficult or even impossible to charge and replace the battery, so the power consumption of the system directly determines the working life of the system. The use of tens or even hundreds of times power consumption to trade several percent increase in accuracy is obviously not a sensible option for underwater positioning systems, especially real-time positioning systems. In summary, the huge amount of computation is the biggest disadvantage of the target positioning system based on the CPF algorithm. In conclusion, how to improve the filtering algorithm of the positioning system has great significance in obtaining the compromise between the precision and the efficiency.
Disclosure of Invention
The invention aims to overcome the technical defects and provides a target positioning method based on simplified volume particle filtering, which can obviously reduce the calculation amount.
In order to achieve the purpose, the technical scheme of the invention is as follows:
a method of target localization based on simplified volumetric particle filtering, the method comprising:
step 1) establishing a state space model of target motion at the moment k; predicting the target state at the k moment by using a state space model to obtain a first filtering result
Figure GDA0002377264170000021
Sum covariance matrix
Figure GDA0002377264170000022
Step 2) recording
Figure GDA0002377264170000023
To generate "seeds" of a collection of particles, to
Figure GDA0002377264170000024
Is used as the center of the device,
Figure GDA0002377264170000025
generating a particle set at the moment k for the radius, and calculating a weight value and normalizing the weight value for each particle in the particle set;
step 3) selecting high-quality particles from the particle set in the step 2), and normalizing the weight of the high-quality particles; these "good-quality particles" constitute a new set of particles;
step 4) resampling the new particle set obtained in the step 3) to obtain a resampled particle set;
step 5) calculating the mean value of the particles in the resampled particle set as
Figure GDA0002377264170000026
Figure GDA0002377264170000027
And
Figure GDA0002377264170000028
is the output of the target state vector at time k.
As an improvement of the above method, the step 1) specifically includes:
step 1-1) motion of an object described using a state space model as follows:
Xk＝f(Xk-1)+wk, (1)
Zk＝h(Xk)+vk, (2)
wherein k is an integer, and the target state at time k is Xk∈Rn；ZkMeasurement information collected for the sensor; w is ak∈RnIs input white noise, vk∈RmTo observe noise; the formula (1) is a state equation, the formula (2) is an observation equation, f (-) is a state transfer function, h (-) is an observation information function, and an initial value X of a target state is given by combining prior knowledge0；
Step 1-2) predicting the state of the target at the k moment by using the cubature Kalman filtering in combination with a state space model to obtain a first filtering result
Figure GDA0002377264170000035
And its corresponding covariance matrix
Figure GDA0002377264170000036
As an improvement of the above method, the step 2) specifically includes:
step 2-1) recording
Figure GDA0002377264170000037
To generate "seeds" of a collection of particles, to
Figure GDA0002377264170000038
Is used as the center of the device,
Figure GDA0002377264170000039
generating a set of particles at time k for the radius, comprising N particles
Figure GDA00023772641700000310
Figure GDA0002377264170000031
Wherein the content of the first and second substances,
Figure GDA00023772641700000311
is shown in
Figure GDA00023772641700000312
Is taken as the mean value of the average value,
Figure GDA00023772641700000313
is a gaussian distribution of variance;
step 2-2) calculating a weight for each particle in the set of particles
Figure GDA00023772641700000314
Figure GDA0002377264170000032
Wherein the content of the first and second substances,
Figure GDA00023772641700000315
representing a given a priori distribution function sum
Figure GDA00023772641700000316
Under the conditions of (1), Z is obtainedkThe probability of (d);
step 2-3) is right
Figure GDA00023772641700000317
Normalization weight is obtained after normalization
Figure GDA00023772641700000318
Figure GDA0002377264170000033
As an improvement of the above method, the step 3) is specifically:
given a weight threshold ωthMake the weight less than omegathAll the particles are discarded, and the rest are marked as 'high-quality particles', and the total number is M; the "good-quality particles" are numbered again from 1
Figure GDA00023772641700000319
ωiIs the normalized weight.
As an improvement of the above method, the weight threshold ωth＝1/N。
As an improvement of the above method, the resampling is: random resampling, polynomial resampling, systematic resampling, or residual resampling.
As an improvement of the above method, the step 5) is specifically:
the resampling particle set in the step 4) is as follows:
Figure GDA00023772641700000320
ωj' -1/M; mean of resampled particle sets
Figure GDA00023772641700000321
Comprises the following steps:
Figure GDA0002377264170000034
outputting the target state vector at the k moment: mean of resampled particle sets
Figure GDA0002377264170000041
And the covariance matrix of step 1)
Figure GDA0002377264170000042
The invention has the advantages that:
1. the method of the invention keeps the advantage of high estimation performance of the CPF tracking and positioning method; but the operation amount is reduced by about one order of magnitude;
2. the method of the invention can also achieve high positioning accuracy for strong nonlinear and non-Gaussian motion models.
Drawings
FIG. 1 is a flow chart of a simplified volumetric particle filter based target location method at time k according to the present invention;
FIG. 2 is a flow chart of a method for continuous simplified volumetric particle filter based object localization according to the present invention;
FIG. 3 is a graph comparing the estimated trajectories of a standard CPF and a simplified CPF (population 100);
FIG. 4 is a graph of the positioning error of CKF, PF, standard CPF, ICPF proposed by the present invention at a certain filtering (population 100);
FIG. 5 is a graph comparing CPF to ICPF for a certain filtered localization error curve (population 500);
FIG. 6 is a particle set contrast diagram (particle count 100) at a time after a resampling update;
FIG. 7 is a particle set contrast diagram (particle number 200) after resampling and updating at a certain time;
FIG. 8 is a particle set contrast diagram (particle number 500) at a time after a resampling update;
FIG. 9 is a comparison graph of the location error RMSE for CKF, PF, standard CPF, ICPF of the present invention (10 Monte Carlo experiments);
FIG. 10 is a graph comparing the run times of CKF, PF, standard CPF, ICPF of the present invention (10 Monte Carlo experiments);
FIG. 11 is a comparison graph of estimated trajectories of CKF, PF, standard CPF, ICPF of the present invention (population 100);
FIG. 12 is a plot of the location error of a certain filtering of CKF, PF, standard CPF, ICPF of the present invention (population 100);
fig. 13 is a particle set contrast diagram (particle number 100) after resampling and updating at a certain time;
FIG. 14 is a particle set contrast diagram (particle count 200) at a time after a resampling update;
FIG. 15 is a particle set contrast diagram (particle number 500) at a time after a resampling update;
FIG. 16 is a comparison of the location error RMSE for CKF, PF, standard CPF, ICPF of the present invention;
FIG. 17 is a comparison of the operating times of CKF, PF, standard CPF, and ICPF of the present invention.
Detailed Description
The present invention will be described in detail with reference to the accompanying drawings.
The invention provides an Improved filtering scheme, which is called an Improved volume particle Filter (ICPF) target positioning method. The method provided by the invention can avoid the particle degradation phenomenon, and has the advantages of simple operation and low calculation amount (the calculation amount is reduced by one order of magnitude). Meanwhile, the estimation precision of the method is not reduced. We believe that this more compact new algorithm will push the further application of nonlinear filtering and greatly reduce the difficulty of real-time application. Both simulation and lake test results prove the superiority of the new method.
1. State system model
Consider a dynamic system described by a state space model
Xk＝f(Xk-1)+Wk, (1)
Yk＝h(Xk)+vk, (2)
Where k is a discrete time and the state of the system at time k is Xk∈Rn；Yk∈RmAs observed signal of corresponding state, wk∈RnIs input white noise, vk∈RmTo observe the noise.
Wherein, the formula (1) is a state equation, the formula (2) is an observation equation, f is a state transfer function, and h is an observation information function.
2. Standard volumetric particle filter (CPF)
The core of the algorithm for improving the PF of the particle filter by using the CKF of the cubature Kalman filter is as follows: in the sampling phase, the mean and covariance are calculated for each particle using the CKF algorithm, then used, and then "guided" the sampling using the mean and covariance. Because the function of approximate posterior filtering density is used in the process of calculating the mean value and the variance by using the CKF algorithm, namely the latest observation information Z is absorbedkIn the frame of particle filtersUnder the shelf, the CKF algorithm produces a density distribution for each particle that conforms to the gaussian recommendation. In other words, the CKF algorithm and the latest observation information Z are used at time kkThe mean and variance of the ith particle are calculated and the mean is used to sample and update the particle.
The standard CPF algorithm flow is as follows.
(1) Initialization, k is 0. Giving an initial particle value X in combination with a priori knowledge0。
(2)For k＝1:K，
a) And (5) importance sampling stage.
(3)For i＝1:N，
Estimating each particle by using CKF algorithm to obtain the mean value of each particle
Figure GDA0002377264170000052
Sum covariance
Figure GDA0002377264170000053
The set of particles is updated and,
Figure GDA0002377264170000051
Figure GDA0002377264170000054
representing the distribution generated for each particle that conforms to the gaussian proposed density,
Figure GDA0002377264170000055
so as to make
Figure GDA0002377264170000056
Is the mean value,
Figure GDA0002377264170000057
Is a gaussian distribution of variance.
(4) The weights are calculated.
For i＝1:N，
The weights are recalculated and normalized for each particle in the set of particles.
Figure GDA0002377264170000061
Figure GDA0002377264170000062
p (-) is the posterior probability density distribution.
b) The phase is selected (resampling).
And generating N random sample sets according to the approximate distribution, calculating weights, normalizing, copying and eliminating the particle sets to obtain updated particle sets.
c) And (3) outputting: calculating the mean X of the particle setskAs the output of the algorithm.
The greatest difference between the CPF algorithm and the PF algorithm is that the process of generating particles is based on the CKF algorithm, and other steps are almost the same.
The essence of this approach is to improve the problem of suggesting a density distribution, transferring a collection of particles distributed a priori to the likelihood regions at the cost of gaussian assumptions on the system. As we know, the fundamental particle filter is not constrained by a linear gaussian model, and the CPF is then constrained by a gaussian model. This is also a disadvantage of the algorithm.
The standard CPF algorithm introduces the CKF algorithm to "guide" the sampling process, which has the most prominent advantage of high estimation accuracy, and also has the following disadvantages, resulting in limitations in practical applications.
1. The complexity of the process of generating new particles is proportional to the number of particles. (according to the standard CPF algorithm steps, each particle after the last process is finished is processed by CKF)
2. In the resampling stage, bipolar differentiation of new particle weights and lack of particle diversity may be caused, resulting in particle degradation. Particles with many small weights (very large errors) may also be re-sampled, introducing new errors.
3. If the number of particles is large, the computational complexity of the resampling stage will be high.
3. Simplified volumetric particle filter (ICPF)
The simplified volume particle filter ICPF of the invention not only retains the advantage of high estimation precision of the standard CPF, but also solves the two biggest problems of particle filtering: large computation and particle degradation.
Specific steps of ICPF are given below.
(1) Initialization, k is 0. Setting an initial value X in combination with a priori knowledge0。
(2)For k＝1:K，
The first step of prediction updating: updating the prediction result by using CKF algorithm to obtain the first filtering result
Figure GDA0002377264170000074
Sum covariance matrix
Figure GDA0002377264170000075
Note the book
Figure GDA0002377264170000076
To generate a "seed" for a collection of particles.
(3) And a second step of prediction updating:
a) and (5) importance sampling stage.
Using the "seed" and the latest covariance matrix, a new set of particles is generated,
Figure GDA0002377264170000077
Figure GDA0002377264170000078
weights are calculated and normalized for each particle.
Figure GDA0002377264170000071
Figure GDA0002377264170000072
In the formula
Figure GDA0002377264170000079
Representing a given a priori distribution function sum
Figure GDA00023772641700000710
Under the conditions of (1), Z is obtainedkThe probability of (c).
b) The phase is selected (resampling).
Given a weight threshold ωthMake the weight less than omegathAll the particles in (2) are discarded, the rest are marked as "good-quality particles", and the weight omega of the "good-quality particles" is recordediAnd (6) carrying out normalization. I.e. the weight ω of each particleiIs updated to
Figure GDA00023772641700000711
The 'high-quality particles' and their corresponding weights form a new particle set
Figure GDA00023772641700000712
Selecting a resampling method (random resampling, polynomial resampling, system resampling, residual resampling and the like), and updating the particle set again; updated particle set
Figure GDA00023772641700000713
ωi′＝1/M。
c) Output of
Mean of resampled particle sets
Figure GDA00023772641700000714
Comprises the following steps:
Figure GDA0002377264170000073
Figure GDA00023772641700000715
and
Figure GDA00023772641700000716
is the output of the ICPF algorithm at time k.
It can be seen that the algorithm in the invention has the following characteristics:
1. the process of generating new particles is simple and fast.
2. The phenomenon of particle degradation is avoided because particles are regenerated in each recursion process.
3. In the resampling stage, a plurality of particles with small weight are directly discarded, and only valuable 'good-quality' particles are reserved.
4. Even if the number of particles is large, the computational complexity in the resampling stage is not high.
5. The advantage of high estimation performance of the CPF algorithm is kept.
As shown in fig. 1, the target location method based on the simplified volume particle filter (ICPF) includes the following steps:
1. setting parameters P, R, Q, T, N, f (·), ωthAnd, in combination with a priori knowledge, giving an initial value X of the target coordinates0. In general, it is recommended to take ωth＝1/N。
2. The sensor collects target information, such as azimuth angle and distance of the target relative to the sensor, the collected information is called measurement information Z, the measurement information at the moment k is recorded as Zk。
3. Determining a measurement equation h (-) and a weight function ω according to the properties of the measurement informationiThe form of calculation of (1).
4. And (3) prediction updating: updating the prediction result by using CKF algorithm to obtain the first filtering result
Figure GDA0002377264170000081
Sum covariance matrix
Figure GDA0002377264170000082
Note the book
Figure GDA0002377264170000083
To generate a "seed" for a collection of particles.
5. Generating a set of particles: to be provided with
Figure GDA0002377264170000084
Is used as the center of the device,
Figure GDA0002377264170000085
for radius scattering of particles, the number of particles is N set in the first step, and a particle set at time k is obtained.
6. An importance sampling stage, namely calculating the weight of each particle and normalizing the weight, selecting ' good-quality particles ', and weighting omega of the ' good-quality particlesiAnd (6) carrying out normalization. These "good quality particles" constitute a new set of particles
Figure GDA0002377264170000086
M is the number of 'high-quality particles'.
7. And (3) a resampling stage: resampling the new particle set obtained in the last step: the particles are copied and discarded.
8. The mean value of the results obtained by resampling is calculated and recorded as
Figure GDA0002377264170000087
Figure GDA0002377264170000088
And
Figure GDA0002377264170000089
is the output of the tracking system at time k.
If the tracking of the target is not finished, repeating the steps 2-8 and continuing downward processing; otherwise, outputting the result, and ending the whole tracking stage; as shown in fig. 2.
4. Performance analysis
The Monte-Carlo experiment was performed below to analyze the performance of the ICPF algorithm proposed by the present invention and to compare it with the original algorithm. First consider the state vector at time k as Xk＝[x,y,vx,vy,ax,ay]TThe equation of state and the observation equation are established according to the equations (1) and (2) as follows:
Xk＝ΦXk-1+wk(3)
Yk＝h(Xk，vk)＝ dist(Xk,{A,B,C}) +vk(4)
let T be the sampling interval, the state transition matrix in equation (3) is:
Figure GDA0002377264170000091
wkand vkWhite gaussian noise with mean zero and variance Q and R, respectively; dist (X)kAnd { A, B, C }) denotes XkThe distance of the determined coordinates from the nodes { A, B, C }.
The target firstly makes a uniform linear (CV) motion from the origin at a speed vx＝5m/s，vy3m/s, 5s rear acceleration ax＝-0.1m/s2，ay＝0.1m/s2And continues until the acceleration of 55s, 55s to 105s is
Figure GDA0002377264170000092
Figure GDA0002377264170000093
ay＝-0.1m/s2105s to 110s make a CV motion with the velocity of the target at the end of the previous stage (105 s). The coordinates of three nodes are given as [0m,500m ] respectively]、[250m,100m]、[300m,400m]. The distance between the target and the measurement information is measured by three nodes, and the covariance of the measurement information is R ═ diag [5, 5 [ ]]. Selecting a uniform acceleration motion (CA) model, given an initial state vector X0＝[0,0,0,0,0,0]TAnd respectively tracking and estimating the position information of the passive target by using a standard CPF algorithm and an ICPF algorithm provided by the invention. Assume that the process noise variance is Q ═ diag [1,1,0.1,0.1,0.01]The initial covariance matrix is the identity matrix, i.e.
Figure GDA0002377264170000094
Where n is the dimension of the state vector, where n is 6.
Fig. 3 shows the comparison of the trajectory estimation results of several methods, CKF, PF, standard CPF, ICPF proposed by the present invention, with the real trajectory when the number of particles is 100. It can be seen from fig. 4 that the trace curves for the standard CPF and ICPF are almost coincident, indicating that the estimates for both methods are almost identical. The positioning error curves for these several methods at each sample point in fig. 4 also confirm the above conclusions. Fig. 5 shows the error curves of the standard CPF and ICPF when the population is 500, and it can be seen that the error curves of the two are closer to each other when the population is increased. Fig. 6 to 8 show the particle set comparison diagrams after resampling and updating at a certain sampling time, which are randomly cut when the number of particles is 100, 200, and 500, respectively. From these several comparison figures, it is clear that the set of particles produced by the ICPF of the present invention is very similar to the set of particles produced by the standard CPF, with the two sets having a tendency to coincide as the number of particles increases. The above results show that as the number of particles increases, the effect of both the standard CPF and ICPF methods becomes more and more similar.
The occurrence of this trend is not difficult to understand. As the number of particles increases, the resulting particles cover the likelihood region of the true value better and better. In addition, since the method of screening particles is the same (the formula for calculating the weight and the resampling algorithm are not changed), the particles left after screening are all particles that are clustered around the true value. As the number of particles increases, the area covered by the set of particles generated by the standard CPF can also be covered by the set of particles of the ICPF, so the results estimated by the two algorithms are also very similar.
The results of the CKF, PF, CPF methods are all near true using the same metrology information and estimation parameters (initial estimate, initial covariance matrix, noise variance matrix, etc.). Moreover, as can be seen from the practical situation, the estimation result can be obtained by using only the CKF or PF method, but the performance is worse than that of the CPF. In systems requiring real-time processing or other systems requiring processing speed, the CKF and PF methods are no longer good alternatives to CPF. Also, when the number of particles is infinite, PF and CPF are equivalent. Thus, for the method of the invention, we can understand that: when the system is subjected to CKF filtering once, a preliminary result is obtained
Figure GDA0002377264170000101
Sum covariance matrix
Figure GDA0002377264170000102
Figure GDA0002377264170000103
With the true value XkWith a certain error in between. Can be handled
Figure GDA0002377264170000104
Considered as a guide or "seed" for the production of particles in the PF, in combination with another parameter
Figure GDA0002377264170000106
To generate a new set of particles omega. As long as
Figure GDA0002377264170000105
Is authentic, then XkShould fall within the region covered by Ω. This is the core idea of ICPF. Compared to the standard PF algorithm, the ICPF algorithm utilizes the latest observations when generating a set of particles, so the particles generated are closer to the true values; compared with the standard CPF algorithm, the ICPF algorithm simulates a particle set obtained after each particle in the CPF is subjected to CKF filtering by using multiple sampling of results of early-stage CKF prediction.
FIG. 9 shows the error comparison curves of several methods obtained after 50 Monte Carlo simulations under the above parameters. The calculation time of the standard CPF method and the ICPF method of the invention is recorded as Tcpf、Ticpf. FIG. 10 shows the running time of the two methods, the multiple T of the operating time of the standard CPF method and the newly proposed ICPF methodcpf/TicpfCurve of change with increasing number of particles. We can clearly see that the estimation accuracy of the ICPF method is almost equal to that of the standard CPF method, but the amount of computation is reduced by at least an order of magnitude. The computer running the program is the associative 90D4CT01WW, the processor is AMD A10PR0-7800B R7, 4CPUs, 3.5GHz, the memory is 4GB, the operating system is Windows 7, and the MATLAB version is 2015 a.
To further validate the method proposed herein, we performed underwater localization experiments in the thousand island lake on day 22 of 3 months in 2018. By comparing the running time of the two methods and the tracking result of the target position coordinate, the method provided by the invention is proved to obtain a good compromise between high precision and high efficiency. In the test, the target was a transmitting transducer placed against the bottom of the boat, and the trajectories of the target and boat were considered to coincide. The real trajectory of the boat is given by the differential GPS. Three nodes (node name A, B, C) used in this experiment were placed in a lake water area approximately one kilometer square. Each node is divided into two parts: the receiving and transmitting combination energy converter module is used for receiving signals underwater and the buoy module is connected with a GPS signal receiver. The underwater acoustic transducer in the node is positioned 5 meters deep under water, and the buoy at the top of the node floats on the water surface. In the test, only two-dimensional coordinate information (namely east direction and north direction) of the target is considered, and a reference point of a coordinate system is selected as the position of the C node at the initial moment. The GPS sends positioning information every 1s, and the underwater acoustic ranging signals are sent and collected at intervals of 2 s.
Similar to the model in the simulation, we still chose the CA model to track the above target. Consider the state vector at time k to be Xk＝[x,y,vx,vy,ax,ay]TThe dimension n is 6; assume that the process noise variance is Q ═ diag [0.15, 0.15, 0.01,0.01]Measuring the variance of the noise as
Figure GDA0002377264170000111
m is 3; initial covariance matrix of P0＝diag[1，1，0.01，0.01，0.01，0.01]. The state transition matrix Φ is unchanged.
It is worth noting that, because the experimental conditions are relatively bad, all the information can not be collected by all three nodes, and in order to improve the utilization rate of the measurement information and the positioning accuracy as much as possible, the filtering is performed on the condition that the measurement information is greater than or equal to 2, that is, the dimension of the measurement information can be 2 or 3. The target tracking algorithm used in the experiment was the same as in the simulation experiment. The results of the experimental data processing are similar to the results of the simulation in the previous section. Fig. 11 shows the comparison of the trajectory estimation results of the two algorithms with the real trajectory for a number of particles of 100. As can be seen in fig. 11, the standard CPF and ICPF curves are nearly coincident, indicating that the estimates for both methods are nearly identical. The positioning error curves for these several methods at each sample point in fig. 12 also confirm the above conclusions. Fig. 13 to 15 show the particle set comparison diagrams after resampling and updating at a certain sampling time, which are randomly cut when the number of particles is 100, 200, and 500, respectively. Like the simulation results, the ICPF-generated particle sets in these several comparison plots are very similar to the standard CPF-generated particle sets, with the two sets having a tendency to coincide as the number of particles increases.
In addition, the new method provided by the invention ensures the estimation precision, greatly reduces the operation time compared with the standard method, and greatly improves the operation efficiency. FIG. 16 shows the error contrast curve of the centralized algorithm obtained after 50 Monte Carlo simulations under the above parameter conditions. The RMSE of the standard CPF algorithm and the ICPF algorithm are very poor, about 0.2m, and we consider that the performance of both algorithms in terms of target location estimation is consistent. FIG. 17 shows the running time of the two methods, the multiple T of the operating time of the standard CPF algorithm and the newly proposed ICPF algorithmcpf/TicpfCurve of change with increasing number of particles. The amount of ICPF computation is reduced by about one order of magnitude compared to CPF. In addition, the PF algorithm easily diverges when the number of particles is not large, because the variance parameter settings that produce the particle sets are not reasonable enough. In this test, the PF algorithm is prone to failure because of the severe "jumping point" situation, which often causes failure of the measurement information, and the points that can be located in some areas are not continuously distributed, but may be separated by a large distance that exceeds the coverage of the particle set. In this case, the main reason for poor performance of the PF is data incompleteness, and if the region for generating particle subsets is blindly enlarged, the particle set generated by the algorithm at the stage of complete measurement data is too sparse, and too few particles fall in the valid interval, which reduces the estimation accuracy.
Finally, it should be noted that the above embodiments are only used for illustrating the technical solutions of the present invention and are not limited. Although the present invention has been described in detail with reference to the embodiments, it will be understood by those skilled in the art that various changes may be made and equivalents may be substituted without departing from the spirit and scope of the invention as defined in the appended claims.

Claims (5)

1. A method of target localization based on simplified volumetric particle filtering, the method comprising:
step 1) establishing a state space model of target motion at the moment k; predicting the target state at the k moment by using a state space model to obtain a first filtering result
Figure FDA0002354726150000011
Sum covariance matrix
Figure FDA0002354726150000012
Step 2) recording
Figure FDA0002354726150000013
To generate "seeds" of a collection of particles, to
Figure FDA0002354726150000014
Is used as the center of the device,
Figure FDA0002354726150000015
generating a particle set at the moment k for the radius, and calculating a weight value and normalizing the weight value for each particle in the particle set;
step 3) selecting high-quality particles from the particle set in the step 2), and normalizing the weight of the high-quality particles; these "good-quality particles" constitute a new set of particles;
step 4) resampling the new particle set obtained in the step 3) to obtain a resampled particle set;
step 5) calculating the mean value of the particles in the resampled particle set as
Figure FDA0002354726150000016
And
Figure FDA0002354726150000017
is the output of the target state vector at time k;
the step 2) specifically comprises the following steps:
step 2-1) recording
Figure FDA0002354726150000018
To generate "seeds" of a collection of particles, to
Figure FDA0002354726150000019
Is used as the center of the device,
Figure FDA00023547261500000110
generating a set of particles at time k for the radius, comprising N particles
Figure FDA00023547261500000111
Figure FDA00023547261500000112
Wherein the content of the first and second substances,
Figure FDA00023547261500000113
is shown in
Figure FDA00023547261500000114
Is taken as the mean value of the average value,
Figure FDA00023547261500000115
is a gaussian distribution of variance;
step 2-2) calculating a weight for each particle in the set of particles
Figure FDA00023547261500000116
Figure FDA00023547261500000117
Wherein the content of the first and second substances,
Figure FDA00023547261500000118
representing a given a priori distribution function sum
Figure FDA00023547261500000119
Under the conditions of (1), Z is obtainedkThe probability of (d);
step 2-3) is right
Figure FDA00023547261500000120
Normalization weight is obtained after normalization
Figure FDA00023547261500000121
Figure FDA00023547261500000122
The step 3) is specifically as follows:
given a weight threshold ωthMake the weight less than omegathAll the particles are discarded, and the rest are marked as 'high-quality particles', and the total number is M; the "good-quality particles" are numbered again from 1
Figure FDA00023547261500000123
ωiIs the normalized weight.
2. The simplified volumetric particle filter based target localization method according to claim 1, wherein the step 1) specifically comprises:
step 1-1) motion of an object described using a state space model as follows:
Xk＝f(Xk-1)+wk， (1)
Zk＝h(Xk)+vk， (2)
wherein k is an integer, and the target state at time k is Xk∈Rn；ZkMeasurement information collected for the sensor; w is ak∈RnIs input white noise, vk∈RmTo observe noise; the formula (1) is a state equation, the formula (2) is an observation equation, f (-) is a state transfer function, h (-) is an observation information function, and an initial value X of a target state is given by combining prior knowledge0；
Step 1-2) predicting the state of the target at the k moment by using the cubature Kalman filtering in combination with a state space model to obtain a first filtering result
Figure FDA0002354726150000021
And its corresponding covariance matrix
Figure FDA0002354726150000022
3. The simplified volumetric particle filter based object localization method of claim 2, wherein the weight threshold ω isth＝1/N。
4. The simplified volumetric particle filter based target localization method of claim 3, wherein the resampling is: random resampling, polynomial resampling, systematic resampling, or residual resampling.
5. The simplified volumetric particle filter-based target localization method according to claim 4, wherein the step 5) is specifically:
the resampling particle set in the step 4) is as follows:
Figure FDA0002354726150000023
ωj' -1/M; mean of resampled particle sets
Figure FDA0002354726150000024
Comprises the following steps:
Figure FDA0002354726150000025
outputting the target state vector at the k moment: mean of resampled particle sets
Figure FDA0002354726150000026
And the covariance matrix of step 1)
Figure FDA0002354726150000027
CN201811196808.6A 2018-10-15 2018-10-15 Target positioning method based on simplified volume particle filtering Active CN109460539B (en)

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 CN109460539A (en) 2019-03-12
CN109460539B true 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)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110361744B (en) * 2019-07-09 2022-11-01 哈尔滨工程大学 RBMCDA underwater multi-target tracking method based on density clustering
CN110702093B (en) * 2019-09-27 2022-10-25 五邑大学 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
CN112631430B (en) * 2020-12-30 2022-08-16 安徽鸿程光电有限公司 Gesture motion trajectory processing method, device, equipment and medium
CN113051529B (en) * 2021-03-17 2023-05-30 哈尔滨工程大学 Localized uniform weight particle filtering data assimilation method based on statistical observation

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110084871A1 (en) * 2009-10-13 2011-04-14 Mcmaster University Cognitive tracking radar

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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)

* Cited by examiner, † Cited by third party
Title
Square Root Cubature Information Filter;Kumar Pakki Bharani Chandra et al.;《IEEE Sensors Journal》;20121025;第23卷(第2期);第750-758页 *
容积粒子滤波算法及其应用;穆静等;《西安交通大学学报》;20110524;第45卷(第08期);第13-17页 *
马尔可夫链蒙特卡罗容积粒子滤波器;鹿传国等;《电子科技大学学报》;20121130;第41卷(第06期);第859-864页 *

Also Published As

Publication number Publication date
CN109460539A (en) 2019-03-12

Similar Documents

Publication Publication Date Title
CN109460539B (en) Target positioning method based on simplified volume particle filtering
CN107038717B (en) A method of 3D point cloud registration error is automatically analyzed based on three-dimensional grid
CN109798896B (en) Indoor robot positioning and mapping method and device
CN107247961B (en) Track prediction method applying fuzzy track sequence
CN111046018B (en) Multi-objective optimization-based power generation equipment operation condition library construction method and device
CN111127523B (en) Multi-sensor GMPHD self-adaptive fusion method based on measurement iteration update
CN104156984A (en) PHD (Probability Hypothesis Density) method for multi-target tracking in uneven clutter environment
CN108871365B (en) State estimation method and system under course constraint
CN111259332B (en) Fuzzy data association method and multi-target tracking method in clutter environment
CN114137562B (en) Multi-target tracking method based on improved global nearest neighbor
CN114386466B (en) Parallel hybrid clustering method for candidate signal mining in pulsar search
CN109115228B (en) Target positioning method based on weighted least square volume Kalman filtering
CN108469609B (en) Detection information filtering method for radar target tracking
CN111765883B (en) Robot Monte Carlo positioning method, equipment and storage medium
CN108490429B (en) TWS Radar Multi Target tracking and system
CN110233608A (en) A kind of particle filter method and radar system based on Weight number adaptively
CN105353353B (en) Multiple search particle probabilities assume the multi-object tracking method of density filtering
CN117075631A (en) Detection tracking identification method based on unmanned aerial vehicle cluster target
CN114710744B (en) Indoor positioning method integrating WiFi ranging and PDR calculation in depth
CN116437290A (en) Model fusion method based on CSI fingerprint positioning
CN112880659B (en) Fusion positioning method based on information probability
Chandio et al. Towards adaptable and tunable cloud-based map-matching strategy for GPS trajectories
Huang et al. Developing map matching algorithm for transportation data center
CN114091578A (en) Ship track clustering method based on curve length distance
CN111954158A (en) RSS map-based joint filtering indoor single-target tracking method, device and medium

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

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

TR01 Transfer of patent right