Kalman filtering parameter self-tuning method

Abstract

The invention belongs to the technical field of Kalman filtering target tracking, and particularly relates to a Kalman filtering parameter self-tuning method, which comprises the following steps of: 1, acquiring original sensor data for Kalman filtering; 2, defining a target function, and then performing calculation according to a Kalman filtering standard process by using original sensor data; 3, Gaussian process regression is carried out, and updating of a regression model is completed; 4, finding a next sampling point by using the expectation improvement function; 5, substituting the new sampling point into the target function for calculation, and adding the sampling point and a loss function thereof into a sample point set for a next optimization iteration process; and 6, evaluating whether the error at the moment is smaller than a threshold value or whether the number of iterations at the moment reaches the maximum, and outputting the minimum point of the corresponding objective function value as an optimal result of optimization in the sample set. According to the method, the Kalman filtering parameter setting method is faster, and the algorithm use efficiency is improved.

Description

Technical Field

[0001] The invention belongs to the technical field of Kalman filter target tracking, and in particular relates to a Kalman filter parameter self-tuning method. Background Art

[0002] Kalman filter is a recursive estimation algorithm for linear dynamic systems, which is widely used in various practical scenarios. It provides the best estimate of the system state by predicting and measuring the system state. Most of the existing Kalman filter parameter tuning methods are based on empirical methods. After setting the parameters based on engineering experience, they are adjusted according to the filtering results, which requires repeated experiments and adjustments.

[0003] For the current Kalman filter algorithm and its variants such as the extended Kalman filter and the unscented Kalman filter, most of the parameters of the process noise matrix and the measurement noise matrix are still in the stage of manual parameter setting. It is more reasonable to measure the variance of the sensor measurement value sequence as the value of the measurement noise matrix, but due to the inconsistency in the dimensions with the process noise and state covariance, and the fact that the process noise introduced by the inaccurate prediction model cannot be measured, the measurement noise value obtained in this way cannot be used directly.

[0004] In the prior art, for example, patent number CN116522055A discloses a method for designing a Kalman filter based on an expanded state and with self-tuning parameters. The method's self-tuning parameters are aimed at an inaccurate prediction model and automatically tune the state transfer matrix parameters. However, it does not achieve self-tuning for the two noise matrix parameters that greatly affect the filtering effect.

[0005] In order to solve the problems not considered by the existing methods and to make the Kalman filter parameter setting method faster and improve the algorithm usage efficiency, the present invention proposes a self-tuning method for the process noise and measurement noise parameters of the Kalman filter. Summary of the invention

[0006] The purpose of the present invention is to provide a Kalman filter parameter self-tuning method, which realizes the self-tuning of Kalman filter parameters based on Bayesian optimization of Gaussian process.

[0007] The technical solution adopted by the present invention is as follows:

[0008] A Kalman filter parameter self-tuning method comprises the following steps:

[0009] Step 1: Prepare data;

[0010] Collect raw sensor data for Kalman filtering and use it for offline parameter self-tuning;

[0011] Step 2: Initialize sample points;

[0012] First, define the objective function, then use the raw sensor data to calculate it according to the standard Kalman filter process;

[0013] In step 2, the collected raw sensor data is X={x1, x2, x3, x4..., x n}, the Kalman filter result after calculation by the standard Kalman filter process is X est ={x est 1. x est 2. x est 3.x est 4. ...x est n}, the process noise is Q, the measurement noise is R, and the objective function is the mean square error calculated using the Kalman filter result and the sensor data according to formula (1):

[0014]

[0015] Secondly, generate several Q and R values ​​within a numerical range. j ∈(q min ,q max );R j ∈(r min , r max ), and substitute the Q and R values ​​into the objective function for calculation to complete the initialization of the sample points.

[0016] Step 3: Perform Gaussian process regression to complete the regression model update;

[0017] The initialization sample points obtained in step 2 are subjected to Gaussian process regression to fit the regression model of the objective function; and after this sample point fitting, the corresponding sample point with the smallest objective function value is marked as mse min ;

[0018] Step 4: Use the expected improvement function to find the next sampling point;

[0019] In step 4, the Gaussian process model established by the sample points in step 3 is used to predict the mean μ(Qj, R) of each candidate point. j ), variance σ(Q j , R j );

[0020] Then the improvement amount is calculated by formula (2);

[0021]

[0022] The expected improvement is calculated according to the following formula (3), where Φ(Z(Q j , Rj )) is the normal cumulative distribution function corresponding to the improvement amount at this point, p(Z(Q j , R j )) is the normal probability density function value corresponding to the improvement amount at this point;

