CN112836354A - 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 PDF

Info

Publication number
CN112836354A
CN112836354A CN202110038824.8A CN202110038824A CN112836354A CN 112836354 A CN112836354 A CN 112836354A CN 202110038824 A CN202110038824 A CN 202110038824A CN 112836354 A CN112836354 A CN 112836354A
Authority
CN
China
Prior art keywords
target
tracking
estimation value
state estimation
constraint
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110038824.8A
Other languages
Chinese (zh)
Other versions
CN112836354B (en
Inventor
邹逸群
邹双
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Central South University
Original Assignee
Central South University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Central South University filed Critical Central South University
Priority to CN202110038824.8A priority Critical patent/CN112836354B/en
Publication of CN112836354A publication Critical patent/CN112836354A/en
Application granted granted Critical
Publication of CN112836354B publication Critical patent/CN112836354B/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

Target tracking and positioning method, system and device and readable storage medium
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 previous time of the maneuver object. 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. In general, kalman filters and their extensions perform well for the tracking of moving targets under gaussian noise, but in the case of non-gaussian noise, the performance can get 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 criterion of information theory learning is 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 theorem can capture high-order statistics and remarkably improve the performance of the information theorem in the application of machine learning and signal processing. 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 a road constraint condition, so that the 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: a model order reduction method, a perfect measurement method and an 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.
In one aspect, the present invention provides a target tracking and positioning method, including 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:
Mxk＝m
where M is a constraint matrix, M is a constraint value, and xkThe state estimation value of the target at the current k moment meets the linear equation constraint;
the initialization settings are as follows:
Figure BDA0002894717630000021
i denotes the first i constraints, the maximum of which is equal to the dimension q of m,
Figure BDA0002894717630000022
respectively using the state estimation value and covariance of the previous i constraint transformations at the current k moment;
Figure BDA0002894717630000023
Pk|krespectively 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
Figure BDA0002894717630000031
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
Figure BDA0002894717630000032
As a result of the positioning of the target at the current k time, the iterative formula is as follows:
Figure BDA0002894717630000033
Figure BDA0002894717630000034
Figure BDA0002894717630000035
respectively using the state estimation value and covariance of the previous i +1 constraint transformations at the current k moment; ρ is an orthogonal matrix, S is a diagonal matrix, U is an orthogonal matrix, and S and U are covariances
Figure BDA0002894717630000036
T is the transposed symbol of the matrix;
Figure BDA0002894717630000037
Figure BDA0002894717630000038
Figure BDA0002894717630000039
both are written definitions given for the convenience of formula simplification, and μ, σ are the mean and variance, respectively, of the state estimate after adding the constraint.
From the above formula, the present invention directly utilizes the Kalman filter to track and obtain the state of the targetThe estimated value is corrected, and the final output is finally calculated in an iterative way
Figure BDA00028947176300000310
Wherein, will
Figure BDA00028947176300000311
As a result of the positioning of the target at the current k instant.
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
Figure BDA00028947176300000312
Respectively as
Figure BDA00028947176300000313
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 entropy criterion kalman filter at each time is related to the target state estimation value output by the maximum 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:
Figure BDA0002894717630000041
Pk|k-1＝FPk-1|k-1FT+Qk
the posterior estimation equation:
Figure BDA0002894717630000042
Figure BDA0002894717630000043
Figure BDA0002894717630000044
Figure BDA0002894717630000045
in the formula, F and HkRespectively a state transition matrix and an observation matrix of the maneuvering target,
Figure BDA0002894717630000046
Pk|k-1to estimate the state and covariance of the filter in the prediction phase for time k,
Figure BDA0002894717630000047
Pk|kis the state estimate and covariance for the filter at time k in the update phase; qk，RkAre all known covariance; t is a matrix transposition symbol, Gσ() Is a Gaussian kernel function, ZkFor systematic measurement, KkIs the filter gain, LkIs defined for writing.
Optionally, initializing the maximum entropy criterion kalman filter sets:
Figure BDA0002894717630000048
Figure BDA0002894717630000049
wherein, X0Corresponding 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 observation 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 target state estimation value 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 for execution by a processor to perform the steps of a method for target tracking and positioning based on a kalman filter 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-based target tracking and positioning method under constraint conditions, which is used for realizing tracking and positioning. In the following embodiments, the following two aspects are mainly included, on one hand, in order to reduce the interference of non-gaussian noise, the invention selects a kalman filter with the maximum correlation entropy criterion; 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, carrying out principle explanation on the constraint of a maximum correlation entropy criterion Kalman filter and a state estimation value of a target by using a probability density function truncation method:
1. maximum correlation entropy criterion Kalman filter
The equation for a conventional kalman filter is as follows:
assuming a linear discrete system:
Figure BDA0002894717630000061
x in linear discrete systemskAnd ZkRespectively, system state quantity and observed quantity (in a motor-driven targetTrace X in practical applicationkIndicating the position and speed, Z, of a mobile object in the x, y direction of the roadkRepresenting the measured distance of the sensor to the location of the mobile target). F and HkRespectively a state transition matrix and an observation matrix, wkIs the process noise sum vkFor measuring the noise, there is a known covariance QkAnd Rk。
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:
Figure BDA0002894717630000062
wherein the content of the first and second substances,
Figure BDA0002894717630000063
Figure BDA0002894717630000064
where the kernel function κ (x, y) in the correlation entropy definition is a gaussian kernel function:
Figure BDA0002894717630000065
e＝x-y,σ>0 is the bandwidth and x, y are random variables.
The upper type is to
Figure BDA0002894717630000066
And (5) obtaining a derivative:
Figure BDA0002894717630000067
further solve for
Figure BDA0002894717630000071
Figure BDA0002894717630000072
To obtain the state estimation value, the right side of the above equation is added or subtracted
Figure BDA0002894717630000073
Comprises the following steps:
Figure BDA0002894717630000074
the method is simplified and can be obtained:
Figure BDA0002894717630000075
Figure BDA0002894717630000076
the maximum correlation entropy criterion Kalman filtering algorithm known by the theoretical reasoning is as follows:
(i) state prediction
Figure BDA0002894717630000077
(ii) Covariance prediction
Pk|k-1＝FPk-1|k-1FT+Qk
(iii) Filter gain
Figure BDA0002894717630000078
(iv) Status update
Figure BDA0002894717630000079
(v) Covariance update
Figure BDA00028947176300000710
In the above formula, F and HkRespectively a state transition matrix and an observation matrix of the maneuvering target,
Figure BDA00028947176300000711
Pk|k-1to estimate the state and covariance of the filter in the prediction phase for time k,
Figure BDA00028947176300000712
Pk|kfor the state estimate and covariance of the filter in the update phase at time k, Qk，RkAll are known covariance, ZkAnd (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
Figure BDA00028947176300000713
And covariance Pk|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:
Mxkm where M is the full rank, a probability density function truncation is used here to contain this linear constraint.
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
Figure BDA0002894717630000081
Sum covariance
Figure BDA0002894717630000082
The steps of the method are as follows:
by using
Figure BDA0002894717630000083
The state estimates representing the first i constraints,
Figure BDA0002894717630000084
is the corresponding covariance. Initialization:
Figure BDA0002894717630000085
the following transformations are performed:
Figure BDA0002894717630000086
ρ is an orthogonal matrix, S is a diagonal matrix, and U is an orthogonal matrix. S and U are
Figure BDA0002894717630000087
Singular value decomposition of (c).
Figure BDA0002894717630000088
Solving the matrix rho by Gram-Schmidt orthogonalization and enabling the matrix rho to meet the requirement
Figure BDA0002894717630000089
Knowing from the linear constraint:
Figure BDA00028947176300000810
both sides of the above formula are divided by
Figure BDA00028947176300000811
And converted to obtain
Figure BDA00028947176300000812
Definition of
Figure BDA00028947176300000813
Further comprises
[1 0…0]yki＝cki
Transformed state estimate y incorporating constraintskiThe mean μ and variance σ of (c) are:
Figure BDA00028947176300000814
Figure BDA00028947176300000815
wherein the mean μ and variance σ of the transformed state estimates incorporating the constraint are transformed y with the first i constraints added in sequencekiThe mean and variance are calculated.
And obtaining a state estimation mean value and a variance which meet the constraint condition:
Figure BDA0002894717630000091
Figure BDA0002894717630000092
repeating the above process until i is the dimension q of m, and outputting
Figure BDA0002894717630000093
Wherein the content of the first and second substances,
Figure BDA0002894717630000094
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
Figure BDA0002894717630000095
As corresponding to time k
Figure BDA0002894717630000096
And Pk|kAnd 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 formulas 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 formulas is still the state estimation value and covariance calculated by the 5 formulas at the previous time, and the state estimation value and covariance obtained under the non-constraint condition.
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; 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 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:
Figure BDA0002894717630000097
Figure BDA0002894717630000098
(ii) and (3) a priori estimation:
Figure BDA0002894717630000099
Pk|k-1＝FPk-1|k-1FT+Qk
(iii) and (3) posterior estimation:
Figure BDA0002894717630000101
Figure BDA0002894717630000102
Figure BDA0002894717630000103
Figure BDA0002894717630000104
(iv) and (3) state constraint:
Mxk＝m
Figure BDA0002894717630000105
Figure BDA0002894717630000106
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:
Figure BDA0002894717630000107
)。
Figure BDA0002894717630000108
Xkis the maneuvering target state, T is the filter sampling time period, θ is the vehicle travel angle, uk-1Is an input of acceleration, qk-1Is 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:
Figure BDA0002894717630000109
Zkis a sensor measurement value, x1,x2The north and east coordinate values of the maneuvering target state, respectively, (p)n1,pe1) Is the coordinate value of the first sensor, (p)n2,pe2) 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 iskIs notGaussian measures noise.
Wherein, what adopted is that mix the Gaussian noise as the non-Gaussian measurement noise, as follows:
qk-1＝λN(μx1,Q1)+(1-λ)N(μx2,Q2),
rk＝λN(μz1,R1)+(1-λ)N(μz2,R2)
μx1,μx2,μz1,μz2,Q1,Q2,R1,R2the 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 X0。
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 is:
Figure BDA0002894717630000111
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 utilizing 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 utilizing 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, and the like. 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 present invention.

