CN110208792B - Arbitrary straight line constraint tracking method for simultaneously estimating target state and track parameters - Google Patents

Arbitrary straight line constraint tracking method for simultaneously estimating target state and track parameters Download PDF

Info

Publication number
CN110208792B
CN110208792B CN201910559780.6A CN201910559780A CN110208792B CN 110208792 B CN110208792 B CN 110208792B CN 201910559780 A CN201910559780 A CN 201910559780A CN 110208792 B CN110208792 B CN 110208792B
Authority
CN
China
Prior art keywords
measurement
state
covariance
equation
calculating
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910559780.6A
Other languages
Chinese (zh)
Other versions
CN110208792A (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.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
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 Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN201910559780.6A priority Critical patent/CN110208792B/en
Publication of CN110208792A publication Critical patent/CN110208792A/en
Application granted granted Critical
Publication of CN110208792B publication Critical patent/CN110208792B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems
    • G01S13/72—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar Systems Or Details Thereof (AREA)
  • Navigation (AREA)

Abstract

The invention relates to an arbitrary straight line constraint tracking method for simultaneously estimating a target state and a track parameter, which comprises the following steps: acquiring target position measurement information from an observation radar; amplifying the state vector by using the slope and the y-axis intercept parameter component to obtain an amplified state vector and an amplified state equation, and constructing a pseudo measurement description constraint relation; and processing the measurement information, performing nonlinear filtering by using the processed measurement, the augmented state equation and the pseudo measurement, updating the state estimation and the state estimation covariance, and realizing target tracking. The method provided by the invention expands the track parameters into the state vector, reasonably utilizes the shape characteristic information in the target motion track, avoids information waste and improves the estimation precision; the calculation burden is effectively reduced by sequentially processing the radar measurement and the pseudo measurement, and the filtering result comprises a track parameter estimation result, so that the subsequent information processing is facilitated.

Description

Arbitrary straight line constraint tracking method for simultaneously estimating target state and track parameters
Technical Field
The invention relates to the technical field of space target tracking, in particular to an arbitrary straight line constraint tracking method for simultaneously estimating a target state and a track parameter.
Background
Constrained state estimation is a method of performing state estimation on a target state under the condition that the target state obeys an equality or inequality constraint condition. In many actual target tracking scenes, the target motion trajectory is not completely determined by the motion speed of the target itself, but is influenced or limited by the physical environment where the target is located or the motion characteristics of the target, and is not unconstrained free motion. The constraint information contained in the actual scenes is reasonably utilized, so that the estimation performance and the filtering precision can be effectively improved.
In an actual tracking scene, a constraint condition exists, which is derived from a shape feature common to all straight-line tracks instead of a specific track, and is called an arbitrary straight-line constraint. For example, in the ground object tracking problem, when tracking a vehicle traveling on a road, the road location information provided by the map is often used as a constrained prior information for object motion. However, if the map information cannot be obtained or the map information cannot be guaranteed to be accurate and reliable in some cases, for example, if the map itself is not updated in time or has drawing errors, the specific information on the road position and direction on the map is not reliable. At this time, the road position information provided by the map is introduced into the filter as the constraint prior information, which may cause the filter performance to deteriorate or even diverge.
This is more common when tracking naval or airborne targets, in addition to ground targets, due to the nature of the naval and aircraft itself, which is highly likely to remain in linear motion for a significant period of time. However, specific information about its course is generally difficult to obtain accurately, and the target true motion trajectory does not necessarily coincide exactly with the course. Therefore, in the constraint target tracking, only the shape feature of the target track is available for constraint prior information, and the shape feature cannot be described by a traditional constraint modeling method. The constraint condition is not approximate to a specific straight line, but is obtained based on the shape characteristic shared by all straight line tracks, wherein useful prior information about the motion state of the target is contained, and the introduction of the constraint condition into a tracking system increases the available information amount of a filter, thereby improving the filtering precision. Therefore, the method for searching accurate and efficient arbitrary straight line constraint modeling and state estimation has important significance.
Disclosure of Invention
The invention aims to provide a target tracking method under any straight line constraint for simultaneously estimating a target state and a track parameter, aiming at least part of defects of the prior art, and the filtering precision can be improved by using any straight line constraint.
In order to achieve the above object, the present invention provides an arbitrary straight line constraint tracking method for simultaneously estimating a target state and a trajectory parameter, comprising:
s1, obtaining target position measurement information from the observation radar;
s2, amplifying the state vector by using the slope and y-axis intercept parameter component to obtain an amplified state vector and an amplified state equation, and constructing a pseudo measurement description constraint relation;
s3, processing the measurement information obtained in the step S1, performing nonlinear filtering by using the processed measurement, the pseudo measurement obtained in the step S2 and the augmented state equation, and updating the state estimation and the state estimation covariance to realize target tracking.
Preferably, when the nonlinear filtering is performed in step S3, the measurement information obtained in step S1 is processed in a sequential measurement processing manner, and an unconstrained estimation result is obtained by using the processed measurement and a corresponding measurement equation; and then processing the pseudo measurement, and updating the constrained state estimation and the state estimation covariance by using a measurement equation corresponding to the pseudo measurement and combining an unconstrained estimation result.
Preferably, when the nonlinear filtering is performed in step S3, one or more of a transition measurement kalman filtering method, an unscented kalman filtering method, an extended kalman filtering method, or a particle filtering method is/are used.
Preferably, the step S2 further includes establishing a target motion model in a cartesian coordinate system:
x k+1＝Φ kx k+Γ kv k；
wherein x is kIs a state vector containing the position components x in the x and y directions at time k k、y kAnd velocity component
Figure BDA0002107920320000031
Φ kIs a state transition matrix; v. of kIs the process noise vector; gamma-shaped kIs a noise distribution matrix;
the expressions of the state transition matrix and the noise distribution matrix are respectively:
Figure BDA0002107920320000032
the corresponding state vector is
Figure BDA0002107920320000033
T is a radar scanning period;
when the state vector is augmented by the slope and the y-axis intercept parameter component in step S2, the expression of the augmented state vector is obtained as follows:
Figure BDA0002107920320000034
wherein a represents the slope of the unknown track, b represents the y-axis intercept of the unknown track, and the superscript "a" represents the vector, matrix and function associated with the pseudo-metrology;
in step S2, after the state vector is augmented, the state transition matrix and the noise distribution matrix in the state equation corresponding to the augmented state vector are correspondingly augmented, and the augmented state equation expression is obtained as follows:
Figure BDA0002107920320000035
wherein, I nAnd 0 m×nRespectively representing an identity matrix of dimension n x n and a zero matrix of dimension m x n.
Preferably, when the pseudo metrology is constructed in S2 to describe the constraint relationship, the equation of the constraint relationship is:
Figure BDA0002107920320000036
further, the expression for constructing the pseudo metrology description constraint relationship is obtained as follows:
Figure BDA0002107920320000037
the pseudo measurement is not affected by noise, and is in a nonlinear relation with the state vector, so that a measurement equation expression corresponding to the pseudo measurement is obtained as follows:
Figure BDA0002107920320000041
preferably, when the step S3 performs the nonlinear filtering, the measurement information obtained in the step S1 is processed by using a kalman filter, and the original measurement information is converted from the polar coordinate measurement to the rectangular coordinate measurement, where the conversion formula is:
Figure BDA0002107920320000042
wherein the content of the first and second substances,
Figure BDA0002107920320000043
distance and azimuth measurement obtained from radar;
Figure BDA0002107920320000044
is a converted cartesian coordinate measurement along the x and y directions,
Figure BDA0002107920320000045
is the converted measurement vector; mu.s θIs a coefficient of depolarization, and the variance of the noise is measured according to the azimuth angle Obtaining:
Figure BDA0002107920320000047
the measurement equation expression corresponding to the converted measurement is as follows:
the covariance matrix expression is:
Figure BDA0002107920320000049
wherein the content of the first and second substances,
Figure BDA00021079203200000410
the superscript "c" represents the vector, matrix and function associated with the transformed measurement.
Preferably, when performing the nonlinear filtering in step S3, the following steps are sequentially performed for each tracking time:
s3-1, if the current time is the first or the second time, initializing the augmented state vector by using an unscented transformation method to obtain an initial value and an initial covariance of the augmented state vector; otherwise, executing the next step;
s3-2, calculating state one-step prediction and one-step prediction covariance by using the augmented state equation;
s3-3, combining the state one-step prediction and the one-step prediction covariance, calculating the measurement prediction and the measurement prediction covariance by using the converted measurement and the corresponding measurement equation, and further obtaining the unconstrained state estimation updated by using the converted measurement and the corresponding state estimation covariance;
s3-4, processing the pseudo measurement, and calculating the cross covariance of the measurement and the state vector by using a measurement equation corresponding to the pseudo measurement and combining the unconstrained state estimation and the corresponding state estimation covariance;
and S3-5, updating the constrained state estimation and the corresponding state estimation covariance by combining the unconstrained state estimation and the corresponding state estimation covariance as well as the cross covariance of the measurement and state vectors, and realizing target tracking.
Preferably, the initializing the augmented state vector in step S3-1 by using an unscented transformation method includes:
converting to obtain rectangular coordinate measurement obeying mean value
Figure BDA0002107920320000051
Variance (variance)
Figure BDA0002107920320000052
Is selected from 2n according to the distribution m+1 δ sample points:
Figure BDA0002107920320000053
wherein the content of the first and second substances,
Figure BDA0002107920320000054
represents
Figure BDA0002107920320000055
Row i of the matrix, λ α 2(n+κ)-n mα and kappa are empirical parameters, α is used to determine the spread of the delta samples around the mean, β is used to introduce a priori knowledge about the distribution, W i meanAnd W i covRespectively calculating corresponding weights when the mean value and the covariance are calculated according to delta sampling points:
Figure BDA0002107920320000061
each sample point is:
Figure BDA0002107920320000062
according to the non-linear equation:
Figure BDA0002107920320000064
mapping each projection point to obtain a posterior delta sampling point:
Figure BDA0002107920320000065
and (3) weighting and summing to obtain an initial value and an initial covariance of the state vector as follows:
Figure BDA0002107920320000066
Figure BDA0002107920320000067
preferably, when the state one-step prediction and the one-step prediction covariance are calculated by using the augmented state equation in step S3-2, the state at the time k is predicted one-step according to the constraint state estimation updated at the time k-1, and an expression of the state one-step prediction calculation is as follows:
Figure BDA0002107920320000071
the expression for calculating the state one-step prediction covariance is:
Figure BDA0002107920320000072
in step S3-3, when the measurement prediction and the measurement prediction covariance are calculated by using the converted measurement and the corresponding measurement equation, and further the unconstrained state estimation updated by using the converted measurement and the corresponding state estimation covariance are obtained, the expression for calculating the measurement prediction is as follows:
Figure BDA0002107920320000073
the expression for calculating the measurement prediction covariance is:
Figure BDA0002107920320000074
calculating an expression of the corresponding filter gain:
Figure BDA0002107920320000075
the updated unconstrained state estimate and the corresponding state estimate covariance are obtained as:
Figure BDA0002107920320000076
Figure BDA0002107920320000077
preferably, the step S3-4 uses a measurement equation corresponding to the pseudo measurement in combination with the pseudo measurementUnconstrained state estimation and corresponding state estimation covariance, while calculating the cross-covariance of the metrology and state vectors, the computation is based on an unscented transformation
Figure BDA0002107920320000078
Selected 2n a+1 δ sample points:
Figure BDA0002107920320000079
calculating posterior delta sampling points according to a measurement equation corresponding to the pseudo measurement:
Figure BDA00021079203200000710
calculating a prediction measurement mean value according to sampling points:
Figure BDA00021079203200000711
calculating a covariance matrix corresponding to the predicted measurement:
Figure BDA0002107920320000081
the expression for calculating the cross-covariance of the metrology and state vectors is:
Figure BDA0002107920320000082
the expression for calculating the corresponding filter gain is:
wherein n is aIn order to extend the dimension of the state vector,
Figure BDA0002107920320000087
in step S3-5, the unconstrained state estimate and the corresponding state estimate covariance are combined with the cross covariance of the measurement and state vectors, and the constrained state estimate and the corresponding state estimate covariance are updated, and when target tracking is implemented, the expression for calculating the constrained state estimate is:
Figure BDA0002107920320000085
the expression for calculating the corresponding state estimate covariance is:
the technical scheme of the invention has the following advantages: the invention provides an arbitrary straight line constraint tracking method for simultaneously estimating a target state and a track parameter, which utilizes a slope and a y-axis intercept parameter component (track parameter) to amplify a state vector, constructs a pseudo-measurement description constraint relation, updates state estimation and state estimation covariance through a corresponding amplified state equation, and realizes target tracking. For example, in a road target tracking problem, a road may be estimated, and then the estimation result is used as constraint prior information to help improve the estimation accuracy of other driving targets along the road.
Drawings
FIG. 1 is a schematic diagram illustrating steps of an arbitrary straight-line constraint tracking method for simultaneously estimating a target state and a trajectory parameter according to an embodiment of the present invention;
FIG. 2 shows a schematic diagram of a target true trajectory and possible linear equality constraints;
FIG. 3 is a schematic diagram illustrating a filter structure corresponding to an arbitrary straight-line constraint tracking method for simultaneously estimating a target state and a trajectory parameter according to an embodiment of the present invention;
FIG. 4 shows a target track of uniform motion along a straight line in a Cartesian coordinate system constructed in a simulation experiment;
fig. 5 shows the comparison results of the position estimation root mean square error obtained by using an unconstrained nonlinear filtering method (transition measurement kalman filtering), a target tracking method (an augmented past time state method) under the constraint of an arbitrary straight line in the prior art, and a method (an augmented trajectory parameter method) provided by the embodiment of the present invention, respectively;
FIG. 6 shows the comparison of the root mean square error of the velocity estimates obtained by the above three methods;
FIG. 7 illustrates a root mean square error of a slope estimate of a trajectory obtained using a method provided by an embodiment of the invention;
FIG. 8 shows the root mean square error of the y-axis intercept of the trajectory obtained by the method provided by the embodiment of the invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be obtained by a person skilled in the art without any inventive step based on the embodiments of the present invention, are within the scope of the present invention.
As shown in fig. 1, an embodiment of the present invention provides an arbitrary straight-line constraint tracking method for simultaneously estimating a target state and a trajectory parameter, including:
and S1, acquiring target position measurement information from the observation radar.
In the tracking process, target position measurement information, namely original sensor measurement, is obtained from an observation radar. In one embodiment, the target position measurement information includes a range measurement of the target relative to the origin of the radar coordinate system And azimuth measurement k denotes the current time. As to how the radar obtains the information and how the method of the embodiment of the present invention obtains the measurement information from the radar, those skilled in the art can implement the method in various ways in the prior art, and details are not described herein.
And S2, amplifying the state vector by using the slope and y-axis intercept parameter components to obtain an amplified state vector and an amplified state equation, and constructing a pseudo-measurement description constraint relation. By introducing the track parameters, namely the slope and y-axis intercept parameter components, an arbitrary straight line constraint model for simultaneously estimating the target state and the track parameters is established.
Preferably, first, under a cartesian coordinate system, an object motion model is established, and the object motion can be modeled as:
x k+1＝Φ kx k+Γ kv k；
wherein x is kIs a state vector containing the position components x in the x and y directions at time k k、y kAnd velocity component
Figure BDA0002107920320000103
Etc.; phi kIs a state transition matrix; v. of kIs a process noise vector, assuming the process noise is white gaussian noise with known zero mean variance, with a covariance matrix of cov (v) k)＝Q k≥0；Γ kIs a noise distribution matrix.
Taking a near uniform velocity model (NCV) commonly used in tracking a linearly moving target as an example, a state transition matrix and a noise distribution matrix are respectively as follows:
Figure BDA0002107920320000104
the corresponding state vector at this time is
Figure BDA0002107920320000105
And T is a radar scanning period.
Preferably, in order to accurately describe the constraint relationship, the state is first augmented, and when the state vector is augmented by using the slope and the y-axis intercept parameter component in step S2, the expression of the augmented state vector is obtained as follows:
where a represents the slope of the unknown trajectory, b represents the y-intercept of the unknown trajectory, and the superscript "a" represents the vector, matrix, and function associated with the pseudo-metrology.
Further, after the state vector is augmented in step S2, the state transition matrix and the noise distribution matrix in the state equation corresponding to the augmented state vector also need to be augmented correspondingly, and preferably, the expression of the corresponding augmented state equation is obtained as:
Figure BDA0002107920320000112
wherein, I nAnd 0 m×nRespectively representing an identity matrix of dimension n x n and a zero matrix of dimension m x n.
Preferably, using the augmented state vector, the equation for obtaining the constraint relationship according to the shape characteristics of the prior target trajectory can be described as:
Figure BDA0002107920320000113
it can be seen that the constraint is not limited to a specific straight line, the direction and the position of the constraint can be arbitrary, the prior information contained in the constraint condition is all derived from the shape characteristics of the target real track, no approximation or noise influence is involved, and the estimation accuracy can be effectively improved by introducing a filtering system.
Then, the constraint equation is rewritten into a form of pseudo measurement, that is, an expression for constructing pseudo measurement description constraint relationship is:
Figure BDA0002107920320000114
the pseudo measurement is not affected by noise, and is in a nonlinear relation with the state vector, so that a measurement equation expression corresponding to the pseudo measurement is obtained as follows:
Figure BDA0002107920320000115
according to the method, the augmented track parameters enter the state vector, the complexity of the obtained arbitrary linear constraint model is low, and the nonlinearity of the pseudo-measurement constructed according to the model is also low. The augmented state vector also makes trajectory parameter estimation possible, and the trajectory estimation result is helpful for improving the tracking performance of the follow-up constrained target.
S3, processing the measurement information obtained in the step S1, performing nonlinear filtering by using the processed measurement, the pseudo measurement obtained in the step S2 and the augmented state equation, and updating the state estimation and the state estimation covariance to realize target tracking.
Preferably, when the nonlinear filtering is performed in step S3, the measurement information obtained in step S1 is processed in a sequential measurement processing manner, and an unconstrained estimation result is obtained by using the processed measurement and a corresponding measurement equation; and then processing the pseudo measurement, and updating the constrained state estimation and the state estimation covariance by using a measurement equation corresponding to the pseudo measurement and combining an unconstrained estimation result. The invention adopts a sequential measurement processing mode to process the radar measurement firstly and then process the pseudo measurement, thereby effectively reducing the operation burden.
Because the original sensor measurement and the pseudo measurement are in a nonlinear relationship with the state, a nonlinear filtering method is required in the filtering process, and preferably, the commonly used nonlinear filtering methods include a conversion measurement kalman filtering method, an unscented kalman filtering method, an extended kalman filtering method, a particle filtering method and the like.
In one embodiment, preferably, when performing the nonlinear filtering in step S3, the original measurement information is processed by using a transform measurement kalman filter method, and the pseudo measurement is processed by using an unscented kalman filter method.
Preferably, in step S3, the original measurement information obtained in step S1 is processed by using a transform measurement kalman filter method, and the original measurement information is first transformed from a polar coordinate measurement to a rectangular coordinate measurement. In one embodiment, an unbiased measurement transformation method is preferably adopted, and the specific transformation formula is as follows:
wherein the content of the first and second substances,
Figure BDA0002107920320000122
distance and azimuth measurement obtained from radar;
Figure BDA0002107920320000123
is a converted cartesian coordinate measurement along the x and y directions,
Figure BDA0002107920320000124
is the converted measurement vector; mu.s θIs a de-bias coefficient, and can measure the noise variance according to the azimuth angle
Figure BDA0002107920320000125
Obtaining:
Figure BDA0002107920320000131
the measurement equation expression corresponding to the converted measurement is as follows:
Figure BDA0002107920320000132
the covariance matrix expression is:
Figure BDA0002107920320000133
wherein the content of the first and second substances,
Figure BDA0002107920320000134
the superscript "c" represents the vector, matrix and function associated with the transformed measurement.
Preferably, in step S3, the following steps are performed in sequence for each tracking time in the measurement information by performing nonlinear filtering using the processed measurement, the pseudo measurement obtained in step S2, and the augmented equation of state:
s3-1, if the current time is the first or second time, that is, k is 1 or k is 2, initializing the augmented state vector by using an unscented transformation method to obtain an initial value and an initial covariance of the augmented state vector, that is, implementing filter initialization by using the unscented transformation method; if k >2, the initialization step is skipped and the next step is performed directly.
Further preferably, when the augmented state vector is initialized by the traceless transformation method in step S3-1, the method includes the following steps:
firstly, obtaining a rectangular coordinate measurement obedient mean value through conversion
Figure BDA0002107920320000135
Variance (variance)
Figure BDA0002107920320000141
Is selected from 2n according to the distribution m+1 δ sample points:
Figure BDA0002107920320000142
wherein the content of the first and second substances,
Figure BDA0002107920320000143
represents
Figure BDA0002107920320000144
Row i of the matrix, λ α 2(n+κ)-n mα and kappa are empirical parameters, α is used to determine the spread of the delta samples around the mean, β is used to introduce information about the distributionA priori knowledge of. W i meanAnd W i covThe corresponding weights when the mean value and the covariance are calculated according to the delta sampling points can be obtained through the following formulas:
Figure BDA0002107920320000145
each sampling point can be expressed as:
Figure BDA0002107920320000146
then, according to the non-linear equation:
Figure BDA0002107920320000148
mapping each projection point to obtain a posterior delta sampling point:
Figure BDA0002107920320000149
and finally, weighting and summing to obtain the initial value and the initial covariance of the state vector as follows:
initial value of state vector:
Figure BDA0002107920320000151
corresponding initial covariance:
Figure BDA0002107920320000152
the invention adopts the unscented transformation method to initialize the parameter component according to the relation between the transformed rectangular coordinate position measurement and the parameter, thereby solving the problem that the conventional initial method can not calculate the initial value and covariance of the augmented parameter component.
And S3-2, calculating the state one-step prediction and the one-step prediction covariance by using the augmented state equation.
Starting filtering from the time k-3, preferably, in step S3-2, performing one-step prediction on the state at the time k according to the constrained state estimation (filtering result) obtained after the time k-1 is updated, and calculating the state one-step prediction by the following expression:
Figure BDA0002107920320000153
the expression for calculating the state one-step prediction covariance is:
Figure BDA0002107920320000154
s3-3, combining the state one-step prediction and the one-step prediction covariance, calculating the measurement prediction and the measurement prediction covariance by using the converted measurement and the corresponding measurement equation, and further obtaining the unconstrained state estimation updated by using the converted measurement and the corresponding state estimation covariance.
Preferably, the expression of the measurement prediction is calculated by using the converted position measurement information and the corresponding measurement equation as follows:
the expression for calculating the measurement prediction covariance is:
Figure BDA0002107920320000156
calculating an expression of the corresponding filter gain:
Figure BDA0002107920320000157
and then obtaining the unconstrained state estimation updated by the processed target position measurement information and the corresponding state estimation covariance as follows:
and (3) state estimation:
Figure BDA0002107920320000161
state estimation covariance:
Figure BDA0002107920320000162
in the above-mentioned part, namely, in the manner corresponding to the sequential measurement processing, the measurement information obtained in step S1 is processed, and the unconstrained estimation result part is obtained by using the processed measurement and the corresponding measurement equation.
S3-4, processing the pseudo measurement, and calculating the cross covariance of the measurement and the state vector by using the measurement equation corresponding to the pseudo measurement and combining the unconstrained state estimation and the corresponding state estimation covariance.
And the unscented Kalman filtering method is preferably adopted for processing the pseudo measurement. Preferably, an unscented transformation is performed, the computation being based on an unconstrained state estimate
Figure BDA0002107920320000163
Selected 2n a+1 δ sample points:
Figure BDA0002107920320000164
calculating posterior delta sampling points according to a measurement equation corresponding to the pseudo measurement:
Figure BDA0002107920320000165
calculating a prediction measurement mean value according to sampling points:
Figure BDA0002107920320000166
calculating a covariance matrix corresponding to the predicted measurement:
the expression for calculating the cross-covariance of the metrology and state vectors is:
Figure BDA0002107920320000168
calculating an expression of the corresponding filter gain:
Figure BDA0002107920320000169
wherein n is aFor the purpose of the augmented state vector dimension,
Figure BDA00021079203200001610
Figure BDA00021079203200001611
the calculation of the weight values is performed in the same manner as in the case of filter initialization (k is 1,2), and the description thereof will not be repeated.
And S3-5, updating the constrained state estimation and the corresponding state estimation covariance by combining the unconstrained state estimation and the corresponding state estimation covariance as well as the cross covariance of the measurement and state vectors, and realizing target tracking.
Preferably, the last updating of the state estimate and the covariance comprises:
the expression for computing the constrained state estimate is:
the expression for calculating the corresponding state estimate covariance is:
the above part is corresponding to the mode of sequential measurement processing, pseudo measurement is processed, and the constrained state estimation and state estimation covariance part is updated by using the measurement equation corresponding to the pseudo measurement and combining the unconstrained estimation result.
Augmented filtering results by augmented state vector calculationI.e. the resulting constrained state estimate
Figure BDA0002107920320000173
The target state result at the time k and the track parameter estimation result are included in the target state result. The method provides a new solution for the target tracking problem under the constraint of any straight line, reasonably utilizes the shape characteristic information in the target motion track, avoids information waste and improves the estimation precision.
In summary, the method provided by the invention has the following advantages:
(1) an arbitrary straight line constraint modeling method for simultaneously estimating a target state and a track parameter is provided. As shown in fig. 2, for a target moving along an unknown linear trajectory, a pseudo measurement is constructed to describe a constraint relationship according to the constraint relationship commonly satisfied by all linear trajectories by using a position and speed state component in an augmented state and a slope and y-axis intercept parameter component, and the specific form is as follows:
Figure BDA0002107920320000174
(2) a corresponding effective filtering method is provided for the constructed pseudo measurement. The state and covariance are updated by sequentially utilizing the original measurement and the pseudo measurement of the sensor, so that constraint prior information contained in the sensor and related to a target state is introduced into a tracking system, the available information quantity of a filter is increased, and the purpose of improving the estimation precision is achieved. Fig. 3 shows a schematic flow diagram of a method according to an embodiment of the present invention (which can also be regarded as a corresponding schematic filter structure).
(3) A corresponding filter initialization method is designed for the proposed filtering method. Firstly, a position measurement and covariance thereof under a rectangular coordinate system are obtained by using an unbiased measurement conversion method. And then, an initial value and an initial covariance of the state vector are obtained according to the relation between the target position and the track parameter by using an unscented transformation method.
To verify the effect of the present invention, a Monte Carlo experiment was performed using the simulation data. The target in the simulation test moves at a constant speed along a straight line, and the motion track is shown in fig. 4. At this time, it is assumed that specific prior information of the real straight-line trajectory of the target is unavailable or unreliable, and therefore, the estimation cannot be performed by adopting the traditional constraint estimation method. A standard conversion measurement Kalman filtering method without any constraint and a target tracking method under any straight line constraint for increasing the state of the past moment into a state vector (referred to as an increased past moment state method) are adopted to compare with an arbitrary straight line constraint tracking method for simultaneously estimating the state of the target and the track parameters (referred to as an increased track parameter method). The target tracking method under the constraint of any straight line for amplifying the past time state into the state vector constructs the pseudo measurement by using the relation between the states in the amplified state vector, and introduces the constraint prior information into the tracking system by using the pseudo measurement, so that the performance is improved compared with the unconstrained target tracking method, but the calculated amount of the method is actually increased when the past time state is amplified into the state vector, and the method is not beneficial to application. In the simulation, the radar sampling interval is set to be 1s, the movement of a target is simulated for 200s, and 500 Monte Carlo experiments are repeatedly performed.
Fig. 5 shows the comparison of the root mean square error estimates corresponding to the position filtering results of the three methods, and fig. 6 shows the comparison of the root mean square error estimates corresponding to the speed filtering results of the three methods. It can be seen from fig. 5 and 6 that the estimation accuracy of the target tracking method under both constraints is better than that of the unconstrained target tracking method. This is because the inclusion of useful information about the target state in any straight line constraint increases the amount of information available to the filter, thereby improving the filtering accuracy. The performance of the target tracking method under the constraint of two arbitrary straight lines is very close, which shows that the two methods can effectively utilize constraint information. The method shown in the present invention, however, can provide highly accurate estimation results of trajectory parameters while providing the state estimation results, as shown in fig. 7 and 8. The parameter estimation results can be used as prior information to improve the tracking performance of the tracking system on subsequent targets obeying the same constraint. Furthermore, the method shown in the present invention effectively reduces the computational burden compared to the method of broadening the past time state, as shown in table 1. Under the same simulation condition, the calculation burden is only 71% of that of the method for expanding the past time state, and is reduced by nearly one third compared with the method for expanding the past time state, so that the optimization effect is very obvious.
TABLE 1
Figure BDA0002107920320000191
Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.

