FUBKF-based pure azimuth target tracking method and device

Abstract

The invention discloses an orientation-pure target tracking method and device based on FUBKF. The method comprises the following steps: S1, collecting and storing target azimuth information; s2, filtering the target azimuth information by adopting a PLKF algorithm to obtain an estimated target position; s3, carrying out noise separation on the target azimuth information and then carrying out observation matrix accurate calculation; and S4, fusing the observation matrix and the estimated target position to obtain a final target position. According to the method, the tracking precision can be remarkably improved in pure azimuth target tracking, relatively high stability is kept, and the requirements of different application scenes can be met.

Description

Technical Field

[0001] The present invention relates to the technical field of target tracking and signal processing, and more specifically, to a bearing-only target tracking method and device based on FUBKF. Background Art

[0002] Bearings-Only Tracking (BOT) technology is a method that estimates the position, velocity and other parameters of the target using only the acquired angle measurement data. This technology can be applied to target tracking in a variety of environments, including air, land and sea targets. Its main application areas include defense, navigation and meteorology. It can improve the accuracy of positioning by extracting accurate target parameter information from the data collected in the tracking task. However, there is a significant nonlinear mapping relationship between the angle measurement and the target state vector, making the BOT problem a challenging research topic in the field of nonlinear filtering.

[0003] In order to solve the nonlinear problem of BOT, researchers use the Pseudo-Linear Kalman Filter (PLKF) method. It converts nonlinear relationships into pseudo-linear forms, has low computational complexity, strong robustness, and is easy to implement, and is widely applicable to practical scenarios.

[0004] However, in PLKF, the correlation between the observation matrix and pseudo-linear noise can lead to serious bias problems, which significantly weakens the tracking performance. To address this problem, we propose a bearings-only target tracking method based on FUBKF (Fusion Unbiased PLKF). The algorithm first performs overall pseudo-linear processing on the observation equation, then separates the noise in the observation matrix, and uses the Unscented Kalman filter (UKF) to accurately estimate the observation matrix, thereby effectively reducing the estimation error caused by the correlation between the observation matrix and noise. Summary of the invention

[0005] The object of the present invention is to provide a bearing-only target tracking method and device based on FUBKF to overcome the defects of the prior art.

[0006] In order to achieve the above object, the technical solution adopted by the present invention is as follows:

[0007] A bearings-only target tracking method based on FUBKF, comprising:

[0008] S1, collecting target position information and storing it;

[0009] S2, using the PLKF algorithm to filter the target position information to obtain an estimated target position;

[0010] S3, performing noise separation on the target position information and then accurately calculating the observation matrix;

[0011] S4. Fusing the observation matrix and the estimated target position to obtain the final target position.

[0012] Furthermore, the step S2 specifically includes:

[0013] S20, using vector and They represent the state information of the target and the observation station at time k=1,...,N, respectively, where p k =[p x,k ,p y,k ] T and v k =[v x,k ,v y,k ] T Respectively represent the position information and speed information of the target on the xy plane, r k =[r x,k ,r y,k ] T represents the location information of the observation station, and it is assumed that the location information of the observation station is known;

[0014] S21. If the target is set to uniform motion, the dynamic model of the target is:

[0015] x k =Fx k-1 +w k-1

[0016] In the formula, is the state transfer matrix, process noise w k-1 is a zero-mean Gaussian noise sequence, whose covariance matrix is The state transfer matrix F and covariance matrix Q are expressed as follows:

[0017]

[0018] Where T is the sampling period of the observation station, q x and q y They are process noise w k The power spectral density in the x- and y-coordinate directions for the noisy azimuth measurements collected by the observation station at time k is:

[0019]

[0020] Where, the measurement noise nk The mean is 0 and the variance is Gaussian noise, assuming is known a priori, and the measurement noise n k and process noise w k are statistically independent, tan -1 is the inverse tangent function, Δx k =p x,k -r x,k Represents the distance difference between the target and the observation station on the x-axis, Δy k =p y,k -r y,k Represents the distance difference between the target and the observation station on the y-axis;

[0021] S22, the distance between the observation station and the target is d k =p k -r k , and ||d k ||sinα k =Δy k =p y,k -r y,k ,||d k ||cosα k =Δx k =p x,k -r x,k , according to the formula The following equation relationship can be obtained:

[0022]

[0023] At the same time, multiply both sides of the above formula by ||d k ||, we can get:

