Target tracking system under condition of unstable distance measurement

Abstract

The invention discloses a target tracking system under the condition of unstable distance measurement. The system is carried on an observation platform with a passive sensor and a laser range finder. The system comprises a data preprocessing module used for receiving target ranging data from a laser range finder and target angle measurement data from a passive sensor; the navigation data coordinate reference system conversion module is used for converting the platform speed and the platform acceleration into a local horizontal sight line system; the angle tracker is used for predicting the state of the target in the horizontal plane and the state of the target in the vertical plane in the next period by adopting a Kalman filter, and estimating the angular velocity perpendicular to the sight line; the distance tracker is used for estimating the state of the target slant distance direction in the next period by adopting a Kalman filter; and the priori knowledge processing module is used for converting velocity components in estimation results of the distance tracker and the angle tracker to a local earth system, performing rationality judgment and performing correction according to a judgment result.

Description

Technical Field

[0001] The present invention relates to the technical field of tracking systems, and in particular to a target tracking system under the condition of unstable ranging. Background Art

[0002] For platforms such as aircraft and vehicles to detect and track targets in three-dimensional space, a ranging machine and passive sensors are usually configured. The ranging machine measures the slant range of the target relative to itself, and the passive sensors measure the azimuth angle and elevation angle of the target relative to the center of the sensor. The measured values of the slant range, azimuth angle, and elevation angle of the target are sent to a tracking processor for filtering to obtain the position and velocity estimation of the target in three-dimensional space. Modern target trackers generally use a Kalman filter or a degraded form of the Kalman filter, the alpha-beta filter. The filtering process is divided into two steps: prediction (also known as state update) and measurement update. In prediction, the future target state is predicted based on the current target state by assuming a target motion model. Common target motion models take the earth as a reference, assuming that the target is stationary, moving at a constant speed, or moving with a constant acceleration, etc. This assumption is inaccurate. In the Kalman filter, this inaccuracy of target motion is modeled as system noise. Measurement update depends on the sensor model, which can be simplified to a geometric relationship, and sensors generally have measurement noise.

[0003] Ranging machines such as laser rangefinders are often affected by smoke or backscattering, prone to target loss or unstable situations where the target appears and disappears intermittently, and their continuous working time is short. After continuous ranging, they need to stop for a period of time. When the laser ranging is lost or stops working, if the data is still used to estimate the target motion state, it will cause the tracker to diverge.

[0004] An existing solution to solve the above problems is to keep the slant range measurement unchanged after the laser ranging is lost or stops working, and use passive sensors to continue tracking the target. This solution assumes that the velocity of the target in the slant range direction relative to the sensor platform is 0 during the laser ranging loss process, that is, the velocity value of the target relative to the earth is equal to the velocity value of its own platform and in the opposite direction. This assumption is not true. If the platform where the sensor is located itself has a large velocity, the estimated target velocity will generate a large reverse velocity, and once the laser ranging recaptures the target, it will cause a second impact. Summary of the Invention

[0005] The purpose of the present invention is to provide a target tracking system under the condition of unstable ranging to solve the problem of excessive target velocity error under the condition of unstable ranging.

[0006] To achieve the above task, the present invention adopts the following technical solutions:

[0007] A target tracking system under the condition of unstable ranging. This system is carried on an observation platform equipped with passive sensors and a laser rangefinder. The system includes:

[0008] A data preprocessing module, which is used to receive the target ranging data from the laser rangefinder and the target angle measurement data from the passive sensor, and preprocess the target angle measurement data.

[0009] A navigation data coordinate reference system conversion module, which is used to receive the platform speed in the local geodetic system and the platform acceleration in the body coordinate system output by the navigation sensor of the observation platform, and convert the platform speed and platform acceleration to the local horizontal line of sight system.

[0010] An angle tracker, which is used to predict the state of the target in the horizontal plane and the vertical plane in the next cycle by using a Kalman filter according to the azimuth angle and elevation angle of the target in the local geodetic system in the preprocessed target angle measurement data, the distance in the direction of the slant range from the target to the observation platform itself estimated by the distance tracker, the horizontal plane component and vertical plane component of the platform speed in the local horizontal line of sight system, and the horizontal plane component and vertical plane component of the platform acceleration in the local horizontal line of sight system, and estimate the angular velocity perpendicular to the line of sight.

[0011] A distance tracker, which is used to estimate the state of the target in the slant range direction in the next cycle by using a Kalman filter according to the slant range from the target to itself included in the target ranging data of the laser rangefinder, the slant range direction component of the local speed of the machine in the local horizontal line of sight system, the slant range direction component of the local acceleration of the machine, and the angular velocity perpendicular to the line of sight estimated by the angle tracker.

[0012] A prior knowledge processing module, which is used to convert the speed component in the estimation results of the distance tracker and the angle tracker to the local geodetic system and make a rationality judgment, and make corrections according to the judgment results.

