A UKF filtering method based on two-stage Sigma points

Abstract

The application discloses a UKF filtering algorithm based on secondary Sigma points and belongs to the field of filtering algorithms; specifically, firstly, the state estimation mean value of a trajectory reentry target plane model is selected as a center point, UT transformation is carried out according to the state estimation variance, and 2n+1 primary Sigma points are obtained; then, UT transformation is respectively carried out with each primary Sigma point as a center point, and 2n+1 secondary Sigma point sets corresponding to the dispersion range of the adaptive adjustment Sigma point set are calculated according to the state covariance matrix P zz,k+1 Finally, through a Sigma point set estimation effect evaluation algorithm, the degree that the secondary Sigma point set approximates to the real observation is represented by the scalarized innovation, and the secondary Sigma point set or the primary Sigma point set with the minimum scalarized innovation is selected as the Sigma point set with the optimal estimation precision, which is used for calculating the Kalman gain and updating the state estimation. The application has better convergence speed and stability, and based on the symmetry distribution characteristics of the Sigma point set, the calculation amount is reduced to a certain extent without affecting the estimation effect of the filtering algorithm.

Description

Technical Field

[0001] This invention belongs to the field of filtering algorithms and relates to a UKF (Unscented Kalman Filter) filtering method based on second-order Sigma points. Background Technology

[0002] Fast and accurate state estimation is crucial in navigation and positioning. The UKF filtering algorithm approximates a nonlinear probability distribution using Sigma point sampling. Its core algorithm utilizes the UT transform to handle the nonlinear propagation of the mean and covariance. Many navigation and positioning models exhibit strong nonlinear relationships. When the initial filtering value of a strongly nonlinear system has a large deviation, the convergence speed of the UKF algorithm will slow down, and it may even diverge.

[0003] When the system is highly nonlinear, the filtering algorithm is more sensitive to the estimation error of the current state. Methods to overcome the adverse interference of the estimation error of the current state on the algorithm include: expanding the range of the Sigma point set in the UT transformation and updating the estimated mean and error covariance of the current state of the system.

[0004] While expanding the range of the Sigma point set can more accurately represent the true state, the range of the Sigma point set cannot be too large due to the limitation of nonlinear equations. The UKF algorithm with second-level Sigma points calculates second-level Sigma points based on the first-level Sigma point set, thereby calculating the mean and error covariance of multiple sets of state estimates. After screening, it can improve the estimation speed and stability of the algorithm without increasing the Sigma point set.

[0005] The ballistic reentry target plane model is a typical nonlinear model. Taking the ballistic reentry target plane model as an example, due to the influence of the initial state estimation bias, the standard UKF algorithm has a slow convergence speed and is prone to divergence. Summary of the Invention

[0006] To address the problem that the UKF algorithm has slow convergence speed and is prone to divergence when the system model is highly nonlinear and the initial value of the filter has a large deviation, this invention provides a UKF filtering method based on second-order Sigma points. Taking the ballistic reentry target plane model as an example, it achieves the optimal estimation of the highly nonlinear system and has the advantages of fast convergence speed and good stability.

[0007] The UKF filtering method based on second-order Sigma points is implemented through the following steps:

[0008] First, the mean of the state estimate of the ballistic reentry target plane model is selected as the center point. Based on the variance of the state estimate, a UT transformation is performed to obtain (2n+1) first-order Sigma points; n is the dimension of the state variables.

[0009] Then, using each first-order Sigma point as the center point, perform UT transformation respectively, and calculate based on the variance. The distribution range of the Sigma point set is adaptively adjusted to calculate multiple sets of second-order Sigma point sets;

[0010] Finally, the scalarized innovation is used to characterize how well the second-order Sigma point set approximates the actual observations, and the second-order Sigma point set or the first-order Sigma point set with the smallest scalarized innovation is selected as the Sigma point set with the best estimation accuracy, which is used to calculate the Kalman gain and update the state estimate.

[0011] Furthermore, this invention also designs a simplified calculation method for the UKF filtering method of second-order Sigma points, which utilizes the symmetrical distribution of Sigma points to reduce the number of second-order Sigma point sets to be calculated, thereby further reducing the computational load of the algorithm.

[0012] The advantages and positive effects of this invention are as follows:

[0013] (1) The present invention provides a UKF filtering method based on second-order Sigma points, which is simple and easy to implement. It is used for strongly nonlinear systems. When the initial filtering value is inaccurate, the UKF filtering algorithm based on second-order Sigma points has better convergence speed and stability than the standard UKF algorithm.

[0014] (2) The present invention provides a simplified calculation method for the UKF filtering method based on second-order Sigma points. Considering the impact of computational complexity, the symmetrical distribution of the Sigma point set can reduce the computational complexity without affecting the estimation effect of the filtering algorithm. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of the UKF filtering method for constructing second-order Sigma points according to the present invention;

[0016] Figure 2 This is a flowchart of a UKF filtering method based on second-order Sigma points according to the present invention;

[0017] Figure 3 This is a comparison chart of the filtering results of the UKF filtering method based on second-order Sigma points in this invention and the standard UKF filtering algorithm. Detailed Implementation

[0018] The technical solution of the present invention will be described below with reference to the accompanying drawings and embodiments.

[0019] This invention provides a UKF filtering method based on second-order Sigma points, including a second-order Sigma point set calculation algorithm, a novelty scalarization algorithm, a Sigma point set estimation performance evaluation algorithm, a simplified Sigma point estimation performance evaluation algorithm, and a posterior estimation algorithm based on second-order Sigma point sets.

[0020] like Figure 1 As shown, taking two-dimensional state variables as an example, the basic principle of the UKF filtering method based on second-order Sigma points is: by calculating the second-order Sigma point set, the center point of the UT transformation can be moved to a position closer to the true value, thereby improving the convergence speed and stability of the filtering algorithm.

[0021] Specifically, for systems with high nonlinearity, a second-level Sigma point set calculation algorithm is used to calculate the second-level Sigma point set based on the first-level Sigma points. An innovation scalarization algorithm is then used to convert the observed innovation into a scalar for easier and more intuitive analysis. An Sigma point set estimation performance evaluation algorithm is used to select the optimal second-level Sigma point set based on how closely it approximates the actual observations. This optimal set is then used for prior estimation of the state in the subsequent Kalman filter algorithm, thereby improving the conversion accuracy of the nonlinear transfer function. Finally, a posterior estimation algorithm based on the second-level Sigma point set is used to update the state estimate of the filtering algorithm.

[0022] Furthermore, by using a simplified Sigma point estimation performance evaluation algorithm, the symmetrical distribution of Sigma points can be utilized to reduce the number of secondary Sigma points to be calculated, thereby reducing the computational load of the UKF filtering algorithm based on secondary Sigma points.

[0023] The entire filtering algorithm is applied to strongly nonlinear systems, and it can converge the state estimates that deviate from those of the standard UKF filtering algorithm to the true values. Especially when the initial values ​​are inaccurate, this UKF filtering algorithm based on second-order Sigma points shows a significant improvement over the standard UKF.

[0024] The present invention is based on a UKF filtering method using second-order Sigma points, and the flowchart is as follows: Figure 2 As shown, the specific content is as follows:

[0025] Step 1: Set the current state of the filter, select the current state estimate of the ballistic reentry target plane model as the center point, and obtain the Sigma point set as the first-level Sigma point set;

[0026] The specific process is as follows:

[0027] State prior estimation based on the ballistic reentry target plane model As the center point, based on variance Calculate the first-order Sigma point set: ;

[0028] (1)

[0029] (2)

[0030] (3)

[0031] (4)

[0032] In equations (1)-(4), , where n is the prior state estimate dimensionality , For constants, Sigma point To the center point distance and Related, Representation matrix The i-th column.

[0033] Step 2: Using the second-level Sigma point calculation algorithm, with the sample points in the first-level Sigma point set as the center points, perform UT transformation to obtain 2n+1 second-level Sigma point sets, where n is the dimension of the state;

[0034] Specifically:

[0035] Using first-level Sigma sample points , Calculate the second-order Sigma point set at the center point of the UT transform. The calculation formulas are shown in equations (5)-(8):

[0036] (5)

[0037] (6)

[0038] (7)

[0039] (8)

[0040] In equations (5)-(8), ; This represents the scaling factor, used to adjust the second-order Sigma point. The dispersion range; if the state variable takes values ​​within the second-order Sigma point set. If the surrounding area is more sensitive to jitter, reduce the scaling factor. Reduce the second-order Sigma point The dispersion range is determined by the scale factor, and vice versa. Add a second-level Sigma point The dispersion range. Therefore, the scaling factor. It can adaptively scale the scattering range of Sigma points according to the nonlinearity of the model; the smaller the scattering range of Sigma points, the higher the accuracy of the UT transformation.

[0041] Using variance The elements on the diagonal represent the sensitivity of the state variable to jitter in the vicinity of the Sigma point set, when the variance When the variance is large, it indicates that the j-dimensional state variable is more sensitive to jitter in the vicinity of the Sigma point set, and when the variance is large... When the value is small, it indicates that the j-dimensional state variable is less sensitive to jitter in the vicinity of the Sigma point set.

[0042] Therefore, the scaling factor is set. The adjustment rate is as shown in equation (9).

[0043] (9)

[0044] In equation (9), and They represent the error covariance respectively. The threshold is set to prevent scaling factor scaling coefficients. Too large or too small It is the upper bound of the scale factor. It is the lower bound of the scale factor.

[0045] Step 3: Calculate the 2n+1 second-order Sigma point sets and the innovation of the observed variables converted to scalars for each second-order Sigma point set using the innovation scalarization algorithm.

[0046] Specifically:

[0047] Based on observation information Calculate the second-order Sigma point set New observation information See equations (10)-(12).

[0048] (10)

[0049] (11)

[0050] (12)

[0051] The observation equation for the filtering model is... For the State estimation after transformation. To calculate the mean The weights satisfy:

[0052] (13)

[0053] (14)

[0054] use Represents the observational information of a state variable with unit i. The absolute value of the observation information is averaged over the absolute values ​​of the observation information of the observed variables with the same units, and multiplied over the absolute values ​​of the observation information of the observed variables with different units, to obtain a scalarized observation information that can unify all dimensions. :

[0055] (15)

[0056] For example, in this instance, the observed information is the angle and magnitude measured by radar. The innovation scalarization algorithm calculates the product of the angle and magnitude to obtain the scalarized observation innovation in units of length. .

[0057] Step 4: Using the Sigma point set estimation performance evaluation algorithm, select the Sigma point set with the best estimation accuracy based on the scalarized observation information, and use it as the mean and variance of the prior estimation of the state in the subsequent Kalman filter algorithm.

[0058] The principle for selecting the optimal Sigma point is shown in equation (16), based on the scalarized observation information. , Select the second-order Sigma point set center point As a priori estimate of the state, a second-order Sigma point set is selected. New observation information As a means of observing new information.

[0059] (16)

[0060] Step 5: Based on the posterior estimation algorithm of the second-order Sigma point set, calculate the innovation and Kalman gain, adjust the prior estimate, and output the filtering result.

[0061] The Kalman gain of the UKF filtering algorithm is calculated according to equations (17)-(21). And update the state estimate to obtain .

[0062] (17)

[0063] (18)

[0064] (19)

[0065] (20)

[0066] (twenty one)

[0067] To calculate covariance , The weight, To measure the noise covariance matrix.

[0068] (twenty two)

[0069] Furthermore, this invention uses a simplified computational algorithm to evaluate the effectiveness of Sigma point estimation. It categorizes Sigma points based on their expansion direction, grouping sets of Sigma points with the same or opposite expansion directions into the same class, resulting in n classes, where n represents the dimension of the system's state. Each class of Sigma points is represented as follows:

[0070] (twenty three)

[0071] In equation (22), i represents the i-th dimension of the state variable.

[0072] (twenty four)

[0073] Based on the collected observation information, calculate The degree to which the three first-order Sigma points are close to the true state of the system is considered, and the first-order Sigma point that is closest to the true value is selected. Center point:

[0074] (25)

[0075] Calculate the corresponding first-order Sigma point set , n second-order Sigma point sets after UT transformation :

[0076] (26)

[0077] (27)

[0078] (28)

[0079] (29)

[0080] (30)

[0081] Select according to formula (24) Choosing one of the three Sigma points to determine the degree of distance from the true state of the system ignores terms in the UT transform whose mean order is greater than 1. Therefore, some accuracy in judging the degree of distance of the Sigma point from the true state of the system is lost. However, to a certain extent, it is still possible to choose a second-level Sigma point, which is better than the first-level Sigma point, and can correct for biased initial values.