[0024]

[0025] The formula The nonlinear form of is reformulated into a pseudolinear form:

[0026] z k =H k x k +η k

[0027] Where η k =-||d k ||sinα k is pseudo-linear noise, H k is a pseudo linear observation matrix, and the observation variables is one-dimensional, z k and H k It is expressed as:

[0028]

[0029] The pseudo-linear noise η is obtained k Variance of:

[0030]

[0031] S23, prediction status:

[0032]

[0033] S23, covariance matrix of predicted state:

[0034] P k|k-1 =FP k-1|k-1 F T +Q

[0035] S24, gain matrix

[0036]

[0037] S25. Status update:

[0038]

[0039] S26. Update the covariance matrix of the state

[0040] P k|k =(I 4×4 -K k H k ) k|k-1

[0041] In the formula, and P k|k-1 Denote the one-step forecast and the corresponding forecast error covariance matrix, K k represents the Kalman gain, and P k|k denote the posterior state estimate and the corresponding estimation error covariance matrix, respectively.

[0042] Furthermore, the step S3 specifically includes:

[0043] S30, performing noise separation on the collected target orientation information to obtain an observation matrix;

[0044] S31, using UKF algorithm to measure the matrix G k Make precise estimates.

[0045] Furthermore, the step S31 specifically includes:

[0046] S311, assuming cos n k≈1, sin n k ≈n k , substitute it into the pseudo linear measurement matrix H k middle:

[0047]

[0048] S312, H k Substitute the formula into the formula z k =H k x k +η k In, we get:

[0049] z k =G k x k +n k ([cosα k ,sinα k ,0,0]x k -||d k ||)

[0050] G k =[sinα k ,-cosα k ,0,0]

[0051] S313, let m k =[cosα k ,sinα k ,0,0]x k -||d k ||, and in H k Multiply both sides of the formula by 1 / m k ,get:

[0052]

[0053] Furthermore, the specific formula of step S32 is:

[0054] Furthermore, the step S5 specifically includes:

[0055] Status prediction:

[0056]

[0057] Covariance matrix prediction:

[0058]

[0059] Fusion:

[0060]

[0061] Gain matrix:

[0062]

[0063] Status Update:

[0064]

[0065] The covariance matrix after state update:

[0066]

[0067] The present invention also provides a device for implementing the above-mentioned FUBKF-based bearings-only target tracking method, comprising:

[0068] A position information processing module is used to collect and store target position information;

[0069] A PLKF target tracking module, used for filtering the target orientation information using a PLKF algorithm to obtain an estimated target position;

[0070] UKF target tracking module, used to perform noise separation on the target position information and then accurately calculate the observation matrix;

[0071] The fusion module fuses the observation matrix and the estimated target position to obtain the final target position.

[0072] Compared with the prior art, the advantages of the present invention are: the present invention can significantly improve the tracking accuracy in pure bearing target tracking, maintain high stability, and meet the needs of different application scenarios; the FUBKF algorithm effectively reduces the target tracking estimation error caused by the correlation between the observation matrix and the noise, and the reduction of this error not only improves the accuracy of target tracking, but also enhances the robustness of the entire system in complex environments. The present invention provides a new solution for precise and efficient target tracking, which can play an important role in many fields such as aerospace, robot navigation, etc. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0074] Figure 1 It is a flow chart of the bearing-only target tracking method based on FUBKF of the present invention.

[0075] Figure 2 It is a logical structure diagram of step S1 in the present invention.

[0076] Figure 3 It is a geometric diagram of the target motion in the present invention.

[0077] Figure 4 This is a diagram showing the effect of the FUBKF-based bearing-only target tracking method proposed in the present invention and other methods. DETAILED DESCRIPTION

[0078] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings so that the advantages and features of the present invention can be more easily understood by those skilled in the art, thereby making a clearer and more definite definition of the protection scope of the present invention.

[0079] See also Figure 1 As shown, this embodiment discloses a bearing-only target tracking method based on FUBKF, including:

[0080] Step S1, collecting target position information and storing it.

[0081] Step S2: Using the PLKF algorithm to filter the target position information to obtain an estimated target position.

[0082] Step S3: After noise separation of the target position information, accurate calculation of the observation matrix is ​​performed.

[0083] Step S4: Fusing the observation matrix and the estimated target position to obtain the final target position.