[0013] Further, the preprocessing of the target angle measurement data includes:

[0014] Converting the direction angles of the azimuth angle and elevation angle of the target included in the target angle measurement data relative to the aiming cross to the local geodetic system, judging whether it is new target angle measurement data, and updating the data identifier.

[0015] Further, the local horizontal line of sight system is used as the solution coordinate system. Its R axis is along the connection line from itself to the target, that is, the slant range direction. The H axis is located in the horizontal plane and is orthogonal to the R axis. The V axis is located in the vertical plane and is orthogonal to the R axis. The RHV forms a right-handed system. The local geodetic system uses the north celestial east coordinate system. The body coordinate system takes the forward direction of the axis of the observation platform as the X axis, the right direction perpendicular to the axis as the Y axis, and the upward direction perpendicular to the axis as the Z axis.

[0016] Further, the state of the horizontal plane includes the azimuth of the local horizontal line-of-sight system relative to the local geodetic system, the horizontal plane component of the target relative to the platform velocity in the local horizontal line-of-sight system, and the horizontal plane component of the platform acceleration in the local horizontal line-of-sight system; the state of the vertical plane includes the elevation angle of the local horizontal line-of-sight system relative to the local geodetic system, the vertical plane component of the target relative to the platform velocity in the local horizontal line-of-sight system, and the vertical plane component of the platform acceleration in the local horizontal line-of-sight system; the state in the target slant range direction includes the distance in the slant range direction, the component of the platform velocity in the slant range direction, and the component of the platform acceleration in the slant range direction.

[0017] Further, the conversion of the velocity components in the estimation results of the distance tracker and the angle tracker to the local geodetic system and the rationality judgment, and the correction according to the judgment result include:

[0018] Convert the component of the local machine velocity in the slant range direction of the local horizontal line-of-sight system estimated by the distance tracker in the next cycle, the horizontal plane component and the vertical plane component of the local machine velocity in the local horizontal line-of-sight system estimated by the angle tracker to the local geodetic system, and combine with the local machine velocity to calculate the target velocity relative to the earth.

[0019] When the velocity relative to the earth is less than the possible minimum velocity, set the target velocity relative to the earth to zero; and, the prior knowledge processing module also inversely calculates its components in the three directions of the local horizontal line-of-sight system according to the local machine velocity, and corrects the velocity state estimation of the distance tracker and the angle tracker.

[0020] Further, the processing process of the angle tracker is as follows:

[0021] When receiving new data, if currently in the idle state, initialize the angle tracking state using the target angle measurement data, initialize the initial values of the states of the vertical plane and the horizontal plane using the horizontal and vertical plane components of the platform velocity in the local horizontal line-of-sight system, and initialize the covariance matrices of the horizontal plane and the vertical plane at the same time;

[0022] When receiving new data, if currently in the calculation state, perform Kalman prediction, then calculate the Mahalanobis distance of the horizontal plane and the vertical plane. When the Mahalanobis distance is within the predetermined angle threshold, update the states of the horizontal plane and the vertical plane, and calculate the angular velocity estimation perpendicular to the line of sight;

[0023] When no new data is received and the interval from the last target angle measurement data update time to the current time does not exceed the preset shelf life, if currently in the calculation state, only perform Kalman prediction;

[0024] When no new data is received and the interval from the last target angle measurement data update time to the current time exceeds the preset shelf life, if the current state is the calculation state, it will transition to the idle state and wait for new target angle measurement data to re-initialize the angle tracking.

[0025] Further, perform Kalman prediction, and then calculate the Mahalanobis distances in the horizontal plane and the vertical plane. When the Mahalanobis distance is within the predetermined angle threshold, update the states of the horizontal plane and the vertical plane:

[0026] Step 151: Discretize the continuous system state transition matrix. The calculation method is as follows:

[0027]

[0028] In the formula, I is the identity matrix, dT is the solution period, A H and A V are the results after discretization of the continuous state transition matrices in the horizontal plane and the vertical plane respectively. F H and F V are the continuous state transition matrices in the horizontal plane and the vertical plane respectively:

[0029]

[0030] Where: is the estimated distance of the range tracker for the slant range from the target to the aircraft itself, r H is the projection of the range in the horizontal plane:

[0031]

[0032] Where el is the elevation angle of the target; t H is the target horizontal plane maneuver time constant, t V is the target vertical plane maneuver time constant;

[0033] Step 152: Discretize the continuous system control transfer matrix:

[0034]

[0035] Among them, B H and B V are the results after discretization of the continuous system control transfer matrix in the horizontal plane and the vertical plane respectively; G is the continuous system control transfer matrix of the angle tracker;

