A design method of parameter self-tuning kalman filter based on extended state

Abstract

The application discloses a parameter self-tuning Kalman filter design method based on an extended state, which is used for reducing estimation error of a filter when estimating a state of a nonlinear and variable trajectory motion target, enhancing smoothness of a filtering curve, and meeting higher-precision target tracking requirements. Although traditional Kalman filtering can realize optimal estimation under Gaussian noise, it is only applicable to linear state equations of a tracked target and requires accurate state equations. When the traditional Kalman filtering is applied to the field of target state estimation, estimation error problems will inevitably occur when facing tracked targets that cannot be modeled in advance or even have maneuverability. The application estimates parameters of state equations of a tracked target in real time, so that the filter can be applied to the field of target tracking of nonlinear or variable trajectory motion. The application breaks through the limitation of the traditional Kalman filtering method, effectively improves estimation accuracy and smoothness of a filtering curve of the filter, and optimizes estimation effect of the filter.

Description

Technical Field

[0001] This invention belongs to the field of target tracking, specifically relating to a parameter self-tuning Kalman filter design method based on extended states. It is mainly used for state estimation and filtering of maneuvering targets with variable trajectories that cannot be pre-modeled. This invention effectively extends the state estimation capability of the Kalman filter algorithm for nonlinear, variable-trajectory moving targets, improves filtering accuracy and the smoothness of the filtering curve, and optimizes the estimation effect of the filter. Background Technology

