Self-adaptive strong tracking square root unscented particle filtering method

Abstract

The invention relates to the field of filtering methods in radar target tracking, in particular to a self-adaptive strong-tracking square root unscented particle filtering method, which comprises the following steps of: predicting state information of a current tracking target by adopting a prediction system combining unscented Kalman filtering and particle filtering, constructing a fading factor, and calculating the state information of the current tracking target; and if the value of the fading factor is greater than 1, performing updating by adopting strong tracking filtering to obtain the state of the tracking target, otherwise, performing updating by adopting a Sage-Husa self-adaptive method to obtain the state of the tracking target. According to the method, strong tracking filtering and a Sage-Husa self-adaptive method are combined, the estimation deviation is corrected by using the fading factor, the tracking performance during sudden maneuvering of the target is improved, and meanwhile, the numerical stability and the tracking precision are enhanced.

Description

Technical Field

[0001] The invention relates to the field of filtering methods in radar target tracking, and in particular to an adaptive strong tracking square root unscented particle filtering method. Background Art

[0002] The essence of target tracking is to use Bayesian filter to estimate the trajectory of the target to eliminate the uncertainty caused by environmental noise, etc. Therefore, the filtering method is an important part of the target tracking system. Kalman filter (KF) is only applicable to linear state estimation problems and can achieve optimal estimation under linear conditions. However, in strong nonlinear and non-Gaussian conditions, the filter will diverge, resulting in a significant reduction in target tracking accuracy. In order to deal with more complex situations, many improvements have been made based on Kalman filtering. The prior art has proposed some nonlinear filtering methods, such as Unscented Kalman Filter (UKF), Extended Kalman Filter (EKF), Cubatue Kalman Filter (CKF), etc. In order to cope with multi-target tracking, multi-Bernoulli filter based on random finite sets, probability hypothesis density filter and its variants have also been proposed.

[0003] Particle filter (PF) and its improved method are optimal regression Bayesian filtering methods based on Monte Carlo simulation. This method is a filtering method suitable for nonlinear and non-Gaussian noise systems. The main idea of ​​PF is to use a set of sampled particles with weights to represent the posterior probability density of the state. The method can theoretically represent any form of probability distribution. The PF method is not limited by linearization error or Gaussian noise assumptions, which greatly improves the estimation accuracy of nonlinear filtering problems. However, its huge amount of calculation seriously limits its application in practical engineering. In addition, the proposed distribution function used in the standard particle filter method does not incorporate the latest observation information, which will cause serious particle degradation problems. Many scholars have conducted in-depth research on this issue and proposed a variety of methods. Representative methods such as introducing EKF to filter and optimize the proposed distribution function of the state. The proposed distribution function of this method incorporates the latest observation information, so the filtering performance is improved. However, when the system is highly nonlinear, it will lead to poor filtering performance. In addition, this method also requires the Jacobian matrix, which is cumbersome to calculate and cannot cope with the situation where the Jacobian matrix does not exist. Therefore, based on the improved idea of ​​the EPF method, UKF is used to filter the sampled particles estimated by the system, and an unscented particle filter (UPF) method is proposed.

[0004] In actual situations, the system motion model may not match the actual motion state of the target, which will cause the state estimation accuracy to decrease and even cause the filter to diverge. Strong tracking filtering introduces a fading factor γ into the state error covariance matrix P to adjust the gain coefficient K in real time and use more observations for correction, thereby suppressing the divergence of the filtering result. Strong tracking filtering uses the fading factor to force the output residual sequence to remain orthogonal and extract all valid information in the output residual sequence to the greatest extent. Therefore, STF can still maintain the ability to track the system state when the system model is inaccurate. Sage and Husa proposed an adaptive filtering method based on maximum a posteriori estimation. Deng Zili et al. introduced the forgetting factor on this basis, which enabled the method to have the ability to estimate unknown time-varying noise in real time. However, this method is not suitable for situations where partial derivatives do not exist and the Jacobian matrix cannot be calculated. Summary of the invention

[0005] In order to improve the filtering accuracy and reduce the filtering error in the target tracking process, the present invention proposes an adaptive strong tracking square root unscented particle filtering method, which adopts a prediction system combining unscented Kalman filtering and particle filtering to predict the state information of the current tracking target, and constructs a fading factor. If the value of the fading factor is greater than 1, a strong tracking filter is used to update the state of the tracking target, otherwise the Sage-Husa adaptive method is used to update the state of the tracking target.

[0006] The present invention generates Sigma points by variable-scale minimum slope simplex sampling, reducing the number of points from 2n+1 of traditional traceless transformation to n+2, thereby improving calculation efficiency; adopts square root transfer of covariance matrix to ensure matrix positive definiteness and symmetry, and avoids filtering failure; combines strong tracking filtering and Sage-Husa adaptive method, uses fading factor to correct estimation deviation, improves tracking performance during sudden maneuvers of targets, and enhances numerical stability and tracking accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0007] Figure 1 This is a flow chart of an adaptive strong tracking square root unscented particle filtering method of the present invention;

[0008] Figure 2 Schematic diagram of the comparison between the real trajectory and the trajectory estimated by the unscented Kalman filter;

[0009] Figure 3 It is a schematic diagram comparing the actual trajectory and the estimated trajectory of the present invention;

[0010] Figure 4 Schematic diagram of error comparison between the trajectory estimated by the present invention and the trajectory estimated by the unscented Kalman filter. DETAILED DESCRIPTION

[0011] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0012] The present invention proposes an adaptive strong tracking square root unscented particle filtering method, which comprises adopting a prediction system combining an unscented Kalman filter and a particle filter to predict the state information of the current tracking target, constructing a fading factor, and adopting a strong tracking filter to update the state of the tracking target if the value of the fading factor is greater than 1, otherwise adopting a Sage-Husa adaptive method to update the state of the tracking target.

[0013] The filtering method of the present invention mainly includes the steps of generating Sigma sampling particles and their weights, estimating the state of the next moment by propagating the Sigma point through the state equation, estimating the measured state of the Sigma point through the measurement equation, sampling from the proposed distribution obtained by strong tracking filtering or the Sage-Husa adaptive method and calculating the weight of each particle, and outputting the estimation result.

[0014] In this embodiment, the system model is as follows:

[0015]

[0016] Among them, X k is the state vector of the target at time k, Z k is the observation vector of the target at time k; w k represents the system noise, v k is the measurement noise, Q k is the covariance of the system noise, R k is the covariance of the measurement noise; f() is the state transfer function, which is related to the target motion model; h() is the target measurement function, which depends on the sensor detection method; technicians in this field can set the state transfer function and the target measurement function according to actual needs.

[0017] like Figure 1 The specific detailed steps of the adaptive strong tracking square root unscented particle filter method based on minimum simplex sampling are derived as follows:

[0018] Step 1: k represents the time, when k = 0, it means initialization, randomly select N initial particles P(X0) represents the probability distribution of the initial state X0. X0 represents the initial state of the system and can be a one-dimensional or multi-dimensional vector. The weight of each particle is Assumptions in, represents the mth initial particle in the initial state, represents the estimated value of the mth initial particle, represents the mth Cholesky decomposition factor at the initial moment, chol{·} is the Cholesky decomposition operator, and E[·] is the expectation operation in mathematics.

[0019] Step 2: When k ≥ 1, generate n + 2 Sigma points χ by minimum slope simplex sampling i And calculate its weight In order to solve the non-local effect caused by the expansion of dimensions in simplex sampling, the α and β parameters are introduced to ensure the positive definiteness of the covariance matrix. The scale factor α controls the distance between the Sigma point and the center point and is a small positive number with a value range of [10 -4 ,1], β introduces the prior distribution information of the random variable, and under Gaussian conditions, it usually takes a value of 2. Specifically, this step includes:

[0020] Step 2.1: Calculate the initial weights. Select Other weights are calculated through proportional correction, as shown in the following formula:

[0021]

[0022] in, is the weight of the i-th Sigma point when calculating the mean, and all weights satisfy Represents the weight when calculating the covariance matrix of i Sigma points.

[0023] Step 2.2: According to the minimum slope traceless transformation, the one-dimensional minimum slope traceless transformation of a single row of x points is:

[0024]

[0025] Step 2.3: Expand the one-dimensional minimum slope traceless transform single-row x-point vector sequence to n dimensions according to the following formula, where j = 2, 3, …, n.

[0026]

[0027] in, Indicates the iterative vector needed to generate the Sigma point, j represents the dimension of the state, i represents the number of vectors, ranging from 0-j+1; Represents the corresponding value calculated by the aforementioned weight formula, that is, the weight when calculating the mean of the i-th Sigma point The weight value of the j+1th dimension after expanding to n dimensions.

[0028] Step 3: Calculate the Sigma sampling point, that is:

[0029]

[0030] Step 4: Sigma points are propagated through the state equation, namely:

[0031]

[0032] Step 5: X k+1k The state mean and the square root of the state error covariance matrix are predicted, and the state error covariance matrix is ​​calculated, that is:

[0033]

[0034]

[0035] Step 6: Calculate the new Sigma sampling point, namely:

[0036]

[0037] Step 7: The Sigma point is propagated through the measurement equation, namely:

[0038]

[0039] Step 8: Predict the state mean and the square root of the innovation covariance matrix of the measured prediction, and calculate the innovation covariance matrix, that is:

[0040]

[0041] Step 9: Calculate the fading factor γ that needs to be introduced by the following formula: k+1 ,Right now:

[0042] ε k+1 =z k+1 -Z k+1k (18)

[0043]

[0044]

[0045] Among them, ε k+1 is the observed value z k+1 The new information between the observed predicted value at time k+1 calculated iteratively according to the kth time; V k+1 is the covariance matrix of the innovation sequence, represents the state error covariance matrix when no fading factor is introduced, Represents the innovation covariance matrix when no fading factor is introduced.

[0046] In the prior art, when calculating the vanishing factor, the Jacobian matrix is ​​generally obtained by differentiation and then subsequent calculations are performed, and the prior art calculation method is not applicable to the case where partial derivatives do not exist. In this embodiment, when calculating the vanishing factor, it is not necessary to use the differentiation method to obtain the Jacobian matrix and then perform subsequent calculations like the prior art method, that is, the present invention introduces traceless transformation to calculate the vanishing factor, which can be applicable to the case where partial derivatives do not exist or the Jacobian matrix is ​​difficult to calculate and has strong nonlinearity. For the adaptive estimation of the noise matrix, because the present invention uses the square root filtering concept, the adaptive estimation of the observation noise covariance matrix also introduces traceless transformation for estimation, and there is no need to calculate the Jacobian matrix, and the result estimation is performed under the square root condition, and the obtained estimation result can be directly used in the filtering algorithm.

[0047] Step 10: Determine whether to select the strong tracking branch or the observation noise estimation branch according to the fading factor, that is, if γ k+1 A value greater than 1 selects a strong tracking branch if γ k+1 A value of less than or equal to 1 selects the estimated observation noise estimation branch.

[0048] The data processing process of the strong tracking branch includes:

[0049] (1) Calculate the square root of the new state error covariance matrix, that is:

[0050]

[0051] (2) Calculate the square root of the innovation covariance matrix S from equations 11 to 17 Z and the cross-covariance matrix P XZ ;

[0052] (3) Get the new observation value z k+1 After that, the state estimation and S are performed by the following formula k+1k+1 Update calculation of:

[0053]

[0054] X k+1k+1 =X k+1k +K k+1 (z k+1 -Z k+1k ) (26)

[0055] S k+1k+1 =cholupdate{S k+1k ,K k+1 S Z ,-1} (27)

[0056] The data processing process of the estimated observation noise estimation branch includes:

[0057] (1) Calculate the measurement innovation as: ε k+1 =z k+1 -Z k+1k ;

[0058] (2) Calculate the square root of the state error covariance matrix:

[0059]

[0060] in, Represents the difference between the observed value and the predicted observed value, d k represents the forgetting factor, The value of b usually ranges from 0.95 to 0.995.

[0061] (3) Calculate the square root of the innovation covariance matrix:

[0062]

[0063] (4) State estimation and S are performed through equations 25 to 27 k+1k+1 The updated calculation of .

[0064] Step 11: Extract one particle from the calculated important density function of each particle, namely:

[0065]

[0066] in, Represents the mth particle extracted at time k+1, m=1,2,…,N.

[0067] Step 12: Calculate the weight of each particle, namely:

[0068]

[0069] in, Represents a given particle In the case of k+1 The likelihood function is used to measure the consistency between the current particle state and the observed data; It means that the particle starts at time k Transfer to the state at time k+1 The state transition probability of Represents the importance distribution of generating new particles in particle filtering obtained by the filtering algorithm.

[0070]

[0071] in, represents the weight of the mth particle at time k+1, m=1,2,…,N.

[0072] Step 13: If the particle is degenerate, resample it.

[0073] Step 14: Output the optimal estimation result, namely:

[0074]

[0075] In order to verify the effectiveness of the present invention, the present invention is simulated under the test conditions: the process noise covariance matrix is ​​0.04, the noise variance matrix is ​​0.04, and the state transfer function set in this embodiment is expressed as: where x k is the predicted state, x k-1 is the optimal estimate of the state at the previous moment, k is a certain moment of recursion, and w k is the system noise; the target measurement function set in this embodiment is expressed as where z k is the actual observed value, v k Based on the above two strongly nonlinear equations, the adaptive strong tracking square root unscented particle filter method with minimum slope sampling is compared with the unscented Kalman filter algorithm. The comparison results are shown in Figures 2 to 4 ,in Figure 2 Given the true trajectory and the unscented Kalman filter estimated trajectory, Figure 3 Given the true trajectory and the trajectory estimated by the improved filtering method, Figure 4 A schematic diagram of the error comparison between the improved filtering method of the present invention and the unscented Kalman filter is given, from which it can be seen that the price of the present invention is kept at a relatively low level as a whole.

[0076] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. An adaptive strong tracking square root unscented particle filtering method, characterized in that: A prediction system combining unscented Kalman filter and particle filter is used to predict the state information of the current tracking target and construct a fading factor. If the value of the fading factor is greater than 1, a strong tracking filter is used to update the state of the tracking target. Otherwise, the Sage-Husa adaptive method is used to update the state of the tracking target.

2. The adaptive strong tracking square root unscented particle filtering method according to claim 1, characterized in that: The fading factor is calculated during the filtering process. When the fading factor is greater than 1, the state information is updated by combining the strong tracking filter. Otherwise, the state information is updated by combining the Sage-Husa adaptive method. The calculation of the fading factor includes: ε k+1 =from k+1 -WITH k+1|k Among them, γ k+1 represents the fading factor introduced in the strong tracking filtering process at the current moment; tr[·] represents the trace of the matrix; N k+1 、M k+1 are two intermediate parameters; V k+1 Represents the covariance matrix of the new information sequence; R k+1 represents the observation noise covariance matrix at time k+1; P XZ represents the cross-covariance matrix; represents the prior state error covariance matrix when no fading factor is introduced; Q k represents the covariance matrix of the system noise at time k; [·] T Indicates the transpose of a matrix; [·] -1 It means to find the inverse matrix of a matrix; represents the innovation covariance matrix when no fading factor is introduced at the kth moment; ε1 represents the innovation value calculated at the moment k = 1; ε k+1 is the observed value z k+1 The new information between the observed predicted value at time k+1 calculated iteratively at time k; ρ represents the forgetting factor; z k+1 represents the observed value obtained at the k+1th moment; Z k+1|k It represents the observed value at time k+1 predicted based on the predicted value from time k to time k+1.

3. The adaptive strong tracking square root unscented particle filtering method according to claim 1 or 2, characterized in that: The process of updating the state of the tracking target using strong tracking filtering includes: Calculate the square root of the new state error covariance matrix, that is: According to the square root of the new state error covariance matrix, update the square root of the new information covariance matrix S Z and the cross-covariance matrix P XZ ; Get the observed value z of the k+1th iteration k+1 Then perform state estimation and S k+1|k+1 The update calculation is: X k+1|k+1 =X k+1|k +K k+1 (z k+1 -Z k+1|k ) S k+1|k+1 =cholupdate{S k+1|k ,K k+1 S Z ,-1} in, Indicates that time k to k+1 is not included The square root of the prior state error covariance matrix of the two effects; qr{·} represents the qr decomposition operation in mathematics; Indicates the weight when calculating the covariance matrix from the 1st to the n+1st Sigma points; Indicates the predicted value of the generated sigma points 1 to n+1 after the state equation transfer; X k+1|k represents the predicted value transferred from time k to time k+1; Q k represents the covariance matrix of system noise; S k+1|k Indicates that the transfer from time k to time k+1 includes The square root of the prior state error covariance matrix of the two effects; cholupdate{·} is the Cholesky decomposition operator; Indicates that k is transferred to k+1 and is not included The square root of the prior state error covariance matrix for the two effects; It represents the predicted value of the generated 0th Sigma point after the state equation transfer; Indicates the weight of the generated 0th Sigma point when calculating the covariance matrix; K k+1 represents the Kalman gain at the k+1th moment; S Z The square root of the computed innovation covariance matrix.

4. The adaptive strong tracking square root unscented particle filtering method according to claim 3, characterized in that: According to the square root of the new state error covariance matrix, update the square root of the new information covariance matrix S Z and the cross-covariance matrix P XZ The process includes: in, represents the i-th Sigma point newly generated in the strong tracking filter; α represents the scale factor; S k+1|k Indicates that k transfers to k+1 time The square root of the prior error covariance matrix of the two effects; Indicates the iterative vector used to generate Sigma sampling points, j indicates the dimension of the state, and i indicates the number of vectors ranging from 0 to j+1; represents the predicted value of the i-th Sigma point generated in the strong tracking filtering process after being transferred by the observation equation; h(·) represents the target observation function; Z k+1|k represents the predicted value of the calculated observation; Represents the weight of the i-th Sigma point; Indicates not included The square root of the innovation covariance matrix of the two effects; It represents the predicted value generated by the observation equation after the newly generated 1st to n+1st Sigma points are propagated in the strong tracking filtering process; Z k+1|k represents the predicted value of the calculated observation; R k represents the observation noise covariance matrix at time k; It represents the predicted value of the 0th Sigma point generated in the strong tracking filter after being transferred by the observation equation; Represents the i-th Sigma point newly generated in the strong tracking filtering process; It represents the predicted value of the i-th Sigma point generated in the strong tracking filtering process after being transferred by the observation equation.

5. The adaptive strong tracking square root unscented particle filtering method according to claim 1 or 2, characterized in that: The process of updating the state of the tracking target using the Sage-Husa adaptive method includes: Get the observed value z of the k+1th iteration k+1 The new information between the post-calculation and the observed predicted value at time k+1 predicted by the iterative calculation at time k; Estimate the statistical characteristics of the observation noise, and bring the estimated observation noise matrix into the square root of the adjusted Kalman gain to calculate the innovation covariance matrix, that is: Perform state estimation and S k+1|k+1 The update calculation is: X k+1|k+1 =X k+1|k +K k+1 (z k+1 -Z k+1|k ) S k+1|k+1 =cholupdate{S k+1|k ,K k+1 S Z ,-1} in, Indicates not included The square root of the innovation covariance matrix of the two effects; w c,1:n+1 Indicates the corresponding weights when calculating the covariance matrix for the 1st to n+1st Sigma points; It represents the predicted value generated by the observation equation after the 1st to n+1st Sigma points generated for the second time in the filtering process are propagated; Z k+1|k represents the predicted value of the calculated observation; R k represents the k-time observation noise covariance matrix estimated by adaptive calculation; S Z represents the square root of the innovation covariance matrix; It represents the predicted value of the 0th Sigma point generated for the second time after being transferred by the observation equation; Z k+1|k represents the predicted value of the calculated observation; Indicates the weight of the generated 0th Sigma point when calculating the covariance matrix; S k+1|k Indicates that k transfers to k+1 time The square root of the prior error covariance matrix of the two effects; Indicates that the transfer from time k to time k+1 is not included The square root of the prior state error covariance matrix for the two effects; It represents the predicted value of the generated 0th Sigma point after the state equation transfer; X k+1|k represents the predicted value transferred from time k to time k+1; γ k+1 represents the fading factor introduced in the strong tracking filter at the current moment; P XZ represents the cross-covariance matrix; K k+1 represents the Kalman gain at time k+1; X k+1|k+1 represents the optimal estimate at time k+1; S k+1|k+1 Represents the square root of the posterior state error covariance matrix at time k+1.

6. The adaptive strong tracking square root unscented particle filtering method according to claim 5, characterized in that: The process of estimating the statistical characteristics of measurement noise involves: Among them, R ** , R * To calculate the intermediate parameters for estimating the statistical characteristics of the observation noise; d k represents the forgetting factor; Represents the difference between the observed value and the predicted observed value; represents the weight of the generated i-th Sigma point when calculating the covariance matrix; diag(·) represents constructing a diagonal matrix through the diagonal elements of the matrix.

7. The adaptive strong tracking square root unscented particle filtering method according to claim 1, characterized in that: In the unscented Kalman filtering process, Sigma points are generated by variable-scale minimum slope simplex sampling, reducing the number of sampling points from 2n+1 in traditional unscented transformation to n+2.

8. The adaptive strong tracking square root unscented particle filtering method according to claim 5, characterized in that: Initialize the weight of the first Sigma point, calculate the weights of other Sigma points through proportional correction, and the weights of n+2 Sigma points are expressed as: in, Represents the weight of the i-th Sigma point; A parameter for initialization setting; α is the scale factor; It represents the weight when calculating the covariance matrix of the i-th Sigma point; β is the prior distribution information of the introduced random variable.

9. The adaptive strong tracking square root unscented particle filtering method according to claim 8, characterized in that: The update process of a Sigma point is expressed as: The update process of calculating a new Sigma point is expressed as: in, The calculated Sigma point; represents the i-th initial particle; represents the Cholesky decomposition factor of the i-th particle at the initial moment; Indicates the iterative vector used to generate Sigma sampling points, j indicates the dimension of the state, and i indicates the number of vectors ranging from 0 to j+1; Indicates the Sigma point where the observation equation is generated; X k+1|k represents the predicted value transferred from time k to time k+1; S k+1|k Indicates that it contains The square root of the prior state error covariance matrix for the two effects.

Images