[0036] Step 153: Discretize the continuous system noise variance matrix: Using the numerical approximation method, according to the continuous-time noise variance matrices Qc H and Qc V in the horizontal plane and the vertical plane and the system noise driving matrix D, obtain the discrete noise variance matrices Q H and QV ;

[0037] Step 154: Predict the states of the next cycle target in the horizontal plane and the vertical plane:

[0038]

[0039] Step 155: Predict the covariance matrices P H , P V :

[0040] P H = A H * P H * A H '+ Q H , P V = A V * P V * A V '+ Q V

[0041] where the parameter superscript'indicates matrix transpose, and the same applies hereinafter;

[0042] Step 16: Perform Kalman measurement update, including the following sub-steps:

[0043] Step 161: Calculate the Kalman gain matrices K H , K V :

[0044] Pxy H = P H * C', Pxy V = P V * C',

[0045] Pyy H = Rn + C * P H * C', Pyy V = Rn + C * P V * C'

[0046] K H = Pxy H / Pyy H , K V = Pxy V / Pyy V

[0047] In the formula, Pxy H , Pyy H are intermediate variables, C is the measurement matrix of the optoelectronic sensor, and Rn is the measurement noise covariance matrix of the optoelectronic sensor;

[0048] Step 162: Calculate the measurement residuals e in the horizontal plane and the vertical planeH , e V :

[0049]

[0050] Step 163: Calculate the square d2 of the Mahalanobis distance of the horizontal plane and the vertical plane H , d2 V :

[0051] d2 H = e H ' * Pyy H -1 * e H , d2 V = e V ' * Pyy V -1 * e V

[0052] Step 164: Determine whether it is within the threshold. When the squares d2 of the Mahalanobis distances of the horizontal plane and the vertical plane H , d2 V are both less than the predetermined threshold size d2Max, continue to execute Step 165; otherwise, do not execute.

[0053] Step 165: Update the states of the horizontal plane and the vertical plane and the covariance matrices P of the horizontal plane and the vertical plane H , P V :

[0054]

[0055] P H = P H - K H * C * P H , P V = P V - K V * C * P V .

[0056] Furthermore, calculate the angular velocity estimation perpendicular to the line of sight, including:

[0057]

[0058] where ω p represents the angular velocity perpendicular to the line of sight, is the distance in the direction of the slant range from the target estimated by the distance tracker to the aircraft itself, is the state of the horizontal plane and contains the horizontal plane component of the target relative speed to the aircraft in the local horizontal line-of-sight system, is the state of the vertical plane The vertical plane component of the relative speed of the target contained therein in the local horizontal line of sight system.

[0059] Furthermore, the processing process of the distance tracker is as follows:

[0060] When new data is received, if it is currently in the idle state, initialize the slant range tracking state using the target ranging data, initialize the initial value of the state in the slant range direction using the component of the platform speed in the slant range direction in the local horizontal line of sight system, and at the same time initialize the covariance matrix of the slant range direction;

[0061] When new data is received, if it is currently in the calculation state, perform Kalman prediction, calculate the Mahalanobis distance in the slant range direction, and update the state in the slant range direction when the Mahalanobis distance is within a predetermined threshold;

[0062] When no new data is received and the interval from the last update time of the target ranging data to the current time does not exceed the preset shelf life, if it is currently in the calculation state, only perform Kalman prediction;

[0063] When no new data is received and the interval from the last update time of the target ranging data to the current time exceeds the preset shelf life, if it is currently in the calculation state, switch to the idle state and wait for new target ranging data to re-initialize the distance tracking.

[0064] Compared with the prior art, the present invention has the following technical features:

[0065] This solution takes the movement amount of its own platform as a deterministic input, avoiding adding its own movement speed to the target speed during the ranging interruption, resulting in incorrect results. In this solution, the distance tracker and the angle tracker operate independently, and the unstable ranging will not cause divergence in the angle dimension. This solution uses a Kalman filter, which can maintain the prediction without updating the data value after the ranging interruption, uses a dimensionless Mahalanobis distance to eliminate abnormal data, and combines prior knowledge to adjust the estimated state, improving the accuracy and robustness of target tracking. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] Figure 1 It is a schematic diagram of the functional structure of the system of the present invention;

[0067] Figure 2 It is the execution control of distance tracking and angle tracking in the embodiment of the present invention;

[0068] Figure 3 It is the flow chart of Kalman prediction and measurement update in the embodiment. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0069] In this embodiment, an aircraft equipped with an optoelectronic sensor and a laser rangefinder is taken as an example to illustrate the specific implementation process of the present invention. In this embodiment, the local horizontal line-of-sight system of the aircraft is used as the solution coordinate system, where the R-axis is along the line connecting itself to the target, i.e., the slant range direction, the H-axis is in the horizontal plane and orthogonal to the R-axis, the V-axis is in the vertical plane and orthogonal to the R-axis, and the RHV forms a right-handed system; this coordinate system is a rotating coordinate system; the local geodetic system adopts the north-east-down coordinate system; the body coordinate system has the forward direction of the aircraft axis as the X-axis, the right direction perpendicular to the axis as the Y-axis, and the upward direction perpendicular to the axis as the Z-axis.

