Robust noise covariance estimation method based on iterative reweighted least square

Abstract

The invention relates to an iterative reweighted least square-based robust noise covariance estimation method, which comprises the following steps of: 1, constructing a state space model of a linear time-invariant system, collecting measurement data in a system operation process to calculate an innovation sequence, and estimating an observation vector formed by auto-covariance of the system; step 2, on the basis of the observation vector, constructing a linear regression equation set of an innovation covariance and a noise covariance by using a system state space model and a steady-state Kalman gain; step 3, acquiring a robust noise covariance estimated value for the linear regression equation set by adopting an iterative reweighted least square algorithm; and step 4, feeding back the robust noise covariance estimation value to the Kalman filter, carrying out Kalman filter parameter updating and circulation, and realizing on-line adaptive adjustment of noise statistical parameters. According to the method, the robustness of abnormal values is remarkably improved in noise statistical parameter estimation, and the accuracy and stability of Kalman filtering can still be ensured in an environment containing noise and abnormal data.

Description

Technical Field

[0001] This invention relates to the fields of signal processing and automatic control technology, and in particular to a robust noise covariance estimation method based on iterative reweighted least squares. Background Technology

[0002] Kalman filters exhibit optimal performance in state estimation for linear Gaussian systems. However, in practical applications, the statistical characteristics (covariance) of process noise and measurement noise are often unknown or change over time. Improper noise covariance settings can significantly weaken the filtering effect, leading to increased state estimation bias or even filter divergence. To address this issue, scholars both domestically and internationally have proposed various methods for estimating noise covariance using historical data, including Bayesian estimation, maximum likelihood estimation (MLE), covariance matching, minimax methods, subspace identification, and correlation function methods. Among these, the correlation function method has received widespread attention due to its moderate computational complexity and lack of specific assumptions about the noise model. Mehra and Belanger first proposed a three-step noise statistical estimation method based on the autocorrelation of the innovation sequence, subsequently developing a single-step autocovariance least squares (ALS) method. The ALS method constructs a system of linear equations between the autocovariance of the innovation sequence and the noise covariance, and solves them using least squares, enabling simultaneous estimation of both process noise and measurement noise covariance, and has been applied in numerous practical systems.

[0003] However, most of the methods mentioned above assume that the observed noise approximately follows a Gaussian distribution, failing to consider the impact of outliers (outliers) introduced by sensor malfunctions or external interference. In real-world systems, outliers frequently occur, such as when sensors are subjected to instantaneous shocks, communication data is lost, or strong interference signals appear in the environment. These can lead to anomalous samples in the innovation sequence that deviate significantly from the main data distribution. These outliers can undermine the Gaussian noise assumption of Kalman filtering, causing severe bias or variance inflation in the filtering results. In particular, when using the least squares principle to estimate the noise covariance, even a small amount of outlier data can dominate the cost function, causing the estimated Q and R to deviate from their true values, thereby degrading the filter performance.

[0004] To address the aforementioned issues, researchers have developed various robust Kalman filter (ORKF) algorithms to enhance the filter's tolerance to outliers. Methodologically, these mainly include: First, parametric adaptive methods, which assume a heavy-tailed noise distribution and introduce a prior distribution to offset the influence of outliers, such as using Gaussian mixture priors or Student-t distributions to enhance robustness; Second, nonparametric methods, which do not directly assume a specific distribution but utilize information theory or statistical methods to reduce the impact of outliers, such as filtering methods based on correlation entropy; Third, robust filtering powered by M-estimation theory. M-estimation methods reduce the influence function corresponding to outlier residuals by selecting a non-squared robust cost function, thereby suppressing the interference of outliers on the estimation. Typical approaches include the weighted least squares (WLMS) method, which introduces weights to the residuals to weaken the contribution of outliers; the least absolute deviation (LAD) method using the L1 norm; and Huber's piecewise function and its improved loss function for filter weighting. These robust filtering methods, to a certain extent, ensure that the filtering residuals are not dominated by outliers, thereby improving the anti-interference capability of state estimation.

[0005] However, the robust methods mentioned above typically employ fixed-form influence functions, determined by the selected loss function and adjustment parameters. Performance may degrade when the outlier distribution characteristics deviate from the pre-set assumptions, and they struggle to adaptively adjust for different types of outliers. For example, the Huber function requires a pre-set threshold; when the proportion or magnitude of outlier noise exceeds expectations, a fixed threshold may not yield optimal results. To overcome the limitations of fixed influence functions, the field of robust statistics has proposed the Iterative Reweighted Least Squares (IRLS) algorithm. IRLS approximates the optimal p-norm minimization solution by continuously updating the weights of the observation data and solving a weighted least squares problem. Simply put, in each iteration, IRLS adjusts the weights based on the magnitude of the residual estimated in the previous iteration. Higher weights indicate that the observation is more in line with the mainstream trend (smaller residuals), while lower weights suggest that the observation may be an outlier. Through several iterations, IRLS can effectively reduce the impact of outliers and has been successfully applied to problems such as rotational averaging, triangulation, and point cloud registration in computer vision. It has also been proven to be applicable to nonlinear regression problems and possesses the flexibility to be compatible with various robust loss functions. In summary, there is a pressing need to introduce an adaptive robust technique in the problem of noise covariance estimation that is both insensitive to outliers and automatically adjusts the estimation strategy according to data characteristics, thereby compensating for the insufficient reliability of existing techniques in complex environments. Based on this motivation, this invention combines the ALS method and the IRLS algorithm to propose a new scheme that can automatically resist interference from outlier data in noise statistical parameter estimation. Summary of the Invention

