A design method of variable parameter Kalman filter based on historical state

Abstract

The present invention discloses a variable parameter Kalman filter design method based on historical state, which is used to improve the prediction effect of the classic Kalman filter method when the model is nonlinear or uncertain, and can effectively reduce the estimation error of the filter and improve the smoothness of the prediction curve to meet the state estimation requirements with higher accuracy. The present invention proposes a variable parameter Kalman filter algorithm based on historical state, and its parameter setting method includes five types: constant proportion parameters, variable proportion parameters, offline linear regression parameters, online fixed storage linear regression parameters, and online incremental linear regression parameters, all of which can effectively improve the prediction effect of the filter while only spending less additional computational complexity. The present invention breaks through the limitations of the traditional Kalman filtering method, uses less additional computational complexity, effectively improves the prediction accuracy of the Kalman filter for maneuvering targets and the smoothness of the prediction curve, and optimizes the estimation effect of the filter.

Description

Technical Field

[0001] The present invention belongs to the field of predictive filtering, and specifically relates to a variable parameter Kalman filter design method based on historical states. The method is mainly used to predict the position of a tracked target when the target makes maneuvers rather than at a constant speed, constant acceleration, or uniform acceleration. The method can effectively improve the estimation accuracy of state prediction and the smoothness of the estimation curve, and optimize the estimation effect of the filter. Background Art

[0002] The present invention can be applied to high-speed tracking fields that require prediction of target positions. In these application fields, such as optoelectronic tracking systems, sensors (such as charge coupled device (CCD) cameras) require time for imaging and image processing, so the position of the tracked target cannot be obtained in real time; and the tracked target often moves at high speed or even maneuvers, and accurate tracking of the target cannot be achieved without a prediction algorithm "Research Progress on Precision Control Technology in Optoelectronic Tracking Systems" (Tang Tao, "Optoelectronic Engineering", 2020); Therefore, the Kalman filter algorithm "Kalman Filter Principles and Applications" (Huang Xiaoping, 2015) is generally used in the field to achieve position prediction.

[0003] In the classical control theory "Principles of Automatic Control" (Hu Shousong), control methods are mostly based on the analysis of the controlled quantity (position value) and its various order derivatives (speed, acceleration, etc.), so variables such as position and speed are also commonly used as estimated state quantities of the Kalman filter algorithm. The limitation of the Kalman filter method is that it requires an accurate state equation of the tracked target, which is often not available; the tracked target may also maneuver, which further reduces the effect of the Kalman filter position prediction.

[0004] Advanced intelligent algorithms for prediction, such as recurrent neural networks (Schuster M, IEEE Transactions on Signal Processing, 1997), contain recurrent units in the hidden layer, which can effectively process time series data. This has inspiring significance for the present invention, but the neural network itself requires a lot of computing resources for training and cannot be carried out in real time; the trained neural network also requires more computing resources to run, and the real-time performance is poor; therefore, it is difficult to directly apply it to the field of high-speed tracking that requires the prediction of the target position.

[0005] As the requirements for filter prediction accuracy and speed in various fields continue to increase, it is particularly important to design a low-computational complexity filter that can effectively track maneuvering targets. We urgently need to improve the Kalman filter method and enhance its adaptability under new tracking requirements. Summary of the invention

[0006] In order to maintain the low computational complexity advantage of the Kalman filter algorithm and enhance its position prediction accuracy and smoothness when dealing with maneuvering targets, the present invention proposes a variable parameter Kalman filter design method based on historical states.

[0007] In order to achieve the purpose of the present invention, a variable parameter Kalman filter design method based on historical state is provided. The present invention first clarifies the state equations corresponding to each sub-state estimator and the meanings of the parameters therein:

[0008] x(k+1)=F(k)x(k)+G(k)u(k),k≥0

[0009] y(k)=H(k)x(k)+v(k)

[0010] In this state equation, x(k) and y(k) are the estimated state value of the variable parameter Kalman filter and the observed value of the tracked target position, respectively; x(0), u(k), and v(k) conform to the Gaussian distribution; F(k) and G(k) are the state transfer matrix and the process noise driving matrix, respectively.

