Design method of interactive robust state estimator based on multiple adaptive factors

Abstract

The application discloses a kind of based on multiple adaptive factors interactive robust state estimator design method, for improving the estimation accuracy of estimator and the curve smoothness of filtering under the condition that observation noise occurs unpredictable surge, to meet the filtering estimation demand of higher accuracy.Standard robust state estimation method requires known observation noise covariance value, therefore cannot be normally used under the condition that observation noise surges;Robust filtering method combined with single adaptive factor can compensate the estimation error caused by noise surge to a certain extent, but its performance cannot achieve universal optimization under different noise surge degrees.The application can effectively improve the estimation effect of state estimator under complex observation noise surge condition, break through the limitation of traditional robust state estimation method, realize fast estimation of actual observation noise under complex observation noise surge condition, effectively improve the estimation accuracy and estimation curve smoothness of state estimator, optimize the estimation effect of estimator.

Description

Technical Field

[0001] This invention belongs to the field of filtering estimation, specifically involving an interactive robust state estimator design method based on multiple adaptive factors. It is mainly used to effectively improve the estimation accuracy and smoothness of the estimation curve of the state estimator and optimize the estimation effect of the state estimator when the observation noise causes an unpredictable surge. Background Technology

[0002] This invention can be applied to photoelectric tracking, motion capture, and other applications. In these fields, the state equations of the object being estimated are unknown or cannot be accurately modeled, and uncertainties are widespread in the state transition equations and observation equations of the object being estimated (see "Real-time Tracking and Modeling System for Human 3D Motion" by Xu Yihua, Acta Automatica Sinica, 2006). The most classic Kalman state estimation method has the ability to optimally estimate Gaussian white noise interference, but it requires accurate modeling; when the state equation parameters are inaccurate, its filtering effect will significantly deteriorate (see "Applied optimal estimation" by Arthur G. Proceedings of the IEEE, 1974). Traditional robust state estimation methods can compensate for uncertainties in the motion model of the object being estimated, effectively dealing with motion model uncertainties within the allowable range (see "A Framework for State-Space Estimation with Uncertain Models" by Ali H. Sayed, IEEE TRANSACTIONSON AUTOMATIC CONTROL, 2001). However, in application fields such as photoelectric tracking systems, observation noise may surge due to factors such as atmospheric turbulence and tracking platform vibration, which traditional robust state estimation methods cannot effectively handle. To address this issue, in 2019, a state estimator based on Bayesian methods for identifying observation noise was proposed and showed good performance. However, while it can be applied to nonlinear systems using algorithms such as the unscented Kalman spectroscopy, it cannot be applied to robust state observers, thus failing to solve the aforementioned difficulties ("Dual-Scale Adaptive Kalman Filtering Based on Variational Bayesian Estimation Method" (Wu Junfeng, Xu Song. Journal of Air Force Engineering University, 2019)). Another method was proposed, utilizing the Mahalanobis adaptive factor to identify outliers exceeding the chi-square test confidence interval in real time and then adjusting the observation noise variance in real time. This method was successfully applied in a robust state estimation paper ("A Robust State Estimator With Adaptive Factor" (Sun Minxing, Mao Yao, Liu Huabo. IEEE Access 2020)). Although this state estimation method can address the uncertainty and noise surge problems of step-size motion models to some extent, its estimation performance cannot achieve optimal results across different noise surge magnitudes, leaving considerable room for improvement. As the performance requirements for state estimation methods continue to increase across various fields, and assuming the observed noise is Gaussian white noise (i.e., the noise amplitude distribution follows a Gaussian distribution and the power spectral density is uniformly distributed), it is particularly important to utilize the statistical properties of noise to perform real-time estimation of the observed noise covariance matrix in robust filters. There is an urgent need to improve robust state estimation so that it can maintain good estimation performance even under varying noise conditions. Summary of the Invention