[0070] The working principle of this embodiment is shown in Figure 1 , and is described in detail as follows:

[0071] The data preprocessing module 01 is used to preprocess the target ranging data from the laser rangefinder and the target angle measurement data from the optoelectronic sensor. The data period of the laser rangefinder is 2 s, and the data period of the optoelectronic sensor is 20 ms. The preprocessing process is as follows:

[0072] Convert the direction angle of the target angle measurement data (including the azimuth angle az and elevation angle el of the target) of the optoelectronic sensor relative to the aiming cross to the local geodetic system, and determine whether it is new target angle measurement data to update the data identifier.

[0073] Navigation data coordinate reference system conversion module: In this embodiment, the output of the aircraft navigation sensor for the local speed of the aircraft includes the northward v IN , eastward v IE and downward v ID components, and the output of the local acceleration of the aircraft includes the forward a bx , rightward a by and downward a bz components in the body coordinate system of the aircraft. However, the solution of the tracking system uses the local horizontal line-of-sight system; therefore, the first coordinate system conversion unit 02 is used to convert the local acceleration of the aircraft to the local horizontal line-of-sight system, and the output is the local acceleration of the aircraft relative to the inertia expressed in the local horizontal line-of-sight system, including the slant range direction component a IR , horizontal plane component a IH and vertical plane component a IV ;

[0074] Use the second coordinate system conversion unit 03 to convert the local speed of the aircraft to the local horizontal line-of-sight system, and the output is the local speed of the aircraft relative to the earth expressed in the local horizontal line-of-sight system, including the slant range direction component v IR , horizontal plane component v IH and vertical plane component v IV .

[0075] An angle tracker 04, which is used to predict the state of the target in the horizontal plane and the vertical plane in the next cycle by using a Kalman filter according to the azimuth angle az and elevation angle el of the target in the local geodetic system in the preprocessed target angle measurement data and the estimated distance of the target to the slant range of the aircraft itself by a distance tracker 05 The horizontal plane component v of the local aircraft speed in the local horizontal line of sight system IV and the vertical plane component v IV 、The horizontal plane component a of the local aircraft acceleration in the local horizontal line of sight system IH and the vertical plane component a IV ,The state of the target in the horizontal plane in the next cycle is predicted by using a Kalman filter and the state in the vertical plane Among them, the state in the horizontal plane Includes the azimuth angle of the local horizontal line of sight system relative to the local geodetic system The horizontal plane component of the target relative to the local aircraft speed in the local horizontal line of sight system and the horizontal plane component of the local aircraft acceleration in the local horizontal line of sight system The state in the vertical plane Then includes the elevation angle of the local horizontal line of sight system relative to the local geodetic system The vertical plane component of the target relative to the local aircraft speed in the local horizontal line of sight system and the vertical plane component of the local aircraft acceleration in the local horizontal line of sight system

[0076] In this embodiment, the angle tracker 04 runs according to the state machine shown in Figure 2 The execution conditions and actions of the state machine are as follows:

[0077] Condition 11: When new data is received, if the current is in the idle state, initialize the angle tracker 04; the initialization content in this embodiment is as follows:

[0078] The initial values of the 3 states in the vertical plane are as follows:

[0079]

[0080] The initial value of the covariance matrix in the vertical plane is as follows:

[0081]

[0082] The initial values of the 3 states in the horizontal plane are as follows:

[0083]

[0084] The initial value of the covariance matrix in the horizontal plane is:

[0085]

[0086] After the above initialization is completed, save the current angle measurement update time and enter the calculation state.

[0087] Condition 12: When new data is received, if the current state is the calculation state, then sequentially execute Step 15 Kalman prediction and Step 16 Kalman measurement update.

[0088] Condition 13: When no new data is received and the interval from the last target ranging data update time to the current time does not exceed the preset shelf life, if the current state is the calculation state, then only execute Step 15 Kalman prediction; in this embodiment, the shelf life of the angle tracker is 10 s.

[0089] Condition 14: When no new data is received and the interval from the last target angle measurement data update time to the current time exceeds the preset shelf life, if the current state is the calculation state, then transfer to the idle state and wait for new target angle measurement data to re-initialize the angle tracking.

[0090] Step 15: Execute Kalman prediction, as Figure 3 shown, including the following sub-steps:

[0091] Step 151: Discretize the continuous system state transition matrix, and the calculation method is as follows:

[0092]

