A robust kalman filter method based on joint policy

Abstract

The application discloses a kind of robust Kalman filtering methods based on joint strategy, including step 1, establishing Kalman filtering state transition equation and state observation equation;Step 2, estimate target initial state initialization estimation error variance P 0,0 ;Step 3, obtain k-1 time target state estimation and estimation error variance P k‑1,k‑1 ;Input the expected sequence of indicator variable z k of previous L time;Step 4, utilize the observation sample weight w k of k time and indicator variable z k It is modified to observation likelihood;Step 5, with the aid of variational bayesian inference, the posterior estimation of hidden variable is updated by iteration;Step 6, according to the update result, save current time target state estimation error variance P k,k And the expected sequence of indicator variable z k of previous L-1 time and k time;Step 7, jump to step 3, carry out state estimation at k=k+1 time.The application meets the demand of real-time positioning tracking system low power consumption, and improves the precision of target state estimation.

Description

Technical Field

[0001] This invention belongs to the field of target tracking and localization technology, and particularly relates to a robust Kalman filtering method based on a joint strategy. Background Technology

[0002] Obtaining accurate state estimation using sensor measurements is a fundamental problem in various tasks, such as target tracking, navigation, state feedback control, monitoring and optimization, and many other applications. The Kalman Filter (KF) algorithm, based on linear minimum variance estimation, is a commonly used state estimation method. However, in practical applications, measurement anomalies are a common phenomenon, which may be caused by unexpected environmental interference, temporary sensor malfunctions, calibration errors, etc. From the perspective of the correlation of outliers in the measurement sequence, outliers can be divided into isolated outliers and continuous outliers. Isolated outliers correspond to random outliers, which suddenly appear in the measurement due to some reason. Continuous outliers, on the other hand, are caused by environmental interference, sensor malfunctions, sensor wear, and manifest as outliers that persist for a period of time. The occurrence of these anomalous measurements may degrade the performance of the standard Kalman filter, resulting in poor estimation of the target of interest.

[0003] For tracking outliers in observations, the two main robust filtering strategies are: one is based on heavy-tailed noise models, such as modeling the observation probability density function as a Student's t or Laplace distribution model. Other methods assess the actual contribution of each input observation by weighting the observation covariance matrix; for example, by introducing variables modeled by gamma or Wishart distributions into the process noise covariance matrix for weighting.

[0004] Another commonly used robust strategy is based on the detection-rejection approach, which automatically identifies and eliminates outliers so that they cannot contribute to state estimation. Common methods include using projective statistics to distinguish outliers. Other approaches transform classic recursive methods into batch pattern regression, using iterative reweighted least squares algorithms to estimate the state. Alternatively, indicator variables modeled using Beta-Bernoulli multilevel prior distributions can be used to detect and reject outliers in real time.

[0005] The above algorithm is based on a single robust strategy and has a good effect on suppressing randomly occurring outliers. However, it is prone to problems such as the tracking trajectory deviating significantly from the actual value and the filtering diverging when dealing with outliers that persist for a period of time.

[0006] With the development of technology, target localization and tracking technologies require increasingly higher estimation accuracy. Meanwhile, in practical applications, factors such as high measurement noise, channel instability, and limited system power consumption make the observation scenarios increasingly complex. Therefore, finding a method that can adapt to complex tracking and localization scenarios while ensuring high-precision positioning is of great significance. Summary of the Invention

[0007] The purpose of this invention is to provide a robust Kalman filtering method based on a joint strategy to solve the technical problem of adapting to complex tracking and positioning scenarios while ensuring high-precision positioning.

[0008] To solve the above-mentioned technical problems, the specific technical solution of the present invention is as follows:

[0009] A robust Kalman filter method based on a joint strategy includes the following steps:

[0010] Step 1: Establish the motion equations for the Kalman filter;

[0011] Step 2: Establish the observation equations for the Kalman filter;

[0012] Step 3, obtain the state of the target at time k-1. and estimation error variance P k-1,k-1 Measurement information y at time k k ; and the expected sequence of the indicator variables at the first L time steps { <z k-L >, <z k-L+1 >,… <z k-1 >};

