Target tracking method based on heterogenous fusion sequential Kalman filtering

Abstract

The invention discloses a target tracking method based on heterogenous fusion sequential Kalman filtering. The target tracking method comprises the following steps: 1) acquiring multi-sensor measurement information; the sensor comprises a radar and a photoelectric device; 2) performing time registration on multi-sensor measurement information; and 3) based on sequential Kalman filtering, fusing the multi-sensor measurement information as a final tracking result. According to the invention, the measurement information of the radial speed and the radial acceleration is constructed by using the angle information acquired by the photoelectric equipment, and the measurement information is fused with the measurement information of the radar, so that the tracking precision of the target is improved.

Description

Technical Field

[0001] The invention relates to sensor signal processing technology, and in particular to a target tracking method based on heterogeneous fusion sequential Kalman filtering. Background Art

[0002] The reliability and stability of multi-sensor joint tracking are strong. Common target tracking sensors include radar, optoelectronic equipment, etc. Radar, as a common active sensor, can provide three-dimensional measurement information of the target. Optoelectronic equipment is usually a passive sensor and can provide high-precision target angle dimension information. When radar and optoelectronic equipment are used to jointly track the target, the data measured by the heterogeneous sensors need to be spatiotemporally aligned due to the inconsistent sampling frequency and coordinate system between the radar and optoelectronic equipment. The existing multi-heterogeneous sensor information fusion is mainly data-level fusion, which is fused after spatial coordinate conversion. However, the data rates of multi-heterogeneous sensors are inconsistent, resulting in differences in target revisit cycles of different sensors. In addition, the measurement data of heterogeneous sensors have differences due to sensor characteristics and data characteristics. The accuracy of direct fusion cannot be guaranteed, and the problems of time alignment and heterogeneous information fusion need to be solved. Summary of the invention

[0003] The technical problem to be solved by the present invention is to provide a target tracking method based on heterogeneous fusion sequential Kalman filtering in view of the defects in the prior art.

[0004] The technical solution adopted by the present invention to solve the technical problem is: a target tracking method based on heterogeneous fusion sequential Kalman filtering, comprising the following steps:

[0005] 1) Acquiring multi-sensor measurement information; the sensors include radar and optoelectronic equipment;

[0006] 2) Temporal registration of multi-sensor measurement information;

[0007] Time registration refers to aligning the sampling time of asynchronous measurement information of each sensor to the same time when multiple sensors are measuring data;

[0008] To implement time registration, time interpolation must be performed first and then time alignment. The main time interpolation methods include linear interpolation, curve interpolation, etc.

[0009] 3) Based on sequential Kalman filtering, multi-sensor measurement information is fused;

[0010] 3.1) Construct the measurement equation with radial velocity and radial acceleration using the angle dimension information obtained by the radar and the distance dimension information obtained by the optoelectronic device;

[0011] 3.2) The radar measurement information is first filtered using the standard Kalman filter, and then the state posteriori is used to perform nonlinear filtering on the optoelectronic device measurement information with radial velocity and radial acceleration as the final tracking result.

[0012] According to the above scheme, in step 2), a time interpolation method based on cubic spline interpolation is used for time alignment.

[0013] According to the above scheme, in step 2), the time interpolation method based on cubic spline interpolation is as follows:

[0014] Assume that the sensor measures the target n+1 times in a certain time period [a, b], and divides the entire time period into a=t0<t1<...<t n = b, the observation value corresponding to a given moment is f(t i )=y i , i=0,1,…,n;

[0015] Construct a cubic spline interpolation function s(x), and use the Hermit interpolation method to construct a bivariate cubic spline interpolation function:

[0016]

[0017] Interpolate the sensor measurement information.

[0018] According to the above scheme, the interpolation processing is: using the constructed cubic spline interpolation function to obtain a smooth curve, using the curve to obtain the measurement values ​​of the radar and the optoelectronic device at any time, and then aligning the sampling times of all sensors to complete the time interpolation processing.

[0019] According to the above scheme, in step 3.1), the measurement equation with radial velocity and radial acceleration is measured by radar and photoelectric equipment measurement Together we get:

[0020]

[0021] Where h(·) is the observation function, X(k) is the state vector, which consists of the position term, velocity term, and acceleration term of each dimension; W(k) is the measurement noise;

[0022] The terms of h(·) are:

[0023]

[0024] Among them, the measurement information of optoelectronic equipment Include azimuth and pitch angle

[0025] According to the above scheme, the step 3.2) first uses the standard Kalman filter to filter the radar measurement information, and then uses the state posteriori to perform nonlinear filtering on the optoelectronic device measurement information with radial velocity and radial acceleration;