Claims (10)

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; 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.
2. The method of claim 1, wherein: 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:
Mxk＝m
where M is a constraint matrix, M is a constraint value, and xkThe state estimation value of the target at the current k moment meets the linear equation constraint;
the initialization settings are as follows:
i＝0,
Figure FDA0002894717620000011
i denotes the first i constraints, the maximum of which is equal to the dimension q of m,
Figure FDA0002894717620000012
respectively using the state estimation value and covariance of the previous i constraint transformations at the current k moment;
Figure FDA0002894717620000013
Pk|krespectively 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
Figure FDA0002894717620000014
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
Figure FDA0002894717620000015
As a result of the positioning of the target at the current k time, the iterative formula is as follows:
Figure FDA0002894717620000016
Figure FDA0002894717620000017
Figure FDA0002894717620000018
respectively using the state estimation value and covariance of the previous i +1 constraint transformations at the current k moment; ρ is an orthogonal matrix, S is a diagonal matrix, U is an orthogonal matrix, and S and U are covariances
Figure FDA0002894717620000019
T is the transposed symbol of the matrix;
Figure FDA00028947176200000110
Figure FDA00028947176200000111
Figure FDA0002894717620000021
both are written definitions, and μ and σ are the mean and variance of the state estimate after adding the constraint.
3. The method of claim 2, 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.
4. The method of claim 2, 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.
5. 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:
Figure FDA0002894717620000022
Pk|k-1＝FPk-1|k-1FT+Qk
the posterior estimation equation:
Figure FDA0002894717620000023
Figure FDA0002894717620000024
Figure FDA0002894717620000025
Figure FDA0002894717620000026
in the formula, F and HkRespectively a state transition matrix and an observation matrix of the maneuvering target,
Figure FDA0002894717620000027
Pk|k-1to estimate the state and covariance of the filter in the prediction phase for time k,
Figure FDA0002894717620000031
Pk|kis the state estimate and covariance for the filter at time k in the update phase; qk，RkAre all known covariance; t is a matrix transposition symbol, Gσ() Is a Gaussian kernel function, ZkFor systematic measurement, KkIs the filter gain, LkIs defined for writing.
6. The method of claim 5, wherein: setting when initializing the maximum correlation entropy criterion Kalman filter:
Figure FDA0002894717620000032
Figure FDA0002894717620000033
wherein, X0Corresponding to the tracking initial state of the target.
7. 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.
8. A system based on the method of any one of claims 1-7, 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.
9. An electronic terminal, characterized by: comprising a processor and a memory, the memory storing a computer program that the processor calls to perform: the process steps of any one of claims 1 to 7.
10. 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-7.
CN202110038824.8A 2021-01-12 2021-01-12 Target tracking and positioning method, system and device and readable storage medium Expired - Fee Related CN112836354B (en)

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 true CN112836354A (en) 2021-05-25
CN112836354B 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)