[0084] In this embodiment, Figure 3 The geometric position relationship between the observation station and the target in the BOT model is described, and the step S2 specifically includes:

[0085] Step S20: Use vector and They represent the state information of the target and the observation station at time k=1,...,N, respectively, where p k =[p x,k ,p y,k ] T and v k =[v x,k ,v y,k ] T Respectively represent the position information and speed information of the target on the xy plane, r k =[r x,k ,r y,k ] T It represents the location information of the observation station, and it is assumed that the location information of the observation station is known.

[0086] Step S21: Set the target to be in uniform motion, and the dynamic model of the target is:

[0087] x k =Fx k-1 +w k-1 (1)

[0088] In the formula, is the state transfer matrix, process noise w k-1 is a zero-mean Gaussian noise sequence, whose covariance matrix is The state transfer matrix F and covariance matrix Q are expressed as follows:

[0089]

[0090] Where T is the sampling period of the observation station, which is assumed to be 1s in this embodiment, and q x and q y They are process noise w k The power spectral density in the x-coordinate and y-coordinate directions. The noisy azimuth measurement collected by the observation station at time k is:

[0091]

[0092] Where, the measurement noise n k The mean is 0 and the variance is Gaussian noise, assuming is known a priori, and the measurement noise n k and process noise w k are statistically independent, tan -1 is the inverse tangent function, Δx k =p x,k -r x,k Represents the distance difference between the target and the observation station on the x-axis, Δy k =p y,k -r y,k Represents the distance difference between the target and the observation station on the y-axis.

[0093] The above BOT model explains the observation azimuth and the state variable x k The nonlinear relationship between them. Next, the PLKF algorithm is used to linearize the above nonlinear model to achieve the estimation of the target state.

[0094] Step S22: Figure 3 The geometric model shows that the distance between the observation station and the target is d k =p k -r k , and ||d k ||sinα k=Δy k =p y,k -r y,k ,||d k ||cosα k =Δx k =p x,k -r x,k According to formula (5), the following equation can be obtained:

[0095]

[0096] At the same time, multiply both sides of equation (6) by ||d k ||, we can get:

[0097]

[0098] The nonlinear form of equation (4) is reformulated into a pseudo-linear form:

[0099] z k =H k x k +η k (8)

[0100] Where η k =-||d k ||sinα k is pseudo-linear noise, H k is a pseudo linear observation matrix, and the observation variables is one-dimensional, z k and H k It is expressed as:

[0101]

[0102]

[0103] The pseudo-linear noise η is obtained k Variance of:

[0104]

[0105] According to the above derivation, this step establishes a pseudo-linear tracking model and rewrites the measurement noise into pseudo-linear measurement noise.

[0106] Finally, the detailed iterative process of the PLKF target tracking module is given:

[0107] Step S23: Prediction status:

[0108]

[0109] Step S23, predict the covariance matrix of the state:

[0110] P k|k-1 =FP k-1|k-1 F T +Q (15)

[0111] Step S24, gain matrix

[0112]

[0113] Step S25: Status update:

[0114]

[0115] Step S26, update the covariance matrix of the state:

[0116] P k|k =(I 4×4 -K k H k ) k|k-1 (18)

[0117] In the formula, and P k|k-1 Denote the one-step forecast and the corresponding forecast error covariance matrix, K k represents the Kalman gain, and P k|k denote the posterior state estimate and the corresponding estimation error covariance matrix, respectively.

[0118] In this embodiment, step S3 specifically includes:

[0119] Step S30: noise separation is performed on the collected target position information to obtain an observation matrix, which is specifically:

[0120] Step S311, assuming cos n k ≈1, sin n k ≈n k , substitute it into the pseudo linear measurement matrix H k middle:

[0121]

[0122] Step S312: Substitute formula (19) into formula (8) to obtain:

[0123] z k =G k x k +n k ([cosα k ,sinα k ,0,0]x k -||d k ||) (20)

[0124] G k =[sinα k ,-cosα k ,0,0] (21)

[0125] Step S313: Set m k =[cosα k ,sinα k ,0,0]x k -||d k ||, and multiply both sides of formula (19) by 1 / m k ,get:

[0126]

[0127] At this point, it can be seen that the measurement matrix and the noise are uncorrelated.

[0128] Step S31: Use UKF algorithm to measure the measurement matrix G k Make precise estimates.

[0129]