[0026] The specific steps are as follows:

[0027] Assume that the state estimation and covariance matrix of the target at time k-1 are and P k-1|k-1 , state quantity One-step prediction for:

[0028]

[0029] Among them, F k-1 is the state transfer matrix. The corresponding prediction error covariance matrix P k|k-1 for:

[0030]

[0031] Among them, Q k-1 is the process noise covariance matrix;

[0032] Utilization Measurement The new information v k for:

[0033]

[0034] Among them, H k is the observation matrix. The corresponding innovation covariance matrix S k for:

[0035]

[0036] Among them, R k-1 is the measurement noise covariance matrix. Kalman gain K k for:

[0037] State Posterior Estimation for:

[0038]

[0039] The error covariance matrix P corresponding to the state posterior estimate k|k For: P k|k =(IK k H k ) k|k-1 ;

[0040] One step prediction for optoelectronic device measurements is:

[0041]

[0042] in,

[0043]

[0044] Utilization Measurement The new information obtained e,k for:

[0045]

[0046] The corresponding innovation covariance matrix S e,k for:

[0047]

[0048] Kalman gain K e,k for:

[0049]

[0050] State Posterior Estimation for:

[0051]

[0052] The error covariance matrix P corresponding to the state posterior estimate e,k|k for:

[0053] P e,k|k =(IK e,k H e,k ) k|k ;

[0054] The final state posterior is the filtered position estimate in the rectangular coordinate system, P e,k|k is the error covariance matrix estimate after filtering, which can be used for error analysis;

[0055] The posterior state in the polar coordinate system can be obtained through the nonlinear coordinate transformation relationship. The transformation formula is as follows:

[0056]

[0057] The beneficial effects produced by the present invention are:

[0058] 1. In order to solve the problem of time registration of multi-sensor measurement information, the present invention applies the cubic spline interpolation method to construct a cubic spline interpolation function for time interpolation, which can obtain higher registration accuracy and smoothness.

[0059] 2. The present invention utilizes the angle information obtained by the optoelectronic device to construct the measurement information of radial velocity and radial acceleration, fuses it with the measurement information of the radar, and then designs a heterogeneous fusion sequential Kalman filter algorithm to improve the tracking accuracy of the target. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] The present invention will be further described below with reference to the accompanying drawings and embodiments, in which:

[0061] Figure 1 is a method flow chart of an embodiment of the present invention;

[0062] Figure 2 is a time interpolation effect diagram of a multi-source sensor according to an embodiment of the present invention;

[0063] Figure 3 is a comparison diagram of interpolation errors of different interpolation algorithms according to an embodiment of the present invention;

[0064] Figure 4 is a comparison diagram of the trajectory and heterogeneous measurement fusion sequential Kalman filtering posterior of an embodiment of the present invention;

[0065] Figure 5 1 is a comparison diagram of position errors between a linear Kalman filter and a heterogeneous fusion sequential Kalman filter according to an embodiment of the present invention;

[0066] Figure 6 is a comparison diagram of position filtering errors in the X / Y / Z directions of an embodiment of the present invention;

[0067] Figure 7 It is a speed error comparison diagram of the linear Kalman filter and the heterogeneous fusion sequential Kalman filter according to an embodiment of the present invention. DETAILED DESCRIPTION

[0068] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0069] like Figure 1 As shown, a target tracking method based on heterogeneous fusion sequential Kalman filtering includes the following steps:

[0070] 1) Obtain multi-sensor measurement information; sensors include radar and optoelectronic equipment;

[0071] 2) Temporal registration of multi-sensor measurement information;

[0072] Time registration refers to aligning the sampling time of asynchronous measurement information of each sensor to the same time when multiple sensors are measuring data;

[0073] To implement time registration, time interpolation must be performed first and then time alignment. The main time interpolation methods include linear interpolation, curve interpolation and other methods. In this embodiment, a time interpolation method based on cubic spline interpolation is used for time registration.

[0074] The steps of the time interpolation method based on cubic spline interpolation are as follows:

[0075] Assume that the sensor measures the target n+1 times in a certain time period [a, b], and divides the entire time period into a=t0<t1<...<t n = b, the observation value corresponding to a given moment is f(t i )=y i , i=0,1,…,n;

[0076] Construct a cubic spline interpolation function s(x), and use the Hermit interpolation method to construct a bivariate cubic spline interpolation function:

[0077]