[0011] Before performing Kalman filtering, the dimension of x(k) and the value of F(k) need to be determined based on the application scenario and controller performance. Assuming that the estimated state is n-dimensional, then:

[0012]

[0013] in is the estimated position of the tracked target at time k.

[0014] Position prediction is essentially based on the formula In the classic filtering method that uses the position value of the tracked target and its high-order quantities (velocity, acceleration) as the estimated state, the formula changes to the following formula, where T is the sampling interval of the Kalman filter:

[0015]

[0016]

[0017] The corresponding state transfer matrix is:

[0018]

[0019] The present invention uses historical data value p k ,p k-1 ,...,p k-n+1 get A total of 5 feasible prediction methods and the corresponding F(k) design formulas are provided as follows:

[0020] (1) Equal-proportional constant parameter method. This method is based on the prediction formula:

[0021]

[0022]

[0023] And the corresponding F(k) design formula can be obtained:

[0024]

[0025] (2) Variable proportional constant parameters. This method is based on the prediction formula, where w1…w n It can be designed as a sequence whose sum is 1 according to actual needs.

[0026] p k+1 =p k +w1(p k -p k-1 )+w2(p k-1 -p k-2 )+...+w n (p k-n+2 -p k-n+1 )

[0027] When n When it is designed as an arithmetic sequence, then:

[0028] p k+1 =(1+w1)p k +dp k-1 +dp k-2 +...+dp k-n+2 -w n p k-n+1

[0029] w n =w1+(n-1)d

[0030] p k+1 =(1+w1)p k +dp k-1 +dp k-2 +...+dp k-n+2 -[w1+(n-1)d]p k-n+1

[0031]

[0032]

[0033]

[0034] And the corresponding F(k) design formula can be obtained:

[0035]

[0036] (3) Offline linear regression parameters. This method requires first obtaining a set of position data [p1, p2, ..., p N ]; then set the estimated state dimension used by the Kalman filter algorithm to n according to actual needs; then use the position data at time k, k-1, ..., k-n+1 to form the input vector x, and use the position data at time k+1 as the output vector y; then use N input vectors x to form X, and use the corresponding N output vectors y to form Y; finally, perform multivariate linear regression on X and Y to obtain a set of weights, so that for any x, Wx is close to y. Among them:

[0037] W=(X'X) -1 (X'Y), W=[w1,w2,...,w n ]

[0038] And the corresponding F(k) design formula can be obtained:

[0039]

[0040] (4) Online fixed storage linear regression parameters. This method requires the filter to collect N position data without prediction during operation. From time N+1, the filter uses the data of the most recent N+1 time points to calculate W in real time according to method (3). The corresponding F(k) design formula is also:

[0041]

[0042] (5) Online incremental linear regression parameters. Before introducing this parameter design method, we will first introduce the calculation method of multivariate linear regression. For a vector x with c dimensions and a single-dimensional value y, if they each have n data, the following matrix can be designed:

[0043]

[0044] Design augmented matrix Z = [XY] n×(p+1) , the cross product matrix of Z is:

[0045]

[0046] Then the linear regression weight W can be expressed as

[0047] When new data is added to the dataset:

[0048]

[0049] but The cross product matrix is:

[0050]

[0051] but

[0052] According to the above derivation, before the filter of the present invention starts prediction, it first calculates A(0)=X'X, B(0)=X'Y according to the method (3); after starting prediction, at time k, let X * =[p k-1 ,p k-2 ,...,p n ]',Y * =p k , then A(k)=A(k-1)+X * 'X * , B(k)=B(k-1)+X * 'Y * ; Further calculate W = A(k) -1 B(k), we can get the adaptive weight value W=[w1,w2,...,w n ]'. And further obtain the corresponding F(k):

[0053]

[0054] After obtaining the appropriate F(k) value, the prediction filtering work can be completed according to the classic Kalman filter algorithm:

[0055] (1) State estimation covariance update obtains

[0056]