[0002] This invention can be applied to fields such as photoelectric tracking, motion capture, and trajectory prediction. In these applications, the tracked target is often uncontrolled, which not only makes it impossible to pre-establish the relevant state equations, but also may cause them to change during the filtering process (see "Hypersonic Vehicle Trajectory Tracking and Prediction Based on Singer Model" (Wei Xiqing, Aerospace Control, 2017)). The Kalman filter method is widely used due to its optimal estimation characteristics under Gaussian noise and its speed (see "Stochastic theory of minimal realization" (Clary, JW Proceedings of the IEEE, IEEE Conference on Decision & Control Including the Symposium on Adaptive Processes, 1976)). However, it requires an accurate and linear state equation for the tracked target, so the filtering effect deteriorates significantly when applied to maneuvering target tracking (see "Applied optimal estimation" (Arthur G. Proceedings of the IEEE, 1974)). While algorithms such as Extended Kalman Filter (EDF) and Unscented Kalman Filter (UKF), derived from the classic Kalman Filter algorithm, can achieve tracking and estimation of nonlinear moving targets, they also rely on accurate modeling of the tracked target ("Kalman Filter Principles and Applications" (Huang Xiaoping, 2015"). Common robust state estimation methods can compensate for the uncertainty of the state equation of the tracked target to some extent, but the uncertainty must be finite ("A Framework for State-Space Estimation with Uncertain Models" (Ali H. Sayed. IEEE TRANSACTIONS ON AUTOMATIC CONTROL, 2001"). An effective estimation method for uncertain objects is to use an extended state observer ("An Extended State Observer for a Class of Uncertain Objects" (Han Jingqing, Control and Decision, 1995)). A method for estimating the disturbance of the state equation using extended state was successfully published in 2019 ("A Model Predictive Control Algorithm Based on Extended State Kalman Filter" (Shen Jiong, CN201910614372.6, 2019)). However, it is still limited by the deterministic state equation and is difficult to achieve accurate estimation of maneuvering targets. As the performance requirements for filters in fields such as photoelectric tracking continue to increase, designing nonlinear filters with self-tuning capabilities becomes particularly important when facing maneuvering, uncontrolled targets. We urgently need to improve existing Kalman filtering methods to ensure they maintain good filtering performance when dealing with maneuvering targets. Summary of the Invention

[0003] To address the filtering and state estimation problems of tracked targets that are uncontrolled and have variable trajectories, this invention proposes a parameter self-tuning Kalman filter based on extended state.

[0004] To achieve the objectives of this invention, the technical solution adopted is as follows: a parameter self-tuning Kalman filter design method based on extended states, which estimates the state transition matrix F of a target tracking Kalman filter. KF The parameters are used to achieve state estimation for targets with nonlinear or even variable trajectory motion; the following are the state transition equations of the target tracking Kalman filter and the estimated state of the designed extended state Kalman filter:

[0005]

[0006] x i =[p i p i-1 … p i-m+1 c1 c2 … c m ] 1×2m T

[0007] in, p represents the estimated state of the classical Kalman filter. i F represents the position of the tracked target at time i. KF It is the state transition matrix, B KF It is the process noise driving matrix. It is process noise, c i These are the parameters that make up the state transition matrix, x i It is the estimated state of the parameter self-tuning Kalman filter.

[0008] Furthermore, the state transition equation of the new filter is no longer linear and needs to be estimated using the unscented Kalman filter algorithm; the corresponding state transition equation and observation equation are as follows:

[0009]

[0010] y i =h(x i ,v i = [1 0 … 0] 1×2m x i +v i

[0011] Where, f(x) i ,u i h(x) i ,v i These are the state transition equation and observation equation of a parameter-self-tuning Kalman filter, respectively.i v i These are process noise and observation noise, respectively.

[0012] Furthermore, the dimension of the estimated state can be flexibly selected according to the controller's computing power and the characteristics of the tracking scenario, but the number of weight values ​​c is equal to the number of position values ​​p.

[0013] Furthermore, the unscented Kalman filter algorithm needs to be used to complete the calculation. The specific implementation steps are as follows:

[0014] Step (1): Set parameters α, β, κ 、 initial value of state estimate and the number of states n, the initial value of the posterior state covariance P0, the observation noise covariance R, the process noise covariance Q, and the state transition equation. Observation equations

[0015] Step (2): Calculate the parameter λ;

[0016] Step (3): UT transformation yields 2n+1 Sigma points X (k) ;

[0017] Step (4): Calculate 2n+1 Sigma points X (k) Each of their respective weights w (k) ;

[0018] Step (5): 2n+1 Sigma points X (k) Each step of the prediction is performed to obtain

[0019] Step (6): Calculate the prior state values and state prior covariance P i+1|i ;

[0020] Step (7): Calculate 2n+1 new Sigma points

[0021] Step (8): Observe and simulate to calculate Z (k) ;

[0022] Step (9): Based on 2n+1 Z (k) Points and corresponding weights w (k) Predicted observations Covariance P xz and P zz ;

[0023] Step (10): Calculate the Kalman gain matrix K;

[0024] Step (11): Calculate the posterior state estimate of the system Covariance P i+1 ;

[0025] Step (12): Calculate the estimated position of the tracked target.

[0026] Step (13): Return to step (3) to perform a new round of filtering calculation.

[0027] Furthermore, the target tracking Kalman filter, a commonly used Kalman filtering algorithm in the field of target tracking, uses the kinematic features of the target, such as position and velocity, to form the estimated quantity. This algorithm uses the estimated target's historical position and the linear combination coefficients of each historical position during state transition to form a state transition matrix; in many cases, this algorithm can effectively improve the adaptability of the filter.

[0028] Furthermore, by using a parameter self-tuning Kalman filter based on extended states, the accuracy and smoothness of the estimation results can be significantly improved, and the adaptability of the filter can be enhanced, even when the motion equations of the tracked target cannot be pre-modeled or the target is maneuverable.

[0029] Specifically, this invention first clarifies the state equation of the target tracking Kalman filter and the estimated state:

[0030]

[0031] x i =[p i p i-1 …p i-m+1 c1 c2…c m ] 1×2m T

[0032] Unlike common Kalman filters that use the target's position, velocity, and acceleration as the estimated states, this equation uses the target's current position and the positions of the previous m-1 time steps as the estimated states, and obtains the position value at the next time step by linearly summing them. The problem with this filter is that it can only estimate some nonlinearly moving targets and cannot solve the maneuvering problem of the tracked target. To address this, this invention utilizes an extended state method, setting the state as a combination of the target's current position, the position values ​​of the previous m-1 time steps, and the linear summation coefficients of each position value; through parameter self-tuning, the filter adapts to nonlinearly maneuvering targets. The relevant estimated states and state equations are as follows:

[0033] x i =[p i p i-1 … p i-m+1c1 c2 … c m ]

[0034]

[0035] y i =h(x i ,v i = [1 0 … 0] 1×2m x i +v i

[0036] In this state equation, x i y i These represent the estimated state and the observed value, respectively; the initial value of the estimated state x0, and the state transition noise u. i Observation noise v i It follows a Gaussian distribution; f(x) i ,u i h(x) i ,v i ) are the state transition equation and the observation equation, respectively.

[0037] δ ij It is the Kronecker symbolic equation, that is, when i = j, δ ij =1; when i≠j, δ ij =0. x0, u i v i satisfy:

[0038]

[0039] Because the state transition equation of the extended state filter is f(x) i ,u i The equation is nonlinear, and this invention employs an unscented Kalman filter algorithm for state estimation. It is important to clarify that the core of this algorithm is still the classical Kalman filter; the unscented Kalman filter algorithm is used to achieve self-tuning of the state transition matrix parameters of the classical Kalman filter. The steps of the unscented Kalman filter algorithm are as follows:

[0040] Step (1): Set the parameters α, β, κ used in the UT transform of the Unscented Kalman Filter (UKF); set the initial value P0 of the posterior state covariance, the observation noise covariance R, and the process noise covariance Q, all selected according to the actual application scenario; set the state transition equation of the Unscented Kalman Filter. and observation equations The relevant functions are as described above; set the initial value of the UKF state estimate. The position values ​​are selected based on the actual application scenario, and the linear summation coefficients are c1, c2, ..., c. m The following rules can be used to set it up:

[0041]

[0042] Step (2): Based on the preset parameters and the estimated state The dimension n is calculated using the parameter λ:

[0043] λ=α 2 (n+κ)-n

[0044] Step (3): Based on the preset parameters, the λ calculated in the previous step, and the estimated state With its dimension n and state posterior covariance P i Perform the following UT transformation to obtain 2n+1 Sigma points X. (k) ;

[0045]

[0046] formula Let represent the square root of the i-th column after performing the square root operation on matrix M.

[0047] Step (4): Each Sigma point X (k) The corresponding weight w (k) for:

[0048]

[0049] Step (5): 2n+1 Sigma points X (k) According to the state transition equation Each step of the prediction is performed to obtain

[0050]

[0051] Step (6): Based on 2n+1 Points and corresponding weights w (k) Calculate the estimated state Prior state values and state prior covariance P i+1|i :

[0052]

[0053]

[0054] Step (7): For the prior state values Performing the UT transformation yields 2n+1 new Sigma points.

[0055]

[0056] Step (8): 2n+1 Sigma points According to the observation equation Z was obtained by observation simulation. (k) :

[0057]

[0058] Step (9): Based on 2n+1 Z (k) Points and corresponding weights w (k) Predicted observations Covariance P xz and P zz :

[0059]

[0060]

[0061]

[0062] Step (10): Calculate the Kalman gain matrix K:

[0063]

[0064] Step (11): Calculate the posterior state estimate of the system Covariance P i+1 :

[0065]

[0066] P i+1 =P i+1|i -KP zz K T

[0067] Step (12): Calculate the estimated position of the tracked target. Here, a further prediction and simulation of the observed p is needed to evaluate the posterior estimated state. i To compensate for the lag caused by the self-tuning parameters:

[0068]

[0069] Step (13): Return to step (3) to perform a new round of filtering calculation.

[0070] Compared with the prior art, the present invention has the following advantages:

[0071] (1) Compared with the traditional Kalman filtering method, the present invention can be applied to tracking nonlinear moving targets, and realizes the improvement of targets with nonlinear state equations;

[0072] (2) Compared with the traditional Kalman filtering method and the unscented Kalman filtering method, the present invention does not require the precise state equation of the tracked target, and achieves good estimation of uncontrolled variable trajectory moving targets.

[0073] (3) Compared with common robust state estimation methods, this invention improves the adaptability to maneuvering targets by estimating the parameters of the state equation of the tracked target, rather than by compensating for parameter uncertainty.

[0074] (4) Compared with other extended state Kalman filtering methods, the estimated state of this invention is not a disturbance or noise of the state equation, but a linear summation parameter of the motion equation of the tracked target, which breaks through the limitation of other extended state Kalman filtering methods that rely on the state equation and cannot adapt to maneuvering targets.

[0075] (5) Compared with common filtering methods, when using the present invention, the estimated state dimension can be set independently according to the target tracking scenario, which expands the adaptability to tracking targets with different characteristics and controllers with different performance. Attached Figure Description

[0076] Figure 1 This is a comparison of the estimation results of the classical Kalman filter and the parameter self-tuning Kalman filter based on the extended state under the simulation conditions of a variable sine wave.

[0077] Figure 2 This invention presents a schematic diagram illustrating the cumulative estimation errors of two types of filters.

[0078] Figure 3 This is a comparison of the estimation results of the classical Kalman filter and the parameter self-tuning Kalman filter based on the extended state under the simulation condition of variable triangular wave.

[0079] Figure 4 This is a schematic diagram showing the cumulative estimation errors of the two filters. Detailed Implementation

[0080] The present invention will be further described below with reference to the accompanying drawings and specific embodiments.

[0081] To achieve the objectives of this invention, a method for designing a parameter self-tuning Kalman filter based on extended states is provided, and its algorithm flow is as follows:

[0082] Step (1): Based on the target tracking scenario and controller performance, set the parameters to self-tun the number of estimated states and the initial value of the estimated Kalman filter. Initial values ​​of the posterior state covariance P0, observation noise covariance R, and process noise covariance Q are set; the parameters α, β, and κ used for the unscented Kalman filter are set; and the state transition equations are set according to the principles described above. and observation equations

[0083] Step (2): Calculate the parameter λ;

[0084] Step (3): Calculate 2n+1 Sigma points X (k) ;

[0085] Step (4): Calculate 2n+1 Sigma points X (k) Each of their respective weights w (k) ;

[0086] Step (5): Make a prediction to obtain

[0087] Step (6): Calculate the estimated state Prior state values and state prior covariance P i+1|i ;

[0088] Step (7): Obtain 2n+1 new Sigma points

[0089] Step (8): Perform observation simulation to obtain Z (k) ;

[0090] Step (9): Predict the observed values Covariance P xz and P zz ;

[0091] Step (10): Calculate the Kalman gain matrix K;

[0092] Step (11): Calculate the posterior state estimate of the system Covariance P i+1 ;

[0093] Step (12): Calculate the estimated position of the tracked target.

[0094] Step (13): Return to step (3) to perform a new round of filtering calculation.

[0095] The parameter self-tuning Kalman filter based on the extended state obtained through the above steps significantly improves the filtering accuracy when the motion equation of the tracked target is unknown and it is undergoing variable trajectory motion.

[0096] The following Matlab simulations yielded the filtering results of a classic Kalman filter and a parameter-self-tuned Kalman filter based on extended states when the tracked target undergoes sinusoidal and triangular motions, to illustrate the design process and effects of this invention in detail:

[0097] (1): The relevant formulas and parameters used for trajectories simulation of variable sinusoidal motion and variable triangular motion are as follows:

[0098] Formulas and parameters related to variable sinusoidal motion:

[0099] p i =A i sin(w i t i )+v i

[0100] t i+1 =t i +0.02+u i (1),w i+1 =w i +u i (2),A i+1 =A i +u i (3)

[0101] u i =[t i w i A i ] T

[0102] R = 10, w0 = 2π, A0 = 100

[0103] Formulas and parameters related to variable trigonometric motion:

[0104]

[0105] t i+1 =t i +0.02+u i (1),w i+1 =w i +u i (2),A i+1 =A i +u i (3),T i+1 =T i +u i (4)

[0106] u i =[t i w i A i T i ] T

[0107]

[0108] Formulas and parameters related to the classic Kalman filter:

[0109]

[0110] y i =[1 0],x i +v i

[0111]

[0112] Formulas and parameters for parameter self-tuning Kalman filters based on extended states:

[0113] x i =[p i p i-1 … p i-49 c1 c2 … c 50 ]

[0114]

[0115] y i =h(x i ,v i = [1 0 … 0] 1×2m x i +v i

[0116]

[0117] α=0.01, β=2, κ=0,

[0118] In the above formula, u i ,v i Let Q be Gaussian white noise with covariances Q and R.

[0119] (2): Simulate the classical Kalman filter and the parameter self-tuning Kalman filter based on the extended state according to the method described in steps (2) to (12). Perform 2000 steps of trajectory generation and filtering simulation in the simulation of variable triangular wave and variable sine wave.

[0120] (3): Figure 1 This is a comparison of the estimation results of the classical Kalman filter and the parameter self-tuning Kalman filter based on the extended state, under the simulation condition of a variable sine wave. It can be seen that both filtering methods achieve good position estimation, but the superiority of one method over the other is not immediately apparent. Therefore, in... Figure 2 In this invention, the cumulative estimation errors of two filters are statistically analyzed; by Figure 2It can be seen that the estimation accuracy of the parameter self-tuning Kalman filter based on the extended state is significantly better than that of the classical Kalman filter, and the cumulative error is reduced by about 55%. This shows the advantages of the present invention in estimation accuracy and estimation accuracy when dealing with tracked targets with changing trajectories.

[0121] (4): Figure 3 This chart compares the estimation results of a classical Kalman filter and a parameter-self-tuning Kalman filter based on the extended state under a variable triangular wave simulation. It can be seen that the parameter-self-tuning Kalman filter significantly outperforms the classical Kalman filter in estimation accuracy at the peak of the triangular wave. To quantitatively demonstrate the advantages of the parameter-self-tuning Kalman filter, Figure 4 The cumulative estimation errors of the two filters were statistically analyzed; by Figure 4 It can be seen that the estimation accuracy of the parameter self-tuning Kalman filter based on the extended state is significantly better than that of the classical Kalman filter, and the cumulative error is reduced by about 45%. This further proves the effectiveness of the present invention and its adaptability to different motion trajectories of the tracked target.

Claims

1. A method for designing a parameter self-tuning Kalman filter based on extended states, characterized in that: By estimating the state transition matrix of a target tracking Kalman filter The parameters are used to achieve state estimation of targets with nonlinear and variable trajectory motion; the following are the state transition equations of the target tracking Kalman filter and the estimated state of the designed extended state Kalman filter: in, The estimated state represents the classical Kalman filter. This represents the position of the tracked target at time i. It is the state transition matrix. It is the process noise driving matrix. It's process noise. These are the parameters that make up the state transition matrix. It is the estimated state of the parameter-self-tuning Kalman filter; The state transition equation of the new filter is no longer linear and needs to be estimated using the unscented Kalman filter algorithm; the corresponding state transition equation and observation equation are as follows: in, , The estimated state and the observed value are respectively. , These are the state transition equation and observation equation of a parameter-self-tuning Kalman filter, respectively. , These are process noise and observation noise, respectively. Using the extended state method, the state is set as a combination of the current position of the tracked target, the position values ​​of the previous m-1 time steps, and the linear summation coefficient of each position value; linear summation coefficient value The following rules can be used to set it up: 。 2. The method for designing a parameter self-tuning Kalman filter based on extended states according to claim 1, characterized in that: The dimension of the estimated state can be flexibly selected according to the controller's computing power and the characteristics of the tracking scenario, but the weight values... The number equals the position value The quantity.

3. The method for designing a parameter self-tuning Kalman filter based on extended states according to claim 1, characterized in that: The calculation needs to be performed using the unscented Kalman filter algorithm. The specific implementation steps are as follows: Step (1): Set parameters Initial values ​​of state estimates and the number of states n, and the initial value of the posterior covariance of the states. Observation noise covariance Process noise covariance State transition equations Observation equations ; Step (2): Calculate parameters ; Step (3): UT transformation yields 2n+1 Sigma points. ; Step (4): Calculate 2n+1 Sigma points Each of their respective weights ; Step (5): 2n+1 Sigma points Each step of the prediction is performed to obtain ; Step (6): Calculate the prior state values State prior covariance ; Step (7): Calculate 2n+1 new Sigma points ; Step (8): Observation and simulation calculation ; Step (9): Based on 2n+1 Points and corresponding weights Predicted observations Covariance and ; Step (10): Calculate the Kalman gain matrix ; Step (11): Calculate the posterior state estimate of the system Covariance ; Step (12): Calculate the estimated location of the tracked target. ; Step (13): Return to step (3) to perform a new round of filtering calculation.

4. The method for designing a parameter self-tuning Kalman filter based on extended states according to any one of claims 1-3, characterized in that: By using a parameter-self-tuned Kalman filter based on extended states, the accuracy and smoothness of the estimation results can be improved and the adaptability of the filter can be enhanced when the motion equation of the tracked target cannot be pre-modeled or the target has maneuverability.

Images