[0093] In the formula, I is the identity matrix, dT is the solution period, which is set to a fixed value of 0.02 in this embodiment, A H 、A V are the results after discretization of the continuous state transition matrices in the horizontal plane and the vertical plane respectively, F H and F V are the continuous state transition matrices in the horizontal plane and the vertical plane respectively, and the numerical values are as follows:

[0094]

[0095] Among them: is the estimated distance of the distance tracker 05 to the slant range from the target to the aircraft itself, r H is the projection of the distance in the horizontal plane, which is calculated according to the slant range and the elevation angle as follows:

[0096]

[0097] where el is the elevation angle of the target; t H is the target horizontal plane maneuver time constant, which is related to the motion characteristics of the target. In this embodiment, it is assumed that t H = 60 s; t V is the target vertical plane maneuver time constant, which is related to the motion characteristics of the target. In this embodiment, it is assumed that t V= 60 s.

[0098] Step 152: Discretize the continuous system control transfer matrix, and the calculation method is as follows:

[0099]

[0100] where, R H , R V are the results of discretizing the continuous system control transfer matrix in the horizontal plane and the vertical plane respectively; G is the continuous system control transfer matrix of the angle tracker:

[0101]

[0102] Step 153: Discretize the continuous system noise variance matrix: Adopt the numerical approximation method. According to the continuous-time noise variance matrices Qc H , Qc V in the horizontal plane and the vertical plane and the system noise driving matrix D, obtain the discrete noise variance matrices Q H and Q V ; The continuous-time noise variance matrix Qc in this embodiment characterizes the acceleration noise, and its value is Qc H = Qc V = 4, D = [0; 0; 1].

[0103] Step 154: Predict the state of the target in the horizontal plane and the vertical plane in the next period:

[0104]

[0105] where, the on the right side of the equation is the state of the target in the horizontal plane and the vertical plane in this period, and the on the left side of the equation is the predicted state of the target in the horizontal plane and the vertical plane in the next period; The meanings of the following formulas are similar and will not be elaborated.

[0106] Step 155: Predict the covariance matrices P H , P V :

[0107] P H = A H * P H * A H '+ Q H , P V = A V * P V * A V '+ Q V

[0108] where, the parameter A HThe superscript '′' represents matrix transpose, and the same applies hereinafter.

[0109] Step 16: Perform Kalman measurement update, as Figure 3 shown, including the following sub-steps:

[0110] Step 161: Calculate the Kalman gain matrices K H 、K V :

[0111] Pxy H = P H *C′, Pxy V = P V *C′,

[0112] Pyy H = Rn + C * P H *C′, Pyy V = Rn + C * P V *C′

[0113] K H = Pxy H / Pyy H , K V = Pxy V / Pyy V

[0114] In the formula, Pxy H 、Pyy H are intermediate variables, C is the measurement matrix of the optoelectronic sensor, and Rn is the measurement noise covariance matrix of the optoelectronic sensor. The values in this embodiment are as follows:

[0115] C =

[100] , Rn = 0.00004rad 2 ;

[0116] Step 162: Calculate the measurement residuals e H 、e V :

[0117]

[0118] Step 163: Calculate the squares of the Mahalanobis distances d2 H 、d2 V :

[0119] d2 H = e H ′ * Pyy H -1 * e H , d2 V = e V ′ * Pyy V-1 *e V

[0120] In this embodiment, the calculation of the size of the predetermined gate set to d2max is as follows:

[0121] d2Max = 3 2 *Measurement dimension = 9

[0122] Step 164: Determine whether it is within the threshold. When the squares of the Mahalanobis distances d2 H and d2 V of the horizontal plane and the vertical plane are both less than the predetermined gate size d2Max, proceed to Step 165; otherwise, do not execute.

[0123] Step 165: Update the states of the horizontal plane and the vertical plane and the covariance matrices P H and P V of the horizontal plane and the vertical plane:

[0124]

[0125] P H = P H - K H * C * P H , P V = P V - K V * C * P V

[0126] Step 166: The angle tracker 04 calculates the estimated angular velocity ω perpendicular to the line of sight p :

[0127]

[0128] Among them, is the distance in the direction of the slant range from the target to the aircraft itself estimated by the distance tracker 05, is the state of the horizontal plane and contains the horizontal plane component of the target relative speed to the local aircraft in the local horizontal line of sight system, is the state of the vertical plane and contains the vertical plane component of the target relative speed to the local aircraft in the local horizontal line of sight system.

[0129] Distance tracker 05: Used to calculate, based on the target ranging data of the laser rangefinder, the slant range r from the target to itself, the slant range direction component v IR of the local aircraft speed in the local horizontal line of sight system, the slant range direction component a IR of the local aircraft acceleration, and the estimated angular velocity ω p, the Kalman filter is used to estimate the state of the target slant range direction in the next cycle The distance including the slant range direction The component of the local speed in the slant range direction The component of the local acceleration in the slant range direction