Citations (10)

* Cited by examiner, † Cited by third party
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
FR2958096A1 (en) * 2010-03-29 2011-09-30 Commissariat Energie Atomique METHOD AND SYSTEM FOR CALIBRATION, RECORDING MEDIUM FOR THIS METHOD
CN106487358A (en) * 2016-09-30 2017-03-08 西南大学 A kind of maximal correlation entropy volume kalman filter method based on statistical linear regression
KR20180105876A (en) * 2017-03-16 2018-10-01 한국전자통신연구원 Method for tracking image in real time considering both color and shape at the same time and apparatus therefor
CN109856622A (en) * 2019-01-03 2019-06-07 中国人民解放军空军研究院战略预警研究所 A kind of single radar rectilinear path line target method for estimating state under constraint condition
CN110061716A (en) * 2019-01-15 2019-07-26 河海大学 A kind of improvement kalman filtering method based on least square and the Multiple fading factor
CN111291471A (en) * 2020-01-17 2020-06-16 中山大学 Constraint multi-model filtering method based on L1 regular unscented transformation
CN111798491A (en) * 2020-07-13 2020-10-20 哈尔滨工业大学 Maneuvering target tracking method based on Elman neural network
US20200364472A1 (en) * 2019-05-14 2020-11-19 Neusoft Corporation Vehicle tracking method, computer readable storage medium, and electronic device
CN111985093A (en) * 2020-08-03 2020-11-24 哈尔滨工程大学 Adaptive unscented Kalman filtering state estimation method with noise estimator