[0023] EI(Q j , R j )=(mse min μ(Q j , R j ))*Φ(Z(Q j , R j ))+σ(Q j , R j )*p(Z(Q j , R j )) (3)

[0024] According to the expected improvement of the candidate points calculated previously, the candidate point corresponding to the maximum expected improvement is selected as the next sampling point;

[0025] Step 5: Substitute the new sampling point into the objective function for calculation, and add the sampling point and its loss function to the sample point set for the next optimization iteration process;

[0026] Step 6: After adding the new sampling point to the sample set, evaluate whether the error is less than the threshold or whether the number of iterations reaches the maximum. If one of the conditions is met, the optimization process ends and the point with the minimum value of the objective function in the sample set is output as the optimal result of the optimization. The final result is the optimal result of the independent variable corresponding to the minimum value of the objective function, that is, the process noise matrix and the measurement noise matrix of the Kalman filter.

[0027] The technical effects achieved by the present invention are:

[0028] The self-tuning method of the present invention is based on Bayesian optimization, and can achieve a low-computational optimization for problems such as Kalman filtering where it is impossible to write an analytical expression for the objective function; the existing inventions do not have a self-tuning method for tuning the process noise and measurement noise parameters of the Kalman filter; the present invention uses Bayesian optimization to achieve a parameter optimization that does not require too much computational cost for the problem that it is difficult to obtain an analytical expression for the loss function of a dynamic iterative process such as the Kalman filter. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is an overall flow chart of a Kalman filter parameter self-tuning method of the present invention;

[0030] Figure 2It is an overall flow chart of finding the optimal solution in a Kalman filter parameter self-tuning method of the present invention;

[0031] Figure 3 This is a standard calculation flow chart of the Kalman filter in the present invention. DETAILED DESCRIPTION

[0032] In order to make the purpose and advantages of the present invention more clearly understood, the present invention is specifically described below in conjunction with embodiments. It should be understood that the following text is only used to describe one or several specific embodiments of the present invention, and does not strictly limit the scope of protection of the specific claims of the present invention.

[0033] like Figure 1 As shown, a Kalman filter parameter self-tuning method comprises the following steps:

[0034] Step 1: Prepare data;

[0035] Collect raw sensor data for Kalman filtering and use it for offline parameter self-tuning;

[0036] Step 2: Initialize sample points;

[0037] First, define the objective function, then use the raw sensor data to calculate it according to the standard Kalman filter process;

[0038] In step 2, the collected raw sensor data is X={x1, x2, x3, x4..., x n}, the Kalman filter result after calculation by the standard Kalman filter process is X est ={x est 1. x est 2. x est 3.x est 4. ...x est n}, the process noise is Q, the measurement noise is R, and the objective function is the mean square error calculated using the Kalman filter result and the sensor data according to formula (1):

[0039]

[0040] Secondly, generate several Q and R values ​​within a numerical range. j ∈(q min ,q max );R j ∈(r min , r max ), and substitute the Q and R values ​​into the objective function for calculation to complete the initialization of the sample points;

[0041] In this embodiment, in step 2, the specific process of obtaining the Kalman filter result after the original sensor data is calculated by the Kalman filter standard process is as follows: Figure 3 As shown, the standard Kalman filter process here is common knowledge and will not be described in detail here;

[0042] Step 3: Perform Gaussian process regression to complete the regression model update;

[0043] The initialization sample points obtained in step 2 are subjected to Gaussian process regression to fit the regression model of the objective function; and after this sample point fitting, the corresponding sample point with the smallest objective function value is marked as mse min ;

[0044] In this embodiment, in step three, after the initialization sample points are obtained, when the Gaussian process regression is performed to fit the regression model of the target function, the key to Gaussian process regression is to define a kernel function, also called a covariance function, which is used to measure the similarity between different input variables; commonly used kernel functions include linear kernels, polynomial kernels, and Gaussian kernels, also known as radial basis function kernels; through the kernel function, the covariance between any two input variables can be calculated. When using Gaussian process regression for prediction, the mean and variance of the output variables of the unknown input variables can be obtained by calculating the covariance matrix of the input variables and the covariance matrix of the output variables of the known data points. The mean represents the predicted output of the corresponding input variable, and the variance represents the uncertainty of the predicted output; Gaussian process regression has good flexibility and generalization ability, and can adapt to various data distributions and patterns.

[0045] like Figure 2 As shown, step 4: use the expected improvement function to find the next sampling point;

[0046] In step 4, the Gaussian process model established by using the sample points in step 3 is used to predict the mean μ(Q j , R j ), variance σ(Q j , R j );

[0047] Then the improvement amount is calculated by formula (2);

[0048]