[0003] To enable the state estimator to simultaneously compensate for the uncertainty of the tracked target motion model and various observation noise surges in photoelectric tracking applications, thereby improving estimation accuracy and smoothness, this invention proposes an interactive robust state estimator design method based on multiple adaptive factors.

[0004] To achieve the objectives of this invention, the present invention first clarifies the state equations corresponding to each sub-state estimator and the meaning of the parameters therein:

[0005] x j (k+1)=(F j (k)+δF j (k))x j (k)+(G j (k)+δG j (k))u j (k),k≥0

[0006] y j (k)=H j (k)x j (k)+v j (k)

[0007] In this state equation, x j (k), y j (k) represents the target state and target position observations used by the sub-robust state estimator j of the interactive robust state estimator; initial state value x j (0), Process noise u j (k), observation noise v j (k) conforms to a Gaussian distribution; F j (k), G j (k), H j (k) represents the state transition matrix, process noise driving matrix, and state observation matrix, respectively, where the parameter uncertainties are represented by δF. j (k), δG j (k) represents;

[0008] δ ij It is the Kronecker symbolic equation, that is, when i = j, δ ij =1; when i≠j, δ ij =0. u(k) and v(k) satisfy:

[0009] [δF j (k)δG j [(k)]=M j △ j [E f,j (k)E g,j (k)]

[0010]

[0011] Among them, M j It is a matrix that changes over time, △ j It is a scaling parameter that takes values ​​from the range [0,1].

[0012] E f,j (k)E g,j (k) is the expectation of the model uncertainty, Π0 is the state estimation covariance matrix, and Q j (k) is the process noise covariance matrix, R j (k) is the observation noise covariance matrix. Before starting the interactive robust state estimator based on multiple adaptive factors, it is also necessary to set the initial values ​​of the interactive robust state estimator state estimates. The probability transition matrix P of the sub-state estimator, and the model probability μ of the sub-estimator. j Each sub-estimator estimates the initial value. and posterior estimate of covariance

[0013] It needs to be clarified that the elements p in the probability transition matrix P of the sub-robust state estimator ij , representing the probability that a noise surge will transfer from model i to model j.

[0014]

[0015] Sub-robust state estimator model probability μ j This represents the applicability probability of substate estimator j.

[0016] The first step of the interactive robust state estimator with multiple adaptive factors is called input interaction, and the parameters that need to be calculated in this step are as follows:

[0017] (1) The prediction probability of the sub-robust state estimator j is:

[0018] (2) The mixed probability from sub-robust state estimator i to sub-estimator j is:

[0019] (3) The mixed state estimate of the sub-robust state estimator j is:

[0020]

[0021] (4) The mixture covariance estimate of the sub-robust state estimator j is:

[0022]

[0023] The second step of the interactive robust state estimator with multiple adaptive factors is that the sub-robust state estimators perform estimations separately to obtain the estimated value of estimator j. and estimate covariance

[0024] The robust state estimation algorithm optimizes the following equation, where ||u j (k)|| 2 and These respectively represent the Euclidean norm and its weighted form:

[0025]

[0026] The solution to this optimization formula is:

[0027]

[0028] This requires the following mathematical substitution:

[0029]

[0030]

[0031] A←H i+1 [F i G i ]

[0032] δA←H i+1 M i △ i [E f,i E g,i ]

[0033]

[0034]

[0035]

[0036] H←H i+1 M i

[0037] E a ←[E f,i E g,i ]

[0038]

[0039] △←△ i .

[0040] Considering the uncertainties in the motion model, the above equation needs to be adjusted:

[0041]

[0042]

[0043]

[0044] Among the optimization parameters Solve using the following formula:

[0045]

[0046]

[0047] W(λ)=W+WH(λI-H T WH) + H T W

[0048]

[0049]

[0050] Based on this, the original iterative formula for the robust filter is derived as follows:

[0051]

[0052]

[0053]

[0054]

[0055]

[0056]

[0057]

[0058]

[0059]

[0060]

[0061] To address the surge in observation noise caused by platform vibration, uneven atmospheric thickness, and turbulence, each robust state estimator calls different adaptive factors before performing the above calculations. A second estimation is performed. Based on the application field and filter principles, the estimated value is... The difference between the observed value and the true value follows a Gaussian distribution. k+1The difference between the true values ​​also conforms to a Gaussian distribution, therefore It conforms to a Gaussian distribution.

[0062] The application method of the MR adaptive factor is as follows:

[0063]

[0064]

[0065] Distance γ k+1 It conforms to a chi-square distribution. Where e k+1 The covariance is expressed as

[0066] According to γ k+1 Following a chi-square distribution, the MR adaptive factor is based on two preset confidence thresholds. observation noise covariance Make timely adjustments. That is, preset the threshold corresponding to the information interval 1-α1. Preset threshold corresponding to the information interval 1-α2 when At that time, that is Then it is considered that the observed value at this time is incorrect, and a judgment of non-reliability is made. when At that time, that is This indicates a surge in observation noise, which can be determined using the following formula. Make adjustments so that e k+1 The covariance then meets the confidence requirement again. In the above text, P(.) represents the probability of the event occurring.

[0067]

[0068] like That is, the difference e k+1 If the confidence requirement is met, then The value remains unchanged, and it is substituted into the iterative equation of the robust filter to solve.

[0069] The γ used in Huber's adaptive factor k+1 as follows:

[0070]

[0071]

[0072] According to γ k+1 Following a chi-square distribution, the Huber adaptive factor is based on a preset confidence threshold. observation noise covariance Make timely adjustments. That is, preset the confidence interval to 1-α, corresponding to the threshold. when At that time, the following formula is used to... Adjustments were made to make e k+1 The covariance meets the confidence requirement again; otherwise The value remains unchanged.

[0073]

[0074]

[0075] like That is, the difference e k+1 If the confidence requirement is met, then The value remains unchanged, and it is substituted into the iterative equation of the robust filter to solve.

[0076] The γ used in the RMA adaptive factor k+1 as follows:

[0077]

[0078]

[0079] According to γ k+1 Following a chi-square distribution, the RMA adaptive factor is based on a preset confidence threshold. observation noise covariance Make timely adjustments. That is, preset the confidence interval to 1-α, corresponding to the threshold. when At that time, the following formula is used to... Adjustments were made to make e k+1 The covariance meets the confidence requirement again; otherwise The value remains unchanged.

[0080]

[0081]

[0082] The γ used in Mahalanobis adaptive factor k+1 as follows:

[0083]

[0084]

[0085] According to γ k+1 Following a chi-square distribution, the Mahalanobis adaptive factor has a preset confidence threshold. observation noise covariance Make timely adjustments. That is, preset the confidence interval to 1-α, corresponding to the threshold. when At that time, the following formula is used to... Adjustments were made to make e k+1 The covariance now meets the confidence requirement again.

[0086]

[0087]

[0088] like That is, the difference e k+1 If the confidence requirement is met, then The value remains unchanged, and it is substituted into the iterative equation of the robust filter to solve.

[0089] The third step of the multi-adaptive-factor interactive robust state estimator is to evaluate the confidence μ of each estimator based on the estimation results of each sub-robust state estimator. j (Also known as model probability), the parameters that need to be calculated in this step are as follows:

[0090]

[0091]

[0092]

[0093]

[0094]

[0095] The fourth step of the interactive robust state estimator with multiple adaptive factors is based on the confidence level μ. j The estimation results of each sub-robust state estimator are weighted to obtain the estimation result and covariance of the entire estimator. The relevant formula is as follows:

[0096]

[0097]

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

[0099] (1) Compared with robust state estimation methods that use a single adaptive factor, this invention can effectively integrate the advantages of multiple adaptive factors and enhance the adaptability of the estimator under the condition of observation noise surge at different scales.

[0100] (2) Compared with traditional robust state estimation methods, the sub-robust state estimator in this invention is regularized and the estimation results have good stability under the premise of stability and measurability.

[0101] (3) Compared with traditional robust state estimation methods, the sub-robust state estimator in this invention can guarantee that the estimation error is bounded when applied to a quadratic stable state space model.

[0102] (4) Compared with the traditional Kalman state estimation method, the present invention can effectively compensate for the parameter uncertainty in the motion model of the estimated object. Attached Figure Description

[0103] Figure 1 This is a schematic diagram showing the cumulative error values ​​of six state estimation methods for different values ​​of parameter 'a' (noise surge amplitude). Figure 1 (a) is a comparison chart of cumulative errors when there is no surge in observation noise. Figure 1 (b) is a comparison chart of cumulative errors when there is a maximum 5-fold increase in observation noise. Figure 1 (c) is a comparison chart of cumulative errors when there is a maximum 10-fold increase in observation noise. Figure 1 (d) is a comparison chart of cumulative errors when there is a maximum 20-fold increase in observation noise. Figure 1 (e) is a comparison chart of cumulative errors when there is a maximum 25-fold increase in observation noise. Figure 1 (f) is a comparison chart of cumulative errors when there is a maximum 30-fold increase in observation noise. Figure 1 (g) is a comparison chart of cumulative errors when there is a maximum 40-fold increase in observation noise. Figure 1 (h) is a comparison chart of cumulative errors when there is a maximum 50-fold increase in observation noise;

[0104] Figure 2 The robust state estimator based on multiple adaptive factors, the robust state estimator combined with MR adaptive factors, the robust state estimator combined with Huber adaptive factors, the robust state estimator combined with RMA adaptive factors, and the robust state estimator combined with Mahalanobis adaptive factors help reduce the cumulative estimation error ratio compared with traditional robust filters under different noise surge magnitudes.

[0105] Figure 3 This is a comparison chart showing the performance of the interactive robust state estimator, the original robust estimator, and the single adaptive factor robust estimator. Figure 3 (a) is a comparison chart of the estimation errors of the two estimators. Figure 3 (b) is a comparison of the logarithmic curves of the cumulative estimation errors of the two estimators. Detailed Implementation

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

[0107] To achieve the objectives of this invention, this invention provides a design method for an interactive robust state estimator based on multiple adaptive factors, the algorithm of which is as follows:

[0108] Step (1): Based on the actual application scenario of the photoelectric tracking system, set the initial state estimate value of the interactive robust state estimator. That is, the initial position value of the tracked target, the probability transition matrix P of the sub-state estimator, and the sub-estimator model probability μ. j Each sub-estimator estimates the initial value. and posterior estimate of covariance

[0109] Step (2): Calculate the mixed state estimate of estimator j and mixed covariance estimation

[0110] Step (3): Use a robust state estimator with different adaptive factors to perform state estimation and calculate the state estimates respectively. and estimate covariance

[0111] Step (3.1): Calculate the prior state covariance P j (k|k);

[0112] Step (3.2): Utilize different adaptive factors to adjust the observation noise covariance. Make adjustments;

[0113] Step (3.2.1) The MR adaptive factor needs to preset thresholds K1 and K2, let When γ>K2, let When K2≥γ>K1, let

[0114] Step (3.2.2) The Huber adaptive factor needs to be preset with a threshold K, let When γ>K, let

[0115] Step (3.2.3) The RMA adaptive factor needs to be preset with a threshold K, let When γ>K, let

[0116] Step (3.2.4) The Mahalanobis adaptive factor needs to be preset with a threshold K, let When γ>K, let

[0117] Step (3.3): Calculate the adjustment parameters The possible values ​​of ;

[0118] Step (3.4): Calculate parameters

[0119] Step (3.5): Calculate the posterior state estimate and posterior state covariance

[0120] Step (4): Calculate the model probability μ j ;

[0121] Step (5): Calculate the overall state estimate of the interactive robust state estimator. State estimation covariance Based on the estimated value The current position value of the tracked target is obtained by combining the state equation;

[0122] Step (6): Return to step (2) to perform a new round of state estimation.

[0123] The following simulations demonstrate the design process and effectiveness of this invention, covering an interactive robust state estimator based on multiple adaptive factors, a primitive robust state estimator, and a robust state estimator with only MR adaptive factors, Huber adaptive factors, RMA adaptive factors, or Mahalanobis adaptive factors.

[0124] (1): The parameters used by the sub-robust estimators in each robust estimator or interactive robust state estimator are the same, as follows:

[0125]

[0126]

[0127] The parameters specifically used in the interactive robust state estimator based on multiple adaptive factors are as follows:

[0128]

[0129] Threshold used for MR adaptive factor:

[0130] Threshold used by Huber adaptive factor:

[0131] Threshold used for RMA adaptive factor:

[0132] Threshold used for Mahalanobis adaptive factor:

[0133] (2): Simulate the true value of the estimated object and the observation value of the observer; where the observation noise covariance matrix R is used to simulate the surge of observation noise. i In each iteration, values ​​are randomly generated from the range R0 to aR0.

[0134] x i =(F+M△) i E f )x i-1 +u i

