A Kalman filter parameter self-tuning method

Abstract

The present invention belongs to the technical field of Kalman filter target tracking, and specifically relates to a Kalman filter parameter self-tuning method, comprising the following steps: Step 1: collecting raw sensor data for Kalman filtering; Step 2: defining an objective function, and then using the raw sensor data to perform calculations according to a standard Kalman filter process; Step 3: performing Gaussian process regression to complete regression model update; Step 4: using an expected improvement function to find the next sampling point; Step 5: substituting the new sampling point into the objective function for calculation, and adding the sampling point and its loss function to a sample point set for the next optimization iteration process; Step 6: evaluating whether the error at this time is less than a threshold, or whether the number of iterations at this time has reached a maximum, and outputting the point in the sample set corresponding to the minimum objective function value as the optimal optimization result. The present invention makes the Kalman filter parameter tuning method faster and improves the efficiency of algorithm use.

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] The Kalman filter is a recursive estimation algorithm for linear dynamic systems, widely used in various practical scenarios. It provides an optimal estimate of the system state by predicting and updating the measured state. Existing Kalman filter parameter tuning methods are mostly empirical. After setting parameters based on engineering experience, adjustments are made based on the filtering results, requiring repeated experimentation and adjustments.

[0003] The parameters of the process noise matrix and measurement noise matrix for current Kalman filters and their variants, such as the extended Kalman filter and the unscented Kalman filter, are still largely manually tuned. A more reasonable approach is to measure the variance of the sensor measurement sequence as the value of the measurement noise matrix. However, due to the dimensionality discrepancy between this and process noise and state covariance, and the fact that process noise, introduced by inaccurate prediction models, cannot be measured, the measurement noise value obtained in this way cannot be directly used.

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

[0005] In order to solve the problems not considered by the existing methods and to make the Kalman filter parameter tuning method faster and improve the efficiency of the algorithm, 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 present invention aims 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 solutions adopted by the present invention are 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, and then use the raw sensor data to calculate it according to the standard Kalman filter process;

[0013] In step 2, the collected original sensor data is X={x1, x2, x3, x4..., x n}, the Kalman filter result after calculation through the standard Kalman filter process is X est ={x est 1.x est 2. x est 3.x est 4. ...x est n}, process noise is Q, measurement noise is R, and the objective function is the mean square error calculated using the Kalman filter result and 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 update the regression model;

[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 minimum 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 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 has reached the maximum. If either condition is met, the optimization process ends and the point in the sample set with the minimum objective function value is output as the optimal optimization result. The final result is the optimal solution for the independent variable corresponding to the minimum objective function value, 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 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 solve 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, and achieves a parameter optimization that does not require too much computational cost. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0030] Figure 2This is an overall flow chart for 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 described in detail below with reference to the following examples. It should be understood that the following text is only used to describe one or more specific embodiments of the present invention and does not strictly limit the scope of protection of the present invention.

[0033] like Figure 1 As shown, a Kalman filter parameter self-tuning method includes 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, and then use the raw sensor data to calculate it according to the standard Kalman filter process;

[0038] In step 2, the collected original sensor data is X={x1, x2, x3, x4..., x n}, the Kalman filter result after calculation through the standard Kalman filter process is X est ={x est 1.x est 2. x est 3.x est 4. ...x est n}, process noise is Q, measurement noise is R, and the objective function is the mean square error calculated using the Kalman filter result and 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 original sensor data is calculated through the Kalman filter standard process to obtain the Kalman filter result. Figure 3 As shown, the standard Kalman filter process here is common knowledge and will not be described here in detail;

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

[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 minimum objective function value is marked as mse min ;

[0044] In this embodiment, in step three, after obtaining the initialized sample points, when performing Gaussian process regression to fit the regression model of the target function, the key to Gaussian process regression is to define a kernel function, also known as a covariance function, which is used to measure the similarity between different input variables; commonly used kernel functions include linear kernel, polynomial kernel, and Gaussian kernel, also known as radial basis function kernel; through the kernel function, the covariance between any two input variables can be calculated. When using Gaussian process regression for prediction, by calculating the covariance matrix of the input variables and the covariance matrix of the output variables of the known data points, the mean and variance of the output variables of the unknown input variables can be obtained. 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 different 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 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 five: substitute the new sampling point obtained in step four 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 two 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 to 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 either of these conditions is met, the optimization process ends and the point corresponding to the minimum objective function value in the sample set is output as the optimal optimization result. If any of these conditions are not met, return to step four and continue iterating until the iteration condition is met; the final result is the optimal solution of the independent variable corresponding to the minimum objective function value, that is, the process noise matrix and measurement noise matrix of the Kalman filter.

[0054] The self-tuning method of the present invention is based on Bayesian optimization and can achieve 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 solve 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, and achieves a parameter optimization that does not require too much computational cost.

[0055] The foregoing is merely a preferred embodiment of the present invention. It should be noted that those skilled in the art may make various improvements and modifications without departing from the principles of the present invention, and such improvements and modifications are also within the scope of protection of the present invention. Structures, devices, and operating methods not specifically described or explained herein shall, unless otherwise specified or limited, be implemented in accordance with conventional means in the art.

Claims

1. A Kalman filter parameter self-tuning method, characterized by: 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, and then use the raw sensor data to calculate it according to the standard Kalman filter process; Step 3: Perform Gaussian process regression to update the regression model; 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 minimum 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 adding the new sampling point to the sample set, evaluate whether the error is less than the threshold or whether the number of iterations has reached the maximum. If either of the conditions is met, the optimization process ends and the sample set with the minimum value of the objective function is output as the optimal result of the optimization.

2. A Kalman filter parameter self-tuning method according to claim 1, characterized in that: In step 2, the collected original sensor data is X={x1, x2, x3, x4..., x n }, the Kalman filter result after calculation through the standard Kalman filter process is X est ={x est 1.x est z 、x est 3.x est 4. ...x est n }, process noise is Q, measurement noise is R, and the objective function is the mean square error calculated using the Kalman filter result and 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 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; According to the expected improvement amount of the candidate point calculated previously, the candidate point corresponding to the maximum expected improvement amount is selected as the next sampling point.

Images