[0130] In this embodiment, the angle tracker operates at a 20 - ms cycle as Figure 2 shown in the state machine, and the execution conditions and actions of the state machine are as follows:

[0131] Condition 11: When new data is received, if the current state is the idle state, initialize the distance tracker, and the content is as follows:

[0132] The state of the target slant range direction The initial value is:

[0133]

[0134] The initial value of the variance matrix in the slant range direction is:

[0135]

[0136] Condition 12: When new data is received, if the current state is the calculation state, perform Step 15 Kalman prediction and Step 16 Kalman measurement update.

[0137] Condition 13: When new data is not received and the interval from the last target ranging data update time to the current time does not exceed the preset shelf life, if the current state is the calculation state, only perform Step 15 Kalman prediction; in this embodiment, the shelf life of the distance tracker is set to 10 s.

[0138] Condition 14: When new data is not received and the interval from the last target ranging data update time to the current time exceeds the preset shelf life, if the current state is the calculation state, transition to the idle state and wait for new target ranging data to re - initialize the distance tracking.

[0139] Step 15: Perform Kalman prediction, as Figure 3 shown, and it includes the following sub - steps:

[0140] Step 151: Discretize the continuous system state transition matrix, and the calculation method is as follows:

[0141]

[0142] where I is the identity matrix, dT is the solution period, which is set to a fixed value of 0.02 in this embodiment, and A R is the result after discretizing the continuous state transition matrix in the slant range direction, and F Ris the continuous state transition matrix in the slant range direction:

[0143]

[0144] where t R is the maneuver time constant in the target slant range direction, related to the motion characteristics of the target. Assume t R = 60 s.

[0145] Step 152: Discretize the continuous system control transfer matrix. The calculation method is as follows:

[0146]

[0147] where B R is the discretization result of the continuous system control transfer matrix in the slant range direction, and G R is the continuous system control transfer matrix in the slant range direction:

[0148]

[0149] Step 153: Discretize the continuous system noise variance matrix: Using the numerical approximation method, based on the continuous-time noise variance matrix Q CR in the slant range direction and the system noise driving matrix D, obtain the discrete noise variance matrix Q R in the slant range direction. In this embodiment, Qc represents the acceleration noise, and its value is Q CR = 4, D = [0; 0; 1].

[0150] Step 154: Predict the target slant range direction state in the next cycle

[0151]

[0152] Step 155: Predict the covariance matrix P R in the slant range direction:

[0153] P R = A R * P R * A R '+ Q R

[0154] Step 16: Perform the Kalman measurement update, as Figure 3 shown, including the following sub-steps:

[0155] Step 161: Calculate the Kalman gain matrix K R in the slant range direction:

[0156] Pxy R = P R * C'

[0157] Pyy R = R nR + C * P R * C′

[0158] K R = Pxy R / Pyy R

[0159] Where Pxy R , Pyy R are intermediate variables, C R is the measurement matrix of the laser rangefinder, R nR is the measurement noise covariance matrix of the laser rangefinder. In this embodiment, the values are as follows:

[0160] C R = [10 0], R nR = 16m 2

[0161] Step 162: Calculate the measurement residual in the slant range direction:

[0162]

[0163] Step 163: Calculate the square d2 of the Mahalanobis distance in the slant range direction R :

[0164] d2 R = e R ′ * Pyy R -1 * e R

[0165] In this embodiment, the calculation of the size of the predetermined gate d2max is as follows:

[0166] d2Max = 3 2 * Measurement dimension = 9

[0167] Step 164: Determine whether it is within the threshold. When the square d2 of the Mahalanobis distance in the slant range direction R is less than the size of the predetermined gate d2Max, then continue to execute Step 165, otherwise do not execute.

[0168] Step 165: Update the state estimate and covariance matrix:

[0169]

[0170] P R = P R - K R * C * P R

[0171] Prior knowledge processing module 06: the component of the local speed in the next cycle estimated by the distance tracker 05 in the slant range direction of the local horizontal line of sight system The component of the local speed in the next cycle estimated by the angle tracker 04 in the horizontal plane of the local horizontal line of sight system Vertical plane component Convert it to the local geodetic system, combine with the local speed, and calculate the speed of the target relative to the ground; if the speed of the target relative to the ground is less than the possible minimum speed, then set the speed of the target relative to the ground to zero; and, the prior knowledge processing module 06 also reversely calculates its components in the three directions of the local horizontal line of sight system according to the local speed and corrects the estimation results of the speed by the distance tracker 05 and the angle tracker 04 Make corrections.

[0172] In this embodiment, the target is a ground vehicle. According to prior knowledge, the speed of the vehicle will not be lower than 5 m / s when it is in motion. If the minimum speed of the target relative to the ground estimated by the tracker is less than 5 m / s, then the speed is forced to be set to 0.