[0049] The expected improvement is calculated according to the following formula (3), where Φ(Z(Q j , R j )) is the normal cumulative distribution function corresponding to the improvement amount at this point, p(Z(Q j , R j )) is the normal probability density function value corresponding to the improvement amount at this point;

[0050] EI(Q j , R j )=(mse min μ(Q j , R j ))*Φ(Z(Q j , R j ))+σ(Q j , R j )*p(Z(Q j , R j )) (3)

[0051] According to the expected improvement of the candidate points calculated previously, the candidate point corresponding to the maximum expected improvement is selected as the next sampling point;

[0052] like Figure 2 As shown, step 5: Substitute the new sampling point obtained in step 4 into the objective function of formula (1) for calculation, and add the sampling point and its loss function value to the sample point set in step 2 for the next optimization iteration process;

[0053] like Figure 2 As shown, step six: perform error evaluation on the sample set obtained in step five, and determine whether the minimum value of the objective function value of all sample points in the sample set is less than the threshold, or whether the number of iterations has reached the maximum. If one of the conditions is met, the optimization process ends, and the minimum point of the corresponding objective function value in the sample set is output as the optimal result of the optimization. If any of the conditions is not met, return to step four and continue to iterate until the iteration condition is met; the final result is to solve the optimal result of the independent variable corresponding to the minimum value of the objective function, that is, the process noise matrix and the measurement noise matrix of the Kalman filter.

[0054] The self-tuning method of the present invention is based on Bayesian optimization, and can achieve a low-computational optimization for problems such as Kalman filtering where it is impossible to write an analytical expression for the objective function; the existing inventions do not have a self-tuning method for tuning the process noise and measurement noise parameters of the Kalman filter; the present invention uses Bayesian optimization to achieve a parameter optimization that does not require too much computational cost for the problem that it is difficult to obtain an analytical expression for the loss function of a dynamic iterative process such as the Kalman filter.

[0055] The above is only a preferred embodiment of the present invention. It should be noted that, for those skilled in the art, several improvements and modifications can be made without departing from the principles of the present invention, and these improvements and modifications should also be considered as the protection scope of the present invention. The structures, devices and operating methods not specifically described and explained in the present invention shall be implemented according to the conventional means in the art unless otherwise specified and limited.

Claims

1. A Kalman filter parameter self-tuning method, characterized in that: The following steps are involved: Step 1: Prepare data; Collect raw sensor data for Kalman filtering and use it for offline parameter self-tuning; Step 2: Initialize sample points; First, define the objective function, then use the raw sensor data to calculate it according to the standard Kalman filter process; Step 3: Perform Gaussian process regression to complete the regression model update; The initialization sample points obtained in step 2 are subjected to Gaussian process regression to fit the regression model of the objective function; and after this sample point fitting, the corresponding sample point with the smallest objective function value is marked as mse min ; Step 4: Use the expected improvement function to find the next sampling point; Step 5: Substitute the new sampling point into the objective function for calculation, and add the sampling point and its loss function to the sample point set for the next optimization iteration process; Step 6: After the new sampling point is added to the sample set, evaluate whether the error is less than the threshold or whether the number of iterations has reached the maximum. If one of the conditions is met, the optimization process ends and the point with the minimum objective function value in the sample set is output as the optimal optimization result.

2. A Kalman filter parameter self-tuning method according to claim 1, characterized in that: In step 2, the collected raw sensor data is X={x1, x2, x3, x4..., x n }, the Kalman filter result after calculation by the standard Kalman filter process is X est ={x est 1.x est z 、x est 3.x est 4. ...x est n }, the process noise is Q, the measurement noise is R, and the objective function is the mean square error calculated using the Kalman filter result and the sensor data according to formula (1): Secondly, generate several Q and R values ​​within a numerical range. j ∈(q min ,q max );R j ∈(r min , r max ), and substitute the Q and R values ​​into the objective function for calculation to complete the initialization of the sample points.

3. A Kalman filter parameter self-tuning method according to claim 2, characterized in that: In step 4, the Gaussian process model established by using the sample points in step 3 is used to predict the mean μ(Q j , R j ), variance σ(Q j , R j ); Then the improvement amount is calculated by formula (2); The expected improvement is calculated according to the following formula (3), where Φ(Z(Q j , R j )) is the normal cumulative distribution function corresponding to the improvement amount at this point, p(Z(Q j , R j )) is the normal probability density function value corresponding to the improvement amount at this point; EI(Q j ,R j )=(mse min μ(Q j ,R j ))*Φ(Z(Q j ,R j ))+σ(Q j ,R j )*p(Z(Q j ,R j )) (3) According to the expected improvement amount of the candidate points calculated previously, the candidate point corresponding to the maximum expected improvement amount is selected as the next sampling point.

Images