Claims (7)

1. An arbitrary straight line constraint tracking method for simultaneously estimating a target state and a track parameter is characterized by comprising the following steps:
s1, obtaining target position measurement information from the observation radar;
s2, amplifying the state vector by using the slope and y-axis intercept parameter component to obtain an amplified state vector and an amplified state equation, and constructing a pseudo measurement description constraint relation;
s3, processing the measurement information obtained in the step S1, performing nonlinear filtering by using the processed measurement, the pseudo measurement obtained in the step S2 and the augmented state equation, and updating the state estimation and the state estimation covariance to realize target tracking;
the step S2 further includes establishing a target motion model in a cartesian coordinate system:
x k+1＝Φ kx k+Γ kv k；
wherein x is kIs a state vector containing the position components x in the x and y directions at time k k、y kAnd velocity component
Figure FDA0002288361740000011
Φ kIs a state transition matrix; v. of kIs the process noise vector; gamma-shaped kIs a noise distribution matrix;
the expressions of the state transition matrix and the noise distribution matrix are respectively:
Figure FDA0002288361740000012
the corresponding state vector is
Figure FDA0002288361740000013
T is a radar scanning period;
when the state vector is augmented by the slope and the y-axis intercept parameter component in step S2, the expression of the augmented state vector is obtained as follows:
Figure FDA0002288361740000014
wherein a represents the slope of the unknown track, b represents the y-axis intercept of the unknown track, and the superscript "a" represents the vector, matrix and function associated with the pseudo-metrology;
in step S2, after the state vector is augmented, the state transition matrix and the noise distribution matrix in the state equation corresponding to the augmented state vector are correspondingly augmented, and the augmented state equation expression is obtained as follows:
Figure FDA0002288361740000021
wherein, I nAnd 0 m×nRespectively representing a unit matrix with dimension n multiplied by n and a zero matrix with dimension m multiplied by n;
when the pseudo metrology description constraint relationship is constructed in S2, the equation of the constraint relationship is:
Figure FDA0002288361740000022
further, the expression for constructing the pseudo metrology description constraint relationship is obtained as follows:
the pseudo measurement is not affected by noise, and is in a nonlinear relation with the state vector, so that a measurement equation expression corresponding to the pseudo measurement is obtained as follows:
Figure FDA0002288361740000024
when the nonlinear filtering is performed in the step S3, the measurement information obtained in the step S1 is processed in a sequential measurement processing manner, and an unconstrained estimation result is obtained by using the processed measurement and a corresponding measurement equation; and then processing the pseudo measurement, and updating the constrained state estimation and the state estimation covariance by using a measurement equation corresponding to the pseudo measurement and combining an unconstrained estimation result.
2. The method for tracking any straight line constraint according to claim 1, wherein in the step S3, one or more of a transition measurement kalman filter method, an unscented kalman filter method, an extended kalman filter method, or a particle filter method is used for performing the nonlinear filtering.
3. The method for tracking any straight line constraint according to claim 1, wherein when performing the nonlinear filtering in step S3, the measurement information obtained in step S1 is processed by using a kalman filter to convert the original measurement information from the polar coordinate measurement to the rectangular coordinate measurement, and the conversion formula is as follows:
Figure FDA0002288361740000031
wherein the content of the first and second substances,
Figure FDA0002288361740000032
distance and azimuth measurement obtained from radar;
Figure FDA0002288361740000033
is a converted cartesian coordinate measurement along the x and y directions,
Figure FDA0002288361740000034
is the converted measurement vector; mu.s θIs a coefficient of depolarization, and the variance of the noise is measured according to the azimuth angle
Figure FDA0002288361740000035
Obtaining:
Figure FDA0002288361740000036
the measurement equation expression corresponding to the converted measurement is as follows:
Figure FDA0002288361740000037
the covariance matrix expression is:
Figure FDA0002288361740000038
wherein the content of the first and second substances,
Figure FDA0002288361740000039
Figure FDA00022883617400000310
Figure FDA00022883617400000311
the superscript "c" represents the vector, matrix and function associated with the transformed measurement.
4. The arbitrary straight-line constraint tracking method according to claim 3, wherein, when performing the nonlinear filtering in step S3, the following steps are sequentially performed for each tracking time:
s3-1, if the current time is the first or the second time, initializing the augmented state vector by using an unscented transformation method to obtain an initial value and an initial covariance of the augmented state vector; otherwise, executing the next step;
s3-2, calculating state one-step prediction and one-step prediction covariance by using the augmented state equation;
s3-3, combining the state one-step prediction and the one-step prediction covariance, calculating the measurement prediction and the measurement prediction covariance by using the converted measurement and the corresponding measurement equation, and further obtaining the unconstrained state estimation updated by using the converted measurement and the corresponding state estimation covariance;
s3-4, processing the pseudo measurement, and calculating the cross covariance of the measurement and the state vector by using a measurement equation corresponding to the pseudo measurement and combining the unconstrained state estimation and the corresponding state estimation covariance;
and S3-5, updating the constrained state estimation and the corresponding state estimation covariance by combining the unconstrained state estimation and the corresponding state estimation covariance as well as the cross covariance of the measurement and state vectors, and realizing target tracking.
5. The method according to claim 4, wherein the initializing the augmented state vector in step S3-1 by using an unscented transformation method comprises:
converting to obtain rectangular coordinate measurement obeying mean value
Figure FDA0002288361740000041
Variance (variance)
Figure FDA0002288361740000042
Is selected from 2n according to the distribution m+1 δ sample points:
Figure FDA0002288361740000043
wherein the content of the first and second substances,
Figure FDA0002288361740000044
represents Row i of the matrix, λ α 2(n+κ)-n mα and kappa are empirical parameters, α is used to determine the spread of the delta samples around the mean, β is used to introduce a priori knowledge about the distribution, W i meanAnd W i covRespectively calculating corresponding weights when the mean value and the covariance are calculated according to delta sampling points:
Figure FDA0002288361740000046
each sample point is:
according to the non-linear equation:
Figure FDA0002288361740000052
mapping each projection point to obtain a posterior delta sampling point:
Figure FDA0002288361740000053
and (3) weighting and summing to obtain an initial value and an initial covariance of the state vector as follows:
Figure FDA0002288361740000054
Figure FDA0002288361740000055
6. the method for tracking any straight line constraint according to claim 5, wherein when the state one-step prediction and the one-step prediction covariance are calculated by using the augmented state equation in step S3-2, the state at the time k is predicted one step according to the constraint state estimation updated at the time k-1, and the expression for calculating the state one-step prediction is as follows:
Figure FDA0002288361740000056
the expression for calculating the state one-step prediction covariance is:
Figure FDA0002288361740000057
in step S3-3, when the measurement prediction and the measurement prediction covariance are calculated by using the converted measurement and the corresponding measurement equation, and further the unconstrained state estimation updated by using the converted measurement and the corresponding state estimation covariance are obtained, the expression for calculating the measurement prediction is as follows:
Figure FDA0002288361740000061
the expression for calculating the measurement prediction covariance is:
Figure FDA0002288361740000062
calculating an expression of the corresponding filter gain:
Figure FDA0002288361740000063
the updated unconstrained state estimate and the corresponding state estimate covariance are obtained as:
Figure FDA0002288361740000064
Figure FDA0002288361740000065
7. the method for tracking constraint on any straight line according to claim 6, wherein in step S3-4, when calculating the cross-covariance of the metrology and state vectors by using the metrology equation corresponding to the pseudo metrology and combining the unconstrained state estimation and the covariance of the corresponding state estimation, performing an unscented transformation, the calculation based on the measured covariance and the unconstrained state estimation
Figure FDA0002288361740000066
Selected 2n a+1 δ sample points:
Figure FDA0002288361740000067
calculating posterior delta sampling points according to a measurement equation corresponding to the pseudo measurement:
Figure FDA0002288361740000068
calculating a prediction measurement mean value according to sampling points:
Figure FDA0002288361740000069
calculating a covariance matrix corresponding to the predicted measurement:
Figure FDA00022883617400000610
the expression for calculating the cross-covariance of the metrology and state vectors is:
Figure FDA0002288361740000071
the expression for calculating the corresponding filter gain is:
Figure FDA0002288361740000072
wherein n is aTo extend the state vector dimension, i ═ 0,1 a，
Figure FDA0002288361740000073
In step S3-5, the unconstrained state estimate and the corresponding state estimate covariance are combined with the cross covariance of the measurement and state vectors, and the constrained state estimate and the corresponding state estimate covariance are updated, and when target tracking is implemented, the expression for calculating the constrained state estimate is:
Figure FDA0002288361740000074
the expression for calculating the corresponding state estimate covariance is:
CN201910559780.6A 2019-06-26 2019-06-26 Arbitrary straight line constraint tracking method for simultaneously estimating target state and track parameters Active CN110208792B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910559780.6A CN110208792B (en) 2019-06-26 2019-06-26 Arbitrary straight line constraint tracking method for simultaneously estimating target state and track parameters

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910559780.6A CN110208792B (en) 2019-06-26 2019-06-26 Arbitrary straight line constraint tracking method for simultaneously estimating target state and track parameters