[0173] The above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit it; although the present application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application, and should all be included in the protection scope of the present application.

Claims

1. A target tracking system under the condition of unstable ranging, characterized in that The system is mounted on an observation platform equipped with passive sensors and a laser rangefinder; the system includes: A data preprocessing module, configured to receive target ranging data from the laser rangefinder and target angle measurement data from the passive sensors, and preprocess the target angle measurement data; A navigation data coordinate reference system conversion module, configured to receive the platform speed in the local geodetic system and the platform acceleration in the body coordinate system output by the navigation sensors of the observation platform, and convert the platform speed and platform acceleration to the local horizontal line of sight system; An angle tracker, configured to use a Kalman filter to predict the state of the target in the horizontal plane and the state in the vertical plane in the next cycle, and estimate the angular velocity perpendicular to the line of sight, based on the azimuth and elevation angles of the target in the local geodetic system in the preprocessed target angle measurement data, the distance in the direction of the slant range from the target to the observation platform itself estimated by the distance tracker, the horizontal plane component and the vertical plane component of the platform speed in the local horizontal line of sight system, and the horizontal plane component and the vertical plane component of the platform acceleration in the local horizontal line of sight system; A distance tracker, configured to use a Kalman filter to estimate the state of the target in the slant range direction in the next cycle, based on the slant range from the target to itself included in the target ranging data of the laser rangefinder, the slant range direction component of the local machine speed in the local horizontal line of sight system, the slant range direction component of the local machine acceleration, and the angular velocity estimation perpendicular to the line of sight by the angle tracker; A prior knowledge processing module, configured to convert the speed components in the estimation results of the distance tracker and the angle tracker to the local geodetic system, perform a rationality judgment, and make corrections according to the judgment results.

2. The target tracking system under the condition of unstable ranging according to claim 1, characterized in that The preprocessing of the target angle measurement data includes: Converting the direction angles of the azimuth and elevation angles of the target included in the target angle measurement data relative to the aiming cross to the local geodetic system, determining whether it is new target angle measurement data, and updating the data identifier.

3. The target tracking system under the condition of unstable ranging according to claim 1, characterized in that, The local horizontal line of sight system is used as the solution coordinate system, where the R axis is along the connection line from itself to the target, i.e., the slant range direction, the H axis is located in the horizontal plane and orthogonal to the R axis, the V axis is located in the vertical plane and orthogonal to the R axis, and the RHV forms a right-handed system; the local geodetic system uses the north-east-down coordinate system; the body coordinate system has the forward axis of the observation platform as the X axis, the right axis perpendicular to the axis as the Y axis, and the upward axis perpendicular to the axis as the Z axis.

4. The target tracking system under the condition of unstable ranging according to claim 1, characterized in that The state of the horizontal plane includes the azimuth of the local horizontal line of sight system relative to the local geodetic system, the horizontal plane component of the target relative to the platform speed in the local horizontal line of sight system, and the horizontal plane component of the platform acceleration in the local horizontal line of sight system; the state of the vertical plane includes the elevation of the local horizontal line of sight system relative to the local geodetic system, the vertical plane component of the target relative to the platform speed in the local horizontal line of sight system, and the vertical plane component of the platform acceleration in the local horizontal line of sight system; the state of the target slant range direction includes the distance in the slant range direction, the component of the platform speed in the slant range direction, and the component of the platform acceleration in the slant range direction.

5. The target tracking system under the condition of unstable ranging according to claim 1, wherein The conversion of the speed components in the estimation results of the distance tracker and the angle tracker to the local geodetic system, the rationality judgment, and the correction according to the judgment results include: Convert the component of the local speed in the slant range direction of the local horizontal line of sight system and the components of the local speed in the horizontal plane and vertical plane of the local horizontal line of sight system estimated by the distance tracker and angle tracker in the next cycle to the local geodetic system, and combine with the local speed to calculate the target speed relative to the ground; When the speed relative to the ground is less than the possible minimum speed, set the target speed relative to the ground to zero; and, the prior knowledge processing module also inversely calculates its components in the three directions of the local horizontal line of sight system according to the local speed, and corrects the speed state estimates of the distance tracker and angle tracker.

6. The target tracking system under the condition of unstable ranging according to claim 1, wherein The processing process of the angle tracker is as follows: When receiving new data, if currently in the idle state, initialize the angle tracking state using the target angle measurement data, initialize the initial state values of the vertical plane and horizontal plane using the horizontal and vertical plane components of the platform speed in the local horizontal line of sight system, and initialize the covariance matrices of the horizontal plane and vertical plane at the same time; When receiving new data, if currently in the calculation state, perform Kalman prediction, then calculate the Mahalanobis distances of the horizontal plane and vertical plane. When the Mahalanobis distance is within the predetermined angle threshold, update the states of the horizontal plane and vertical plane, and calculate the angular velocity estimate perpendicular to the line of sight; When no new data is received and the interval from the last target angle measurement data update time to the current time does not exceed the preset shelf life, if currently in the calculation state, only perform Kalman prediction; When no new data is received and the interval from the last target angle measurement data update time to the current time exceeds the preset shelf life, if currently in the calculation state, transition to the idle state and wait for new target angle measurement data to re-initialize the angle tracking.