[0057] (2) Calculate the Kalman gain matrix K(k):

[0058]

[0059] (3) The estimated state is updated

[0060]

[0061] (4) State estimation covariance update is obtained

[0062]

[0063] (5) State prediction acquisition

[0064]

[0065] (6) Return to (1) and enter the next filtering cycle. If F(k) needs to be adjusted, the parameter optimization of F(k) is completed first.

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

[0067] (1) Compared with the classical Kalman filtering method, the present invention can effectively improve the position prediction accuracy of the filter for nonlinear maneuvering targets;

[0068] (2) Compared with the improved Kalman filter algorithms for nonlinear targets, such as the extended Kalman filter and the unscented Kalman filter, the variable parameter Kalman filter based on historical states does not require the precise motion equations of the maneuvering target;

[0069] (3) Compared with the traditional prediction filtering method, the Kalman prediction filtering method in the present invention can correct the algorithm model in real time according to the observed target trajectory, and has stronger adaptability;

[0070] (4) Compared with intelligent algorithms such as recurrent neural networks, the present invention uses less computational effort to achieve good target position prediction accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 This is a schematic diagram of the trajectory prediction of eight Kalman filters.

[0072] Figure 2 This is a schematic diagram of the cumulative prediction errors of several Kalman filters.

[0073] Figure 3 This is a comparison chart of the prediction effects of the variable parameter Kalman filter, robust filter, two types of UKF filters, and IMM filter based on historical states in experiments.

[0074] Figure 4 is a schematic diagram showing the improvement of the effect of the variable parameter Kalman filter based on historical states, where (a) shows the comparison of the cumulative prediction errors of the classic Kalman filter, the robust filter, two UKF filters, and the IMM filter. It can be seen that the prediction effect of the classic Kalman filter is inferior to that of the other four filters; (b) shows the comparison of the cumulative prediction errors of the variable parameter Kalman filter based on historical states (F(k) is obtained by multivariate linear regression) and the robust filter, two UKF filters, and the IMM filter. DETAILED DESCRIPTION

[0075] The present invention is further described below with reference to the accompanying drawings and specific embodiments.

[0076] To achieve the purpose of the present invention, the present invention provides a variable parameter Kalman filter based on historical state, and its algorithm flow is as follows:

[0077] Step (1): According to the application scenario requirements and controller performance, select an appropriate one from the following five methods for calculating F(k):

[0078] 1) The F(k) design formula of the equal-proportional constant parameter method, where n is the dimension of the preset estimated state:

[0079]

[0080] 2) F(k) design formula of variable proportion constant parameter method, where w1 is the preset weight value:

[0081]

[0082] 3) Offline linear regression parameter method: First obtain a set of position data [p1, p2, ..., p N ]; then set the estimated state dimension used by the Kalman filter algorithm to n according to actual needs; then use the position data at time k, k-1, ..., k-n+1 to form the input vector x, and use the position data at time k+1 as the output vector y; then use N input vectors x to form X, and use the corresponding N output vectors y to form Y; finally, perform multivariate linear regression on X and Y to obtain a set of weights W and corresponding F(k) values:

[0083] W=(X'X) -1 (X'Y), W=[w1,w2,...,w n ]

[0084]

[0085] 4) Online fixed storage linear regression parameter method: During operation, the filter first collects N position data without making predictions. From time N+1, the filter uses the data from the most recent N+1 time points to calculate W in real time according to method (3). The corresponding F(k) is also:

[0086]

[0087] 5) Online incremental linear regression parameter method: Before the filter starts prediction, it first calculates A(0) = X'X, B(0) = X'Y based on historical data according to method (3); after starting prediction, at time k, let X * =[p k-1 ,p k-2 ,...,p n ]',Y * =p k , then A(k)=A(k-1)+X * 'X *, B(k)=B(k-1)+X * 'Y * ; Further calculate W = A(k) -1 B(k), obtain the weight value W = [w1,w2,...,w n ]'. And further obtain the corresponding F(k):

[0088]

[0089] Step (2): Update the state estimation covariance to obtain