[0135] y i =Hx i +v i .

[0136] (3): Simulate the interactive robust state estimator based on multiple adaptive factors according to the methods described in steps (1) to (6), and simulate the robust state estimator with only a single adaptive factor as described in step (3) to obtain data; for each R i The range parameter 'a' is generated, and 500 simulations are performed. Each simulation consists of a trajectory simulation of 100 points to obtain the results.

[0137] (4): such as Figure 1 These are the cumulative error values ​​of six state estimation methods corresponding to different values ​​of parameter 'a' (noise surge amplitude). Among them, Figure 1 (a) is a comparison chart of cumulative errors when there is no surge in observation noise. Figure 1 (b) is a comparison chart of cumulative errors when there is a maximum 5-fold increase in observation noise. Figure 1 (c) is a comparison chart of cumulative errors when there is a maximum 10-fold increase in observation noise. Figure 1 (d) is a comparison chart of cumulative errors when there is a maximum 20-fold increase in observation noise. Figure 1 (e) is a comparison chart of cumulative errors when there is a maximum 25-fold increase in observation noise. Figure 1 (f) is a comparison chart of cumulative errors when there is a maximum 30-fold increase in observation noise. Figure 1 (g) is a comparison chart of cumulative errors when there is a maximum 40-fold increase in observation noise. Figure 1 (h) is a comparison of the cumulative error when there is a maximum 50-fold increase in observation noise; the curve with the pentagram node in the figure represents the cumulative estimation error of the interactive robust state estimator based on multiple adaptive factors. It has always had the estimation accuracy that leads other estimation methods in different noise surge situations, proving that it can effectively integrate the advantages of different adaptive factors.