[0130] In this embodiment, step S5 specifically includes:

[0131] The UKF target tracking module is used to track the observation matrix G k To make a precise estimate:

[0132]

[0133] Status prediction:

[0134]

[0135] Covariance matrix prediction:

[0136]

[0137] Fusion:

[0138]

[0139] Gain matrix:

[0140]

[0141] Status Update:

[0142]

[0143] The covariance matrix after state update:

[0144]

[0145] See also Figure 2 As shown, the present invention also provides a device for implementing the above-mentioned pure bearing target tracking method based on FUBKF, including: an bearing information processing module 1, used for collecting and storing target bearing information, the bearing information processing module 1 needs to ensure the real-time and accuracy of the target bearing data, and provide a reliable data basis for subsequent processing steps; a PLKF target tracking module 2, used for using the PLKF algorithm to filter the target bearing information to obtain an estimated target position, the PLKF algorithm is famous for its high efficiency and accuracy in processing linear systems, it realizes continuous estimation of the target position by pseudo-linearizing the target motion model, and provides stable position information for the tracking system; a UKF target tracking module 3, used for filtering the target bearing information After the information is separated from the noise, the observation matrix is ​​accurately calculated. The UKF target tracking module 3 focuses on a more accurate estimation of the observation matrix of the target orientation information after noise separation. The advantage of the UKF algorithm is that it can better capture the dynamic characteristics of the nonlinear system, thereby improving the accuracy of target tracking; the fusion module 4 fuses the observation matrix and the estimated target position to obtain the final target position. Through this multi-source information fusion technology, the advantages of the two algorithms are effectively combined, and the accuracy and reliability of the target position estimation are further improved. Finally, the fusion module 4 outputs the target position estimate after comprehensive processing. This estimation result is closer to the actual position of the target, which provides a strong guarantee for the performance of the entire target tracking system.

[0146] The main purpose of the pure bearings target tracking device based on FUBKF of the present invention is to effectively reduce the target tracking estimation error caused by the correlation between the observation matrix and the noise through the FUBKF algorithm. This error reduction not only improves the accuracy of target tracking, but also enhances the robustness of the entire system in complex environments. In general, the pure bearings target tracking method based on FUBKF proposed in the present invention provides a new solution for accurate and efficient target tracking, and is expected to play an important role in multiple fields such as aerospace, robot navigation, etc.

[0147] Finally, Monte Carlo simulation is used to compare the proposed FUBKF with PLKF, BC-PLKF, IV-PLKF, EKF and UKF. We use the root mean square error (RMSE) as the performance indicator. The initial position of the target is [40,2260] T m, speed is [2,14] T m / s, the observation station adopts Leg-by-Leg maneuvering mode, the first section is from [10,20] T Starting from m, [2,3] Tm / s for 150s, the second segment is [2,-3] T m / s for 150s, the third segment is [2,3] T m / s speed movement for 100s, the total duration of continuous observation is 400s, the sampling interval is 1s, and the power spectral density of the process noise is q x =q y =0.1m 2 / s 3 The initial state of the algorithm The mean is [40,2260,2,14] T , the covariance is P 0|0 = diag([6.25,6.25,0.25,0.25]).

[0148] Next, we compare the algorithms Tracking situation at the time. Figure 4 The RMSE of position and velocity estimates for FUBKF, PLKF, BC-PLKF, IV-PLKF, EKF, and UKF are given. Figure 4 As can be seen from the results, as k increases, the deviation of the PLKF algorithm increases significantly compared to other algorithms, which is due to the principle deviation of PLKF. IV-PLKF and BC-PLKF show similar performance in the root mean square error of position and velocity. However, the deviation of FUBKF is smaller than BC-PLKF and IV-PLKF, and is closer to PCRLB, which indicates that the noise n k From H k Separating the bias from the PLKF is more effective than subtracting the bias from the PLKF. FUBKF performs one more filtering than UKF, so the target tracking effect is more stable and there is no divergence problem. Therefore, compared with the above algorithms, the performance of the FUBKF algorithm is the best.

[0149] Although the embodiments of the present invention are described in conjunction with the accompanying drawings, the patent owner may make various variations or modifications within the scope of the appended claims. As long as they do not exceed the protection scope described in the claims of the present invention, they should be within the protection scope of the present invention.

Claims