[0090] Step (3): Calculate the Kalman gain matrix K(k):

[0091] Step (4): The estimated state is updated to obtain

[0092] Step (5): Update the state estimation covariance to obtain

[0093] Step (6): Obtain state prediction

[0094] Step (7): Return to step (2) to enter the next round of state estimation cycle. If F(k) needs to be adjusted, the parameter optimization of F(k) is completed first.

[0095] Below, the present invention uses simulation to compare the prediction effect of the Kalman filter designed by the equal-proportional parameter method and the variable-proportional parameter method with the prediction effect of the traditional Kalman filter method; in addition, the present invention uses experiments to compare the prediction error of the Kalman filter designed by the multivariate linear regression method with the prediction error of the classic Kalman filter method, three adaptive robust filtering methods, and the interactive multi-model filtering method. This illustrates the effectiveness and superiority of the present invention:

[0096] (1.1): The simulation involved in the present invention performs trajectory simulation according to the following parameters, where Q and R are the covariance matrices of Gaussian white noise u(k) and v(k) respectively:

[0097] x(k+1)=Fx(k)+Gu(k), k≥0

[0098] y(k)=Hx(k)+v(k)

[0099]

[0100]

[0101] G=I 8×8 ,T=0.01,N=200,

[0102]

[0103] x0=[-100 20 0 0 200 20 0 0] T

[0104] (1.2): In the simulation, the present invention designs 4 classical Kalman prediction methods, 3 Kalman prediction methods of equal proportional parameter method, and 1 Kalman prediction method of variable proportional parameter method for comparison. Their estimated states x and state transfer matrices F are respectively:

[0105]

[0106]

[0107]

[0108]

[0109]

[0110]

[0111]

[0112]

[0113] (1.3): Simulate the above 8 Kalman filters according to the methods described in steps (2) to (7), and calculate their cumulative prediction errors.

[0114] (1.4): If Figure 1 These are the trajectory predictions of eight Kalman filters, but the comparison cannot be seen directly. To better show the differences between the filters, Figure 2 The cumulative prediction errors of several Kalman filters are statistically analyzed. It can be seen that the four variable parameter Kalman filters based on historical states all show better prediction effects than the classic Kalman filter with position and its high-order quantities as estimated states. Among them, the Kalman filter method obtained by designing F(k) according to the variable proportion constant parameter method, i.e., the green curve at the bottom of the figure, shows the highest prediction accuracy. This proves the superiority of the present invention.

[0115] (2.1): The present invention has also experimentally proved the superiority of the prediction effect of the Kalman filter designed in one step by using the multivariate linear regression method to obtain F(k). That is, first obtain a set of position data [p1, p2, ..., p N]; then set the estimated state dimension used by the Kalman filter algorithm to n according to actual needs; then use the position data at time k, k-1, ..., k-n+1 to form the input vector x, and use the position data at time k+1 as the output vector y; then use N input vectors x to form X, and use the corresponding N output vectors y to form Y; finally, perform multivariate linear regression on X and Y to obtain a set of weights W and corresponding F(k) values. Here, because the observations contain two dimensions in the x and y directions, the form is slightly different:

[0116] W=(X'X) -1 (X'Y), W=[1.20980.3301-0.54361.21710.2725-0.4938]

[0117]

[0118] (2.2): If Figure 3 It is a comparison chart of the prediction effects of the variable parameter Kalman filter, robust filter, two UKF filters, and IMM filter based on historical states in the experiment. It can be seen that the variable parameter Kalman filter based on historical states has excellent prediction accuracy. To further demonstrate the effect improvement of the variable parameter Kalman filter based on historical states, as shown in Figure 4, (a) shows the cumulative prediction error comparison of the classic Kalman filter and the robust filter, two UKF filters, and IMM filter. It can be seen that the prediction effect of the classic Kalman filter is inferior to the other four filters. (b) shows the cumulative prediction error comparison of the variable parameter Kalman filter based on historical states (F(k) is obtained by multivariate linear regression) and the robust filter, two UKF filters, and IMM filters. It can be seen that the prediction effect of the variable parameter Kalman filter is significantly better than the other four filters, and is also significantly better than the classic Kalman filter. That is, the present invention has once again verified the superiority of the variable parameter Kalman filter based on historical states in prediction accuracy through experiments.