[0078] The sensor measurement information is interpolated. The interpolation process is as follows: a smooth curve is obtained using the constructed cubic spline interpolation function. The measurement values ​​of the radar and optoelectronic equipment at any time are obtained through the curve. Then the sampling times of all sensors are aligned to complete the time interpolation process.

[0079] 3) Based on sequential Kalman filtering, multi-sensor measurement information is fused;

[0080] 3.1) Using the angle dimension information obtained by the radar and the distance dimension information obtained by the active sensor, a measurement equation with radial velocity and radial acceleration is constructed;

[0081] Measurement equations with radial velocity and radial acceleration, measured by radar and photoelectric equipment measurement Together we get:

[0082]

[0083] Where h(·) is the observation function, X(k) is the state vector, which consists of the position term, velocity term, and acceleration term of each dimension; W(k) is the measurement noise;

[0084] The terms of h(·) are:

[0085]

[0086] Among them, the measurement information of optoelectronic equipment Include azimuth and pitch angle

[0087] 3.2) The radar measurement information is first filtered using the standard Kalman filter, and then the state posteriori is used to perform nonlinear filtering on the optoelectronic device measurement information with radial velocity and radial acceleration as the final tracking result.

[0088] The specific steps are as follows:

[0089] Assume that the state estimation and covariance matrix of the target at time k-1 are and P k-1|k-1 , state quantity One-step prediction for:

[0090]

[0091] Among them, F k-1 is the state transfer matrix. The corresponding prediction error covariance matrix P k|k-1 for:

[0092]

[0093] Among them, Q k-1 is the process noise covariance matrix;

[0094] Utilization Measurement The new information obtained k for:

[0095]

[0096] Among them, H k is the observation matrix. The corresponding innovation covariance matrix S k for:

[0097]

[0098] Among them, R k-1 is the measurement noise covariance matrix. Kalman gain K k for:

[0099]

[0100] State Posterior Estimation for:

[0101]

[0102] The error covariance matrix P corresponding to the state posterior estimate k|k For: P k|k =(IK k H k ) k|k-1 ;

[0103] One step prediction for optoelectronic device measurements is:

[0104]

[0105] in,

[0106]

[0107] Utilization Measurement The new information obtained e,k for:

[0108]

[0109] The corresponding innovation covariance matrix S e,k for:

[0110]

[0111] Kalman gain K e,k for:

[0112]

[0113] State Posterior Estimation for:

[0114]

[0115] The error covariance matrix P corresponding to the state posterior estimate e,k|k for:

[0116] P e,k|k =(IK e,k H e,k ) k|k ;

[0117] The final state posterior is the filtered position estimate in the rectangular coordinate system, P e,k|k is the error covariance matrix estimate after filtering, which can be used for error analysis;

[0118] The posterior state in the polar coordinate system can be obtained through the nonlinear coordinate transformation relationship. The transformation formula is as follows:

[0119]

[0120] Effect verification:

[0121] Assume that a weak target at sea starts to maneuver 50 kilometers away from our fleet, and our radar detects the weak target at 50 kilometers. The weak target has the characteristics of strong stealth penetration capability, long range, and low flight speed. Assume that the radar sampling rate is Ts = 2s and the sampling time is T = 200s.

[0122] The radar sampling data is used as the interpolation point, and the data rate after interpolation is 1s, and a total of 100 interpolation results are obtained. The radar measurement data of each interpolation point is brought into the interpolation function of different interpolation methods to verify the time interpolation algorithm, and the output is the interpolation result.

[0123] According to the preset trajectory, the interpolation result in 100s should be 1.75km. The interpolation results obtained by different interpolation methods are as follows Figure 2 As shown in the figure, the interpolation result of linear interpolation is 1.8017km, the interpolation result of second-order Hermit interpolation is 1.7270km, and the interpolation result of cubic spline interpolation is 1.7403km. It can be seen that cubic spline interpolation can obtain more accurate interpolation results. Figure 3 Error analysis for cubic spline interpolation.

[0124] If a weak target at sea starts to fly over the sea 25 kilometers away from our fleet, our radar and optoelectronic equipment will track the target at the same time. The target motion model and observation model are both assumed to be linear Gaussian models, and the state transfer matrix and process noise covariance matrix are respectively:

[0125]

[0126] The observation matrix and observation noise covariance matrix are:

[0127]

[0128] Similarly, it is assumed that the data sampling rate of the optoelectronic device is Ts = 0.5s and the sampling time is T = 100s. The target motion model and observation model are also assumed to be linear Gaussian models, and the observation noise covariance matrix is:

[0129]

[0130] Substituting the above parameter settings into step 3.2) can verify the heterogeneous measurement fusion sequential Kalman fusion filter algorithm.