[0013] Step 4: Employ a robust Kalman filter method based on a joint strategy, introducing the weight w at time k. k and indicator variable z k Correct the observation likelihood function;

[0014] Step 5: Using variational Bayesian inference, iteratively update the target state at time k. Estimation error variance P k,k And perform posterior estimation of latent variables;

[0015] Step 6, output the state of the target at time k. and estimation error variance P k,k ; and the expected sequence of indicator variables at time L-1 and time k { <z k-L+1 >, <z k-L+2 >,… <z k When a new observation is input, return to step 3 to estimate the next time step; if no new observation is input, the filtering ends.

[0016] Furthermore, step 1, establishing the motion equations for the Kalman filter, specifically includes:

[0017] x k =Ax k-1 +u k

[0018] In the formula [·] T Represents the transpose of a vector; x k y k Let k be the target position at time k. Let k be the target velocity. Let k be the target acceleration at time k; Let Σ be the process noise, where Σ is the process noise covariance matrix; and A be the state transition matrix.

[0019]

[0020] Where ΔT is the observation time interval for acquiring the target state, and I2 represents the second-order identity matrix.

[0021] Furthermore, step 2 involves establishing the observation equations for the Kalman filter, including:

[0022] y k =Cx k +v k

[0023] In the formula y k For the measurement information at time k, x k Let k be the target state at time k; Let R be the observation noise, and C be the observation noise covariance matrix.

[0024] C = [I2 02 02].

[0025] Furthermore, step 3 includes the following steps:

[0026] Step 3.1, Initialization at k=0: Given the target state based on prior knowledge. Estimation error variance P 0,0 Hyperparameters c0, d0, e0, f0 and the hidden Markov chain order L; the process noise covariance matrix Σ and the observation noise covariance matrix R are determined based on the target motion and measurement information.

[0027] Step 3.2, at time k≥1, initialize i = 0; <z k >=1; <w k >=1;

[0028] Step 3.3, when 1≤k<L+1 and i=0, input the expected sequence of the indicator variables for the previous k-1 time steps { <z1> , <z2>,… <z k-1 >};

[0029] When k ≥ L+1, i = 0, input the expected sequence of the indicator variables for the previous L time steps { <z k-L >, <z k-L+1 >,… <z k-1 >}.

[0030] Furthermore, step 4 specifically includes the following steps:

[0031] Step 4.1, Standard Gaussian distribution observation likelihood probability density function:

[0032]

[0033] In the formula, C represents the observation matrix and R represents the observation noise covariance matrix;

[0034] Step 4.2, introduce the weight w at time k. k Modify the observation likelihood function:

[0035]

[0036] In the formula w k Follows a Gamma distribution:

[0037] w k ~Gamma(w k |c0,d0)

[0038] Where c0 and d0 are hyperparameters of the Gamma distribution;

[0039] Step 4.3, based on step 4.2, introduces the indicator variable z at time k. k Further modifications were made to the observation likelihood function:

[0040]

[0041] In the formula z k Markov chains belonging to L-order discrete sequences in Modeled using Beta-Bernoulli multilevel prior distribution:

[0042]

[0043] π j ~Beta(π) j |e0,f0),

[0044] in, It is an indicator variable modeled by a hidden Markov chain at time k, and follows a Bernoulli distribution; π j The expected variable of the Bernoulli distribution is modeled by the Beta distribution; e0 and f0 are the hyperparameters of the Beta distribution.

[0045] Furthermore, step 5 specifically includes the following steps:

[0046] Step 5.1, Kalman filter prediction The formula is:

[0047]

[0048] In the formula, A is the state transition matrix;

[0049] Step 5.2, Kalman filter prediction P k,k-1 The formula is:

[0050] P k,k-1 =AP k-1,k-1 A T +Σ;

[0051] In the formula, Σ is the process noise covariance matrix;

[0052] Step 5.3: Enter the i-th iteration and, based on variational Bayesian inference, modify the Kalman gain K. k Represented as:

[0053]

[0054] Expected value using weights <w k > and the expectation of the indicator variable <z k Scaling the noise covariance matrix;

[0055] Step 5.4, when <z k >>10 -3 At that time, the target state and estimation error variance The posterior estimation formula is:

[0056]

[0057]

[0058] when <z k >≤10 -3 At that time, the target state and estimation error variance The posterior estimation formula is:

[0059]

[0060]

[0061] Latent variables in steps 5.5 and 5.3 Following a Bernoulli distribution, and based on variational Bayesian inference, the posterior expectation is updated as follows:

[0062]

[0063] in, and Represented as:

[0064]

[0065]

[0066] in,

[0067] η k = <lnw k >-M ln(2π)-ln|R|- <w k >ρ k ,

[0068] ρ k Represented as:

[0069]

[0070] In the formula, Tr(·) is the trace of the matrix;

[0071] Latent variables in steps 5.6 and 5.5 Following a Beta distribution, and based on variational Bayesian inference, the posterior expectation is updated as follows:

[0072]

[0073]

[0074] in,

[0075]

[0076]

[0077] The implicit variable w in steps 5.7 and 5.3 k Following a Gamma distribution, and based on variational Bayesian inference, the posterior expectation is updated as follows:

[0078]

[0079]

[0080] in,

[0081]

[0082]

[0083] Step 5.8, calculate the value of the relative increment τ of the iteratively estimated state:

[0084]

[0085] Step 5.9, increment the iteration count: i = i + 1;

[0086] Step 5.10, determine if τ > 10 -6 If τ ≤ 10, then exit the loop iteration and execute step 5.11; -6 If so, return to step 5.3 and continue the loop iteration;

[0087] Step 5.11, output the state of the target at time k. and estimation error variance And the expected sequence of indicator variables at time L-1 and time k { <z k-L+1 >, <z k-L+2 >,… <z k >}.

[0088] The robust Kalman filtering method based on a joint strategy proposed in this invention has the following advantages:

[0089] 1. This invention introduces a weight w at time k. k and indicator variable z k Modify the observation likelihood function, w k The weight variables, z, are modeled by the Gamma distribution. k The indicator variable is modeled by the Beta-Bernoulli distribution, thus integrating outlier identification and processing strategies based on heavy-tailed noise models and detection-rejection strategies. This results in better detection and suppression of randomly occurring isolated outliers, thereby improving the robustness of the Kalman filter.

[0090] 2. The indicator variable z of this invention k Modeled by Markov chains, it also has a good suppression effect on outliers with time correlation.

[0091] 3. The tracking filtering framework of this invention is based on Kalman filtering and uses variational Bayesian inference for posterior estimation. It is simple to implement, has low computational cost, and meets the low power consumption requirements of positioning and tracking systems. Attached Figure Description

[0092] Figure 1 This is a flowchart of the robust Kalman filter based on a joint strategy according to the present invention;

[0093] Figure 2 This is a comparison chart of the target motion position components and sensor-collected data in Example 1;

[0094] Figure 2 (a) is a diagram showing the lateral position of the target in Embodiment 1 of the present invention;

[0095] Figure 2 (b) is a diagram showing the actual lateral position measurement values ​​in Embodiment 1 of the present invention;

[0096] Figure 2 (c) is a longitudinal position diagram of the target motion in Embodiment 1 of the present invention;

[0097] Figure 2 (d) is a diagram showing the actual longitudinal position measurement values ​​in Embodiment 1 of the present invention;

[0098] Figure 3 is a comparison of trajectory estimation and positioning error curves for different filtering algorithms in Embodiment 1 of the present invention.

[0099] Figure 3(a) is a comparison of the target position state estimation results in Embodiment 1 of the present invention;

[0100] Figure 3(b) is a comparison of the root mean square error (RMSE) of the position estimation algorithm in Embodiment 1 of the present invention;

[0101] Figure 4 This is a comparison chart of the localization error of different algorithms as a function of the observed anomaly ratio in Embodiment 1 of the present invention;

[0102] Figure 5 is a comparison diagram of the target motion position components and sensor-collected data in Embodiment 2 of the present invention;

[0103] Figure 5(a) is a diagram showing the lateral position of the target movement in Embodiment 2 of the present invention;

[0104] Figure 5(b) shows the actual lateral position measurement values ​​in Embodiment 2 of the present invention;

[0105] Figure 5(c) is a diagram showing the longitudinal position of the target in Embodiment 2 of the present invention;

[0106] Figure 5(d) shows the actual longitudinal position measurement values ​​in Embodiment 2 of the present invention;

[0107] Figure 6 is a comparison of trajectory estimation and positioning error curves for different filtering algorithms in Embodiment 2 of the present invention.

[0108] Figure 6(a) is a comparison of the target position state estimation results in Embodiment 2 of the present invention;

[0109] Figure 6(b) is a comparison of the root mean square error (RMSE) of the position estimation algorithm in Embodiment 2 of the present invention;

[0110] Figure 7 This is a comparison chart of the localization error of different algorithms as a function of the observed anomaly ratio in Embodiment 2 of the present invention. Detailed Implementation

[0111] To better understand the purpose, structure, and function of this invention, a robust Kalman filtering method based on a joint strategy will be described in further detail below with reference to the accompanying drawings.

[0112] Example 1:

[0113] This invention discloses a robust Kalman filtering method based on a joint strategy. This method combines a heavy-tailed noise model and a detection-rejection strategy, effectively suppressing both isolated and continuous outliers in measurements, and achieving real-time robust filtering and tracking of sensors in complex scenarios. To verify the effectiveness of this method, this embodiment takes two-dimensional trajectory tracking as an example. First, it simulates the case where isolated outliers exist in the data collected by the sensor. The formula for generating contaminated observations is expressed as:

[0114] y k =Cx k +v k +r k

[0115] Where, x k The state vector of the target; y k V is the measurement vector; C is the position observation matrix; v k It is additive Gaussian noise. Where R is the observation noise covariance matrix; r k It is the non-Gaussian noise term in the generated observations, expressed as:

[0116]

[0117] c k ~Bernoulli(p k ),

[0118] Where U(·) represents a uniform distribution, [a m ,b m ] represents the domain of uniform distribution of outliers in the m-th dimension; p k This represents the probability of isolated outliers occurring. In this example, [a1,b1] = [-10,10], [a2,b2] = [-20,20], p k =0.2.

[0119] The specific steps of the robust Kalman filtering method based on the joint strategy disclosed in this embodiment are as follows: Figure 1 As shown, it includes:

[0120] Step 1, establish the motion equations for the Kalman filter:

[0121] x k =Ax k-1 +u k

[0122] In the formula x k y k Let k be the target position at time k. Let k be the target velocity. Let k be the target acceleration at time k; Let Σ be the process noise, where Σ is the process noise covariance matrix; and A be the state transition matrix.

[0123]

[0124] Where ΔT = 0.01s is the observation time interval for acquiring the target state; I2 represents the second-order identity matrix.

[0125] Step 2, establish the observation equations for the Kalman filter:

[0126] y k =Cx k +v k

[0127] In the formula y k For the measurement information at time k, x k Let k be the target state at time k; Let R be the observation noise, and C be the observation noise covariance matrix.

[0128] C = [I2 02 02]

[0129] Step 3, obtain the state of the target at time k-1. and estimation error variance P k-1,k-1 Measurement information y at time k k ; and the expected sequence of the indicator variables at the first L time steps { <z k-L >, <z k-L+1 >,… <z k-1 >};

[0130] Step 3 also includes:

[0131] Step 3.1, Initialization at k=0: Given the target state based on prior knowledge. Estimation error covariance P 0,0 In this embodiment P 0,0 =10 -1 I6; Given hyperparameters c0, d0, e0, f0 and the hidden Markov chain order L; determine the process noise covariance matrix Σ and the observation noise covariance matrix R based on the target motion and measurement information. In this embodiment, Σ = 10. -3 I6, R = 10 -1 I2;

[0132] Step 3.2, at time k≥1, initialize i = 0; <z k >=1; <w k >=1;

[0133] Step 3.3, when 1≤k<L+1 and i=0, input the expected sequence of the indicator variables for the previous k-1 time steps { <z1> , <z2>,… <z k-1 >};

[0134] When k ≥ L+1, i = 0, input the expected sequence of the indicator variables for the previous L time steps { <z k-L >, <z k-L+1 >,… <z k-1 >};

[0135] Step 4: Employ a robust Kalman filter method based on a joint strategy, introducing the weight w at time k. k and indicator variable z k Correct the observation likelihood function;

[0136] Step 4.1, Standard Gaussian distribution observation likelihood probability density function:

[0137]

[0138] In the formula, C represents the observation matrix and R represents the observation noise covariance matrix;

[0139] Step 4.2, introduce the weight w at time k. k Modify the observation likelihood function:

[0140]

[0141] In the formula w k Follows a Gamma distribution:

[0142] w k ~Gamma(w k |c0,d0)

[0143] Where c0 and d0 are hyperparameters of the Gamma distribution;

[0144] Step 4.3, based on step 4.2, introduces the indicator variable z at time k. k Further modifications were made to the observation likelihood function:

[0145]

[0146] In the formula z k Markov chains belonging to L-order discrete sequences in Modeled using Beta-Bernoulli multilevel prior distribution:

[0147]

[0148] π j ~Beta(π) j |e0,f0),

[0149] in, It is an indicator variable modeled by a hidden Markov chain at time k, and follows a Bernoulli distribution; π j The expected variable of the Bernoulli distribution is modeled by the Beta distribution; e0 and f0 are the hyperparameters of the Beta distribution.

[0150] Step 5: Using variational Bayesian inference, iteratively update the target state at time k. Estimation error variance P k,k And perform posterior estimation of latent variables;

[0151] Step 5.1, Kalman filter prediction The formula is:

[0152]

[0153] In the formula, A is the state transition matrix;

[0154] Step 5.2, Kalman filter prediction P k,k-1 The formula is:

[0155] P k,k-1 =AP k-1,k-1 A T +Σ;

[0156] In the formula, Σ is the process noise covariance matrix;

[0157] Step 5.3: Enter the i-th iteration and, based on variational Bayesian inference, modify the Kalman gain K. k Represented as:

[0158]

[0159] Expected value using weights <w k > and the expectation of the indicator variable <z k Scaling the noise covariance matrix;

[0160] Step 5.4, when <z k >>10 -3 At that time, the target state and estimation error variance The posterior estimation formula is:

[0161]

[0162]

[0163] when <z k >≤10 -3 At that time, the target state and estimation error variance The posterior estimation formula is:

[0164]

[0165]

[0166] Latent variables in steps 5.5 and 5.3 Following a Bernoulli distribution, and based on variational Bayesian inference, the posterior expectation is updated as follows:

[0167]

[0168] in, and Represented as:

[0169]

[0170]

[0171] in,

[0172] η k = <ln w k >-M ln(2π)-ln|R|- <w k >ρ k ,

[0173] ρ k Represented as:

[0174]

[0175] In the formula, Tr(·) is the trace of the matrix;

[0176] Latent variables in steps 5.6 and 5.5 Following a Beta distribution, and based on variational Bayesian inference, the posterior expectation is updated as follows:

[0177]

[0178]

[0179] in,

[0180]

[0181]

[0182] The implicit variable w in steps 5.7 and 5.3 k Following a Gamma distribution, and based on variational Bayesian inference, the posterior expectation is updated as follows:

[0183]

[0184]

[0185] in,

[0186]

[0187]

[0188] Step 5.8, calculate the value of the relative increment τ of the iteratively estimated state:

[0189]

[0190] Step 5.9, increment the iteration count: i = i + 1;

[0191] Step 5.10, determine if τ > 10 -6 If τ≤10, then exit the loop iteration and execute step 6; -6 If so, return to step 5.3 and continue the loop iteration;