[0006] The purpose of this invention is to overcome the shortcomings of existing noise covariance estimation methods, such as sensitivity to outlier measurements and significant degradation in estimation accuracy and filtering performance in the presence of outliers. This invention provides a robust noise covariance estimation method based on iterative reweighted least squares. The technical problem this method addresses is how to effectively suppress the interference of outliers on the estimation of noise statistical parameters when abnormal disturbances occur in sensor data or system models, ensuring that the Kalman filter can still perform state estimation with near-optimal accuracy. In other words, this invention aims to provide an online noise covariance identification strategy that can adaptively adjust estimation criteria and reduce the impact of outlier data, thereby improving the robustness and reliability of filtering systems in complex environments. This method can significantly reduce estimation bias caused by sensor failures, sudden increases in environmental noise, etc., avoiding the decrease in filtering accuracy or even failure due to inaccurate noise parameters in existing technologies, and meeting the demand for high-precision, highly robust state estimation in practical engineering.

[0007] To achieve the above objectives, the present invention provides the following solution: A robust noise covariance estimation method based on iterative reweighted least squares includes: Step 1. Construct the state-space model of the linear time-invariant system, initialize the Kalman filter, collect measurement data during system operation, calculate the innovation sequence, and estimate the observation vector composed of its autocovariance; Step 2. Based on the observed vector, construct a set of linear regression equations for the innovation covariance and noise covariance using the system state-space model and steady-state Kalman gain; Step 3. Use the iterative reweighted least squares algorithm to obtain the robust noise covariance estimate for the linear regression equations; Step 4. Feed the robust noise covariance estimate back to the Kalman filter to update and iterate the Kalman filter parameters, thereby achieving online adaptive adjustment of the noise statistical parameters.

[0008] Optionally, the observation vector estimated from its autocovariance includes: Construct a state-space model of the linear time-invariant system and initialize the Kalman filter state. and initial value of covariance Given process noise covariance and measurement noise covariance The initial estimate; Acquire measurement data during system operation using sensors. The new information sequence is then calculated using the Kalman prediction update formula. ,in, For the measurement matrix, For time The state is predicted using prior noise covariance; Collect windows of preset length New information sequence data Calculate the sample estimates of the autocovariance of the innovation at each lag: Stack all the autocovariance estimates of the lag order column-wise to form the observation vector: in, For the first time, For the new information sequence, This is the observation vector.

[0009] Optionally, the state-space model is: in, For system status, For sensor measurement data, Here is the state transition matrix. For the observation matrix, and These are the process noise and the measurement noise, respectively, and their covariance matrices are as follows: and , It is a set of positive integers.

[0010] Optionally, the linear regression equations are: in, For the corresponding coefficient matrix, For a vector containing the parameters to be estimated, observation vector .

[0011] Optionally, the steady-state Kalman gain is: in, For steady-state Kalman gain, For steady-state Kalman gain, For the transpose of the observation matrix, To observe the noise covariance matrix.

[0012] Optionally, step 3. Using the iterative reweighted least squares algorithm to obtain robust noise covariance estimates for the linear regression equations includes: Step 3.1. Initialization: All observation data weights are initially set to 1: in The total number of equations; This represents the weight diagonal matrix for step t. This represents its i-th diagonal element, initially set to a uniform weight, i.e. ,make Solve for the initial weighted least squares: ; Step 3.2. Calculate the residuals: based on the current estimate Calculate the residual vector: Each element of the residual Reflecting the The deviation between observed covariance values ​​and model predictions of the new information; Step 3.3. Update weights: Select Huber loss threshold The weights are updated according to the following rules: Thus, a new diagonal weight matrix is ​​constructed. ; Step 3.4. Solve for the new estimate: After updating the weights, solve the new weighted least squares problem. This yields new estimates of the noise covariance parameter. Step 3.5. Iteration Stopping Condition: Let Repeat steps 3.2 to 3.4 until either of the following conditions is met, then stop: 1) The parameter change is small: 2) The cost function changes little: 3) Reaching the maximum number of iterations The final output shows the convergence result: The robust noise covariance estimate is obtained by reconstructing it into matrix form. , .

[0013] Optionally, Kalman filter parameter updates and iterations include: The obtained robust noise covariance estimate , Feedback is sent to the Kalman filter to update its process noise and measurement noise parameters: The filter then continues to run, accumulating new information sequences. Steps 1-3 are repeated every fixed number of steps or when an increase in the variance of the information is detected. The covariance matrices of the process noise and measurement noise in the state-space model are estimated and updated again using the latest data window. and This enables online adaptive adjustment of noise statistical parameters.

[0014] The beneficial effects of this invention are as follows: This invention has significant innovation and beneficial effects compared to existing technologies. On the one hand, it introduces IRLS robust regression into the field of noise covariance identification, solving the problem of traditional ALS methods being unable to resist outliers; on the other hand, it improves the filtering effect by accurately estimating noise statistics, and has important engineering application value. Attached Figure Description

[0015] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0016] Figure 1 This is a schematic diagram of a robust noise covariance estimation method based on iterative reweighted least squares according to an embodiment of the present invention. Figure 2 This is a block diagram of a robust noise covariance estimation method based on iterative reweighted least squares (IRLS) according to an embodiment of the present invention. Figure 3 This is a schematic diagram of the core process of a robust noise covariance estimation method based on iterative reweighted least squares implemented in this invention. Figure 4 This is a schematic diagram comparing the fitting performance of ALS-IRLS and ALS algorithms in the 6th Monte Carlo simulation step according to an embodiment of the present invention. Figure 5 This is a schematic diagram of the weights obtained by the ALS-IRLS algorithm in the 6th Monte Carlo simulation step under the premise that the sensor has noise, according to an embodiment of the present invention. Detailed Implementation

[0017] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0018] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0019] like Figures 1 to 3 As shown, this embodiment proposes a robust noise covariance estimation method based on iterative reweighted least squares, including: Step 1. Construct the state-space model of the linear time-invariant system, initialize the Kalman filter, collect measurement data during system operation, calculate the innovation sequence, and estimate the observation vector composed of its autocovariance; Step 2. Based on the observed vector, construct a set of linear regression equations for the innovation covariance and noise covariance using the system state-space model and steady-state Kalman gain; Step 3. Use the iterative reweighted least squares algorithm to obtain the robust noise covariance estimate for the linear regression equations; Step 4. Feed the robust noise covariance estimate back to the Kalman filter to update and iterate the Kalman filter parameters, thereby achieving online adaptive adjustment of the noise statistical parameters.

[0020] This embodiment's method can be widely applied to various systems involving dynamic state estimation and filtering, especially suitable for robust state estimation and target tracking tasks in complex environments such as sensor anomalies and data mutations. For example, in the field of autonomous driving, this invention achieves accuracy in positioning and trajectory tracking in complex environments or sudden situations through the following four steps: Step 1: Acquire physical signals and calculate innovation deviation. The onboard main control unit acquires IMU (accelerometer / angular velocity), wheel speedometer (vehicle speed), and GPS (position) data in real time. The sensor observations are compared with the system predictions to extract a physical innovation sequence reflecting the deviation between the vehicle's actual driving state and the model prediction. Step 2: Construct a linear mapping of sensor noise characteristics. Combining the vehicle dynamics model, the innovation deviation is transformed into a linear regression equation describing sensor measurement noise (such as environmental electromagnetic interference) and process noise (such as irregular road vibration), establishing a correspondence between mathematical parameters and the distribution of physical environmental noise. Step 3: Perform robust weighting to suppress anomalous interference. The signal is filtered through an iterative reweighting algorithm: when the vehicle encounters tunnel signal shielding, visual obstruction, or tire slippage causing anomalous jumps in data, the algorithm automatically identifies outliers and reduces their weights. Through iterative calculation, high-confidence covariance estimates are extracted from noisy physical signals. Step 4: Correct positioning parameters online and guide vehicle movement. The estimation results are fed back to the filter in real time to dynamically compensate for the vehicle's lateral pose and longitudinal velocity. The system outputs high-precision positioning commands based on the corrected parameters, directly driving the autonomous vehicle's path planning and obstacle avoidance control, ensuring driving safety in complex road conditions. In scenarios such as radar monitoring, UAV navigation, robot positioning, and industrial process control, sensor measurement data often contain outliers or wild points. Traditional filtering algorithms are prone to inaccurate state estimation or even divergence due to the accumulation of noise statistical errors. The robust noise covariance estimation method provided by this invention can be embedded as a module in existing Kalman filter systems, adaptively adjusting filter parameters online to ensure stable operation even in harsh environments. With the development of intelligent sensing and IoT technologies, the risks of data anomalies and attacks are increasing. This robust estimation method has significant application prospects in ensuring system security and reliability. By combining it with adaptive control, fault-tolerant filtering, and other technologies, this invention is expected to play a greater role in modern systems requiring highly reliable state estimation, such as aerospace, autonomous driving, smart grids, and distributed sensor networks, providing technical support for accurate perception and decision-making in complex environments.

[0021] This invention discloses a robust noise covariance estimation method based on iterative reweighted least squares (IRLS), comprising: (1) establishing the connection between noise covariance ALS estimation and robust regression, and for the first time replacing the least squares criterion in the classical ALS method with the Huber mixture norm loss function, transforming the problem of estimating the autocovariance of the innovation sequence contaminated with outliers into a robust regression problem. This design enables the algorithm to theoretically resist a certain proportion of outlier data, thus improving the robustness of existing covariance matching techniques; (2) innovatively designing an iterative reweighted least squares (IRLS) algorithm to solve the above robust estimation problem; (3) the method of this invention assigns different influence weights to each observation data, enabling the algorithm to be adjusted for different outlier distribution structures.

[0022] This invention aims to improve the accuracy and reliability of Kalman filter noise statistical parameter estimation under outlier interference conditions. Addressing the vulnerability of traditional Autocovariance Least Squares (ALS) noise estimation methods to outliers, this method introduces the Huber robust loss function as an optimization criterion and combines iterative reweighted least squares algorithm to adaptively weight the observed data, suppressing the impact of outliers on covariance estimation. Specifically, this invention first uses a Kalman filter to obtain a novelty sequence (i.e., the difference between observed and predicted values, also known as the novelty sequence), and calculates the autocovariance of this sequence as the original observation. Then, the autocovariance sequence containing outliers is modeled as an ε-contamination model, and different weights are assigned to each observation point through iterative optimization: the weight is automatically reduced for observations with large deviations, thereby reducing the impact of outliers on the estimation results. After several iterations of approximation, robust estimates of the process noise covariance Q and measurement noise covariance R can be obtained. Simulation and experimental results show that, compared with traditional algorithms, the method of this invention significantly improves the robustness to outliers in noise statistical parameter estimation, and can still guarantee the accuracy and stability of Kalman filtering in environments with noisy and outlier data.

[0023] Specifically, in this embodiment, step 1, system initialization and data acquisition: establish the state-space model of the linear time-invariant system and initialize the Kalman filter state. and initial value of covariance Based on experience or offline identification, the noise covariance of a given process can be determined. and measurement noise covariance The initial estimated value. Subsequently, measurement data during system operation are acquired through sensors. ( The new information sequence is then calculated using the Kalman prediction update formula. in For the measurement matrix. Collect data from a window of a certain length (length is...). (New information sequence data) Calculate the sample estimates of the autocovariance of the innovation at each lag: Stack all the autocovariance estimates of the lag order column-wise to form the observation vector: This vector contains the variance and autocorrelation of the innovation sequence itself, and serves as the basis for subsequent calculation of the system noise covariance.

[0024] Taking a linear time-invariant discrete system as an example, the system state-space model can be expressed as: in, For system state (dimension) ), For sensor measurement (dimension) ), Here is the state transition matrix. The observation matrix; and These are process noise and measurement noise, respectively, generally assumed to be a random process with zero mean, and their covariance matrices are respectively... (dimension) )and (dimension) ).

[0025] In practical applications, these two noise covariance matrices are often unknown or change over time. and When the noise is known and follows a Gaussian distribution, the Kalman filter can obtain the optimal state estimate with the minimum mean square error. However, when... , When the parameters are unknown or inaccurately set, the filter performance will degrade significantly. Therefore, it is necessary to estimate and adjust the parameters online based on actual measurement data. and .

[0026] Specifically, when the noise covariance matrix and Given the time, the time can be derived using a Kalman filter (KF) with the minimum mean square error (MMSE) criterion. status Covariance The optimal estimate: in This is known as Kalman gain.

[0027] Furthermore, the specific steps for calculating the innovation sequence and its autocovariance are as follows: During operation, the Kalman filter generates a sequence of innovations (i.e., a sequence of measurement residuals): in, For time The state is predicted using prior noise covariance.

[0028] Ideally, the covariance is accurately estimated and there are no outliers, and the innovation sequence... It should be zero-mean white noise, and satisfy the following: in The state prediction covariance. Satisfying the Lyapunov equations Kalman filter steady state Converges to a constant value .

[0029] Kalman gain under steady-state conditions tending to a constant value It can be obtained through offline calculation: Using the innovation sequence, the corresponding autocovariance can be estimated. The innovation autocovariance is defined as follows: in This is the selected covariance lag window length. Specifically, Let be the variance of the new information.

[0030] When a finite length is collected New information sequence Then, the autocovariance of the new information can be estimated by iterative averaging: Expand the autocovariance estimators of each lag order into vector form: Subscript This indicates that the matrix is ​​stacked as a vector by columns or rows. This is the observed autocovariance vector, equivalent to the measurement data.

[0031] Specifically, in this embodiment, step 2, constructing the linear regression equation system, includes: utilizing the system state-space model and steady-state Kalman gain. We construct the analytical relationship between the innovation covariance and the noise covariance. According to Kalman filtering theory, in the ideal case: zero hysteresis covariance: lag covariance ( ) in: This is the closed-loop prediction error transfer matrix. The steady-state prediction error covariance matrix is ​​obtained by solving the algebraic Riccati equation offline. By rearranging the above covariance formulas for each order, we can obtain the covariance matrix for the unknown vector. linear equation system , where: matrix From the known system matrix , and steady-state gain Calculations show that The observation vector of the new information covariance calculated in step (1) Thus, the noise covariance estimation problem is transformed into a standard linear regression problem. .

[0032] Specifically, the linear regression model for noise covariance estimation: Mehra's Autocovariance Least Squares (ALS) method estimates the noise covariance using the autocovariance of the innovation sequence. and Using the Kalman filter steady-state condition, the theoretical innovation autocovariance can be expressed. use , To express. Specifically, there are: in It is the closed-loop state matrix.

[0033] Furthermore, by expanding the Lyapunov equations: Substituting the result into the above equation, it can be proven that the autocovariance of the new information is related to... and It is linear. Therefore, this embodiment can establish a system of linear equations in the following form: in, For a vector containing the parameters to be estimated (i.e., ... and The combination of the flattened elements of a matrix is ​​generally denoted as . ), For the aforementioned true (or observed) information autocovariance vector, The corresponding coefficient matrix has a size of .

[0034] More specifically, through new information Autocovariance, defined as the expectation of the data and its lagged data, is denoted as . Record the residual as Therefore, combining the above equation, we get: ; in With new information The relationship is: ; During the process, K It will converge to the steady-state gain exponentially fast, and the gain can be calculated offline as follows: ; in Let the steady-state residual covariance matrix be denoted by . This is the closed-loop prediction error transfer matrix. This is the closed-loop observation error transfer matrix. for The covariance matrix. The Lyapunov equations must be satisfied, provided that the prediction error transition matrix is... It is stable and the innovation sequence is a stationary process: Using the system state-space model and steady-state Kalman gain An analytical relationship is constructed between the innovation covariance and the noise covariance, where the innovation covariance is expressed as: ; in , It is a user-defined parameter that defines the window size.