[0131] Comparison of the effects of using heterogeneous measurement fusion sequential Kalman filtering and linear Kalman filtering Figures 4 to 7 shown.

[0132] It should be understood that those skilled in the art can make improvements or changes based on the above description, and all these improvements and changes should fall within the scope of protection of the appended claims of the present invention.

Claims

1. A target tracking method based on heterogeneous fusion sequential Kalman filtering, characterized in that: The following steps are involved: 1) Acquiring multi-sensor measurement information; the sensors include radar and optoelectronic equipment; 2) Temporal registration of multi-sensor measurement information; Time registration refers to aligning the sampling time of asynchronous measurement information of each sensor to the same time when multiple sensors are measuring data; 3) Based on sequential Kalman filtering, the multi-sensor measurement information is fused as the final tracking result; 3.1) Construct the measurement equation with radial velocity and radial acceleration using the angle dimension information obtained by the radar and the distance dimension information obtained by the optoelectronic device; 3.2) The radar measurement information is first filtered using the standard Kalman filter, and then the state posteriori is used to perform nonlinear filtering on the optoelectronic device measurement information with radial velocity and radial acceleration as the final tracking result.

2. The target tracking method based on heterogeneous fusion sequential Kalman filtering according to claim 1 is characterized in that: In the step 2), a time interpolation method based on cubic spline interpolation is used for time alignment.

3. The target tracking method based on heterogeneous fusion sequential Kalman filtering according to claim 2 is characterized in that: In step 2), the time interpolation method based on cubic spline interpolation is as follows: Assume that the sensor measures the target n+1 times in a certain time period [a, b], and divides the entire time period into a=t0<t1<...<t n = b, the observation value corresponding to a given moment is f(t i )=y i , i=0,1,…,n; Construct a cubic spline interpolation function s(x), and use the Hermit interpolation method to construct a bivariate cubic spline interpolation function: Interpolate the sensor measurement information.

4. The target tracking method based on heterogeneous fusion sequential Kalman filtering according to claim 3 is characterized in that The interpolation process is as follows: a smooth curve is obtained by using the constructed cubic spline interpolation function, the measurement values ​​of the radar and the optoelectronic device at any time are obtained through the curve, and then the sampling times of all sensors are aligned, thus completing the time interpolation process.

5. The target tracking method based on heterogeneous fusion sequential Kalman filtering according to claim 1 is characterized in that In step 3.1), the measurement equations with radial velocity and radial acceleration are measured by radar and photoelectric equipment measurement Together we get: Where h(·) is the observation function, X(k) is the state vector, which consists of the position term, velocity term, and acceleration term of each dimension; W(k) is the measurement noise; The terms of h(·) are: Among them, the measurement information of optoelectronic equipment Include azimuth and pitch angle 6. The target tracking method based on heterogeneous fusion sequential Kalman filtering according to claim 1 is characterized in that The step 3.2) first uses a standard Kalman filter to filter the radar measurement information, and then uses a state posteriori to perform nonlinear filtering on the optoelectronic device measurement information with radial velocity and radial acceleration; The specific steps are as follows: Assume that the state estimation and covariance matrix of the target at time k-1 are and P k-1|k-1 , state quantity One-step prediction for: Among them, F k-1 is the state transfer matrix. The corresponding prediction error covariance matrix P k|k-1 for: Among them, Q k-1 is the process noise covariance matrix; Utilization Measurement The new information v k for: Among them, H k is the observation matrix. The corresponding innovation covariance matrix S k for: Among them, R k-1 is the measurement noise covariance matrix. Kalman gain K k for: State Posterior Estimation for: The error covariance matrix P corresponding to the state posterior estimate k|k for: P k|k =(I-K k H k )P k|k-1 ; One step prediction for optoelectronic device measurements is: in, Utilization Measurement The new information v e,k for: The corresponding innovation covariance matrix S e,k for: Kalman gain K e,k for: State Posterior Estimation for: The error covariance matrix P corresponding to the state posterior estimate e,k|k for: P e,k|k =(I-K e,k H e,k )P k|k ; The final state posterior is the filtered position estimate in the rectangular coordinate system, P e,k|k is the error covariance matrix estimate after filtering; The posterior state in the polar coordinate system is obtained through the nonlinear coordinate transformation relationship. The transformation formula is as follows:

7. An electronic device, characterized in that: include: one or more processors; as well as a storage device for storing one or more programs, When the one or more programs are executed by the one or more processors, the one or more processors are caused to execute the method according to any one of claims 1 to 6.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 6 is implemented.

Images