Publications (2)

Publication Number Publication Date
CN110208792A CN110208792A (en) 2019-09-06
CN110208792B true CN110208792B (en) 2020-02-11

Family

ID=67794548

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910559780.6A Active CN110208792B (en) 2019-06-26 2019-06-26 Arbitrary straight line constraint tracking method for simultaneously estimating target state and track parameters

Country Status (1)

Country Link
CN (1) CN110208792B (en)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111002302B (en) * 2019-09-09 2021-10-22 浙江瀚镪自动化设备股份有限公司 Mechanical arm grabbing track planning method combining Gaussian mixture model and dynamic system
CN110989856B (en) * 2019-11-07 2024-03-22 北京集创北方科技股份有限公司 Coordinate prediction method, device, equipment and storable medium
CN111426322B (en) * 2020-04-09 2020-12-18 北京航空航天大学 Adaptive target tracking filtering method and system for simultaneously estimating state and input
CN111796271B (en) * 2020-07-13 2024-02-23 哈尔滨工业大学 Target tracking method and device under constraint of proportional guidance destination
CN113008222B (en) * 2021-02-20 2023-03-31 西北工业大学 Track constraint target tracking method based on continuous time track function
CN113175931B (en) * 2021-04-02 2022-08-16 上海机电工程研究所 Cluster networking collaborative navigation method and system based on constraint Kalman filtering
CN113189578B (en) * 2021-04-20 2022-09-16 浙江大学 Extended target tracking method
CN113625262A (en) * 2021-08-05 2021-11-09 长沙祥云瑞风信息技术有限公司 Target track determination method and related equipment
CN116609776B (en) * 2023-05-23 2023-11-14 兰州理工大学 Star convex expansion target tracking method based on artificial potential field method in complex environment

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103247057A (en) * 2013-05-08 2013-08-14 西北工业大学 Road target multi-hypothesis tracking algorithm under target-echo-road network data association
CN105652250A (en) * 2016-01-15 2016-06-08 西北工业大学 Maneuvering target tracking technology based on double-layer expectation maximization
CN106054170A (en) * 2016-05-19 2016-10-26 哈尔滨工业大学 Maneuvering target tracking method under constraint conditions
CN108279412A (en) * 2018-01-30 2018-07-13 哈尔滨工业大学 A kind of lower target tracker of destination constraint and method

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103247057A (en) * 2013-05-08 2013-08-14 西北工业大学 Road target multi-hypothesis tracking algorithm under target-echo-road network data association
CN105652250A (en) * 2016-01-15 2016-06-08 西北工业大学 Maneuvering target tracking technology based on double-layer expectation maximization
CN106054170A (en) * 2016-05-19 2016-10-26 哈尔滨工业大学 Maneuvering target tracking method under constraint conditions
CN108279412A (en) * 2018-01-30 2018-07-13 哈尔滨工业大学 A kind of lower target tracker of destination constraint and method

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
State estimation with a destination constraint using pseudo-measurements;Gongjian Zhou等;《Signal Processing》;20171207;第155-166页 *
State Estimation with a Heading Constraint;Zhuanhua Zhang等;《2018 21st International Conference on Information Fusion》;20181231;第253-258页 *
多测速***中***误差实时估计与校准算法;梁小虎等;《飞行器测控学报》;20121031;第31卷(第5期);第49-53页 *

