CN107132504B - Particle filter-based positioning and tracking device and method and electronic equipment - Google Patents
Particle filter-based positioning and tracking device and method and electronic equipment Download PDFInfo
- Publication number
- CN107132504B CN107132504B CN201610112949.XA CN201610112949A CN107132504B CN 107132504 B CN107132504 B CN 107132504B CN 201610112949 A CN201610112949 A CN 201610112949A CN 107132504 B CN107132504 B CN 107132504B
- Authority
- CN
- China
- Prior art keywords
- particle
- position information
- path
- target terminal
- current moment
- 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
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
Abstract
The embodiment of the invention provides a particle filter-based positioning and tracking device, a particle filter-based positioning and tracking method and electronic equipment, wherein map matching query is carried out according to initial position information of a target terminal at the current moment and position information of the target terminal at the previous moment to obtain path constraint information at the current moment, the weights of all particles are updated according to the path constraint information, a posterior probability density function of the target terminal to a state can be more approximated, so that the positioning precision is improved, meanwhile, the weight updating of all particles can be realized only by carrying out map matching query once at the current moment, the calculation complexity is greatly reduced, and the performance of real-time positioning and tracking is effectively improved.
Description
Technical Field
The present invention relates to the field of communications technologies, and in particular, to a positioning and tracking apparatus and method based on particle filtering, and an electronic device.
Background
In recent years, demand for location-based services has increased, and the application of the localization tracking technology has also become widespread. The filtering technique is beneficial to improving the accuracy performance of dynamic continuous positioning tracking.
Currently, commonly used filtering techniques for localization tracking include kalman filtering, particle filtering, and the like, where the kalman filtering is limited to a linear gaussian system, and the particle filtering technique has a better filtering effect in a nonlinear non-gaussian system. In the wireless positioning and tracking technology, the particle filtering technology is also widely applied, wherein particles at invalid positions can be eliminated by combining with map information, and the positioning and tracking precision is improved.
Fig. 1 is a schematic diagram of a conventional particle filter-based localization tracking method. As shown in fig. 1, the method includes: step 101: acquiring initial position information of a target terminal by using the existing positioning method according to the characteristic parameters of the received signals; step 102: initializing a particle swarm; step 103: obtaining the state transition probability according to the first-order Markov chain and the motion model, thereby predicting the position of each particle in the particle swarm at the current moment; step 104: updating the weight according to the initial position information of the target terminal and the predicted position of each particle at the current moment, wherein map matching query is carried out on each particle, and the particles at invalid positions are eliminated; step 105: and calculating the position information of the target terminal at the current time according to the updated weight of each particle and the predicted position of each particle.
It should be noted that the above background description is only for the sake of clarity and complete description of the technical solutions of the present invention and for the understanding of those skilled in the art. Such solutions are not considered to be known to the person skilled in the art merely because they have been set forth in the background section of the invention.
Disclosure of Invention
When the particle filter-based positioning and tracking method is used for carrying out real-time positioning and tracking on a target terminal, because the particle filter technology needs to generate a large number of particles, and when the weight of the particles is updated at the current moment, map matching query needs to be carried out on each particle, so that the calculated amount is greatly increased, and the performance of real-time positioning and tracking is reduced.
The embodiment of the invention provides a particle filter-based positioning and tracking device, a particle filter-based positioning and tracking method and electronic equipment, wherein map matching query is carried out according to initial position information of a target terminal at the current moment and position information of the target terminal at the previous moment to obtain path constraint information at the current moment, the weights of all particles are updated according to the path constraint information, and the path constraint information can be closer to a posterior probability density function of a state of the target terminal, so that the positioning precision is improved.
According to a first aspect of the embodiments of the present invention, there is provided a particle filter-based localization tracking apparatus, including: the initialization unit is used for initializing the particle swarm and estimating initial position information of the target terminal at the current moment; a prediction unit configured to predict position information of each particle in the particle group at a current time; the updating unit is used for carrying out map matching query according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment so as to obtain the path constraint information at the current moment; the updating unit is used for updating the weight of each particle according to the path constraint information at the current moment and the weight of each particle at the previous moment to obtain the weight of each particle at the current moment; and the calculating unit is used for calculating the position information of the target terminal at the current time according to the weight of each particle at the current time and the predicted position information of each particle at the current time.
According to a second aspect of embodiments of the present invention, there is provided an electronic device comprising the apparatus according to the first aspect of embodiments of the present invention.
According to a third aspect of the embodiments of the present invention, there is provided a particle filter-based localization tracking method, including: initializing a particle swarm, and estimating initial position information of a target terminal at the current moment; predicting the position information of each particle in the particle swarm at the current moment; performing map matching query according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment to obtain path constraint information at the current moment; updating the weight of each particle according to the path constraint information at the current moment and the weight of each particle at the previous moment to obtain the weight of each particle at the current moment; and calculating the position information of the target terminal at the current time according to the weight of each particle at the current time and the predicted position information of each particle at the current time.
The invention has the beneficial effects that: the method comprises the steps of carrying out map matching query according to initial position information of a target terminal at the current moment and position information of the target terminal at the previous moment to obtain path constraint information of the current moment, updating weights of all particles according to the path constraint information, and being capable of approaching to a posterior probability density function of the state of the target terminal, so that positioning accuracy is improved.
Specific embodiments of the present invention are disclosed in detail with reference to the following description and drawings, indicating the manner in which the principles of the invention may be employed. It should be understood that the embodiments of the invention are not so limited in scope. The embodiments of the invention include many variations, modifications and equivalents within the spirit and scope of the appended claims.
Features that are described and/or illustrated with respect to one embodiment may be used in the same way or in a similar way in one or more other embodiments, in combination with or instead of the features of the other embodiments.
It should be emphasized that the term "comprises/comprising" when used herein, is taken to specify the presence of stated features, integers, steps or components but does not preclude the presence or addition of one or more other features, integers, steps or components.
Drawings
The accompanying drawings, which are included to provide a further understanding of the embodiments of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the principles of the invention. It is obvious that the drawings in the following description are only some embodiments of the invention, and that for a person skilled in the art, other drawings can be derived from them without inventive effort. In the drawings:
FIG. 1 is a schematic diagram of a conventional particle filter-based localization tracking method;
fig. 2 is a schematic diagram of a particle filter-based positioning and tracking apparatus according to embodiment 1 of the present invention;
FIG. 3 is a diagram of the prediction unit 202 according to embodiment 1 of the present invention;
fig. 4 is a schematic diagram of the obtaining unit 203 according to embodiment 1 of the present invention;
fig. 5 is a schematic view of an electronic device according to embodiment 2 of the present invention;
fig. 6 is a schematic block diagram of a system configuration of an electronic apparatus according to embodiment 2 of the present invention;
FIG. 7 is a schematic diagram of a particle filter-based localization tracking method according to embodiment 3 of the present invention;
fig. 8 is a schematic diagram of a particle filter-based localization tracking method according to embodiment 4 of the present invention.
Detailed Description
The foregoing and other features of the invention will become apparent from the following description taken in conjunction with the accompanying drawings. In the description and drawings, particular embodiments of the invention have been disclosed in detail as being indicative of some of the embodiments in which the principles of the invention may be employed, it being understood that the invention is not limited to the embodiments described, but, on the contrary, is intended to cover all modifications, variations, and equivalents falling within the scope of the appended claims.
Example 1
Fig. 2 is a schematic diagram of a particle filter-based positioning and tracking apparatus according to embodiment 1 of the present invention. As shown in fig. 2, the apparatus 200 includes:
an initialization unit 201, configured to initialize a particle swarm and estimate initial position information of a target terminal at a current time;
a prediction unit 202 configured to predict position information of each particle in the particle group at the current time;
an obtaining unit 203, configured to perform map matching query according to initial position information of the target terminal at the current time and position information of the target terminal at the previous time, so as to obtain path constraint information at the current time;
an updating unit 204, configured to update the weight of each particle according to the path constraint information at the current time and the weight of each particle at the previous time, so as to obtain the weight of each particle at the current time;
calculating unit 205 is configured to calculate the position information of the target terminal at the current time based on the weight of each particle at the current time and the predicted position information of each particle at the current time.
In this embodiment, the positioning and tracking device 200 obtains the position information of the target terminal at the current time, and obtains the position information of each time within a period of time by the same method, so as to perform real-time dynamic tracking and positioning on the target terminal.
According to the embodiment, map matching query is carried out according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment, the path constraint information at the current moment is obtained, the weights of all the particles are updated according to the path constraint information, the posterior probability density function of the target terminal to the state can be more approximated, and therefore the positioning precision is improved.
In the present embodiment, the initialization unit 201 is configured to initialize the particle swarm and estimate the initial position information of the target terminal at the current time, wherein the initialization of the particle swarm and the estimation of the initial position information of the target terminal can be performed by using the existing method.
For example, when initializing the particle group, N particles may be uniformly distributed at random in the entire positioning region, and the initial weight of each particle may be set to
Is the weight of the ith particle at the initial moment, N, i is a positive integer, and i is less than or equal to N.
For example, when initializing the particle group, initial position information of the target terminal may be obtained by a conventional method, and the initial position information may be located in a region having the position as a center and a radius of R as a radiusN particles are uniformly distributed in the domain, wherein R is larger than the maximum initial positioning error, and the initial weight of each particle is
Is the weight of the ith particle at the initial moment, N, i is a positive integer, and i is less than or equal to N.
In this embodiment, the initialization of the particle swarm and the order of the initial position information of the estimation target terminal at the current time are not limited, for example, in the above method for initializing the first particle swarm, the initialization of the particle swarm may be performed first, and then the initial position information of the estimation target terminal at the current time is estimated, and in the above method for initializing the second particle swarm, the initial position information of the estimation target terminal at the current time needs to be estimated first, and then the initialization of the particle swarm is performed based on the initial position.
In this example, when estimating the initial location information of the target terminal at the current time, the initial location in the target may be determined by using an existing positioning method such as a multilateration method, a fingerprint positioning method, and a region positioning method, according to the characteristic parameters received at the current time, such as one or more of received signal field strength (RSS), time of arrival (TOA), time difference of arrival (TDOA), angle of arrival (AOA), and inertial navigation information.
In the embodiment, the number N of the particles in the initialized particle group can be set according to actual needs, for example, N is a value of 500 to 1000.
In the present embodiment, the prediction unit 202 is configured to predict the position information of each particle in the particle group at the current time, and the following description exemplifies the structure of the prediction unit 202 and the method for predicting the position information of each particle at the current time.
Fig. 3 is a schematic diagram of the prediction unit 202 according to embodiment 1 of the present invention. As shown in fig. 3, the prediction unit 202 includes:
an estimating unit 301, configured to obtain a step length estimate and a heading estimate at a current time;
a value taking unit 302, configured to take values of the step length estimation and the heading estimation uniformly within a predetermined range;
the first prediction unit 303 is configured to predict, according to each step length estimation and the heading estimation after the uniform value taking, position information of each particle at the current time.
In this embodiment, the estimation unit 301 may obtain the step size estimation and the heading estimation at the current time using the existing method. For example, when the target terminal has an inertial navigation system, the step length estimation and the heading estimation can be determined according to the output of the inertial navigation system; in addition, the step size estimate and the heading estimate may also be obtained from an empirical motion model.
In this embodiment, the value taking unit 302 uniformly takes values of the step length estimation and the heading estimation in a predetermined range, wherein when the target terminal has an inertial navigation system, the predetermined range can be determined according to the step length estimation and the heading estimation obtained from the output of the inertial navigation system;
when the target terminal does not have the inertial navigation system, regarding the step length estimation, a range from zero to the maximum possible step length is taken as the predetermined range, and regarding the course estimation, a range of course from zero to 360 degrees is taken as the predetermined range.
In this embodiment, the value obtaining unit 302 obtains the step length estimation and the heading estimation uniformly within a predetermined range, for example, the predetermined range is 0 to 1 for the step length estimation, and the number N of particles in the particle group is 10, then the step length estimation after uniform value obtaining is 0.1,0.2,0.3, … …,0.9,1.0 respectively, for each particle, the step lengths may be randomly assigned to each particle in the order of the particle, or any one of the step length estimations may be randomly assigned to any one particle.
In this embodiment, the position information of each particle at the current time can be predicted by using a conventional method, for example, the position information of each particle at the current time can be predicted by using the following formula (1):
wherein the content of the first and second substances,
indicating the position of the ith particle at time k (i.e., the current time), which may be expressed in terms of coordinates
It is shown that,
indicating the position of the ith particle at time k-1 (i.e. the last time),
representing the step size estimate of the ith particle at time k,
representing the heading estimate of the ith particle at time k,
obey U (min (v)k-Δv,0),vkA uniform distribution of + Δ v),
obey U (alpha)k-Δα,αk+ Δ α), Δ v represents the step maximum estimation error, Δ α represents the heading maximum estimation error, both Δ v and Δ α are greater than 0, vkRepresenting the step size estimate at time k, alpha, obtained by an inertial navigation system or empirical motion modelkRepresents the course estimate at time k, obtained by an inertial navigation system or empirical motion model, i, k being a positive integer.
Therefore, the step length estimation and the course estimation are uniformly valued in a preset range, and the position information of each particle at the current moment is predicted according to each step length estimation and course estimation after uniform value taking, so that the particle degradation effect can be improved when a motion model is inaccurate, and the robustness of the positioning and tracking performance is improved.
In this embodiment, the obtaining unit 203 performs map matching query according to the initial position information of the target terminal at the current time and the position information of the target terminal at the previous time, so as to obtain the path constraint information at the current time.
In the present embodiment, the map information used for the map matching query is map information obtained in advance, for example, a database containing maps or the like.
In this embodiment, the path constraint information may be a conditional probability model of the path constraint, for example, the conditional probability model of the path constraint is a gaussian probability model with a path center as a mean and a path width as a standard deviation.
Fig. 4 is a schematic diagram of the obtaining unit 203 according to embodiment 1 of the present invention. As shown in fig. 4, the acquisition unit 203 includes:
the query unit 401 is configured to query map path information between the two positions according to initial position information of the target terminal at the current time and position information of the target terminal at the previous time, and obtain a path center curve and a path width at the current time;
the establishing unit 402 is configured to establish a conditional probability model of the path constraint at the current time according to the path center curve and the path width.
In this embodiment, after the obtaining unit 203 obtains the path constraint information at the current time, the updating unit 204 is configured to update the weight of each particle according to the path constraint information at the current time and the weight of each particle at the previous time, so as to obtain the weight of each particle at the current time, for example, the updating unit 204 may update the weight of each particle by using the following formula (2):
wherein the content of the first and second substances,
representing the weight of the ith particle at time k,
representing the weight of the ith particle at time k-1,
indicating that the i-th particle is approximately equal to the posterior probability at time k
The prior probability of (a) being,
representing the conditional probability of the path constraint for the ith particle at time k,
initial position information representing the target terminal,
the position information of the ith particle at the current moment is obtained through prediction, f (x, y) represents a path center curve, sigma (x, y) represents the path width, and i and k are positive integers.
The following describes an exemplary method for acquiring path constraint information and updating weights of particles according to this embodiment by using specific examples.
First, a path center curve of M paths is constructed, fj(x, y) is the jth path center curve, where the curve can be set to y ═ a0+a1x+a2x2+…，akIs a coefficient, k is an integer, and the path width is σjM, j is a positive integer, and j is less than or equal to M.
Then, the result of the position estimation at the previous time can be obtained from the following equation (3)
Calculate its distance to each path center curve:
wherein the content of the first and second substances,
for the separation on the ith path curve
Nearest point, fi(x, y) 0 represents [ x, y ]]At any point on the ith path curve, djRepresents the distance from the position estimation result at the last moment to the jth path center curve,
indicating the position estimation result of the previous time, which is the initial position at the initial time
Using the output result of the previous moment at each later moment
i, j are positive integers.
In this embodiment, a newton method or a lagrange-newton method may be used to solve the optimal solution of the linear constraint or the nonlinear constraint, and then a constraint path satisfying the following formula (4) is selected:
where C represents the set of all possible paths for selecting at least one path among {1,2, …, M } paths,
represents that one of all path sets is selected so that sigma in formula (4)j∈C(dj-α×σj) The set of paths corresponding to the smallest value of djThe distance from the position estimation result at the previous moment to the jth path center curve is represented, alpha represents a weighting coefficient of a preset path width, for example, alpha takes a value of 0.5-1，σjRepresents the path width of the jth path, j being a positive integer.
Wherein the function realized by the formula (4) is to select all the curve distances from the center of the path to be less than alpha multiplied by sigmaiWhen there is no path smaller than α × σiThe nearest path is selected as the constrained path information.
Through the steps, the weight of the ith particle is calculated according to the selected path constraint, and is expressed by the following formula (5):
wherein the content of the first and second substances,
representing the weight of the ith particle at time k,
representing the weight of the ith particle at time k-1,
indicating that the i-th particle is approximately equal to the posterior probability at time k
A priori probability of (a), (b), (c), (d) and (d)j(x, y) 0 represents [ x, y ]]At any point on the jth path curve, djRepresenting the distance from the position estimation result at the previous moment to the jth path center curve, wherein alpha represents a weighting coefficient of a preset path width, for example, alpha takes 0.5-1, and sigmajThe width of the jth path is shown, i, j, N, k are positive integers, i is less than or equal toN，j≤N。
Obtaining the weight of each particle at time k, i.e. at the current time
Thereafter, the updating unit 204 may also update the weight of each particle
The normalization process is performed, and for example, the normalized weight of each particle can be calculated according to the following formula (6):
wherein the content of the first and second substances,
representing the normalized weight of the ith particle at time k,
representing the weight of the ith particle at time k,
and the weight of the jth particle at the moment k is shown, i, j, N, k is a positive integer, i is less than or equal to N, and j is less than or equal to N.
In this embodiment, the calculation unit 205 calculates the position information of the target terminal at the current time based on the weight of each particle at the current time and the predicted position information of each particle at the current time. The weight of each particle may use a normalized weight.
For example, the calculation unit 205 may calculate the position information of the target terminal at the current time according to the following formula (7):
wherein the content of the first and second substances,
indicating the location information of the target terminal at time k,
representing the normalized weight of the ith particle at time k,
and the position information of the ith particle at the moment k is shown, i, N and k are positive integers, and i is less than or equal to N.
In this embodiment, as shown in fig. 2, the positioning and tracking apparatus 200 may further include:
an evaluation unit 206, configured to evaluate the degree of particle degradation after obtaining the location information of the target terminal at the current time, where the evaluation of the degree of particle degradation may be performed using an existing method. In this embodiment, the evaluation unit 206 is an optional component, indicated by a dashed box in fig. 2.
For example, the evaluation unit 206 determines the valid sampling parameter NeffIf it is less than the predetermined threshold, when the effective sampling parameter N is less than the predetermined thresholdeffIf the value is smaller than the predetermined threshold, it is determined that the particle degradation is serious and the resampling of the particles is necessary, and the prediction unit 202 predicts the position information of each particle in the particle group at the next time based on the resampled particles.
In this embodiment, the effective sampling parameter N can be calculated using the existing methodeffFor example, the effective sampling parameter N can be calculated using the following equation (8)eff：
Wherein the content of the first and second substances,
and the weight of the ith particle after normalization at the time k is shown, i, N and k are positive integers, and i is less than or equal to N.
In the present embodiment, the particle resampling may use an existing method, such as polynomial resampling, residual resampling, hierarchical resampling, and system resampling.
According to the embodiment, map matching query is carried out according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment, the path constraint information at the current moment is obtained, the weights of all the particles are updated according to the path constraint information, the posterior probability density function of the target terminal to the state can be more approximated, and therefore the positioning precision is improved.
In addition, the step length estimation and the course estimation are uniformly valued in a preset range, and the position information of each particle at the current moment is predicted according to each step length estimation and course estimation after uniform value taking, so that the particle degradation effect can be improved when a motion model is inaccurate, and the robustness of the positioning and tracking performance is improved.
Example 2
An embodiment of the present invention further provides an electronic device, and fig. 5 is a schematic diagram of the electronic device in embodiment 2 of the present invention. As shown in fig. 5, the electronic device 500 includes a particle filter-based location tracking apparatus 501, wherein the structure and function of the particle filter-based location tracking apparatus 501 are the same as those described in embodiment 1, and are not described herein again.
Fig. 6 is a schematic block diagram of a system configuration of an electronic apparatus according to embodiment 2 of the present invention. As shown in fig. 6, the electronic device 600 may include a central processor 601 and a memory 602; the memory 602 is coupled to the central processor 601. The figure is exemplary; other types of structures may also be used in addition to or in place of the structure to implement telecommunications or other functions.
As shown in fig. 6, the electronic device 600 may further include: an input unit 603, a display 604, and a power supply 605.
In one embodiment, the function of the particle filter based localization tracking device described in example 1 can be integrated into the central processor 601. Wherein, the central processor 601 may be configured to: initializing a particle swarm, and estimating initial position information of a target terminal at the current moment; predicting the position information of each particle in the particle swarm at the current moment; performing map matching query according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment to obtain path constraint information at the current moment; updating the weight of each particle according to the path constraint information at the current moment and the weight of each particle at the previous moment to obtain the weight of each particle at the current moment; and calculating the position information of the target terminal at the current time according to the weight of each particle at the current time and the predicted position information of each particle at the current time.
The map matching query is performed according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment to obtain the path constraint information at the current moment, and the method includes: inquiring map path information between the two positions according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment, and obtaining a path center curve and a path width at the current moment; and establishing a conditional probability model of the path constraint at the current moment according to the path center curve and the path width.
The conditional probability model of the path constraint is a gaussian probability model taking the path center as a mean value and the path width as a standard deviation.
Wherein the predicting the position information of each particle in the particle swarm at the current moment comprises: obtaining step length estimation and course estimation at the current moment; uniformly taking values of the step length estimation and the course estimation in a preset range; and predicting the position information of each particle at the current moment according to each step length estimation and the course estimation after uniform value taking.
In another embodiment, the particle filter-based localization and tracking apparatus described in embodiment 1 may be configured separately from the central processing unit 601, for example, the particle filter-based localization and tracking apparatus may be configured as a chip connected to the central processing unit 601, and the function of the particle filter-based localization and tracking apparatus is realized by the control of the central processing unit 601.
It is not necessary for the electronic device 600 to include all of the components shown in fig. 6 in this embodiment.
As shown in fig. 6, a central processing unit 601, sometimes referred to as a controller or operational control, may include a microprocessor or other processor device and/or logic device, where the central processing unit 601 receives input and controls the operation of the various components of the electronic device 600.
The memory 602, for example, may be one or more of a buffer, a flash memory, a hard drive, a removable media, a volatile memory, a non-volatile memory, or other suitable device. And the central processor 601 may execute the program stored in the memory 602 to realize information storage or processing, or the like. The functions of other parts are similar to the prior art and are not described in detail here. The various components of electronic device 600 may be implemented in dedicated hardware, firmware, software, or combinations thereof, without departing from the scope of the invention.
According to the embodiment, map matching query is carried out according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment, the path constraint information at the current moment is obtained, the weights of all the particles are updated according to the path constraint information, the posterior probability density function of the target terminal to the state can be more approximated, and therefore the positioning precision is improved.
In addition, the step length estimation and the course estimation are uniformly valued in a preset range, and the position information of each particle at the current moment is predicted according to each step length estimation and course estimation after uniform value taking, so that the particle degradation effect can be improved when a motion model is inaccurate, and the robustness of the positioning and tracking performance is improved.
Example 3
The embodiment of the invention also provides a particle filter-based positioning and tracking method, which corresponds to the particle filter-based positioning and tracking device of the embodiment 1. Fig. 7 is a schematic diagram of a particle filter-based localization tracking method according to embodiment 3 of the present invention. As shown in fig. 7, the method includes:
step 701: initializing a particle swarm, and estimating initial position information of a target terminal at the current moment;
step 702: predicting the position information of each particle in the particle swarm at the current moment;
step 703: performing map matching query according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment to obtain path constraint information at the current moment;
step 704: updating the weight of each particle according to the path constraint information at the current moment and the weight of each particle at the previous moment to obtain the weight of each particle at the current moment;
step 705: and calculating the position information of the target terminal at the current time according to the weight of each particle at the current time and the predicted position information of each particle at the current time.
In this embodiment, a method of performing particle group initialization, a method of estimating initial position information, a method of predicting position information of each particle, a method of obtaining path constraint information, a method of obtaining a weight of each particle, and a method of calculating position information of a target terminal are the same as those described in embodiment 1, and are not described herein again.
According to the embodiment, map matching query is carried out according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment, the path constraint information at the current moment is obtained, the weights of all the particles are updated according to the path constraint information, the posterior probability density function of the target terminal to the state can be more approximated, and therefore the positioning precision is improved.
Example 4
The embodiment of the invention also provides a particle filter-based positioning and tracking method, which corresponds to the particle filter-based positioning and tracking device of the embodiment 1. Fig. 8 is a schematic diagram of a particle filter-based localization tracking method according to embodiment 4 of the present invention. As shown in fig. 8, the method includes:
step 801: initializing a particle swarm;
step 802: estimating initial position information of a target terminal at a moment k, wherein k is a positive integer;
step 803: predicting the position information of each particle in the particle swarm at the k moment;
step 804: performing map matching query according to the initial position information of the target terminal at the time k and the position information of the target terminal at the time k-1 to obtain path constraint information at the time k;
step 805: updating the weight of each particle according to the path constraint information at the time k and the weight of each particle at the time k-1 to obtain the weight of each particle at the time k;
step 806: calculating the position information of the target terminal at the k moment according to the weight of each particle at the k moment and the predicted position information of each particle at the k moment;
step 807: judging whether the positioning tracking is needed to be continued or not, entering a step 808 when the judgment result is yes, and ending the process when the judgment result is no;
step 808: judging whether the particles are seriously degraded, if so, entering step 809, and if not, entering step 810;
step 809: carrying out particle resampling;
step 810: k is added to 1, i.e., k equals k + 1.
In this embodiment, in step 807, it is determined whether the need for continuing the localization tracking can be determined according to actual needs, for example, the user sets the time that the localization tracking is needed, and when the end time of the time is reached, the localization tracking is stopped, thereby ending the process.
In this embodiment, a method of performing particle group initialization, a method of estimating initial position information, a method of predicting position information of each particle, a method of obtaining path constraint information, a method of obtaining a weight of each particle, a method of calculating position information of a target terminal, a method of determining whether or not a particle is seriously degraded, and a method of performing particle resampling are the same as those described in embodiment 1, and are not described again here.
According to the embodiment, map matching query is carried out according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment, the path constraint information at the current moment is obtained, the weights of all the particles are updated according to the path constraint information, the posterior probability density function of the target terminal to the state can be more approximated, and therefore the positioning precision is improved.
An embodiment of the present invention further provides a computer-readable program, wherein when the program is executed in a particle filter based localization and tracking apparatus or an electronic device, the program causes a computer to execute the particle filter based localization and tracking method according to embodiment 3 or embodiment 4 in the particle filter based localization and tracking apparatus or the electronic device.
An embodiment of the present invention further provides a storage medium storing a computer-readable program, where the computer-readable program enables a computer to execute the particle filter-based localization and tracking method according to embodiment 3 or embodiment 4 in a particle filter-based localization and tracking apparatus or an electronic device.
The particle filter based localization and tracking method performed in the particle filter based localization and tracking device or the electronic device described in connection with the embodiments of the present invention may be directly embodied as hardware, a software module executed by a processor, or a combination of the two. For example, one or more of the functional block diagrams and/or one or more combinations of the functional block diagrams illustrated in fig. 2 may correspond to individual software modules of a computer program flow or may correspond to individual hardware modules. These software modules may correspond to the steps shown in fig. 7, respectively. These hardware modules may be implemented, for example, by solidifying these software modules using a Field Programmable Gate Array (FPGA).
A software module may reside in RAM memory, flash memory, ROM memory, EPROM memory, EEPROM memory, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art. A storage medium may be coupled to the processor such that the processor can read information from, and write information to, the storage medium; or the storage medium may be integral to the processor. The processor and the storage medium may reside in an ASIC. The software module may be stored in the memory of the mobile terminal or in a memory card that is insertable into the mobile terminal. For example, if the apparatus (e.g., mobile terminal) employs a relatively large capacity MEGA-SIM card or a large capacity flash memory device, the software module may be stored in the MEGA-SIM card or the large capacity flash memory device.
One or more of the functional block diagrams and/or one or more combinations of the functional block diagrams described with respect to fig. 2 may be implemented as a general purpose processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field Programmable Gate Array (FPGA) or other programmable logic device, discrete gate or transistor logic, discrete hardware components, or any suitable combination thereof designed to perform the functions described herein. One or more of the functional block diagrams and/or one or more combinations of the functional block diagrams described with respect to fig. XXXX may also be implemented as a combination of computing devices, e.g., a combination of a DSP and a microprocessor, a plurality of microprocessors, one or more microprocessors in communication with a DSP, or any other such configuration.
While the invention has been described with reference to specific embodiments, it will be apparent to those skilled in the art that these descriptions are illustrative and not intended to limit the scope of the invention. Various modifications and alterations of this invention will become apparent to those skilled in the art based upon the spirit and principles of this invention, and such modifications and alterations are also within the scope of this invention.
Claims (9)
1. A particle filter based position tracking device, comprising:
the initialization unit is used for initializing the particle swarm and estimating initial position information of the target terminal at the current moment;
a prediction unit configured to predict position information of each particle in the particle group at a current time;
the acquisition unit is used for carrying out map matching query according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment so as to acquire the path constraint information at the current moment;
the updating unit is used for updating the weight of each particle according to the path constraint information at the current moment and the weight of each particle at the previous moment to obtain the weight of each particle at the current moment;
a calculating unit, configured to calculate, according to the weight of each particle at the current time and the predicted position information of each particle at the current time, position information of the target terminal at the current time;
wherein the updating unit updates the weight of each particle using the following formula (1):
wherein the content of the first and second substances,
representing the weight of the ith particle at time k,
indicates the ith particle at time k-1The weight of (a) is determined,
representing the prior probability of the ith particle at time k,
representing the conditional probability of the path constraint for the ith particle at time k,
initial position information representing the target terminal,
the position information of the ith particle at the current time is obtained by prediction, f (x, y) represents a path center curve, sigma (x, y) represents the path width, and i and k are positive integers.
2. The apparatus of claim 1, wherein the obtaining unit comprises:
the query unit is used for querying the map path information between the two positions according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment, and acquiring a path center curve and a path width at the current moment;
and the establishing unit is used for establishing a conditional probability model of the path constraint at the current moment according to the path center curve and the path width.
3. The apparatus of claim 2, wherein the path-constrained conditional probability model is a gaussian probability model with a path center as a mean and the path width as a standard deviation.
4. The apparatus of claim 1, wherein the prediction unit comprises:
the estimation unit is used for obtaining step length estimation and course estimation at the current moment;
the value taking unit is used for uniformly taking values of the step length estimation and the course estimation in a preset range;
and the first prediction unit is used for predicting the position information of each particle at the current moment according to each step length estimation and the course estimation after uniform value taking.
5. An electronic device comprising the apparatus of any of claims 1-4.
6. A particle filter-based localization tracking method comprises the following steps:
initializing a particle swarm, and estimating initial position information of a target terminal at the current moment;
predicting the position information of each particle in the particle swarm at the current moment;
performing map matching query according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment to obtain path constraint information at the current moment;
updating the weight of each particle according to the path constraint information at the current moment and the weight of each particle at the previous moment to obtain the weight of each particle at the current moment;
calculating the position information of the target terminal at the current moment according to the weight of each particle at the current moment and the predicted position information of each particle at the current moment;
wherein the weight of each particle is updated using the following equation (1):
wherein the content of the first and second substances,
indicates the ith granuleThe weight of the child at time k is,
representing the weight of the ith particle at time k-1,
representing the prior probability of the ith particle at time k,
representing the conditional probability of the path constraint for the ith particle at time k,
initial position information representing the target terminal,
the position information of the ith particle at the current time is obtained by prediction, f (x, y) represents a path center curve, sigma (x, y) represents the path width, and i and k are positive integers.
7. The method of claim 6, wherein the performing a map matching query according to the initial location information of the target terminal at the current time and the location information of the target terminal at the previous time to obtain the path constraint information at the current time comprises:
inquiring map path information between the two positions according to the initial position information of the target terminal at the current moment and the position information of the target terminal at the previous moment, and obtaining a path center curve and a path width at the current moment;
and establishing a conditional probability model of the path constraint at the current moment according to the path center curve and the path width.
8. The method of claim 7, wherein the path-constrained conditional probability model is a gaussian probability model with a path center as a mean and the path width as a standard deviation.
9. The method of claim 6, wherein said predicting location information for each particle in the population at a current time comprises:
obtaining step length estimation and course estimation at the current moment;
uniformly taking values of the step length estimation and the course estimation in a preset range;
and predicting the position information of each particle at the current moment according to each step length estimation and the course estimation after uniform value taking.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610112949.XA CN107132504B (en) | 2016-02-29 | 2016-02-29 | Particle filter-based positioning and tracking device and method and electronic equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610112949.XA CN107132504B (en) | 2016-02-29 | 2016-02-29 | Particle filter-based positioning and tracking device and method and electronic equipment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107132504A CN107132504A (en) | 2017-09-05 |
| CN107132504B true CN107132504B (en) | 2020-12-22 |
Family
ID=59721425
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610112949.XA Active CN107132504B (en) | 2016-02-29 | 2016-02-29 | Particle filter-based positioning and tracking device and method and electronic equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107132504B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111174777A (en) * | 2018-11-09 | 2020-05-19 | 阿里巴巴集团控股有限公司 | Positioning method and device and electronic equipment |
| CN111665470A (en) * | 2019-03-07 | 2020-09-15 | 阿里巴巴集团控股有限公司 | Positioning method and device and robot |
| CN110493731B (en) * | 2019-09-09 | 2022-03-29 | 腾讯科技（深圳）有限公司 | Movement track obtaining method and device, storage medium and equipment |
| CN111132029B (en) * | 2019-10-16 | 2022-03-01 | 张苏 | Positioning method and device based on terrain constraint |
| CN111044050B (en) * | 2019-12-30 | 2022-06-21 | 中电海康集团有限公司 | Bluetooth positioning method based on particle filtering and Kalman filtering |
| CN111352066B (en) * | 2020-03-27 | 2022-02-22 | 西安震有信通科技有限公司 | Particle filter-based positioning method and device, computer equipment and storage medium |
| CN112218239B (en) * | 2020-09-04 | 2023-03-10 | 北京爱笔科技有限公司 | Position determination method, position determination device, computer equipment and storage medium |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101509781A (en) * | 2009-03-20 | 2009-08-19 | 同济大学 | Walking robot positioning system based on monocular cam |
| JP2009205666A (en) * | 2008-02-28 | 2009-09-10 | Hitachi Ltd | Method and apparatus for determining whether moving entity is moving in predetermined direction |
| CN101614817A (en) * | 2009-06-24 | 2009-12-30 | 北京航空航天大学 | A kind of multi-object tracking method based on ground moving target indication radar system |
| CN101644758A (en) * | 2009-02-24 | 2010-02-10 | 中国科学院声学研究所 | Target localization and tracking system and method |
| CN103440764A (en) * | 2013-08-19 | 2013-12-11 | 同济大学 | Urban road network vehicle travel path reconstruction method based on vehicle automatic identification data |
| CN103487047A (en) * | 2013-08-06 | 2014-01-01 | 重庆邮电大学 | Improved particle filter-based mobile robot positioning method |
| CN103644903A (en) * | 2013-09-17 | 2014-03-19 | 北京工业大学 | Simultaneous localization and mapping method based on distributed edge unscented particle filter |
| CN103983266A (en) * | 2014-05-28 | 2014-08-13 | 北京天地方元科技有限公司 | Indoor locating method based on geomagnetic information and indoor locating system based on geomagnetic information |
| CN104215238A (en) * | 2014-08-21 | 2014-12-17 | 北京空间飞行器总体设计部 | Indoor positioning method of intelligent mobile phone |
| CN104375117A (en) * | 2013-08-12 | 2015-02-25 | 无锡知谷网络科技有限公司 | Target locating method and system |
| CN105022055A (en) * | 2015-07-05 | 2015-11-04 | 吉林大学 | IMU indoor positioning method |
-
2016
- 2016-02-29 CN CN201610112949.XA patent/CN107132504B/en active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2009205666A (en) * | 2008-02-28 | 2009-09-10 | Hitachi Ltd | Method and apparatus for determining whether moving entity is moving in predetermined direction |
| CN101644758A (en) * | 2009-02-24 | 2010-02-10 | 中国科学院声学研究所 | Target localization and tracking system and method |
| CN101509781A (en) * | 2009-03-20 | 2009-08-19 | 同济大学 | Walking robot positioning system based on monocular cam |
| CN101614817A (en) * | 2009-06-24 | 2009-12-30 | 北京航空航天大学 | A kind of multi-object tracking method based on ground moving target indication radar system |
| CN103487047A (en) * | 2013-08-06 | 2014-01-01 | 重庆邮电大学 | Improved particle filter-based mobile robot positioning method |
| CN104375117A (en) * | 2013-08-12 | 2015-02-25 | 无锡知谷网络科技有限公司 | Target locating method and system |
| CN103440764A (en) * | 2013-08-19 | 2013-12-11 | 同济大学 | Urban road network vehicle travel path reconstruction method based on vehicle automatic identification data |
| CN103644903A (en) * | 2013-09-17 | 2014-03-19 | 北京工业大学 | Simultaneous localization and mapping method based on distributed edge unscented particle filter |
| CN103983266A (en) * | 2014-05-28 | 2014-08-13 | 北京天地方元科技有限公司 | Indoor locating method based on geomagnetic information and indoor locating system based on geomagnetic information |
| CN104215238A (en) * | 2014-08-21 | 2014-12-17 | 北京空间飞行器总体设计部 | Indoor positioning method of intelligent mobile phone |
| CN105022055A (en) * | 2015-07-05 | 2015-11-04 | 吉林大学 | IMU indoor positioning method |
Non-Patent Citations (3)
| Title |
|---|
| 基于粒子滤波的约束目标被动跟踪研究;薛锋等;《武汉理工大学学报（交通科学与工程版）》;20070228;第31卷(第1期);43-45,52 * |
| 基于规划路径约束的地形辅助导航算法;李世丹等;《中国惯性技术学报》;20120430;第20卷(第2期);196-199 * |
| 融合地理信息的粒子滤波被动跟踪算法;康健等;《火力与指挥控制》;20130731;第38卷(第7期);8-10 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107132504A (en) | 2017-09-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107132504B (en) | Particle filter-based positioning and tracking device and method and electronic equipment | |
| US10852139B2 (en) | Positioning method, positioning device, and robot | |
| CN105549049B (en) | A kind of adaptive Kalman filter algorithm applied to GPS navigation | |
| CN109883423B (en) | Positioning method, system, equipment and storage medium based on Kalman filtering | |
| CN107255795B (en) | Indoor mobile robot positioning method and device based on EKF/EFIR hybrid filtering | |
| WO2017088196A1 (en) | Fusion navigation device and method based on wireless fingerprints and mems sensor | |
| CN109982398B (en) | Indoor and outdoor seamless positioning method, device, equipment and medium | |
| CN108020813B (en) | Positioning method, positioning device and electronic equipment | |
| CN109959894B (en) | Positioning method and terminal equipment | |
| CN107796387B (en) | Positioning method, positioning device and electronic equipment | |
| CN109698999B (en) | Positioning method and terminal equipment | |
| CN113066127B (en) | Visual inertial odometer method and system for calibrating equipment parameters on line | |
| CN111225334B (en) | Positioning method, terminal and computer readable storage medium | |
| JP6284151B2 (en) | Position estimation apparatus, method, and program | |
| CN110572139A (en) | fusion filtering implementation method and device for vehicle state estimation, storage medium and vehicle | |
| CN109696654B (en) | Positioning method and terminal equipment | |
| CN106885567B (en) | Inertial navigation cooperation positioning method and positioning equipment | |
| WO2013096209A1 (en) | System and method for probablistic wlan positioning | |
| Farahmand et al. | Tracking target signal strengths on a grid using sparsity | |
| CN109541541B (en) | Indoor triangulation positioning precision correction method and device | |
| CN109945864B (en) | Indoor driving positioning fusion method and device, storage medium and terminal equipment | |
| CN109769206B (en) | Indoor positioning fusion method and device, storage medium and terminal equipment | |
| WO2014079245A1 (en) | Positioning method, device and system | |
| CN107548033B (en) | Positioning device and method and electronic equipment | |
| CN114894222B (en) | External parameter calibration method of IMU-GNSS antenna and related method and equipment |
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 |