Claims

1. A variable parameter Kalman filter design method based on historical state, characterized in that: The specific implementation steps are as follows: Step (1): First, clarify the state equation of the state estimator and the meaning of its parameters: x(k+1)=F(k)x(k)+G(k)u(k),k≥0 y(k)=H(k)x(k)+v(k) In the state equation, x(k) and y(k) are the estimated state parameters of the variable parameter Kalman filter and the position observation value of the tracked UAV target, respectively; the initial position value x(0) and the noise u(k) and v(k) conform to the Gaussian distribution; F(k) and G(k) are the state transfer matrix and the process noise driving matrix, respectively, which should be set according to the UAV flight environment and the performance of the observation equipment; Before performing Kalman filtering, the dimension of x(k) and the value of F(k) need to be determined based on the maneuverability of the drone and the computing performance of the controller. Assuming that the estimated state is n-dimensional, then: in is the estimated position of the tracked UAV target at time k; Set the initial value of the state estimation of the variable parameter Kalman filter A posteriori estimate covariance P(k), state transfer matrix F(k), noise driver matrix G(k), observation matrix H(k), process noise covariance Q(k), observation noise covariance R(k); Step (2): Update the state estimation covariance to obtain Step (3): Calculate the Kalman gain matrix K(k); Step (4): Status update acquisition Step (5): Update the state estimation covariance to obtain Step (6): Obtain state prediction Step (7): Return to step (2) and loop.

2. The method for designing a variable parameter Kalman filter based on historical states according to claim 1, characterized in that: The estimated state value and predicted values According to the requirements, a vector is composed of a preset number of historical position values ​​of drones:

3. The method for designing a variable parameter Kalman filter based on historical states according to claim 1, characterized in that: The setting state transfer matrix F(k) includes the following five setting methods: (1) Proportional constant parameters: Based on but: Among them, p k is the position of the UAV target at time k, and n is the dimension of the estimated state; (2) Variable proportional constant parameter: based on p k+1 =p k +w1(p k -p k-1 )+w2(p k-1 -p k-2 )+...+w n (p k-n+1 -p k-n ), where w1…w n It can be designed as a sequence whose sum is 1 according to actual needs, and an arithmetic sequence is selected, then: Here, n is the dimension of the state being estimated. (3) Offline linear regression parameters: First, obtain a set of position data of the tracked UAV target; then use the position data at time k, k-1, …, kn to form the input vector x, and use the position data at time k+1 as the output vector y; then use n input vectors x to form X, and use n output vectors y to form Y; finally, perform multivariate linear regression on X and Y to obtain a set of weights, W = (X'X) -1 (X'Y), and get the weight value W = [w1,w2,...,w n ],but: (4) Online fixed storage linear regression parameters: During operation, the filter first collects N drone position data without making predictions. From time N+1, the filter uses the data from the most recent N+1 time points to calculate W in real time according to method (3) to obtain: (5) Online incremental linear regression parameters: Before the filter starts prediction, the intermediate variables A(0) = X'X and B(0) = X'Y at time 0 are designed according to the method in (3); after the prediction starts, at any time k, the variable X is constructed. * =[p k-1 ,p k-2 ,...,p n ]' and Y * =p k , and recursively calculate the intermediate variable A(k)=A(k-1)+X * 'X * , B(k)=B(k-1)+X * 'Y * ; Further calculate W = A(k) -1 B(k), the weight value W = [w1,w2,...,w n ],but:

4. The method for designing a variable parameter Kalman filter based on historical states according to any one of claims 1 to 3, characterized in that: By utilizing the new estimated state quantity design method and state transfer matrix design method, the application of Kalman predictive filtering in the field of UAV target tracking can be realized with less computational complexity, thereby improving the accuracy of target position prediction and the smoothness of the prediction curve, and enhancing the adaptability of the filter.

Images