[0082] In the example:

[0083] Using a ballistic reentry target model, the standard UKF algorithm and the UKF algorithm based on second-order Sigma points were applied respectively. The filtering results are as follows: Figure 3 As shown. This invention is applied to strongly nonlinear systems and can converge the state estimate deviating from the standard UKF to the true state. Especially when the initial values ​​are inaccurate, this UKF filtering method based on second-order Sigma points shows a significant improvement over the standard UKF.

Claims

1. A method of UKF filtering based on second order Sigma points, characterized in that, The specific steps are: First, the state estimation mean value of the ballistic reentry target plane model is selected as the center point, and UT transformation is performed according to the state estimation variance to obtain 2n+1 first-level Sigma points, n being the dimension of the state variable; Then, UT transformation is performed respectively with each primary Sigma point as a center point, and the corresponding 2n+1 secondary Sigma points are calculated according to the variance The dispersion range of the Sigma point set is adaptively adjusted, and the corresponding 2n+1 secondary Sigma point sets are calculated. The specific process is: First, the state prior estimate As the center point, the variance , compute the first Sigma point set: n is a state prior estimate dimension of the matrix , is a constant, Sigma points distance from the center point to the center point is related to denotes the i-th column of the matrix ​ Then, the set of second-level Sigma points is computed , with the center point of the UT transform The computation formula is shown as follows: denotes a scale factor scaling coefficient for adjusting the spread of the secondary Sigma points denotes a scale factor scaling coefficient for adjusting the spread of the secondary Sigma points Finally, the simplified Sigma point set estimation effect evaluation algorithm is used to evaluate the estimation effect, the scalarized innovation is used to represent the degree to which the second-level Sigma point set approximates the true observation, and the second-level Sigma point set or the first-level Sigma point set with the minimum scalarized innovation is selected as the Sigma point set with the optimal estimation accuracy, which is used to calculate the Kalman gain and update the state estimation; The simplified Sigma point set estimation effect evaluation algorithm classifies the Sigma points according to their symmetric distribution, thereby reducing the number of second-level Sigma point sets, and the specific process is as follows: First, the first-level Sigma points are classified according to their expansion directions, and the first-level Sigma point sets with the same or opposite expansion directions are classified into the same class, and a total of n classes are obtained, wherein n represents the dimension of the state of the system; Each class of first-level Sigma points is represented as: i represents the i-th dimension of the state variable; Then, according to the collected observation information, the distances of the center points of the three classes of first-level Sigma points from the true state are calculated, the first-level Sigma point closest to the true value is selected as the center point, UT transformation is performed to expand it into n second-level Sigma point sets, the innovations of the observation variables converted into scalars are calculated, and the Sigma point set with the optimal estimation accuracy is adaptively selected according to the innovations of the observation variables converted into scalars; The observed information is the radar measured angle and the module length, the innovation scalarization algorithm calculates the product of the angle and the module length, and the scalarized observed innovation with the unit of length is obtained ; select the primary sigma point closest to the true value The center point is given by the formula is observation information.

2. The UKF filtering method based on secondary Sigma points according to claim 1, characterized in that, The scale factor scaling coefficient The modulation rate is adjusted as follows: and denote the error covariance a threshold to prevent the scale factor from being too large or too small, is an upper bound on the scale factor, is a lower bound on the scale factor.

3. The UKF filtering method based on secondary Sigma point according to claim 1, characterized in that, The innovations of the observation variables converted into scalars for the 2n+1 second-level Sigma point sets are as follows: According to the observation information computing a set of secondary Sigma points of the observation innovation as follows: is the observation equation for the filter model; is the transformed state estimate; is the transformed state estimate; is the weight for calculating the mean of the weights.

4. The UKF filtering method based on secondary Sigma point according to claim 1, characterized in that, For the Sigma point set with the optimal estimation accuracy, the Kalman gain is calculated, and the state estimation is updated, and the specific process is as follows: First, the Kalman gain of the UKF filtering algorithm is calculated and the state estimation is updated to obtain : To compute the covariance and weights, is the measurement noise covariance matrix.

Images