[0138] (5): such as Figure 2The interactive robust state estimator based on multiple adaptive factors, the robust state estimator combined with MR adaptive factors, the robust state estimator combined with Huber adaptive factors, the robust state estimator combined with RMA adaptive factors, and the robust state estimator combined with Mahalanobis adaptive factors all help reduce the cumulative estimation error compared to traditional robust filters under different noise surge magnitudes. The interactive robust state estimator based on multiple adaptive factors not only significantly reduces the estimation error when noise surges occur, but also exhibits general advantages over the robust state estimator combined with a single adaptive factor.

[0139] (6): such as Figure 3 This is a comparison of the performance of the interactive robust state estimator with the original robust estimator and the single adaptive factor robust estimator. Figure 3 (a) is a comparison chart of the estimation errors of the two estimators. Figure 3 (b) is a comparison graph of the cumulative estimation error logarithmic curves of the two estimators. Figure 3 This further reflects the estimation stability and the ability of the present invention to reduce the estimation error of the system.

Claims

1. A design method for an interactive robust state estimator based on multiple adaptive factors, characterized in that: The specific implementation steps are as follows: Step (1): The initial state estimate of the interactive robust state estimator should be set according to the actual application scenario of the photoelectric tracking system. That is, the initial position value of the tracked target, the probability transition matrix P of the sub-state estimator, and the sub-estimator model probability. Each sub-estimator estimates the initial value. and posterior estimate of covariance ; Step (2): Based on the state estimates from the interactive robust state estimator The probability transition matrix P of the sub-state estimator and the model probability of the sub-estimator. The estimated values ​​of each sub-estimator and estimate covariance The mixed-state estimate of sub-estimator j is obtained. and mixed covariance estimation ; Step (3): According to , and the position observations of the tracked target A robust state estimator incorporating different adaptive factors is used to estimate the state, resulting in a new state estimate. and estimate covariance The robust state estimator combining different adaptive factors for state estimation is implemented through the following steps: Step (A1): Based on the posterior state covariance Obtain the prior state covariance ; Step (A2): According to and Using different adaptive factors to adjust the observation noise covariance Make adjustments; Step (A3): Calculate the adjustment parameters The possible values ​​of ; Step (A4): Using adjustment parameters The actual process noise covariance, the actual observed noise covariance, the state transition matrix, and the driving matrix are optimized to obtain the following results: , , , ; Step (A5): Estimate the state value Prior state covariance Perform state updates to obtain posterior state estimates. and posterior state covariance ; Step (4): Calculate the model probability The calculated parameters are as follows: Step (5): Based on confidence level The estimation results of each sub-robust state estimator are weighted and the overall state estimate of the interactive robust state estimator is calculated. State estimation covariance ; Step (6): Return to step (2) to perform a new round of state estimation.

2. The interactive robust state estimator design method based on multiple adaptive factors according to claim 1, characterized in that: To address the uncertainties present in the tracked target model, the following state equation is used, where both process noise and observation noise are white noise: In this state equation, , These represent the tracked target state and observations in sub-robust state estimator j of the interactive robust state estimator; initial state values. Process noise Observation noise It conforms to a Gaussian distribution; , , These are the state transition matrix, process noise driving matrix, and state observation matrix, respectively, where the parameter uncertainties are represented by... , express; It is the expectation of the model uncertainty. It is a matrix that changes over time. From Scaling parameters that take values ​​within a range.

3. The interactive robust state estimator design method based on multiple adaptive factors according to claim 1, characterized in that: In step (1), the posterior state covariance value obtained in the previous iteration is used. Observation noise estimate and observation matrix Obtain the prior state covariance value .

4. The interactive robust state estimator design method based on multiple adaptive factors according to claim 1, characterized in that: The adaptive factors mentioned in step (2) include the MR adaptive factor, Huber adaptive factor, RMA adaptive factor, and Mahalanobis adaptive factor, and their respective calculation methods are as follows: (1) The MR adaptive factor needs to be preset with a threshold. , ,make , ,when season ;when season ; (2) The Huber adaptive factor needs to be preset with a threshold. ,make , ,when season ; (3) The RMA adaptive factor needs to be preset with a threshold. ,make , ,when season ; (4) The Mahalanobis adaptive factor needs to be preset with a threshold. ,make , ,when season .

5. The interactive robust state estimator design method based on multiple adaptive factors according to claim 1, characterized in that: In step (A3), the adjustment parameters are determined using the following formula. The possible values ​​of: 。 6. The design method for an interactive robust state estimator based on multiple adaptive factors according to any one of claims 1-5, characterized in that: By combining robust state estimators with different adaptive factors using an interactive multi-model state estimation method, appropriate adaptive factors can be adaptively selected and the observation noise covariance matrix can be quickly adjusted under observation noise surge scenarios at different scales. This compensates for the impact of observation noise on the state estimator, thereby improving the accuracy and smoothness of state estimation and enhancing the adaptability of the estimator.

Images