Also Published As

Publication number Publication date
CN110208792A (en) 2019-09-06

Similar Documents

Publication Publication Date Title
CN110208792B (en) Arbitrary straight line constraint tracking method for simultaneously estimating target state and track parameters
CN111985093B (en) Adaptive unscented Kalman filtering state estimation method with noise estimator
CN107045125B (en) Interactive multi-model radar target tracking method based on predicted value measurement conversion
CN106950562B (en) State fusion target tracking method based on predicted value measurement conversion
CN108226920B (en) Maneuvering target tracking system and method for processing Doppler measurement based on predicted value
Liu et al. Unscented extended Kalman filter for target tracking
KR100816269B1 (en) Robust fast simultaneous localization and mapping method applying unscented filter
CN108896986B (en) Measurement conversion sequential filtering maneuvering target tracking method based on predicted value
CN109001699B (en) Tracking method based on noisy destination information constraint
CN110501696B (en) Radar target tracking method based on Doppler measurement adaptive processing
CN110231620B (en) Noise-related system tracking filtering method
CN111693984B (en) Improved EKF-UKF moving target tracking method
Shi et al. Adaptive UKF for target tracking with unknown process noise statistics
CN108319570B (en) Asynchronous multi-sensor space-time deviation joint estimation and compensation method and device
CN108871365B (en) State estimation method and system under course constraint
CN110209180B (en) Unmanned underwater vehicle target tracking method based on HuberM-Cubasic Kalman filtering
CN111965618B (en) Conversion measurement tracking method and system integrating Doppler measurement
CN111487612B (en) CPD-based allopatric configuration radar/ESM track robust correlation method
CN111722213B (en) Pure distance extraction method for maneuvering target motion parameters
CN112034445B (en) Vehicle motion trail tracking method and system based on millimeter wave radar
Zhou et al. State estimation with destination constraints
Konatowski et al. A comparison of estimation accuracy by the use of KF, EKF & UKF filters
Huang et al. State estimation with incomplete linear constraint
CN114858166B (en) IMU attitude resolving method based on maximum correlation entropy Kalman filter
CN107886058B (en) Noise-related two-stage volume Kalman filtering estimation method and system

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