[0192] Step 6, output the state of the target at time k. and estimation error variance P k,k ; and the expected sequence of indicator variables at time L-1 and time k { <z k-L+1 >, <z k-L+2 >,… <z k When a new observation is input, return to step 3 to estimate the next time step; if no new observation is input, the filtering ends.

[0193] like Figure 2 As shown, Figure 2 -(a) Figure 2 -(c) represent the actual motion trajectories of the target in the horizontal and vertical positional components, respectively. Figure 2 -(b) Figure 2 -(d) represents the sampled observation data. It can be seen that the observation data input to the sensor has a large number of randomly occurring isolated outliers in the horizontal and vertical positional dimensions, which are significantly outliers that deviate from the observations containing Gaussian noise.

[0194] Furthermore, in this embodiment, the root mean square error (RMSE) of the positioning is used to evaluate the tracking performance at each time step:

[0195]

[0196] Where N0 represents the number of Monte Carlo experiments, in this example N0 = 500; These represent the x and y coordinates estimated by the algorithm at time k in the nth Monte Carlo experiment. Represents the actual x and y coordinates of the target at time k in the nth Monte Carlo experiment;

[0197] Furthermore, the Time-averaged Root Mean Square Error (TRMSE) is defined as follows:

[0198]

[0199] Where K represents the total number of observation sampling points.

[0200] Furthermore, simulation comparison experiments were conducted with existing robust filters, Outlier-robust Kalman Filter (OR-KF) and Outlier Detection Kalman Filter (OD-KF), under different types of outlier interference, fully verifying the superiority of the Contaminated-measurements-robust Kalman Filter (CMR-KF) method of this invention. Notably, the OR-KF method only introduces the weight w. k The observation likelihood function is modified; the OD-KF method only introduces the indicator variable z. k Modify the observation likelihood function.

[0201] Figure 3-(a) shows a comparison between the trajectory estimation results of the OR-KF, OD-KF, and CMR-KF methods and the actual trajectories. Figure 3-(b) shows the RMSE curves of the localization errors of these three methods at each sampling point. Clearly, the CMR-KF method disclosed in this invention exhibits the lowest overall localization error at each sampling point, demonstrates good robustness against randomly occurring single-point outliers, and achieves high state estimation accuracy.

[0202] Furthermore, the measurement anomaly control factor, i.e., the probability p of isolated outliers, will be used. k The value was varied from 0.05 to 0.5, with other conditions remaining unchanged. 500 Monte Carlo simulations were conducted at each outlier proportion, and the time mean root square error (TRMSE) of each tracking method was calculated. Figure 4 As shown, although the TRMSE of all methods increases with the increase of the proportion of outliers, the TRMSE of CMR-KF is always the lowest.

[0203] This invention combines a joint heavy-tailed noise model and a detection-rejection strategy with standard Kalman filtering. For input observation samples, outliers are automatically identified and processed using weight variables modeled by a Gamma distribution and indicator variables modeled by a Beta-Bernoulli distribution. This invention differs from traditional Kalman filtering methods, which indiscriminately process received observation samples. Compared to robust filters with a single strategy, this invention employs a joint outlier processing strategy, resulting in better suppression of outliers. This embodiment verifies that the method of this invention can effectively achieve robust filtering even when isolated outliers exist in the observed samples.

[0204] Example 2:

[0205] To verify the filtering effect of the method disclosed in this invention in more complex observation scenarios, this embodiment, based on Embodiment 1, changes the form of the input observation samples so that the sensor observations contain both randomly occurring isolated outliers and time-correlated continuous outliers. The estimation results of the robust Kalman filter method based on the joint strategy are then observed. This embodiment uses two-dimensional trajectory tracking as an example to simulate the situation where isolated and continuous outliers exist simultaneously in the data collected by the sensor, generating a non-Gaussian noise term r in the contaminated observations. k The formula is expressed as:

[0206]

[0207] In the formula, r k The time intervals 200≤k<250 and 600≤k<650 contain two consecutive outliers. Isolated outliers at other time intervals are represented as follows:

[0208]

[0209] c k ~Bernoulli(p k ),

[0210] Where U(·) represents a uniform distribution, [a m ,b m [] denotes the domain of a uniform distribution of outliers in the m-th dimension. In this example, [a1,b1] = [-30,30], [a2,b2] = [-20,20], p k =0.2. Other filter initialization conditions: target initial state estimation Initial estimation error variance P 0,0 =diag[0.1,0.1,0.1,0.1,0.01,0.01].

[0211] As shown in Figure 5, Figures 5-(a) and 5-(c) show the actual motion trajectories of the target in the lateral and longitudinal position components, respectively, while Figures 5-(b) and 5-(d) show the sampled observation data. It can be seen that the input sensor observation data, in addition to isolated outliers in the lateral and longitudinal position dimensions, also shows two periods of anomalous observations lasting for a period of time. Figure 6-(a) shows the comparison between the trajectory estimation results of the OR-KF, OD-KF, and CMR-KF methods and the actual trajectories. Figure 6-(b) shows the localization error RMSE curves of these three methods at each sampling point. The OR-KF method, because it only introduces w... k The variables are weighted on the observation covariance matrix R, and even when consecutive outliers occur, they still receive information about the anomalies with a certain probability, thus significantly increasing the cumulative error. Clearly, the CMR-KF method disclosed in this invention exhibits the lowest overall localization error at each sampling point, demonstrates good robustness against isolated outliers and time-correlated consecutive outliers, and achieves high state estimation accuracy.

[0212] Furthermore, given the probability p of isolated outliers k The width β of each consecutive outlier segment and the proportion λ of mixed outliers are expressed as:

[0213]

[0214] Furthermore, the probability p of isolated outliers is fixed. k =0.2, and by changing β, the proportion of mixed outliers is altered, causing λ to change from 0.2 to 0.5. At this point, the non-Gaussian noise term r... k Represented as:

[0215]

[0216] In the formula, c k ~Bernoulli(p k With other conditions remaining unchanged, 500 Monte Carlo simulation experiments were conducted at each outlier proportion, and the time mean root square error (TRMSE) of each tracking method was statistically analyzed. Figure 7 As shown, the error of the OR-KF method increases significantly when λ > 0.4, while the error of the CMR-KF method disclosed in this invention, although increasing with the proportion of mixed outliers, maintains the lowest TRMSE. This embodiment verifies that the method of this invention can effectively achieve robust filtering when the observed sample contains both isolated and continuous outliers.

[0217] It is understood that the present invention has been described through some embodiments, and those skilled in the art will recognize that various changes or equivalent substitutions can be made to these features and embodiments without departing from the spirit and scope of the invention. Furthermore, under the teachings of the present invention, these features and embodiments can be modified to adapt to specific situations and materials without departing from the spirit and scope of the invention. Therefore, the present invention is not limited to the specific embodiments disclosed herein, and all embodiments falling within the scope of the claims of this application are within the protection scope of the present invention. < / z1> < / z1>

Claims

1. A robust Kalman filter method based on federated policy for two-dimensional trajectory tracking, characterized in that, The method comprises the following steps: Step 1, establishing a motion equation of Kalman filtering; Step 2, establishing an observation equation of Kalman filtering; Step 3, obtain the state of the target at time k-1 and the estimation error variance P k-1,k-1 ; measurement information y k at time k; and the expected sequence of indicator variables {<z k-L >, <z k-L+1 >, …, <z k-1 >} at the previous L times Step 4, the robust Kalman filter method based on joint strategy is adopted, and the weight w at time k is introduced k and the indication variable z k The observation likelihood function is modified; Step 5, the target state at time k is updated iteratively by means of variational Bayesian inference estimate error variance P k,k and the hidden variables; Step 6, output the state of the target at time k and the estimated error variance P k,k ; and the indicator variable expected sequence {<z k-L+1 >, <z k-L+2 >, … <z k >} at the previous L-1 time and time k; when there is a new observation input, return to Step 3 to estimate the next time; if there is no new observation input, end the filtering; The step 4 specifically comprises the following steps: Step 4.1, a standard Gaussian distribution observation likelihood probability density function: In the formula, C represents an observation matrix, and R represents an observation noise covariance matrix; Step 4.2, introducing the weights w at time k k Modifying the observation likelihood function: where w k Subjection to Gamma distribution: w k ~ Gamma(w k |c0,d0) Wherein, c0 and d0 are hyperparameters of Gamma distribution; Step 4.

3. Introduce the indicator variable z at time k based on step 4.2 k Further modification of the observation likelihood function: where z k Markov chain belonging to a discrete sequence of order L where Modelled by a Beta-Bernoulli hierarchical prior distribution: π j ~ Beta(π j |e0,f0), where, is an indicator variable modeled by a k-state hidden Markov chain, following a Bernoulli distribution; π j is an expected variable of the Bernoulli distribution, modeled by a Beta distribution; e0and f0are hyperparameters of the Beta distribution.

2. The robust Kalman filter based on joint policy according to claim 1, characterized in that, The step 1 of establishing the motion equation of Kalman filtering specifically comprises: x k = Ax k-1 +u k wherein [·] T represents the transpose of the vector; x k , y k is the target position at time k, is the target velocity at time k, is the target acceleration at time k; is the process noise, where∑ is the process noise covariance matrix; A is the state transition matrix: Wherein, ΔT is an observation time interval for obtaining a target state, and I2 represents a second-order unit matrix.

3. The robust Kalman filter based on joint policy according to claim 1, characterized in that, The step 2 of establishing the observation equation of Kalman filtering comprises: y k = Cx k + v k where y k is the measurement information at time k, x k is the target state at time k; is the observation noise, where R is the observation noise covariance matrix; C is the observation matrix: C=[I20202].

4. The robust Kalman filter based on joint policy according to claim 1, characterized in that, The step 3 comprises the following steps: Step 3.1, initialization at k = 0: given target state with prior knowledge Estimation error variance P 0,0 hyperparameters c0, d0, e0, f0and the number of hidden Markov chain L; process noise covariance matrix Σ and observation noise covariance matrix R are determined by the situation of target motion and the situation of measurement information; Step 3.2, at time k≥1, initialize i = 0; <z k > = 1; <w k > = 1; Step 3.3, at time 1≤kL+1, i=0, input the expected sequence of the indicator variable at previous k-1 time <z1> , <z2> ,…<z k-1 >}；< / z2> < / z1> When k = L + 1, i = 0, the expected sequence of the previous L time indication variables {<z k-L >, <z k-L+1 >, …, <z k-1 >} is inputted.

5. The robust Kalman filter based on joint policy according to claim 1, characterized in that, The step 5 specifically comprises the following steps: Step 5.1, Kalman filter prediction The formula is: In the formula, A is a state transition matrix; Step 5.2, Kalman filter prediction P k,k-1 The formula is: P k,k-1 = AP k-1,k-1 A T +∑; In the formula, Σ is a process noise covariance matrix; Step 5.3, enter the i-th loop iteration, modify the Kalman gain K according to the variational Bayesian inference k is represented as: using the expectation of the weight <w k > and the expectation of the indicator variable <z k > to scale the noise covariance matrix; Step 5.4, when <z k > 10 -3 , the target state and the posterior estimate of the estimated error variance is given by: When <z k >≤10 -3 When <z and the posterior estimate of the estimation error variance is given by Step 5.5, the hidden variable in Step 5.3 Subject to the Bernoulli distribution, according to variational Bayesian inference, the posterior expectation is updated as: wherein and is represented by: Wherein, η k = <ln w k > - M ln(2π) - ln |R| - <w k > p k , ρ k is represented as: In the formula, Tr(·) is a trace of a matrix; Step 5.6, the hidden variable in step 5.5 Subject to the Beta distribution, the posterior expectation is updated according to variational Bayesian inference as: Wherein, Step 5.7, the hidden variable w in Step 5.3 k Subject to the Gamma distribution, according to variational Bayesian inference, the posterior expectation is updated as: Wherein, Step 5.8, calculating a value of a relative increment τ of an iterative estimation state: Step 5.9, increasing the iteration number by 1: i=i+1; Step 5.10, determine if τ > 10 -6 then exit the loop iteration and perform step 5.11; if τ < 10 -6 then return to step 5.3 to continue the loop iteration; Step 5.11, output the state of the target at time k and the estimated error variance and the indicator variable expectation sequence {<z k-L+1 >, <z k-L+2 >, …, <z k} at the previous L - 1 times and time k.

Images