7. The target tracking system under the condition of unstable ranging according to claim 6, characterized in that, Perform Kalman prediction, then calculate the Mahalanobis distances of the horizontal plane and vertical plane. When the Mahalanobis distance is within the predetermined angle threshold, update the states of the horizontal plane and vertical plane: Step 151: Discretize the continuous system state transition matrix, and the calculation method is as follows: where I is the identity matrix, dT is the solution period, and A H , A V are the discretized results of the continuous state transition matrices in the horizontal plane and the vertical plane respectively, and F H and F V are the continuous state transition matrices in the horizontal plane and the vertical plane respectively: Wherein: is the estimated distance of the distance tracker for the slant range from the target to the aircraft itself, r H is the projection of the distance on the horizontal plane: where el is the elevation angle of the target; t H is the maneuvering time constant of the target in the horizontal plane, and t V is the maneuvering time constant of the target in the vertical plane; Step 152: Discretize the continuous system control transition matrix: Among them, B H , B V are respectively the results after discretization of the continuous system control transfer matrix in the horizontal plane and the vertical plane; G is the continuous system control transfer matrix of the angle tracker; Step 153: Discretization of the continuous system noise variance matrix: Using a numerical approximation method, based on the continuous-time noise variance matrices Qc H , Qc V and the system noise driving matrix D, obtain the discrete noise variance matrices Q H and Q V ; Step 154: Predict the states of the target in the horizontal plane and vertical plane in the next cycle: Step 155: Predict the covariance matrices P of the horizontal plane and the vertical plane H , P V : P H = A H * P H * A H ′ + Q H , P V = A V * P V * A V ′ + Q V Where the parameter superscript ′ represents matrix transpose, and the same applies hereinafter; Step 16: Perform Kalman measurement update, including the following sub-steps: Step 161: Calculate the Kalman gain matrices K H and K V : Pxy H = P H * C′, Pxy V = P V * C′, Pyy H = Rn + C * P H * C′, Pyy V = Rn + C * P V * C′ K H = Pxy H / Pyy H , K V = Pxy V / Pyy V where Pxy H , Pyy H are intermediate variables, C is the measurement matrix of the optoelectronic sensor, and Rn is the measurement noise covariance matrix of the optoelectronic sensor; Step 162: Calculate the measurement residuals e of the horizontal plane and the vertical plane H , e V : Step 163: Calculate the squares d2 of the Mahalanobis distances of the horizontal plane and the vertical plane H and d2 V : d2 H = e H ′ * Pyy H -1 * e H , d2 V = e V ′ * Pyy V -1 * e V Step 164: Determine whether it is within the threshold. When the squares of the Mahalanobis distances d2 H and d2 V are both less than the predetermined gate size d2Max, then continue to execute Step 165; otherwise, do not execute. Step 165: Update the states of the horizontal plane and the vertical plane and the covariance matrices P of the horizontal plane and the vertical plane H , P V : P H = P H - K H * C * P H , P V = P V - K V * C * P V 。 8. The target tracking system according to claim 6, wherein Calculate the angular velocity estimate perpendicular to the line of sight, including: Among them, ω p represents the angular velocity perpendicular to the line of sight, is the distance in the direction of the slant range from the target estimated by the distance tracker to the aircraft itself, is the state of the horizontal plane and is the horizontal plane component of the relative velocity of the target included in the state in the local horizontal line-of-sight system, and is the vertical plane component of the relative velocity of the target included in the state 9. The target tracking system under the condition of unstable ranging according to claim 1, wherein The processing process of the distance tracker is as follows: When receiving new data, if currently in the idle state, initialize the slant range tracking state using the target range measurement data, initialize the initial state value in the slant range direction using the slant range direction component of the platform speed in the local horizontal line of sight system, and initialize the covariance matrix in the slant range direction at the same time; When receiving new data, if currently in the calculation state, perform Kalman prediction, calculate the Mahalanobis distance in the slant range direction. When the Mahalanobis distance is within the predetermined threshold, update the state in the slant range direction; When no new data is received and the interval from the last target range measurement data update time to the current time does not exceed the preset shelf life, if currently in the calculation state, only perform Kalman prediction; When no new data is received and the interval from the last target ranging data update time to the current time exceeds the preset shelf life, if the current state is the calculation state, it will transition to the idle state and wait for new target ranging data to re-initialize the distance tracking.

Images