[0035] The problem of noise covariance estimation can be reformulated as a system of linear regression equations, where the corresponding coefficient matrix A is defined as: ; The linear regression equation system is then: ; parameter The semidefinite constrained least squares problem can be solved under the LMS criterion, and the following can be estimated using the ordinary least squares (OLS) method: ; Optimal estimation In the sense of minimum mean square error (MMSE), it is given by the following equation if the matrix inequality holds: ; Where b represents the unbiased estimate of the observation vector b, and the autocovariance matrix is ​​estimated by using the ergodicity of N-innovation. Calculation: ; in This represents the window size for the autocovariance. However, due to unexpected maneuvers and unreliable sensors, relevant measurements may be contaminated by outliers, leading to a significant deterioration in the performance of noisy covariance estimation via the ALS algorithm and state estimation via KF. Therefore, it is necessary to address the sensitivity of the original ALS method to outliers. The original Least Mean Square (LMS) criterion is reformulated as an outlier-resistant regression problem, and updated... Calculation formula: ; in This represents the residual function, i.e., the bias in the noise covariance estimation problem. ,in Represents the design matrix. This represents the noise covariance parameter. The sample represents the autocovariance of the innovation. Robust cost function. Designed to mitigate the impact of outliers, Huber is typically chosen as the loss function. It is a piecewise function that applies squared loss to small residuals and linear loss to large residuals. ; in It is a user-defined threshold that balances bias and robustness. The weights are based on a chosen function, typically the Huber loss function, with respect to bias. The derivative: .

[0036] Step 3, IRLS Iterative Solution: Based on the linear model above, the traditional ALS method uses the minimum mean square error (LMS). Solving using norm criteria : The closed-form solution to the unconstrained problem is: In the absence of outliers, the above equation yields an unbiased estimate. However, when the novel autocovariance is observed... When outliers exist, the LMS criterion assigns excessive weight to the squared term of the deviation, causing the estimation results to deviate from the true value and exhibiting poor robustness.

[0037] Therefore, it is necessary to improve the objective function so that it can still stably estimate the noise covariance parameter when outlier data is present.

[0038] Robust target estimation and Huber loss function: This embodiment transforms the noise covariance problem into a robust regression problem by replacing the least squares squared loss with a loss function that is insensitive to outliers. For example, consider the Huber loss function. To measure the residuals, the following optimization objective is formed: ; in Indicates the first The residuals of the observation equations total One observation equation.

[0039] Huber loss is defined as: ; in This is the preset threshold.

[0040] The corresponding influence function is: Iterative weighted solution based on IRLS: This invention employs the Iteratively Reweighted Least Squares (IRLS) algorithm to solve the Huber M estimation problem. The corresponding weight update strategy is as follows: In each iteration, solve the weighted least squares subproblem: in .

[0041] The algorithm starts from the initial estimate (Usually, a standard OLS solution is used) Start by iteratively updating the weights and parameters until the convergence condition is met.

[0042] In summary, the IRLS iterative solution: For the regression equation obtained in step (2), this embodiment uses the iterative reweighted least squares algorithm to solve θ, thereby obtaining robust Q and R estimates. The specific process is as follows: (3.1) Initialization (t = 0): The initial weights of all observation data are 1: in Let be the total number of equations. (Identity matrix), solve for the initial weighted least squares: (i.e., using the ordinary least squares solution as the starting point).

[0043] (3.2) Calculate the residuals: based on the current estimate Calculate the residual vector: Each element of the residual Reflecting the The deviation between observed covariance values ​​and model predictions.

[0044] (3.3) Update weights: Select Huber loss threshold (commonly used) or The weights are updated according to the following rules: Thus, a new diagonal weight matrix is ​​constructed. .

[0045] This weighting mechanism can automatically identify abnormal residuals: the larger the residual, the closer the weight is to 0, and it has almost no impact on subsequent estimations.

[0046] (3.4) Solving for the new estimate: After updating the weights, solve the new weighted least squares problem. This yields new estimates of the noise covariance parameter.

[0047] (3.5) Iteration stopping condition: Let Repeat (3.2) to (3.4) until any of the following conditions are met, then stop: the parameter changes little: 1) 2) The cost function changes little: 3) Reaching the maximum number of iterations (Convergence typically occurs within 20-50 iterations). Final output: Convergence result. Reconstructing it into matrix form yields the robust estimate. , .

[0048] Specifically, in this embodiment, updating and looping the Kalman filter parameters includes: The obtained robust noise covariance estimate , Feedback is sent to the Kalman filter to update its process noise and measurement noise parameters: Then, the filter continues to run, accumulating new innovation sequences. Steps (1) to (3) can be repeated every fixed number of steps (e.g., every 100 to 500 steps) or when an anomalous increase in the innovation variance is detected, to estimate and update again using the latest data window. and This enables online adaptive adjustment of noise statistical parameters. Thus, when environmental noise levels change or sudden abnormal interference occurs during system operation, this embodiment can sense and automatically correct filter parameters in real time, ensuring that the Kalman filter always operates close to its optimal state, significantly improving the system's robustness and long-term stability.

[0049] Through the above steps, this embodiment effectively reduces the impact of anomalous information data on the results while obtaining noise covariance estimates. The entire method does not require special assumptions about the noise distribution and is applicable to a wide range of scenarios with a small number of non-Gaussian anomalies in a Gaussian background. It can significantly improve the robustness of the filter without significantly increasing the computational load.

[0050] Below, this embodiment uses a simplified linear system as an example to verify the effectiveness of the method of the present invention: (I) System Model and Experimental Setup: This embodiment uses a simplified linear system as the object to verify the effectiveness of the method of the present invention. System state vector , measurement quantity Its state-space model is: In this model, process noise and measuring noise The variable is a zero-mean Gaussian random variable. The experiment sets the noise covariance to reflect the actual process noise. (Scalar, equivalent to a diagonal matrix) ) and the actual measurement noise covariance Initial state of filtering (Take the true value plus a small bias), initial estimate of covariance Without knowing the true noise covariance, this embodiment first provides an imprecise initial guess: And calculate the initial Kalman gain accordingly. Subsequently, the robust covariance estimation algorithm of this invention is embedded into the Kalman filtering process for online correction. and .

[0051] (II) Outlier Scenario Design: To test the algorithm's robustness against anomalies, this embodiment artificially creates a significant anomaly in the measurement data: at time 50, measurement noise is introduced. This increases to five times the normal variance (equivalent to a major sensor shock or failure). The resulting observations... The deviation from the true value is significant, which in turn stimulates innovation. This becomes a significant outlier. Apart from this one anomaly, all other observation noise in this experiment follows a given Gaussian distribution. This embodiment uses a sliding window approach for covariance estimation, obtaining... For each piece of innovative data, the covariance estimation algorithm is updated once. and Huber loss function threshold The initial noise level is set to 0.02, which is the threshold for the convergence criterion of IRLS iteration. The maximum number of iterations was set to 20. To evaluate the statistical performance of the algorithm, this embodiment independently repeated the above experiment 100 times (Monte Carlo simulation) and compared the results with those of the traditional ALS method.

[0052] (III) Algorithm Execution and Result Analysis: Using the initial , Running the Kalman filter, we obtain the following results in sequence. arrive A total of 50 innovative data points were collected. The innovation autocovariance observation vector was then calculated based on these data. (This example uses the lag order) There are a total of 15 self-covariance matrices, which are expanded into vector dimensions. Combined with the known system matrix. , and current gain Constructing a design matrix (dimension) ,because , (Each is a scalar to be estimated). The resulting linear regression model is: ,in .