1. A bearings-only target tracking method based on FUBKF, characterized in that: include: S1, collecting target position information and storing it; S2, using the PLKF algorithm to filter the target position information to obtain an estimated target position; S3, performing noise separation on the target position information and then accurately calculating the observation matrix; S4. Fusing the observation matrix and the estimated target position to obtain the final target position.

2. The bearing-only target tracking method based on FUBKF according to claim 1, characterized in that: The step S2 specifically includes: S20, using vector and They represent the state information of the target and the observation station at time k=1,...,N, respectively, where p k =[p x,k ,p y,k ] T and v k =[v x,k ,v y,k ] T Respectively represent the position information and speed information of the target on the xy plane, r k =[r x,k ,r y,k ] T represents the location information of the observation station, and it is assumed that the location information of the observation station is known; S21. If the target is set to uniform motion, the dynamic model of the target is: x k =Fx k-1 +w k-1 In the formula, is the state transfer matrix, process noise w k-1 is a zero-mean Gaussian noise sequence, whose covariance matrix is The state transfer matrix F and covariance matrix Q are expressed as follows: Where T is the sampling period of the observation station, q x and q y They are process noise w k The power spectral density in the x- and y-coordinate directions for the noisy azimuth measurements collected by the observation station at time k is: Where, the measurement noise n k The mean is 0 and the variance is Gaussian noise, assuming is known a priori, and the measurement noise n k and process noise w k are statistically independent, tan -1 is the inverse tangent function, Δx k =p x,k -r x,k Represents the distance difference between the target and the observation station on the x-axis, Δy k =p y,k -r y,k Represents the distance difference between the target and the observation station on the y-axis; S22, the distance between the observation station and the target is d k =p k -r k , and ||d k ||sinα k =Δy k =p y,k -r y,k ,||d k ||cosα k =Δx k =p x,k -r x,k , according to the formula The following equation relationship can be obtained: At the same time, multiply both sides of the above formula by ||d k ||, we can get: The formula The nonlinear form of is reformulated into a pseudolinear form: z k =H k x k +η k Where η k =-||d k ||sinα k is pseudo-linear noise, H k is a pseudo linear observation matrix, and the observation variables is one-dimensional, z k and H k It is expressed as: The pseudo-linear noise η is obtained k Variance of: S23, prediction status: S23, covariance matrix of predicted state: P k|k-1 =FP k-1|k-1 F T +Q S24, gain matrix S25. Status update: S26. Update the covariance matrix of the state P k|k =(I 4×4 -K k H k )P k|k-1 In the formula, and P k|k-1 Denote the one-step forecast and the corresponding forecast error covariance matrix, K k represents the Kalman gain, and P k|k denote the posterior state estimate and the corresponding estimation error covariance matrix, respectively.

3. The bearing-only target tracking method based on FUBKF according to claim 1, characterized in that: The step S3 specifically includes: S30, performing noise separation on the collected target orientation information to obtain an observation matrix; S31, using UKF algorithm to measure the matrix G k Make precise estimates.

4. The bearing-only target tracking method based on FUBKF according to claim 3 is characterized in that: The step S31 specifically includes: S311, assuming cos n k ≈1, sin n k ≈n k , substitute it into the pseudo linear measurement matrix H k middle: S312, H k Substitute the formula into the formula z k =H k x k +η k In, we get: z k =G k x k +n k ([cosα k ,sinα k ,0,0]x k -||d k ||) G k =[sinα k ,-cosα k ,0,0] S313, let m k =[cosα k ,sinα k ,0,0]x k -||d k ||, and in H k Multiply both sides of the formula by 1 / m k ,get:

5. The bearing-only target tracking method based on FUBKF according to claim 1, characterized in that: The specific formula of step S32 is:

6. The bearing-only target tracking method based on FUBKF according to claim 1, characterized in that: The step S5 specifically includes: Status prediction: Covariance matrix prediction: Fusion: Gain matrix: Status Update: The covariance matrix after state update:

7. A device for implementing the bearings-only target tracking method based on FUBKF according to any one of claims 1 to 6, characterized in that: include: A position information processing module is used to collect and store target position information; A PLKF target tracking module, used for filtering the target orientation information using a PLKF algorithm to obtain an estimated target position; UKF target tracking module, used to perform noise separation on the target position information and then accurately calculate the observation matrix; The fusion module fuses the observation matrix and the estimated target position to obtain the final target position.

Images