Patent Citations (10)

* Cited by examiner, † Cited by third party
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
FR2958096A1 (en) * 2010-03-29 2011-09-30 Commissariat Energie Atomique METHOD AND SYSTEM FOR CALIBRATION, RECORDING MEDIUM FOR THIS METHOD
CN106487358A (en) * 2016-09-30 2017-03-08 西南大学 A kind of maximal correlation entropy volume kalman filter method based on statistical linear regression
KR20180105876A (en) * 2017-03-16 2018-10-01 한국전자통신연구원 Method for tracking image in real time considering both color and shape at the same time and apparatus therefor
CN109856622A (en) * 2019-01-03 2019-06-07 中国人民解放军空军研究院战略预警研究所 A kind of single radar rectilinear path line target method for estimating state under constraint condition
CN110061716A (en) * 2019-01-15 2019-07-26 河海大学 A kind of improvement kalman filtering method based on least square and the Multiple fading factor
US20200364472A1 (en) * 2019-05-14 2020-11-19 Neusoft Corporation Vehicle tracking method, computer readable storage medium, and electronic device
CN111291471A (en) * 2020-01-17 2020-06-16 中山大学 Constraint multi-model filtering method based on L1 regular unscented transformation
CN111798491A (en) * 2020-07-13 2020-10-20 哈尔滨工业大学 Maneuvering target tracking method based on Elman neural network
CN111985093A (en) * 2020-08-03 2020-11-24 哈尔滨工程大学 Adaptive unscented Kalman filtering state estimation method with noise estimator

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
YIQUN ZOU,LU LI,XIAFEI TANG,JINGTIAN YE,LINGLI YU: "Double Arc Path Tracking Control of Driverless Vehicle", 《CHINESE CONTROL CONFERENCE》 *
王恒等: "基于最大相关熵的雷达扩展卡尔曼滤波算法研究", 《中国传媒大学学报(自然科学版)》 *
许艳萍等: "具有状态约束的集员卡尔曼滤波器设计", 《福州大学学报(自然科学版)》 *

Also Published As

Publication number Publication date
CN112836354B (en) 2022-08-30

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
EP3928123B1 (en) System and method for tracking expanded state of an object
CN109141413B (en) EFIR filtering algorithm and system with data missing UWB pedestrian positioning
CN111077518B (en) Tracking filtering method and device based on distance-Doppler measurement
WO2021162018A1 (en) System and method for tracking expanded state of moving object with model geometry learning
CN108871365B (en) State estimation method and system under course constraint
Huang et al. Consistent unscented incremental smoothing for multi-robot cooperative target tracking
CN116047498A (en) Maneuvering target tracking method based on maximum correlation entropy extended Kalman filtering
CN115457510A (en) Self-adaptive cooperative target tracking method based on variational Bayes
CN109542093B (en) Method and device for processing data
CN112836354B (en) Target tracking and positioning method, system and device and readable storage medium
CN114139109A (en) Target tracking method, system, equipment, medium and data processing terminal
CN113030945B (en) Phased array radar target tracking method based on linear sequential filtering
CN107886058B (en) Noise-related two-stage volume Kalman filtering estimation method and system
CN104614751A (en) Constraint information-based target positioning method
CN114111796B (en) Parallel fusion positioning method and system of underwater unmanned robot based on information gain
CN112256054B (en) Unmanned aerial vehicle trajectory planning method and device
Lu et al. Research on matching accuracy of underwater terrain matching algorithm based on state augmented unscented Kalman filter
Cheng et al. Linear sequential filtering based on the debiased measurement matrix of the converted pseudo measurement for radar target tracking
Zhao Covariance intersection fusion estimator for nonlinear system with correlated noise
CN109343013A (en) A kind of spatial registration method and system based on Restart mechanisms
Yu et al. An improved Gaussian filter with asynchronously correlated noises
CN111796271B (en) Target tracking method and device under constraint of proportional guidance destination
Xu et al. Maximum correntropy extend Kalman filter for multiple AUVs cooperative localization

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