[0053] In the first covariance estimation, this embodiment compares the fitting performance of traditional ALS and the method of this invention (ALS-IRLS). At the beginning of the first iteration, the initial weights of each observation are all 1. The calculated observation residuals... The 9th element (correspond The innovation covariance with lag of 8 showed an abnormally large increase, which is due to the inclusion of the aforementioned outliers within the window. This leads to sample covariance The residual deviates significantly from the true value. During the first iteration of IRLS weight calculation, this residual exceeds the threshold. Therefore, its corresponding weight decreases from 1 to (Approximately 30% of the normal value), while the absolute values ​​of the vast majority of other residuals are within the threshold, with a weight of 1. It is evident that this embodiment has automatically identified abnormal residuals caused by anomalous data and reduced their impact. Subsequently, IRLS is used to solve for weighted least squares to obtain the updated... ,Right now and The fitting results show that traditional ALS direct fitting yields... In this embodiment, robust fitting is obtained. Compared to the true value and The robust estimation is clearly closer. This is because after the outliers are reduced in weight, the corresponding... The bias no longer excessively drags down the estimation results, allowing the estimation to be primarily driven by other normal data. When the fitted lines of these two methods are compared with the observed data in this embodiment (as shown in the figure), it can be found that: in traditional ALS fitting, although the 9th observation point deviates significantly from the overall trend, it exerts a large pulling effect on the fitting due to its equal weight, causing the obtained line to deviate from the true solution; while in robust IRLS fitting, the 9th point has almost no impact on the fitted line due to its small weight, and most of the remaining data that conform to the model are fitted more accurately, thus the line is closer to the true solution. - This demonstrates that the method of the present invention effectively reduces the interference of outliers, making the estimation results more accurate and reliable.

[0054] The new estimate will be obtained after the first update is completed. Used to replace the noise covariance in the filter and recalculate the steady-state Kalman gain. The system then continues to run, collecting the next 50 innovative data points (steps 51 to 100), repeating the covariance estimation process described above. As the iterations proceed, it can be observed that... , The estimated values ​​gradually converge to the true values: for example, after the second update After the third update After the fourth update and It was already very close to the true values ​​of 0.5 and 0.1, with a variation within 1%. In subsequent rounds, the estimate remained relatively stable at... The proximity indicates that the algorithm has converged. Meanwhile, the iterations within IRLS during each round of computation are also very fast: in most cases, there are fewer than 10 weight update iterations. Right now The convergence condition is then met. Throughout the simulation, outliers were injected only once, in step 50. This is because the method of this invention effectively reduced the impact of these outlier measurements during the first round of estimation. The estimated impact was negligible, and subsequent iterations were unaffected, with the estimated value quickly returning to the correct trajectory. In contrast, the traditional ALS method suffers from significant deviations in the first-round estimation due to outliers. Even with subsequent filter updates, the gain and prediction remain biased, impacting later innovation distributions and making it difficult to converge or even causing the estimation error to diverge. This further demonstrates the robustness of the method in abnormal environments.

[0055] (iv) Performance statistics comparison: In order to quantitatively evaluate the performance, this embodiment compares the average error of this embodiment with that of traditional ALS in 100 Monte Carlo simulations.

[0056] Figure 4 The fitting performance of ALS-IRLS and ALS algorithms is compared in the 6th Monte Carlo simulation step. Adjusted data points are marked with a blue cross. The 9th data point is an outlier, far from the 95% confidence interval.

[0057] Figure 5 The weights obtained in the 6th Monte Carlo simulation step using the ALS-IRLS algorithm are shown. Normal data are marked in blue, and outliers are marked in purple.

[0058] The results show that, when using the method in this embodiment, the final convergence is achieved. The estimated root mean square error was 0.098, which is lower than the 0.126 of traditional ALS. ; The estimated root mean square error was reduced from 0.033 to 0.025, a decrease of approximately 24%. Furthermore, using the corrected noise covariance for Kalman filtering resulted in an average reduction of approximately 15% in the mean square error of the state estimation compared to the uncorrected version. Therefore, the method of this invention outperforms traditional methods in various simulation experiments, significantly improving the accuracy of noise covariance estimation and filtering performance while maintaining a minimal increase in computational cost.

[0059] Through the above embodiments, the technical solution and advantages of this embodiment have been fully verified. This method can reliably estimate accurate noise statistical parameters even when sensor data exhibits abnormal interference, providing an effective approach for adaptive optimization of Kalman filters. Those skilled in the art can implement this invention based on the above implementation steps, achieving similar results in different systems and application scenarios.

[0060] The robust noise covariance estimation method based on IRLS provided by this invention achieves significant technical effects: First, it ensures that the estimation accuracy of the noise covariance remains stable even with outliers. Simulation results demonstrate that, in the presence of outlier interference, the estimated process noise covariance Q and measurement noise covariance R in this embodiment are closer to the true values, and the root mean square error is significantly reduced compared to the traditional ALS method. For example, in a target tracking experiment, the estimated RMSE of Q is reduced by more than 20% and the RMSE of R is reduced by about 25% after applying this invention, with a synchronous reduction in the filter state estimation error, verifying the effectiveness and superiority of the method. Second, this embodiment improves the robustness and reliability of the filtering system. When sensor malfunctions or sudden noise occurs in the environment, the algorithm can automatically reduce the weight of abnormal data, preventing the filter from being driven by "erroneous information" and causing drastic deviations, thereby preventing filter divergence or tracking misalignment. In practical applications, this means that even under harsh environments or attacks, the system can still maintain stable operation, ensuring the safety of the controlled object. Third, this embodiment has low computational overhead and is easy to implement in engineering. In the IRLS iteration process, the size of the weighted least squares problem solved each time is the same as that of the original ALS method, and the typical number of iterations converges within a finite number of steps. Therefore, the overall computational complexity is only a few times that of conventional least squares, and it can run in real time on modern processors. Furthermore, the method of this invention has good compatibility with existing Kalman filtering frameworks. Developers only need to add weight update iterations to the covariance estimation stage to upgrade existing filtering systems to robust versions without changing other modules. In summary, this invention greatly enhances the system's resilience to anomalous data while improving filtering accuracy, generating significant engineering application value and economic and social benefits. The robust noise covariance estimation method provided by this invention, after adopting the above technical solution, can achieve significant beneficial effects: Enhancing the robustness and stability of the estimation process: This invention continuously adjusts the weights through IRLS iteration, making the estimation process highly adaptable to outliers of different types and magnitudes. Even when intermittent and significant deviations occur in the observed data, the algorithm can still converge stably without being severely affected by single-point distortions. In contrast, robust methods with fixed weight functions are prone to performance degradation when the distribution characteristics of outliers change, while the adaptive weighting strategy of this invention makes it robust to changes in outlier patterns. In simulation experiments, even if the measurement noise of a certain observation is increased several times to make it an outlier, this embodiment only uses a very small weight to process this outlier information, while maintaining a high weight for other normal data, thus ensuring the accuracy of the overall estimation. This adaptive robustness is particularly suitable for complex dynamic systems with unknown outlier noise distributions.

[0061] Maintaining moderate computational complexity and ease of engineering implementation: This invention adds an iterative weight update layer to the traditional ALS algorithm. However, each iteration mainly solves a linear equation with a weighted matrix, resulting in computational complexity on the same order of magnitude as the original least squares method. Simulations show that convergence is typically achieved within 10 iterations, thus maintaining a relatively low overall computational cost, making it suitable for real-time applications. Furthermore, this algorithm does not rely on prior statistical assumptions about anomalous noise, achieving robust estimation solely through data-driven methods, thus exhibiting good versatility. It can be seamlessly integrated into existing Kalman filter systems, requiring only the addition of a covariance estimation module to the filter architecture, resulting in minimal intrusion into the original system and low implementation costs. These advantages make this invention a promising application in fields requiring online calibration of noise parameters, such as navigation, tracking, and signal processing.

[0062] Improving the overall performance of the Kalman filter: By obtaining more accurate state noise covariance matrices Q and R using the method of this invention, the updated noise covariance can be used in the Kalman filtering process, improving the rationality of the filter gain calculation. Results show that after introducing correct noise statistics during the filtering process, the mean square error of state estimation is significantly reduced, and the tracking accuracy of the target trajectory is improved. For example, in target tracking simulations, using the corrected noise covariance of this embodiment, the filtered state estimation error is reduced by approximately 15% compared to the uncorrected version. Simultaneously, because robust estimation avoids state estimation divergence or oscillation caused by outliers, this invention can improve the stability and safety of system operation. In summary, the method of this invention indirectly optimizes the performance of the Kalman filter by improving the accuracy and anti-interference capability of noise covariance estimation, enabling it to operate reliably even in complex environments.

[0063] In summary, this invention has significant innovation and beneficial effects compared to existing technologies. On the one hand, this invention introduces IRLS robust regression into the field of noise covariance identification, solving the problem of traditional ALS methods being insensitive to outliers; on the other hand, it improves the filtering effect by accurately estimating noise statistics, and has important engineering application value.

[0064] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Various modifications and improvements made to the technical solutions of the present invention by those skilled in the art without departing from the spirit of the present invention should fall within the protection scope defined by the claims of the present invention.

Claims

1. A robust noise covariance estimation method based on iterative reweighted least squares, characterized in that, include: Step 1. Construct the state-space model of the linear time-invariant system, initialize the Kalman filter, collect measurement data during system operation, calculate the innovation sequence, and estimate the observation vector composed of its autocovariance; Step 2. Based on the observed vector, construct a set of linear regression equations for the innovation covariance and noise covariance using the system state-space model and steady-state Kalman gain; Step 3. Use the iterative reweighted least squares algorithm to obtain the robust noise covariance estimate for the linear regression equations; Step 4. Feed the robust noise covariance estimate back to the Kalman filter to update and iterate the Kalman filter parameters, thereby achieving online adaptive adjustment of the noise statistical parameters.

2. The robust noise covariance estimation method based on iterative reweighted least squares as described in claim 1, wherein estimating the observation vector composed of its self-covariance includes: Construct a state-space model of the linear time-invariant system and initialize the Kalman filter state. and initial value of covariance Given process noise covariance and measurement noise covariance The initial estimate; Acquire measurement data during system operation using sensors. The innovation sequence is then calculated using the Kalman filter's prediction update formula. ,in, For the measurement matrix, For time The state is predicted using prior noise covariance; Collect windows of preset length New information sequence data Calculate the sample estimates of the autocovariance of the innovation at each lag: Stack all the autocovariance estimates of the lag order column-wise to form the observation vector: in, For the first time, For the new information sequence, This is the observation vector.

3. The robust noise covariance estimation method based on iterative reweighted least squares according to claim 1, characterized in that, The state-space model is as follows: in, For system status, For sensor measurement data, Here is the state transition matrix. For the observation matrix, and These are the process noise and the measurement noise, respectively, and their covariance matrices are as follows: and .

4. The robust noise covariance estimation method based on iterative reweighted least squares according to claim 1, characterized in that, The linear regression equations are as follows: in, For the corresponding coefficient matrix, For a vector containing the parameters to be estimated, observation vector .

5. The robust noise covariance estimation method based on iterative reweighted least squares according to claim 1, characterized in that, The steady-state Kalman gain is: in, For steady-state Kalman gain, For steady-state Kalman gain, For the transpose of the observation matrix, To observe the noise covariance matrix.

6. The robust noise covariance estimation method based on iterative reweighted least squares according to claim 1, characterized in that, Step 3. Using the iterative reweighted least squares algorithm, obtain robust noise covariance estimates for the linear regression equations, including: Step 3.

1. Initialization: All observation data weights are initially set to 1: in The total number of equations; This represents the weight diagonal matrix for step t. This represents its i-th diagonal element, initially set to a uniform weight, i.e. ,make Solve for the initial weighted least squares: ; Step 3.

2. Calculate the residuals: based on the current estimate Calculate the residual vector: Each element of the residual Reflecting the The deviation between observed covariance values ​​and model predictions of the new information; Step 3.

3. Update weights: Select Huber loss threshold The weights are updated according to the following rules: Thus, a new diagonal weight matrix is ​​constructed. ; Step 3.

4. Solve for the new estimate: After updating the weights, solve the new weighted least squares problem. This yields new estimates of the noise covariance parameter. Step 3.

5. Iteration Stopping Condition: Let Repeat steps 3.2 to 3.4 until either of the following conditions is met, then stop: 1) The parameter change is small: 2) The cost function changes little: 3) Reaching the maximum number of iterations The final output shows the convergence result: The robust noise covariance estimate is obtained by reconstructing it into matrix form. , .

7. The robust noise covariance estimation method based on iterative reweighted least squares according to claim 1, characterized in that, Performing Kalman filter parameter updates and loops includes: The obtained robust noise covariance estimate , Feedback is sent to the Kalman filter to update its process noise and measurement noise parameters: The filter then continues to run, accumulating new information sequences. Steps 1-3 are repeated every fixed number of steps or when an abnormal increase in the variance of the information is detected. The covariance matrices of the process noise and measurement noise in the state-space model are estimated and updated again using the latest data window. and This enables online adaptive adjustment of noise statistical parameters.

Images