CN112836354B - Target tracking and positioning method, system and device and readable storage medium - Google Patents
Target tracking and positioning method, system and device and readable storage medium Download PDFInfo
- Publication number
- CN112836354B CN112836354B CN202110038824.8A CN202110038824A CN112836354B CN 112836354 B CN112836354 B CN 112836354B CN 202110038824 A CN202110038824 A CN 202110038824A CN 112836354 B CN112836354 B CN 112836354B
- Authority
- CN
- China
- Prior art keywords
- target
- tracking
- estimation value
- constraint
- state estimation
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 101
- 230000033001 locomotion Effects 0.000 claims abstract description 24
- 238000005259 measurement Methods 0.000 claims abstract description 15
- 230000006870 function Effects 0.000 claims description 34
- 239000011159 matrix material Substances 0.000 claims description 27
- 230000008569 process Effects 0.000 claims description 22
- 238000004590 computer program Methods 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 5
- 238000000844 transformation Methods 0.000 claims description 5
- 230000007704 transition Effects 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 3
- 230000009897 systematic effect Effects 0.000 claims description 2
- 230000017105 transposition Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 5
- 230000009467 reduction Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000000691 measurement method Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000003745 diagnosis Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000014509 gene expression Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/23—Clustering techniques
- G06F18/232—Non-hierarchical techniques
- G06F18/2321—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2111/00—Details relating to CAD techniques
- G06F2111/04—Constraint-based CAD
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2111/00—Details relating to CAD techniques
- G06F2111/08—Probabilistic or stochastic CAD
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Evolutionary Computation (AREA)
- General Physics & Mathematics (AREA)
- General Engineering & Computer Science (AREA)
- Life Sciences & Earth Sciences (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Biology (AREA)
- Bioinformatics & Computational Biology (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Artificial Intelligence (AREA)
- Probability & Statistics with Applications (AREA)
- Computer Hardware Design (AREA)
- Geometry (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
The invention discloses a target tracking and positioning method, a system, a device and a readable storage medium, wherein the method comprises the following steps: constructing a motion model of a target and acquiring a tracking initial state of the target; wherein the motion model comprises a constraint condition, a state update equation of the target and a measurement update equation of the target; tracking and positioning the target by utilizing the Kalman filter with the constraint condition; the method comprises the steps of utilizing a Kalman filter to track to obtain a state estimation value of a target, then constraining the state estimation value of the target based on a probability density function truncation method to obtain a target state estimation value under constraint, and achieving target tracking. The method can improve the tracking precision of the maneuvering target by utilizing the road constraint information.
Description
Technical Field
The invention belongs to the technical field of target tracking, and particularly relates to a target tracking and positioning method, a system and a device and a readable storage medium.
Background
Target tracking and positioning are real-time estimation processes of the position, the speed and other motion parameters of a tracked target obtained through measurement data. The target tracking and positioning problem is embodied in a plurality of scientific and engineering fields such as environmental monitoring, network security, medical diagnosis, military reconnaissance and the like. The method comprises the following steps of tracking and positioning a maneuvering target, namely tracking vehicles running on a road. For example, vehicles traveling on a highway are constrained by the shape of the highway, which is typically a constrained maneuvering target tracking problem.
The kalman filter is one of the main tools used in the field of target tracking and localization, and it comprises two phases: and (4) predicting and updating. In the prediction phase, the filter makes a state estimate for the current time using the state estimate for the maneuver target at a previous time. In the updating stage, the filter optimizes the predicted value obtained in the predicting stage by using the observed value of the current state of the maneuvering target to obtain a more accurate new estimated value. Kalman filters and their extensions generally perform well for tracking moving targets under gaussian noise, but in the case of non-gaussian noise, performance can become worse, especially when the system is disturbed by impulse noise. It is common to encounter non-gaussian noise in real scenes for target tracking.
In recent years, the optimization criteria of information theory learning are more and more focused, and the information theory quantity (entropy) of data estimation is directly used instead of the common second-order statistical measurement (variance). The information theory quantity can capture high-order statistics and remarkably improve the performance of the information theory quantity in the machine learning and signal processing application. The correlation entropy is directly related to the information entropy, defines a measure of data space, and can represent a measure of similarity between two random variables. In supervised learning (e.g., regression), a problem can be formulated to maximize the entropy of correlation between the model output and the expected response. This optimization criterion is referred to as the maximum correlation entropy criterion in information theory learning. The maximum correlation entropy criterion has been successfully applied to the kalman filter, and therefore, how to apply it to the state estimation of a maneuvering target under non-gaussian noise is of great significance.
In addition, when the problem of tracking a road constraint target is solved, the conventional target tracking method has obvious defects: the conventional method does not contain prior information of road constraint conditions, so that information is wasted; secondly, the tracking result is difficult to satisfy the constraint condition, and the performance is lacked. Therefore, the road information is reasonably utilized, and the corresponding tracking method is provided, so that the tracking precision of the maneuvering target can be improved. However, there are some constraint target tracking methods, which incorporate constraint conditions into the estimator to solve the state estimation problem, and the following are common: model order reduction method, perfect measurement method and estimation projection method. The model order reduction method utilizes equality constraint to reduce the dimensionality of a state vector to obtain the optimal estimation of a simple model constraint state, but the physical meaning is unclear after the order reduction; the perfect measurement method introduces equality constraint as pseudo measurement without noise, converts the constrained state estimation problem into the conventional problem of observation dimension expansion, and solves the problem, but the calculation is complex and the numerical calculation problem is easy to cause.
Therefore, how to reasonably utilize the road information to carry out effective constraint to improve the precision of tracking the maneuvering target is particularly important for researching a target tracking method with road constraint under the condition of non-Gaussian noise.
Disclosure of Invention
The invention aims to provide a target tracking and positioning method, a target tracking and positioning system and a readable storage medium, which improve the tracking precision of a maneuvering target by utilizing road constraint information.
On one hand, the invention provides a target tracking and positioning method, which comprises the following steps:
constructing a motion model of a target and acquiring a tracking initial state of the target; wherein the motion model comprises a constraint condition, a state update equation of the target and a measurement update equation of the target;
tracking and positioning a target by utilizing a maximum correlation entropy criterion Kalman filter with the constraint condition;
the state estimation value of the target is obtained by tracking through a maximum correlation entropy criterion Kalman filter, and then the state estimation value of the target is restrained on the basis of a probability density function truncation method to obtain the state estimation value of the target under restraint, so that the target tracking is realized.
According to the method, the constraint condition is combined with the maximum correlation entropy criterion Kalman filter by using a probability density function truncation method to obtain a maximum correlation entropy criterion Kalman filter algorithm with constraint, and the tracking performance of the maneuvering target under non-Gaussian noise is improved. The probability density function of the state estimation of the target is cut off on the constrained boundary by using a probability density function cut-off method, and the obtained estimated value with constraint is closer to the true value, so that the accuracy of the estimated value is improved, and the tracking and positioning precision is improved. A brand new technical means is used for realizing the tracking and positioning problem of the Kalman filter with constraint conditions. And establishing a state updating equation of the target and a measurement updating equation of the target which are obtained in the motion model as configuration functions in the Kalman filter.
Alternatively,
in the process of obtaining the state estimation value of the target under constraint by constraining the state estimation value of the target based on the probability density function truncation method, the state constraint obeys the linear equation constraint as follows:
Mx k ＝m
where M is a constraint matrix, M is a constraint value, and x k The state estimation value of the target at the current k moment meets the linear equation constraint;
the initialization settings are as follows:
i denotes the first i constraints, the maximum of which is equal to the dimension q of m,
respectively using the state estimation value and covariance of the previous i constraint transformations at the current k moment;
P k|k respectively obtaining a state estimation value and a covariance of a target at the current k moment by utilizing a maximum correlation entropy criterion Kalman filter;
i is sequentially valued from 0 to q, and is iteratively calculated according to the following formula to finally obtain
As the state estimation value and covariance of the target under the constraint corresponding to the current k time, and the state estimation value of the target is used
As a result of the positioning of the target at the current k time, the iterative formula is as follows:
According to the formula, the state estimation value of the target obtained by directly tracking by using the Kalman filter is corrected, and the final output is finally calculated in an iterative manner
Wherein, will
As a result of the positioning of the target at the current time k.
Optionally, the tracking and positioning of the target by using the maximum entropy criterion kalman filter with the constraint condition
A: tracking by using a maximum correlation entropy criterion Kalman filter to obtain a state estimation value corresponding to the target at the current moment, and constraining the state estimation value of the target based on a probability density function truncation method to obtain a state estimation value corresponding to the target under constraint at the current moment;
then, utilizing a state estimation value corresponding to the target under constraint at the current moment and a maximum correlation entropy criterion Kalman filter to obtain a state estimation value of the target at the next moment, and then constraining the state estimation value of the target based on a probability density function truncation method to obtain a state estimation value corresponding to the target under constraint at the next moment;
and circulating the above processes to realize the tracking and positioning of the target.
From the above description, it can be seen that the present invention uses the state estimation value modified by the constraint condition as the related data calculated at the next time, i.e. the next time
Respectively as
And the state estimation value is used for participating in the state estimation value output by the maximum correlation entropy criterion Kalman filter at the next moment, and performing cross iteration on the state estimation value output by the maximum correlation entropy criterion Kalman filter and the state estimation value limited by the constraint condition.
Optionally, in the process of performing tracking and positioning on the target by using the maximum entropy criterion kalman filter with the constraint condition, for each tracking time, the following process is performed:
b: tracking by utilizing a maximum correlation entropy criterion Kalman filter at each tracking moment to obtain a state estimation value corresponding to the target at each tracking moment; and then, constraining the state estimation value of the target based on a probability density function truncation method to obtain the state estimation value corresponding to the constrained target at each tracking moment.
From the above description, in the present invention, the target state estimation value output by the maximum correlation entropy criterion kalman filter at each time is related to the target state estimation value output by the maximum correlation entropy criterion kalman filter at the previous time, and is unrelated to the target state estimation value under the constraint of the previous time.
Optionally, the prior estimation equation and the posterior estimation equation corresponding to the maximum correlation entropy criterion kalman filter are as follows:
prior estimation equation:
P k|k-1 ＝FP k-1|k-1 F T +Q k
the posterior estimation equation:
in the formula, F and H k Respectively a state transition matrix and an observation matrix of the maneuvering target,
P k|k-1 to obtain the state estimate and covariance in the prediction phase for the filter at time k,
P k|k is the state estimate and covariance for the filter at time k in the update phase; q k ，R k Are all known covariance; t is a matrix transposition symbol, G σ () Is a Gaussian kernel function, Z k For systematic measurement, K k Is filter gain, L k Is defined for writing.
Optionally, initializing the maximum entropy criterion kalman filter sets:
wherein, X 0 Corresponding to the tracking initial state of the target.
Optionally, if the target is a maneuvering target, the state estimation value of the target represents the position and the speed of the maneuvering target on the road, and the system observed quantity is the measured distance from the sensor to the position of the maneuvering target.
In a second aspect, the present invention provides a system for a target tracking and positioning method based on a kalman filter under constraint conditions, including:
a motion model construction module: the method comprises the steps of constructing a motion model of a target and acquiring a tracking initial state of the target;
a tracking and positioning module: the tracking and positioning module is used for tracking and positioning a target by utilizing a Kalman filter with the maximum correlation entropy criterion of the constraint condition, and comprises: a maximum correlation entropy criterion Kalman filter tracking unit and a constraint unit;
the maximum correlation entropy criterion Kalman filter tracking unit is used for tracking by using a maximum correlation entropy criterion Kalman filter to obtain a state estimation value of a target;
and the constraint unit is used for constraining the state estimation value of the target based on a probability density function truncation method to obtain the state estimation value of the target under constraint.
In a third aspect, the present invention provides an electronic terminal comprising a processor and a memory, the memory storing a computer program, the processor calling the computer program to perform: a target tracking and positioning method based on a Kalman filter under constraint conditions comprises the following steps.
In a fourth aspect, the present invention provides a readable storage medium storing a computer program, which is invoked by a processor to perform the steps of a kalman filter based target tracking and positioning method under constraint conditions.
Advantageous effects
The invention discloses a Kalman filter target tracking and positioning method based on constraint conditions, wherein a selected Kalman filter with a maximum correlation entropy criterion reduces the interference of non-Gaussian noise, the proposed target tracking method is optimized for the non-Gaussian noise, and the advantages of priori knowledge (such as road constraint) are fully utilized, so that the problems that the existing target tracking method cannot process the interference of the non-Gaussian noise and the tracking precision is not high are solved, wherein a probability density function truncation method truncates a probability density function estimated by the state of a maneuvering target at a constraint boundary, the obtained constrained estimation value is closer to a true value, and the positioning accuracy of the estimation value is effectively improved
And can be widely applied to target tracking and positioning under non-Gaussian noise.
Drawings
FIG. 1 is a flow chart of target tracking and positioning based on a Kalman filter with maximum entropy criterion of constraint conditions in the embodiment of the invention.
FIG. 2 is a diagram of a model according to an embodiment of the present invention, where (a) is a schematic view of a scene and (b) is a schematic view of a coordinate model.
FIG. 3 is a diagram of the estimated values obtained by applying the maximum correlation entropy criterion Kalman filter in the example of the present invention.
FIG. 4 is a diagram of estimated values obtained using a Kalman filter with maximum entropy criterion for state constraints in an example of the present invention.
FIG. 5 is a diagram of the estimated difference of the maximum entropy criterion Kalman filter with state constraint.
Detailed Description
The invention provides a Kalman filter target tracking and positioning method based on constraint conditions, which is used for realizing tracking and positioning. In the following embodiments, which are specifically described below, the following two aspects are mainly included, on one hand, in order to reduce the interference of non-gaussian noise, the maximum correlation entropy criterion kalman filter is selected; in a second aspect, the invention sets constraint conditions, such as road constraint conditions, and further constrains the state estimation value of the target of the kalman filter by using the constraint conditions, thereby improving the accuracy of the positioning result. Therefore, the following invention will be described by taking an example of an embodiment having both the above-described technical points.
Example 1:
firstly, a maximum correlation entropy criterion Kalman filter and a probability density function truncation method are used for carrying out constraint on a state estimation value of a target for principle explanation:
1. maximum correlation entropy criterion Kalman filter
The equation for a conventional kalman filter is as follows:
assuming a linear discrete system:
x in linear discrete systems k And Z k Respectively, system state quantity and observed quantity (X in the practical application of moving target tracking) k Indicating the position and speed, Z, of a mobile object in the x, y direction of the road k Representing the measured distance of the sensor to the location of the mobile target). F and H k Respectively a state transition matrix and an observation matrix, w k Is the process noise sum v k For measuring the noise, there is a known covariance Q k And R k 。
Based on the definition of a Kalman filter and a maximum correlation entropy criterion, in the design of the Kalman filter based on the maximum correlation entropy criterion, an objective function based on the correlation entropy comprises the following components:
wherein the content of the first and second substances,
where the kernel function κ (x, y) in the correlation entropy definition is a gaussian kernel function:
e＝x-y,σ>0 is the bandwidth and x, y are random variables.
The upper type is to
And (5) obtaining a derivative:
further solve for
To obtain the state estimation value, the right side of the above equation is added or subtracted
Comprises the following steps:
the method is simplified and can be obtained:
the maximum correlation entropy criterion Kalman filtering algorithm can be known by the theoretical reasoning as follows:
(i) state prediction
(ii) Covariance prediction
P k|k-1 ＝FP k-1|k-1 F T +Q k
(iii) Filter gain
(iv) Status update
(v) Covariance update
In the above formula, F and H k Respectively a state transition matrix and an observation matrix of the maneuvering target,
P k|k-1 to obtain the state estimate and covariance in the prediction phase for the filter at time k,
P k|k for the state estimate and covariance of the filter in the update phase at time k, Q k ，R k All are known covariance, Z k And (4) observing the system.
From the above 5 formulas of (i) state prediction- (v) covariance update, it can be seen that the state estimation value of the target can be obtained by iterative computation using the initially input tracking initial information
And covariance P k|k 。
2. Constraining the state estimation value of the target by utilizing a probability density function truncation method
If the road constraint obeys the linear equality constraint:
Mx k where M is full rank, this linear constraint is contained using probability density function truncation.
2) Probability density function truncation method
The probability density function truncation method truncates the probability density function of the state estimation of the maneuvering target at the constrained boundary, thereby obtaining the mean value of the truncated probability density function
Sum covariance
The steps of this method are as follows:
by using
The state estimates representing the first i constraints,
is the corresponding covariance. Initialization:
the following transformations are performed:
ρ is an orthogonal matrix, S is a diagonal matrix, and U is an orthogonal matrix. S and U are
Singular value decomposition of (c).
Solving the matrix rho by Gram-Schmidt orthogonalization and enabling the matrix rho to meet
Knowing from the linear constraint:
both sides of the above formula are divided by
And converted to obtain
Definition of
Further comprises
[1 0…0]y ki ＝c ki
Transformed state estimate y incorporating constraints ki The mean μ and variance σ of (c) are:
wherein the mean [ mu ] and variance [ sigma ] of the transformed state estimates with the constraint added are determined by sequentially adding the first i constraintsPost-transformed y ki The mean and variance are calculated.
And obtaining a state estimation mean value and a variance which meet the constraint condition:
repeating the above process until i is the dimension q of m, and outputting
Wherein, the first and the second end of the pipe are connected with each other,
as a result of the positioning of the target at the current k instant.
For a state estimation value under constraint, in one implementation, the k time is obtained
As corresponding to time k
And P k|k And then participating in 5 formulas corresponding to (i) state prediction- (v) covariance updating at the next moment, and further outputting the state estimation value at the next moment by the maximum correlation entropy criterion Kalman filter. In another implementation, for each time, 5 equations of (i) state prediction- (v) covariance update are used to calculate the state estimation value at each time, where the data of the previous time applied in the equations is still the state estimation value and covariance calculated by the 5 equations at the previous time, rather than the state estimation value and covariance obtained under the constraint.
Based on the above theoretical statement, the target tracking and positioning method based on the kalman filter under the constraint condition provided by this embodiment includes the following steps:
constructing a motion model of a target and acquiring a tracking initial state of the target; the motion model comprises a constraint condition, a state update equation of the target and a measurement update equation of the target;
tracking and positioning a target by utilizing a maximum correlation entropy criterion Kalman filter with the constraint condition;
the method comprises the steps of tracking by using a maximum correlation entropy criterion Kalman filter to obtain a state estimation value of a target, constraining the state estimation value of the target based on a probability density function truncation method to obtain a target state estimation value under constraint, and realizing target tracking.
The formula for the tracking process is as follows:
(i) initialization:
(ii) and (3) a priori estimation:
P k|k-1 ＝FP k-1|k-1 F T +Q k
(iii) and (3) posterior estimation:
(iv) and (3) state constraint:
Mx k ＝m
application example:
to fully illustrate the content of the method of the present invention, the present embodiment is modeled based on the actual problem of the vehicle traveling on the road, and a motion model is obtained, wherein the noise is simulated as non-gaussian noise. The motion model content is as follows:
(i) establishing a state update equation according to the motion condition of a maneuvering target
In this embodiment, the equation of state describes the dynamic position and velocity of the vehicle, and the selection of the state of the maneuver object is made up of four elements, north, east, north, and east. It should be understood that other embodiments are not limited thereto, and other equations that characterize the update of the maneuver target state can be applied to the method of the present invention. The state update equation of the maneuvering target set in this embodiment is as follows (the following equation corresponds to the equation of the kalman model:
)。
X k is the maneuvering target state, T is the filter sample time periodTheta is the vehicle running angle u k-1 Is an input of acceleration, q k-1 Is non-gaussian process noise.
(ii) Establishing a measurement update equation from sensor observations
The observed value in the model refers to the measured distance from the sensor to the maneuvering target. Measurement update equation for maneuvering target:
Z k is the sensor measurement, x 1 ,x 2 The coordinate values of the maneuvering target state in the north and east directions, (p) n1 ,p e1 ) Is the coordinate value of the first sensor, (p) n2 ,p e2 ) The coordinate value of the second sensor, and the arrangement position of the sensors is set according to actual requirements, such as the initial point and the terminal point according to the tracking road section. r is k Is non-gaussian measurement noise.
Wherein, what adopted is that mix the Gaussian noise as the non-Gaussian measurement noise, as follows:
q k-1 ＝λN(μ x1 ,Q 1 )+(1-λ)N(μ x2 ,Q 2 ),
r k ＝λN(μ z1 ,R 1 )+(1-λ)N(μ z2 ,R 2 )
μ x1 ,μ x2 ,μ z1 ,μ z2 ,Q 1 ,Q 2 ,R 1 ,R 2 the size is set according to actual conditions. Lambda is a mixing proportion coefficient, and the value range is 0-1; and N is xx.
(iii) Acquiring initial state information of the maneuvering target, such as initial position and speed of the maneuvering target as X 0 。
Road constraint:
ground object movement is often constrained by external factors, including road networks or varying terrain, where road constraints are of greater appeal. A road network will typically be represented by a number of road segments, each of which will be described by its two end points. The direction θ of the link may be previously obtained for the relevant link. An equation is established for the known road by using an equality constraint equation, which can be known from the equation:
Mx＝m
and the middle road constraint matrix M is related to the angle theta of the moving target running along the road, and corresponding expressions of M and M are constructed. In this example, the following:
each row in the road constraint matrix M may be regarded as a constraint, and the dimension q of M is 2.
And finally, tracking the target based on the road constraint and the motion model by using the formula of the tracking process stated above. The obtained state estimation value is shown in fig. 5, and fig. 5 is an estimation difference diagram of a maximum entropy criterion kalman filter and a maximum entropy criterion kalman filter with state constraint.
Based on the above method, the present invention provides a positioning system, which includes: the system comprises a motion model building module and a tracking and positioning module which are in communication connection with each other.
Wherein, the motion model construction module: the method comprises the steps of constructing a motion model of a target and acquiring a tracking initial state of the target; a tracking and positioning module: and the Kalman filter is used for tracking and positioning the target by utilizing the maximum correlation entropy criterion with the constraint condition.
Further, the tracking and positioning module comprises: a maximum correlation entropy criterion Kalman filter tracking unit and a constraint unit;
the maximum correlation entropy criterion Kalman filter tracking unit is used for tracking by using a maximum correlation entropy criterion Kalman filter to obtain a state estimation value of a target; and the constraint unit is used for constraining the state estimation value of the target based on a probability density function truncation method to obtain the target state estimation value under constraint.
For the specific implementation process of each unit module, refer to the corresponding process of the foregoing method. It should be understood that, the specific implementation process of the above unit module refers to the method content, and the present invention is not described herein in detail, and the division of the above functional module unit is only a division of a logic function, and there may be another division manner in the actual implementation, for example, multiple units or components may be combined or may be integrated into another system, or some features may be omitted, or may not be executed. Meanwhile, the integrated unit can be realized in a hardware form, and can also be realized in a software functional unit form.
Based on the above method, the present invention provides an apparatus, comprising a processor and a memory, wherein the memory stores a computer program, and the processor calls the computer program to execute:
constructing a motion model of a target and acquiring a tracking initial state of the target; tracking and positioning the target by using the Kalman filter with the constraint condition;
the method comprises the steps of tracking by using a maximum correlation entropy criterion Kalman filter to obtain a state estimation value of a target, constraining the state estimation value of the target based on a probability density function truncation method to obtain a target state estimation value under constraint, and realizing target tracking.
The specific implementation process of each step refers to the explanation of the foregoing method.
Based on the above method, the present invention provides a readable storage medium storing a computer program, which is called by a processor to execute:
constructing a motion model of a target and acquiring a tracking initial state of the target; tracking and positioning the target by using the Kalman filter with the constraint condition;
the method comprises the steps of tracking by using a maximum correlation entropy criterion Kalman filter to obtain a state estimation value of a target, constraining the state estimation value of the target based on a probability density function truncation method to obtain a target state estimation value under constraint, and realizing target tracking.
The specific implementation process of each step refers to the explanation of the foregoing method.
It should be understood that in the embodiments of the present invention, the Processor may be a Central Processing Unit (CPU), and the Processor may also be other general purpose processors, Digital Signal Processors (DSPs), Application Specific Integrated Circuits (ASICs), Field-Programmable gate arrays (FPGAs) or other Programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like. The memory may include both read-only memory and random access memory, and provides instructions and data to the processor. The portion of memory may also include non-volatile random access memory. For example, the memory may also store device type information.
The readable storage medium is a computer readable storage medium, which may be an internal storage unit of the controller according to any of the foregoing embodiments, for example, a hard disk or a memory of the controller. The readable storage medium may also be an external storage device of the controller, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card), and the like provided on the controller. Further, the readable storage medium may also include both an internal storage unit of the controller and an external storage device. The readable storage medium is used for storing the computer program and other programs and data required by the controller. The readable storage medium may also be used to temporarily store data that has been output or is to be output.
Based on such understanding, the technical solution of the present invention essentially or partially contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product stored in a storage medium and including instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned readable storage medium includes: various media capable of storing program codes, such as a usb disk, a removable hard disk, a Read-only Memory (ROM), a Random Access Memory (RAM), a magnetic disk, or an optical disk.
It should be emphasized that the examples described herein are illustrative and not restrictive, and thus the invention is not to be limited to the examples described herein, but rather to other embodiments that may be devised by those skilled in the art based on the teachings herein, and that various modifications, alterations, and substitutions are possible without departing from the spirit and scope of the invention as defined by the appended claims.
Claims (9)
1. A target tracking and positioning method is characterized in that: the method comprises the following steps:
constructing a motion model of a target and acquiring a tracking initial state of the target; the motion model comprises a constraint condition, a state update equation of the target and a measurement update equation of the target;
tracking and positioning a target by utilizing a maximum correlation entropy criterion Kalman filter with the constraint condition;
tracking by using a maximum correlation entropy criterion Kalman filter to obtain a state estimation value of a target, and constraining the state estimation value of the target based on a probability density function truncation method to obtain a state estimation value of the target under constraint to realize target tracking;
in the process of obtaining the state estimation value of the target under constraint by constraining the state estimation value of the target based on the probability density function truncation method, the state constraint obeys the linear equation constraint as follows:
Mx k ＝m
where M is a constraint matrix, M is a constraint value, and x k The state estimation value of the target at the current k moment meets the linear equation constraint;
the initialization settings are as follows:
i＝0，
i denotes the first i constraints, the maximum of which is equal to the dimension q of m,
respectively using the state estimation value and covariance of the previous i constraint transformations at the current k moment;
P k|k respectively obtaining a state estimation value and a covariance of a target at the current k moment by utilizing a maximum correlation entropy criterion Kalman filter;
i is sequentially valued from 0 to q, and is iteratively calculated according to the following formula to finally obtain
As the state estimation value and covariance of the target under the constraint corresponding to the current k time, and the state estimation value of the target is used
As a result of the positioning of the target at the current k time, the iterative formula is as follows:
2. The method of claim 1, wherein: in the process of tracking and positioning the target by using the maximum correlation entropy criterion Kalman filter with the constraint condition, the following process is executed for each tracking moment:
a: tracking by using a maximum correlation entropy criterion Kalman filter to obtain a state estimation value corresponding to the target at the current moment, and constraining the state estimation value of the target based on a probability density function truncation method to obtain a state estimation value corresponding to the target under constraint at the current moment;
then, utilizing a state estimation value corresponding to the target under constraint at the current moment and a maximum correlation entropy criterion Kalman filter to obtain a state estimation value of the target at the next moment, and then constraining the state estimation value of the target based on a probability density function truncation method to obtain a state estimation value corresponding to the target under constraint at the next moment;
and circulating the above processes to realize the tracking and positioning of the target.
3. The method of claim 1, wherein: in the process of tracking and positioning the target by using the maximum correlation entropy criterion Kalman filter with the constraint condition, the following process is executed for each tracking moment:
b: tracking by utilizing a maximum correlation entropy criterion Kalman filter at each tracking moment to obtain a state estimation value corresponding to the target at each tracking moment; and then, constraining the state estimation value of the target based on a probability density function truncation method to obtain the state estimation value corresponding to the constrained target at each tracking moment.
4. The method of claim 1, wherein: the prior estimation equation and the posterior estimation equation corresponding to the maximum correlation entropy criterion Kalman filter are as follows:
prior estimation equation:
P k|k-1 ＝FP k-1|k-1 F T +Q k
the posterior estimation equation:
in the formula, F and H k Respectively a state transition matrix and an observation matrix of the maneuvering target,
P k|k-1 to estimate the state and covariance of the filter in the prediction phase for time k,
P k|k is the state estimate and covariance for the filter at time k in the update phase; q k ，R k Are all known covariance; t is a matrix transposition symbol, G σ () Is a Gaussian kernel function, Z k For systematic observation, K k Is the filter gain, L k Is defined for writing.
5. The method of claim 4, wherein: setting when initializing the maximum correlation entropy criterion Kalman filter:
wherein X 0 Corresponding to the tracking initial state of the target.
6. The method of claim 1, wherein: if the target is a maneuvering target, the state estimation value of the target represents the position and the speed of the maneuvering target on the road, and the system observation is the measurement distance from the sensor to the position of the maneuvering target.
7. A system based on the method of any one of claims 1-6, characterized by: the method comprises the following steps:
a motion model construction module: the method comprises the steps of constructing a motion model of a target and acquiring a tracking initial state of the target;
a tracking and positioning module: the tracking and positioning module is used for tracking and positioning a target by utilizing a Kalman filter with the maximum correlation entropy criterion of the constraint condition, and comprises: a maximum correlation entropy criterion Kalman filter tracking unit and a constraint unit;
the maximum correlation entropy criterion Kalman filter tracking unit is used for tracking by using a maximum correlation entropy criterion Kalman filter to obtain a state estimation value of a target;
and the constraint unit is used for constraining the state estimation value of the target based on a probability density function truncation method to obtain the target state estimation value under constraint.
8. An electronic terminal, characterized by: comprising a processor and a memory, said memory storing a computer program that said processor invokes to perform: the process steps of any one of claims 1 to 6.
9. A readable storage medium, characterized by: a computer program is stored, which is called by a processor to perform the steps of the method of any of claims 1-6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110038824.8A CN112836354B (en) | 2021-01-12 | 2021-01-12 | Target tracking and positioning method, system and device and readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110038824.8A CN112836354B (en) | 2021-01-12 | 2021-01-12 | Target tracking and positioning method, system and device and readable storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112836354A CN112836354A (en) | 2021-05-25 |
| CN112836354B true CN112836354B (en) | 2022-08-30 |
Family
ID=75927844
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110038824.8A Expired - Fee Related CN112836354B (en) | 2021-01-12 | 2021-01-12 | Target tracking and positioning method, system and device and readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112836354B (en) |
Family Cites Families (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5051751A (en) * | 1991-02-12 | 1991-09-24 | The United States Of America As Represented By The Secretary Of The Navy | Method of Kalman filtering for estimating the position and velocity of a tracked object |
| FR2958096B1 (en) * | 2010-03-29 | 2012-04-13 | Commissariat Energie Atomique | METHOD AND SYSTEM FOR CALIBRATION, RECORDING MEDIUM FOR THIS METHOD |
| CN106487358B (en) * | 2016-09-30 | 2019-05-10 | 西南大学 | A kind of maneuvering target turning tracking |
| KR102275452B1 (en) * | 2017-03-16 | 2021-07-12 | 한국전자통신연구원 | Method for tracking image in real time considering both color and shape at the same time and apparatus therefor |
| CN109856622B (en) * | 2019-01-03 | 2021-04-20 | 中国人民解放军空军研究院战略预警研究所 | Single-radar linear track target state estimation method under constraint condition |
| CN110061716B (en) * | 2019-01-15 | 2021-05-04 | 河海大学 | Improved kalman filtering method based on least square and multiple fading factors |
| CN110245565A (en) * | 2019-05-14 | 2019-09-17 | 东软集团股份有限公司 | Wireless vehicle tracking, device, computer readable storage medium and electronic equipment |
| CN111291471B (en) * | 2020-01-17 | 2021-12-17 | 中山大学 | Constraint multi-model filtering method based on L1 regular unscented transformation |
| CN111798491B (en) * | 2020-07-13 | 2022-09-06 | 哈尔滨工业大学 | Maneuvering target tracking method based on Elman neural network |
| CN111985093B (en) * | 2020-08-03 | 2022-06-21 | 哈尔滨工程大学 | Adaptive unscented Kalman filtering state estimation method with noise estimator |
-
2021
- 2021-01-12 CN CN202110038824.8A patent/CN112836354B/en not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| CN112836354A (en) | 2021-05-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106950562B (en) | State fusion target tracking method based on predicted value measurement conversion | |
| CN107677272B (en) | AUV (autonomous Underwater vehicle) collaborative navigation method based on nonlinear information filtering | |
| CN110208792B (en) | Arbitrary straight line constraint tracking method for simultaneously estimating target state and track parameters | |
| CN109141413B (en) | EFIR filtering algorithm and system with data missing UWB pedestrian positioning | |
| WO2021161682A1 (en) | System and method for tracking expanded state of an object | |
| CN111077518B (en) | Tracking filtering method and device based on distance-Doppler measurement | |
| CN108871365B (en) | State estimation method and system under course constraint | |
| WO2021162018A1 (en) | System and method for tracking expanded state of moving object with model geometry learning | |
| CN111008364A (en) | Method and system for cooperative passive positioning of double observers | |
| CN115457510A (en) | Self-adaptive cooperative target tracking method based on variational Bayes | |
| CN112836354B (en) | Target tracking and positioning method, system and device and readable storage medium | |
| CN114440880A (en) | Construction site control point positioning method and system based on adaptive iteration EKF | |
| CN114139109A (en) | Target tracking method, system, equipment, medium and data processing terminal | |
| CN109542093B (en) | Method and device for processing data | |
| CN117614789A (en) | Carrier phase tracking method and device based on Kalman-like unbiased FIR filter | |
| CN113030945B (en) | Phased array radar target tracking method based on linear sequential filtering | |
| CN116577750A (en) | Kalman filtering algorithm based on intermediate transition state | |
| CN114111796B (en) | Parallel fusion positioning method and system of underwater unmanned robot based on information gain | |
| CN104614751A (en) | Constraint information-based target positioning method | |
| CN111998854B (en) | Cholesky decomposition calculation-based accurate expansion Stirling interpolation filtering method | |
| CN115047505A (en) | GNSS positioning method and navigation method based on carrier phase differential assistance | |
| Lu et al. | Research on matching accuracy of underwater terrain matching algorithm based on state augmented unscented Kalman filter | |
| Zhao | Covariance intersection fusion estimator for nonlinear system with correlated noise | |
| Cheng et al. | Linear sequential filtering based on the debiased measurement matrix of the converted pseudo measurement for radar target tracking | |
| CN114593724B (en) | Cluster fusion positioning method and device |
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 | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20220830 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |