Particle filtering algorithm for azimuth-pure target tracking system

Abstract

The invention provides a particle filtering algorithm for an azimuth-pure target tracking system. The particle filtering algorithm comprises the following steps: constructing an azimuth tracking nonlinear discrete state space model and a measurement model; predicting the current state according to the state space model, and constructing a one-step prediction model; wherein the one-step prediction model of the state is updated and corrected through a recursive updating method; constructing an optimal gain model and a covariance model; calculating a posteriori estimated value and an error covariance matrix of the state by using the Bayesian theory and combining the observed value and the prior information; establishing a recursive relationship, and updating the covariance matrix; and re-sampling the particles according to a regularization re-sampling method, re-sampling from continuous approximate distribution to obtain a new particle set, and performing state estimation by using the re-sampled particle set to obtain a final state estimation value. According to the method, a regularization particle filtering algorithm of recursive updating is adopted, and particle filtering is improved, so that the precision and stability of state estimation are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of target tracking, and in particular to a particle filtering algorithm for a bearing-only target tracking system. Background Art

[0002] Particle filter (PF) is a recursive Bayesian filtering technology based on Monte Carlo theory. With the development of modern computer science, particle filtering has become a research hotspot and is currently widely used in state estimation, target tracking, economic forecasting, signal processing and other fields. As a numerical method, particle filtering approximates the posterior probability density distribution of state variables through weighted discretized particles. Since it does not require the state space model to be linear and does not require the noise to obey Gaussian distribution, particle filtering performs better than other basic filtering algorithms in nonlinear and non-Gaussian systems and has the ability to handle nonlinear and non-Gaussian systems.

[0003] However, there are some shortcomings in the basic particle filter. First, the particle filter algorithm is based on the sequential importance sampling algorithm. The variance of the particle weights will increase over time, so there is inevitably a particle degradation problem, which makes the weights concentrated on a few particles, and the weights of most other particles are extremely small. In order to solve the particle degradation problem, in most cases, the resampling method is adopted to discard those small weight particles and copy the large weight particles, but this may lead to the loss of particle diversity, that is, after multiple iterations, the particles are concentrated on a few high-weight particles. Secondly, the particle filter depends on the selection of the Proposal Distribution Function (PDF) to determine the particle sampling strategy.

[0004] In view of the limitations of the above-mentioned particle filters, a variety of improved particle filter algorithms have emerged in recent years, such as the Extended Kalman Particle Filter (EPF) and the Unscented Kalman Particle Filter (UPF); EPF uses the Extended Kalman Filter (EKF) to provide an importance density function for the PF. This algorithm locally linearizes the nonlinear system based on the EKF and relies on the Taylor expansion of the system model; UPF uses the Unscented Kalman Filter (UKF) to provide an importance density function for the PF. This algorithm handles nonlinear problems based on unscented transformation. In addition, there is the Gaussian Particle Filter (GPF), which uses particle filtering to obtain a Gaussian distribution function to approximate the posterior probability density of unknown states in nonlinear systems; the Iterated Extended Kalman Particle Filter (IEKPF) based on the iterative idea, which is different from EPF. Based on EKF, this algorithm obtains a posterior probability distribution that is closer to the truth through multiple iterations, so that PF provides an importance density function, and its effect is better than EPF and UPF. In recent years, new algorithms that combine particle filtering and other intelligent algorithms have also appeared, such as the improved particle filter algorithm combined with particle swarm optimization (Particle Swarm Optimization-Particle Filter, PSO-PF), and particle filtering based on genetic algorithms.

[0005] However, these nonlinear filtering algorithms are essentially an approximation of linear filtering, and are based on the expected posterior estimation to minimize the mean square error to estimate the posterior probability density function; and when using the latest observations for updating, the minimum mean square error estimation is also used to linearly correct the state. This commonly used linear assumption may lead to inaccurate filtering estimation results or even filter divergence. In order to provide a posterior probability density function that is closer to the actual distribution state and reduce the impact of linear approximation, the present invention proposes a new particle filtering algorithm based on recursive update and regularization method. Summary of the invention

[0006] In view of the shortcomings of the prior art, the purpose of the present invention is to provide a particle filter algorithm for a pure bearing target tracking system. The method adopts a recursive update regularized particle filter algorithm to improve the particle filter so as to improve the accuracy and stability of state estimation.

[0007] The present invention achieves the above-mentioned purpose through the following technical solutions:

[0008] A particle filter algorithm for a bearings-only target tracking system, the method comprising the following steps:

[0009] Construct the nonlinear discrete state space model and measurement model for position tracking; define the system process noise and measurement noise;

[0010] The current state is predicted according to the state space model, and a one-step prediction model is constructed; wherein the one-step prediction model of the state is updated and corrected by a recursive update method;

[0011] Construct optimal gain model and covariance model;

[0012] Using Bayesian theory, combined with observations and prior information, the posterior estimate of the state and its error covariance matrix are calculated;

[0013] Establish a recursive relationship and update the covariance matrix;

[0014] The particles are resampled according to the regularized resampling method, and a new particle set is obtained by resampling from the continuous approximate distribution. The state is estimated using the resampled particle set to obtain the final state estimation value.

[0015] According to a particle filter algorithm for a bearings-only target tracking system provided by the present invention, a state space model is constructed, which is expressed as the following formula:

[0016] x k =f(x k-1 )+ω k

[0017] z k =h(x k )+v k

[0018] Among them, x k ∈R n is the n-dimensional state vector, z k ∈R m is an m-dimensional observation vector; f(·) and h(·) are known functions, and the state transfer function f(·): R n →R n , measurement function h(·): R n →R m ;ω k is the system process noise, which obeys Gaussian distribution, that is, ω k ~N(0,Q k );v k is the measurement noise, which obeys Gaussian distribution, that is, v k ~N(0,Rk ), and ω k and v k Independent of each other.

[0019] According to a particle filter algorithm for a bearings-only target tracking system provided by the present invention, when constructing a one-step prediction model, the covariance C between the true state and the measurement noise is defined according to the Kalman filter theory related to the measurement noise and the process noise. k+1 , expressed as the following formula:

[0020]

[0021] in is a priori estimate.

[0022] According to a particle filter algorithm for a bearing-only target tracking system provided by the present invention, the one-step prediction model of the state is updated and corrected step by step, and D is defined. k+1 Such as the following formula:

[0023]

[0024] According to a particle filter algorithm for a bearing-only target tracking system provided by the present invention, when constructing an optimal gain model, the optimal gain matrix is ​​calculated in the nonlinear filter as follows:

[0025]

[0026] Where P z,k+1 represents the covariance matrix of the observed variable z, P xz,k+1 The covariance matrix of the observed variables z and the state variables x.

[0027] According to a particle filter algorithm for a bearings-only target tracking system provided by the present invention, when constructing a covariance model, it is expressed as the following formula:

[0028]

[0029] According to a particle filter algorithm for a bearing-only target tracking system provided by the present invention, according to Bayesian theory, the posterior estimate x k+1 And the error covariance P of the posterior estimate k+1|k+1 The calculation is expressed as the following formula:

[0030]

[0031] According to a particle filter algorithm for a bearings-only target tracking system provided by the present invention, the establishment of a recursive relationship and updating of the covariance matrix include:

[0032] let and A recursive relationship is established and the following formula is used for approximate processing:

[0033]

[0034] in C k+1 Represents x k+1 The covariance between θ and the measurement noise.

[0035] According to a particle filter algorithm for a bearings-only target tracking system provided by the present invention, the weight of the particle is normalized and expressed as the following formula:

[0036]

[0037] According to a particle filter algorithm for a bearing-only target tracking system provided by the present invention, particles are resampled according to a regularized resampling method, which is expressed as the following formula:

[0038]

[0039] Among them, w k (i) is the normalized weight, K h (·) is a kernel density function after rescaling the kernel density function K(·), and its expression is as follows:

[0040]

[0041] Among them, the optimal kernel density K(·) is the Epanechnikov kernel density, which is expressed as the following formula:

[0042]

[0043] The steps of the regularized resampling method include: firstly, calculating the empirical variance S of the particle k , find the solution that satisfies D k (D k ) T =S k D k Then, we sample e from the Epanechnikov kernel density k (i) Finally, resampling is performed to obtain new particles It is expressed as the following formula:

[0044]

[0045] It can be seen that compared with the prior art, the present invention is based on the idea of ​​recursive updating and combines regularized particle filtering to propose a new regularized particle filtering algorithm based on recursive updating. The algorithm uses the Gaussian filtering algorithm of recursive updating to provide an importance density function for the particle filter, which improves the accuracy and stability of state estimation. Through recursive updating and regularization processing, the posterior state estimation is closer to the true distribution. When the particle state is updated, the particle state can be continuously optimized and adjusted in small steps; regularization processing particle resampling can improve the problem of particle diversity deficiency, regularization processing can increase particle diversity, improve the robustness of the algorithm, and improve the accuracy of state estimation. Therefore, the present invention uses observation values ​​for recursive updating to effectively reduce the influence of linearized minimum mean square error in nonlinear models, and combined with RPF can reduce the influence of particle diversity deficiency, improve the accuracy of state estimation, and is suitable for pure bearing target tracking systems, with broad application prospects.

[0046] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 It is a flow chart of a particle filter algorithm embodiment of the present invention for a bearing-only target tracking system.

[0048] Figure 2 It is a schematic diagram of the first simulation result in an embodiment of a particle filter algorithm for a bearing-only target tracking system of the present invention.

[0049] Figure 3 It is a schematic diagram of the second simulation result in an embodiment of a particle filter algorithm for a bearing-only target tracking system of the present invention. DETAILED DESCRIPTION

[0050] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings of the present invention. Obviously, the described embodiments are 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.

[0051] Reference to "embodiments" herein means that a particular feature, structure, or characteristic described in conjunction with the embodiments may be included in at least one embodiment of the present application. The appearance of the phrase in various locations in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments. It is explicitly and implicitly understood by those skilled in the art that the embodiments described herein may be combined with other embodiments.

[0052] See also Figure 1 This embodiment provides a particle filtering algorithm for a bearings-only target tracking system. The method includes the following steps:

[0053] Step S1, constructing a nonlinear discrete state space model and a measurement model for position tracking; defining system process noise and measurement noise;

[0054] Step S2, predicting the current state according to the state space model, constructing a one-step prediction model, and updating and correcting the one-step prediction model of the state by a recursive updating method;

[0055] Step S3, constructing an optimal gain model and a covariance model;

[0056] Step S4, using Bayesian theory, combining the observed values ​​and prior information, to calculate the posterior estimate of the state and its error covariance matrix;

[0057] Step S5, establishing a recursive relationship and updating the covariance matrix;

[0058] Step S6, resample the particles according to the regularized resampling method, resample from the continuous approximate distribution to obtain a new particle set, use the resampled particle set to perform state estimation, and obtain a final state estimation value.

[0059] In the above step S1, a state space model is constructed, which is expressed as the following formula:

[0060] x k =f(x k-1 )+ω k

[0061] z k =h(x k )+v k

[0062] Among them, x k ∈R n is the n-dimensional state vector, z k ∈R m is an m-dimensional observation vector; f(·) and h(·) are known functions, and the state transfer function f(·): R n →R n , measurement function h(·): R n →R m ;ω k is the system process noise, which obeys Gaussian distribution, that is, ω k ~N(0,Q k );v k is the measurement noise, which obeys Gaussian distribution, that is, v k ~N(0,Rk ), and ω k and v k Independent of each other.

[0063] In the above step S2, when constructing the one-step prediction model, according to the Kalman filter theory related to measurement noise and process noise, the covariance C between the true state and the measurement noise is defined as k+1 , expressed as the following formula:

[0064]

[0065] in is a priori estimate.

[0066] In the above step S2, the one-step prediction model of the state is updated and modified step by step, and D is defined k+1 Such as the following formula:

[0067]

[0068] In the above step S3, when constructing the optimal gain model, the optimal gain matrix is ​​calculated in the nonlinear filtering as follows:

[0069]

[0070] Where P z,k+1 represents the covariance matrix of the observed variable z, P xz,k+1 The covariance matrix of the observed variables z and the state variables x.

[0071] In the above step S3, when constructing the covariance model, it is expressed as the following formula:

[0072]

[0073] In the above step S3, the optimization gain model is updated, which is expressed as the following formula:

[0074]

[0075] In the above step S4, according to Bayesian theory, the posterior estimate x k+1 And the error covariance P of the posterior estimate k+1|k+1 The calculation is expressed as the following formula:

[0076]

[0077] In the above step S5, a recursive relationship is established to update the covariance matrix, including:

[0078] let and A recursive relationship is established and the following formula is used for approximate processing:

[0079]

[0080] in C k+1 Represents x k+1 The covariance between θ and the measurement noise.

[0081] In this embodiment, an update model is constructed, which is expressed as the following formula:

[0082]

[0083] In the above step S6, the weight of the particle is normalized, which is expressed as the following formula:

[0084]

[0085] In the above step S6, the particles are resampled according to the regularized resampling method, which is expressed as the following formula:

[0086]

[0087] Among them, w k (i) is the normalized weight, K h (·) is a kernel density function after rescaling the kernel density function K(·), and its expression is as follows:

[0088]

[0089] Among them, the optimal kernel density K(·) is the Epanechnikov kernel density, which is expressed as the following formula:

[0090]

[0091] In this embodiment, the steps of the regularized resampling method include: firstly, calculating the empirical variance S of the particles k , find the solution that satisfies D k (D k ) T =S k D k Then, we sample e from the Epanechnikov kernel density k (i) Finally, resampling is performed to obtain new particles It is expressed as the following formula:

[0092]

[0093] In specific applications, this embodiment is based on the idea of ​​recursive updating and combines regularized particle filtering to propose a regularized particle filtering algorithm based on recursive updating. The algorithm uses the recursive updating Gaussian filtering algorithm to provide an importance density function for the regularized particle filter, so that the posterior state estimate is closer to the true distribution, which can improve the accuracy of RPF; using observed values ​​for recursive updating can effectively reduce the impact of the linearized minimum mean square error in nonlinear models, and combined with RPF can reduce the impact of the lack of particle diversity and improve the accuracy of the algorithm. The specific technical solution is as follows:

[0094] 1. Recursive update: The core idea of ​​recursive update is to use measurement information to recursively update the state one-step prediction multiple times. Each update is small, which can make the linearization of the algorithm more reasonable and reduce the impact of the minimum mean square error on the results; it can also fully explore and utilize nonlinear measurement information, so as to update the target state more accurately.

[0095] First, the covariance C between the true state and the measurement noise is defined according to the Kalman filter related to the measurement noise and process noise. k+1 :

[0096]

[0097] Since the system is nonlinear, in order to make better use of the measurement information, the one-step prediction of the state is gradually updated and corrected to reduce the influence of the linear approximation. k+1 as follows:

[0098]

[0099] Then in nonlinear filtering, the optimal gain matrix can be obtained as

[0100]

[0101] Where P z,k+1 represents the covariance matrix of the observed variable z, P xz,k+1 The covariance matrix of the observed variables z and the state variables x.

[0102] Then according to Bayesian theory, at this time x k+1 The estimated value of and the error covariance P of the posterior estimate k+1|k+1 for:

[0103]

[0104] In order to ensure the smooth progress of the recursion process, and After establishing the recursive relationship, the following method can be used for approximate processing:

[0105]

[0106] in C k+1 Represents x k+1 The covariance between θ and the measurement noise.

[0107] After the above series of processing, the specific steps of recursive update are as follows:

[0108] 1) State prediction:

[0109]

[0110] 2) Measurement recursive update:

[0111] a) Initialization:

[0112] b) Recursive update:

[0113] for i=1:N

[0114]

[0115] end for

[0116] c) Status Update:

[0117] 2. Regularization: Regularization refers to re-establishing the discrete distribution of the posterior probability density into a continuous distribution based on density estimation theory, and then resampling from the continuous approximate distribution to obtain a new set of particles, thereby improving the problem of lack of particle diversity and improving the accuracy of state estimation. The regularized sampling function is as follows:

[0118]

[0119] where w k (i) is the normalized weight, K h (·) is a rescaled kernel density function of the kernel density function K(·), and its expression is as follows:

[0120]

[0121] The optimal kernel density K(·) is the Epanechnikov kernel density:

[0122]

[0123] The sampling steps are as follows: First, calculate the empirical variance S of the particles k , find the solution that satisfies D k (D k )T =S k D k Then, we sample e from the Epanechnikov kernel density k (i) Finally, resampling is performed to obtain new particles

[0124]

[0125] 3. Recursive update regularized particle filter: The selection of importance density function plays a vital role in the realization of particle filter and is also a key factor in the design of particle filter. Standard particle filter randomly extracts particles from the prior probability density function, which may cause a large number of particles to be in the low likelihood area, and when the observed value of the system is at the tail of the prior probability density function, particle degradation will occur, so selecting a suitable importance density function can solve this problem well. The selection of importance density function should be as close to the true distribution of the state as possible. The recursive update mentioned in this embodiment divides the one-step prediction update process into N small steps for progressive update, which can more accurately capture state changes and reduce the error accumulation of each step, thereby improving the overall prediction accuracy, and can effectively use the observed value to correct the state, and adjust the position and weight of the particles in time, so that the state posterior estimate is closer to the true distribution. At the same time, since sequential importance resampling will repeatedly sample high-weight particles and ignore low-weight particles, this may lead to a lack of particle diversity, thereby affecting the estimation results; compared to resampling from discrete functions, when resampling in continuous functions, the weight of the particle is represented as a smooth probability distribution rather than a discrete point, making the sampling closer to the true posterior distribution, which can more accurately describe the uncertainty in the state space and improve the accuracy of particle filtering. Based on the above ideas, this embodiment uses the RUG F algorithm to provide particles with an importance probability density function that is closer to the true state distribution, and uses a regularized method to resample particles from continuous functions when resampling particles, reducing particle degradation and ensuring particle diversity, and realizing a regularized particle filter based on recursive update (Recursive Update Gaussian Re gularized Particle Filter, RUGRPF), and the specific algorithm process is as follows:

[0126] Known and P k , measured value z k+1

[0127] 1) Prediction:

[0128] for i=1:M

[0129]

[0130] end for

[0131] 2) Update:

[0132] for i=1:M

[0133]

[0134] Regenerate particles:

[0135]

[0136] end for

[0137] Weight normalization:

[0138] 3) Resampling

[0139] Resample according to the above regularized resampling method to obtain a new particle set

[0140] 4) State Estimation

[0141]

[0142] Let k=k+1 and repeat step 1) until the number of iterations is met.

[0143] In practical applications, in order to verify the feasibility of the new algorithm, an experimental verification is carried out in a target tracking system with a certain orientation. The model has strong nonlinearity, and its equation is expressed as follows:

[0144]

[0145] Where state x = [x(1); x(2)] T =[s;t] T Represents two-dimensional coordinates;

[0146] System process noise ω k-1 ~N(0,Q),Q=[1 0.05; 0.05 1];

[0147] The observer is located at the point (cosk, sink);

[0148] Observation equation:

[0149] like Figure 2 and 3 As shown, Figure 2 and 3The simulation result diagram is used to show the performance comparison of two algorithms (RUGPF and UPF) in dynamic systems or processes. The initial true value of the state of this simulation is x 0 =[20;5] T ; Initial estimate P 0 =[0.1 0; 0 0.1] T ; Number of particles M = 2000; Number of recursive updates N = 20; Number of Monte Carlo times L = 20; Time T = 60s.

[0150] In summary, this embodiment is based on the idea of ​​recursive update and combines regularized particle filtering to propose a new regularized particle filtering algorithm based on recursive update. The algorithm uses the Gaussian filtering algorithm of recursive update to provide an importance density function for the particle filter, which improves the accuracy and stability of state estimation. Through recursive update and regularization processing, the posterior state estimation is closer to the real distribution. When the particle state is updated, the particle state can be continuously optimized and adjusted in small steps; regularization processing particle resampling can improve the problem of particle diversity deficiency. Regularization processing can increase the diversity of particles, improve the robustness of the algorithm, and improve the accuracy of state estimation. Therefore, this embodiment uses observation values ​​for recursive update to effectively reduce the influence of linearized minimum mean square error in nonlinear models, and combined with RPF can reduce the influence of particle diversity deficiency, improve the accuracy of state estimation, and is suitable for pure bearing target tracking systems, with broad application prospects.

[0151] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0152] The above-mentioned embodiments are only preferred embodiments of the present invention and cannot be used to limit the scope of protection of the present invention. Any non-substantial changes and substitutions made by technicians in this field on the basis of the present invention shall fall within the scope of protection required by the present invention.

Claims

1. A particle filter algorithm for a bearings-only target tracking system, characterized in that: The method comprises the following steps: Construct the nonlinear discrete state space model and measurement model for position tracking; define the system process noise and measurement noise; The current state is predicted according to the state space model, and a one-step prediction model is constructed; wherein the one-step prediction model of the state is updated and corrected by a recursive update method; Construct optimal gain model and covariance model; Using Bayesian theory, combined with observations and prior information, the posterior estimate of the state and its error covariance matrix are calculated; Establish a recursive relationship and update the covariance matrix; The particles are resampled according to the regularized resampling method, and a new particle set is obtained by resampling from the continuous approximate distribution. The state is estimated using the resampled particle set to obtain the final state estimation value.

2. The method according to claim 1, characterized in that Construct the state space model, expressed as the following formula: x k =f(x k-1 )+ω k z k =h(x k )+v k Among them, x k ∈R n is the n-dimensional state vector, z k ∈R m is an m-dimensional observation vector; f(·) and h(·) are known functions, and the state transfer function f(·): R n →R n , measurement function h(·): R n →R m ;ω k is the system process noise, which obeys Gaussian distribution, that is, ω k ~N(0,Q k );v k is the measurement noise, which obeys Gaussian distribution, that is, v k ~N(0,R k ), and ω k and v k Independent of each other.

3. The method according to claim 1, characterized in that: When building a one-step prediction model, according to the Kalman filter theory related to measurement noise and process noise, the covariance C between the true state and the measurement noise is defined as k+1 , expressed as the following formula: in is a priori estimate.

4. The method according to claim 3, characterized in that: Gradually update and modify the one-step prediction model of the state, and define D k+1 Such as the following formula:

5. The method according to claim 4, characterized in that: When constructing the optimal gain model, the optimal gain matrix is ​​calculated in nonlinear filtering as follows: Where P z,k+1 represents the covariance matrix of the observed variable z, P xz,k+1 The covariance matrix of the observed variables z and the state variables x.

6. The method according to claim 5, characterized in that: When constructing a covariance model, it is expressed as the following formula:

7. The method according to claim 6, characterized in that: According to Bayesian theory, the posterior estimate x k+1 And the error covariance P of the posterior estimate k+1|k+1 The calculation is expressed as the following formula:

8. The method according to claim 7, characterized in that The step of establishing a recursive relationship and updating the covariance matrix includes: let and A recursive relationship is established and the following formula is used for approximate processing: in C k+1 Represents x k+1 The covariance between θ and the measurement noise.

9. The method according to any one of claims 1 to 8, characterized in that: The weight of the particle is normalized and expressed as the following formula:

10. The method according to claim 9, characterized in that: The particles are resampled according to the regularized resampling method, which is expressed as the following formula: Among them, w k (i) is the normalized weight, K h (·) is a kernel density function after rescaling the kernel density function K(·), and its expression is as follows: Among them, the optimal kernel density K(·) is the Epanechnikov kernel density, which is expressed as the following formula: The steps of the regularized resampling method include: firstly, calculating the empirical variance S of the particle k , find the solution that satisfies D k (D k ) T =S k D k Then, we sample e from the Epanechnikov kernel density k (i), finally resampling to obtain new particles It is expressed as the following formula:

Images