A method and device for enhanced robust state estimation based on spatio-temporal constrained expectation maximization

Abstract

The application provides a method and device for enhanced robust state estimation based on space-time constraint expectation maximization, which comprises the following steps: constructing an incomplete measurement model based on a two-state Markov process, modeling time inertia of multi-sensor measurement loss, and calculating measurement arrival probability; deriving posterior Cramer-Rao lower bound under a measurement loss superposition heavy tail noise scenario, and explicitly defining a state estimation error theoretical lower limit. Taking the error lower limit as a precision constraint, a modified variational Bayesian robust student t Kalman filter is constructed. The filter output result is used to modify the noise covariance prior inverse scale matrix, and the filter is updated synchronously. The filter update and prior inverse scale matrix modification are iterated in an alternating closed loop, and the prior parameters and posterior distribution are optimized in linkage during the iteration process. After the iteration converges, the state estimation result is output, and the precision and overall robustness of state estimation under complex working conditions are effectively improved.

Description

Technical Field

[0001] This invention relates to the field of multi-sensor fusion state estimation technology, and specifically to an enhanced robust state estimation method and apparatus based on spatiotemporal constraint expectation maximization. Background Technology

[0002] Multi-sensor localization and tracking systems are widely used in drones, autonomous driving, radar detection, and wireless sensor networks. In real-world scenarios, sensors often experience incomplete measurements due to communication packet loss, energy-saving transmission, and occlusion. Complex environments can also introduce heavy-tailed non-Gaussian noise, and the coupling of these factors leads to a significant decrease in the accuracy of traditional Kalman filtering and robust filtering.

[0003] The existing technology has the following drawbacks:

[0004] The measurement missing often uses the Bernoulli model, which does not reflect the time inertia and only discards the missing dimension information, resulting in low spatiotemporal utilization; the variational Bayesian robust student t-filter depends on the nominal process noise covariance, and the filter diverges due to inaccurate priors; the lack of theoretical quantification of the coupling effect of incomplete measurement and heavy-tailed noise results in insufficient robustness and stability.

[0005] In summary, the existing technology has the following problems: In multi-sensor positioning systems, incomplete measurements coupled with heavy-tailed non-Gaussian noise form ill-conditioned measurement conditions. Existing robust filtering methods cannot fully utilize the spatiotemporal correlation information contained in the missing measurements, and the covariance of process noise and measurement noise depends on inaccurate prior nominal values, resulting in low state estimation accuracy, poor robustness, and easy filtering divergence. Summary of the Invention

[0006] The purpose of this invention is to address the problems in multi-sensor positioning systems where incomplete measurements coupled with heavy-tailed non-Gaussian noise create ill-conditioned measurement conditions. Existing robust filtering methods cannot fully utilize the spatiotemporal correlation information contained in the missing measurements, and the covariance between process noise and measurement noise depends on inaccurate prior nominal values, resulting in low state estimation accuracy, poor robustness, and easy filtering divergence.

[0007] On one hand, embodiments of the present invention provide an enhanced robust state estimation method based on spatiotemporal constraint expectation maximization, comprising:

[0008] An incomplete measurement model based on a two-state Markov process is constructed to model the time inertia of multi-sensor measurement gaps and calculate the measurement arrival probability.

[0009] By combining the incomplete measurement model with the measurement arrival probability, the posterior Cramer-Rao lower bound under the condition of incomplete measurement coupled with heavy-tailed noise is derived to quantify the lower bound of the state estimation error.

[0010] Using the posterior Cramerlow lower bound as the accuracy constraint, the inverse Wishart distribution is adopted as the conjugate prior of the noise covariance. Adaptive estimation of the noise covariance is achieved through moment matching, and a modified variational Bayesian robust student t-Kalman filter is constructed based on the incomplete measurement model.

[0011] Based on the variational Bayesian robust student t-Kalman filter, the prior inverse scaling matrix of the noise covariance is corrected;

[0012] The variational Bayesian robust student t-Kalman filter is updated and iterated alternately with the modified prior inverse scaling matrix to output the state estimate, error covariance, and optimized noise covariance.

[0013] On the other hand, embodiments of the present invention provide an apparatus for enhanced robust state estimation based on spatiotemporal constraint expectation maximization, comprising:

[0014] The measurement missing modeling unit is used to construct an incomplete measurement model based on a two-state Markov process, perform time inertial modeling of multi-sensor measurement missing, and calculate the measurement arrival probability.

[0015] The error lower bound derivation unit is used to combine the incomplete measurement model with the measurement arrival probability to derive the posterior Cramer-Rao lower bound under the coupling condition of incomplete measurement and heavy-tailed noise, so as to quantify the state estimate of the error lower bound.

[0016] The filter construction unit is used to use the posterior Cramerlow lower bound as the accuracy constraint, adopt the inverse Wishart distribution as the conjugate prior of the noise covariance, realize the adaptive estimation of the noise covariance through moment matching, and construct a modified variational Bayesian robust student t-Kalman filter based on the incomplete measurement model.

[0017] The prior matrix correction unit is used to correct the prior inverse scaling matrix of the noise covariance based on the variational Bayesian robust student t-Kalman filter.

[0018] The iterative operation output unit is used to update the variational Bayesian robust student t-Kalman filter and alternately iterate with the modified prior inverse scaling matrix to output the state estimate, error covariance and optimized noise covariance.

[0019] The above technical solution has the following beneficial effects:

[0020] This invention employs a two-state Markov process to model measurement missing behavior, accurately characterizing the time inertial features of measurement missing. It combines the observation equation to solve for the measurement arrival probability, closely aligning with the dynamic changes in the missing data of actual multi-sensor measurements, resulting in a higher degree of model fit.

[0021] This invention introduces a posterior Cramer-Rao lower bound as an accuracy constraint, clearly defining the upper limit of the theoretical error of state estimation, providing a quantitative basis for the optimization of filtering algorithms, and effectively avoiding the state estimation results from exceeding the reasonable error range.

[0022] This invention employs an inverse Wishart distribution combined with a moment matching algorithm to achieve adaptive estimation of noise covariance, overcoming the limitations of fixed noise parameters. It also combines an incomplete measurement model with an optimized robust student t-Kalman filter, which can efficiently adapt to heavy-tailed non-Gaussian noise scenarios and significantly improve noise suppression performance.

[0023] This invention integrates time series and spatial structure constraints to construct an expectation-maximizing Q function, optimizes the prior inverse scaling matrix correction logic, fully explores the correlation information of multi-sensor data, and improves the accuracy of parameter correction.

[0024] This invention employs an alternating iterative mode of filter update and matrix correction closed loop to achieve dynamic linkage optimization of prior parameters and posterior distribution. It has a fast iteration convergence speed and the final state estimation result has high accuracy and strong stability, and can be widely adapted to various complex application scenarios such as vehicle navigation, industrial equipment condition monitoring, and UAV trajectory estimation. Attached Figure Description

[0025] Figure 1 This is a flowchart of an enhanced robust state estimation method based on spatiotemporal constraint expectation maximization provided by an embodiment of the present invention;

[0026] Figure 2 This is a schematic diagram of the structure of an enhanced robust state estimation device based on spatiotemporal constraint expectation maximization provided in an embodiment of the present invention. Detailed Implementation

[0027] 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.

[0028] The technical solution of this invention is as follows: First, real-time monitoring data from multiple sensors is collected, and the measurement missing modeling process is initiated. All sensor measurement states are divided into two categories: arrival and absence. The state transition matrix is ​​determined based on the two-state Markov process. Measurement indicator variables are introduced and combined with the Hadamard product to build the system observation equation. The real-time measurement arrival probability is calculated based on the above model, thus completing the preliminary modeling work.

[0029] After completing the modeling, we combine the completed incomplete measurement model with the obtained measurement arrival probability to carry out the posterior Cramer-Rao lower bound derivation, quantify the minimum error value that the state estimation can achieve in the current scenario, and use this as a hard accuracy constraint standard for subsequent filtering optimization.

[0030] Based on the established accuracy constraints, the inverse Wishart distribution is selected as the conjugate prior distribution of the process noise covariance and the measurement noise covariance. Moment matching is used to complete the adaptive closed-loop estimation of the two types of noise covariance. At the same time, combined with the incomplete measurement model built in the early stage, invalid and missing measurement data are eliminated, and the modified variational Bayesian robust student t-Kalman filter is built. After the construction is completed, it can be put into use for preliminary state estimation.

[0031] During the filtering process, the posterior distribution results of the generated state and noise covariance are extracted in real time. Combined with the spatial structure of the on-site sensor deployment and the variation law of historical measurement missing time, the expected maximum Q function adapted to the scenario is constructed. The derivative of the constructed Q function around the prior inverse scaling matrix is ​​calculated and zeroed out to simplify the calculation process and obtain the maximum likelihood estimation update model. The update model is used to simultaneously complete the correction and optimization of the prior inverse scaling matrix corresponding to the process noise covariance and the measurement noise covariance.

[0032] After completing a single round of matrix correction, the corrected prior inverse scaling matrix is ​​used as a new prior parameter and substituted back into the variational Bayesian robust student t-Kalman filter to perform filtering update operations, generating new posterior distribution data again. The prior inverse scaling matrix EM correction operation is repeated, and alternating cyclic iterations are continuously performed.

[0033] Throughout the entire iteration process, the corrected prior matrix is ​​continuously passed to the next round of filtering operations, forming a stable closed-loop optimization logic. The iteration process stops when the iterative operation meets the convergence condition, and the accurate state estimation value, error covariance data, and noise covariance parameter after optimization and debugging are uniformly output, thus successfully completing the entire process of multi-sensor enhanced robust state estimation.

[0034] The enhanced robust state estimation device designed in this invention, in actual operation, has the following steps: the measurement missing modeling unit completes all modeling and probability calculation work; the error lower bound derivation unit completes the estimation error quantification and judgment; the filter construction unit builds a dedicated robust filtering algorithm; the prior matrix correction unit completes the prior parameter optimization and adjustment based on the filtered data; and finally, the iterative operation output unit completes the closed-loop iterative operation and outputs all estimation results in a unified manner. All units work together to complete the entire process of state estimation automatically without manual parameter debugging, making it suitable for various complex industrial measurement and control scenarios.

[0035] In embodiments of the present invention, such as Figure 1This application provides an enhanced robust state estimation method based on spatiotemporal constraint expectation maximization. The method includes the following steps:

[0036] S101: Construct an incomplete measurement model based on a two-state Markov process, perform time-inertial modeling for the measurement omissions of multiple sensors, and calculate the measurement arrival probability; specifically, this includes: modeling the arrival and omission of measurements in each dimension of multiple sensors as a two-state Markov process and determining the state transition matrix; introducing measurement indicator variables and constructing the system observation equation with measurement omissions using the Hadamard product form; and solving for the sensor measurement arrival probability based on the state transition matrix and the system observation equation of this application.

[0037] S102: Combining the incomplete measurement model and the measurement arrival probability of this application, the posterior Cramer-Rao lower bound under the coupling condition of incomplete measurement and heavy-tailed noise is derived to quantify the lower bound of the state estimation error. The posterior Cramer-Rao lower bound (PCRLB) here serves not only as a theoretical benchmark for evaluating algorithm performance but is also innovatively introduced as an accuracy constraint for filter design. Since incomplete measurement introduces discrete missing variables and heavy-tailed noise violates the Gaussian assumption, the traditional Cramer-Rao lower bound derivation is no longer directly applicable. This embodiment introduces auxiliary variables to represent the heavy-tailed noise as a Gaussian-scale mixture and, combined with the statistical characteristics of the two-state Markov model, derives an analytical expression for PCRLB applicable to this coupling scenario. This lower bound clearly reveals the joint attenuation mechanism of the measurement missing probability and the heavy-tailed noise parameters on the Fisher information matrix, thus providing quantitative theoretical guidance for subsequent gain adjustment and parameter optimization of the adaptive filter, ensuring that the algorithm can still approach the theoretical optimal accuracy under adverse observation conditions.

[0038] S103: Using the posterior Cramero lower bound of this application as the accuracy constraint, the inverse Wishart distribution is adopted as the conjugate prior of the noise covariance. Moment matching is used to achieve adaptive estimation of the noise covariance, and a modified variational Bayesian robust student t-Kalman filter is constructed based on the incomplete measurement model of this application. Specifically, to maintain robustness in unknown or time-varying noise environments, the inverse Wishart distribution is chosen as the conjugate prior distribution of the process noise covariance matrix and the measurement noise covariance matrix. This ensures that the posterior and prior distributions have the same mathematical form, facilitating analytical derivation. Based on this, the moment matching method is used to replace the traditional fixed parameter settings, achieving online adaptive learning of the noise covariance and completely eliminating the strong dependence on nominal prior information. Simultaneously, to address the failure of the standard variational Bayesian update caused by incomplete measurements, the robust student t-Kalman filter framework is structurally modified to make it compatible with the two-state Markov observation model. This modified filter can still maintain an effective inference of the system state and noise statistics even when some measurements are missing, preventing filter divergence caused by insufficient information utilization.

[0039] S104: Based on the variational Bayesian robust student t-Kalman filter of this application, the prior inverse scaling matrix of the noise covariance is corrected; including:

[0040] Based on the variational Bayesian robust student t-Kalman filter of this application, the posterior distribution result is obtained; based on the posterior distribution result of this application, combined with the time series information of missing measurements and the spatial structure constraints of multi-sensor, the Q function of the expectation-maximization algorithm is constructed; the derivative of the Q function of this application with respect to the prior inverse scaling matrix is ​​taken and set to zero to obtain the maximum likelihood estimation update model;

[0041] This application employs maximum likelihood estimation to update the prior inverse scaling matrix of the process noise covariance and measurement noise covariance in the model. In the standard variational Bayesian framework, when measurements are frequently missing, the update of prior parameters often stagnates or becomes significantly biased. To address this issue, this application introduces the Expectation-Maximization (EM) algorithm to perform a secondary correction on the prior inverse scaling matrix. This step does not directly rely on current instantaneous observations but fully utilizes the posterior distribution of the variational Bayesian filter output to extract the inherent state uncertainty information. By incorporating the temporal series correlation of missing measurements and the spatial structural constraints among multiple sensors into the objective function construction of the EM algorithm, it can statistically optimally compensate for the information loss caused by missing data. The output of this step is a spatiotemporally enhanced prior inverse scaling matrix, whose accuracy is significantly better than estimates obtained solely from currently available measurements, providing high-quality prior input for the next round of filtering.

[0042] S105: Update the variational Bayesian robust student t-Kalman filter of this application, and iterate alternately with the modified prior inverse scaling matrix, outputting the state estimate, error covariance, and optimized noise covariance, including:

[0043] Using the corrected prior inverse scaling matrix as the current prior parameter, a variational Bayesian robust student t-Kalman filter update is performed; using the updated posterior distribution, the prior inverse scaling matrix is ​​corrected again using EM; the variational Bayesian update and EM prior correction are iterated alternately; after the iteration converges, the state estimate, error covariance, and optimized noise covariance are output.

[0044] Specifically, firstly, the prior inverse scaling matrices of the process noise covariance and the measurement noise covariance, corrected by the EM algorithm, are used as the prior input parameters for the variational Bayesian robust student t-Kalman filter at the current time. These are substituted into the corrected variational Bayesian robust student t-Kalman filter, and filtering time and measurement updates are performed to solve for the posterior distributions of the state, noise covariance, and auxiliary variables, yielding the updated posterior distribution. Subsequently, based on the updated posterior distribution, and considering the missing time-series information from multi-sensor measurements and sensor spatial structure constraints, the Q function of the expectation-maximization algorithm is reconstructed. The derivative of the Q function with respect to the prior inverse scaling matrix is ​​taken and set to zero, resulting in the maximum likelihood estimation update formula. This update formula is then used to perform EM correction on the prior inverse scaling matrices of the process noise covariance and the measurement noise covariance, completing a new round of prior parameter optimization. Furthermore, the variational Bayesian robust student t Kalman filter update step and the prior inverse scaling matrix EM correction step are executed alternately in a loop. In each loop, the prior inverse scaling matrix corrected in the previous round is used as the prior parameter for the next round of filter update. This allows the prior parameters to continuously drive the optimization of the posterior distribution, and the posterior distribution to continuously back-engineer the prior parameter correction, forming a closed-loop optimization mechanism in which the prior parameters and the posterior distribution iterate and improve with each other.

[0045] During the iteration process, the changes in state estimation, error covariance, and noise covariance are monitored in real time. When the parameter changes are less than a preset threshold and tend to stabilize, the iteration is considered to have converged. Finally, the final state estimate, state estimation error covariance, and process noise covariance and measurement noise covariance optimized by closed-loop iteration are output, completing the entire process of enhanced robust state estimation.

[0046] As one implementation method, the arrival and loss of measurements from multiple sensors in each dimension are modeled as a two-state Markov process, and a state transition matrix is ​​determined. Specifically, unlike traditional methods that often assume measurement loss follows an independent and identically distributed Bernoulli process, this embodiment fully considers the temporal correlation of measurement data loss in the actual physical system, i.e., the so-called "time inertia." In a multi-sensor localization network, data loss caused by environmental occlusion, communication link congestion, or sensor active energy-saving strategies often does not occur with high-frequency random jumps between adjacent time points, but rather exhibits a trend of state preservation. For example, when a target enters a building occlusion area, the measurement data of the relevant sensors will be continuously missing for multiple consecutive sampling periods; similarly, when the communication channel is interrupted due to interference, its recovery also requires a certain reconnection time. To accurately characterize this dynamic characteristic, this embodiment establishes a Markov chain containing two states, "arrival" and "loss," for each dimension of measurement for each sensor. The state transition matrix is ​​defined by two key parameters: loss probability and recovery probability. The loss probability describes the probability that the measurement arrived at the previous time point and is now missing, while the recovery probability describes the probability that the measurement was missing at the previous time point and is now back to normal measurement.

[0047] Furthermore, a measurement indicator variable is introduced, and the system observation equation with measurement missing is constructed using the Hadamard product form. Specifically, to mathematically unify the description of complete and incomplete measurements, this invention introduces a binary indicator variable vector with the same dimension as the measurement vector. Each element in this vector corresponds to a measurement channel, with a value of 1 indicating that data has arrived and a value of 0 indicating that it is missing. Based on this, the system observation equation no longer uses traditional piecewise functions or dimensionality reduction methods, but instead uses the Hadamard product (i.e., element-wise multiplication) to directly multiply the indicator variable vector with the ideal observation function. This construction method has significant technical advantages: on the one hand, it achieves complete decoupling of the measurement missing mechanism and the system state dynamics in terms of algebraic structure, ensuring that the form of the observation equation remains highly consistent with and without data missing, greatly simplifying the matrix operation complexity in subsequent posterior Cramero lower bound derivation and variational Bayesian filter updates; on the other hand, the Hadamard product form naturally supports asynchronous missing data processing for multidimensional sensor data, eliminating the need to rebuild the observation model for every possible combination of missing data, thus enhancing the algorithm's versatility and scalability for different sensor configurations and missing modes.

[0048] In this invention, the measurement arrival probability of the sensor is solved based on the state transition matrix and the system observation equation. Specifically, the measurement arrival probability is a key bridge connecting the underlying missing data model and the upper-level state estimation algorithm. In this invention, this probability is not preset as a fixed empirical parameter, but is dynamically calculated based on the established two-state Markov process. According to the properties of the state transition matrix, the transient measurement arrival probability at any time can be obtained recursively through the Chapman-Kolmogorov equation, or its steady-state distribution can be solved as prior information after the system has run for a sufficiently long time. This probability value directly reflects the confidence in obtaining effective measurement information at the current time and will be used for the weighting of the Fisher information matrix and the correction of prior parameters in variational Bayesian inference in subsequent steps. Compared with the existing technology that simply sets the noise variance of missing data to infinity or directly ignores the missing dimension, this embodiment retains the indirect information about the system state carried by the missing event itself (for example, continuous missing data may indicate that the target is in a specific area or that the sensor is in a specific failure mode) by accurately solving the measurement arrival probability. This approach not only improves the model's fitting accuracy to the real environment, but also lays a solid theoretical foundation for the subsequent derivation of the tight posterior Cramer-Rao lower bound, ensuring that the performance boundary of the enhanced robust state estimation method under extreme observation conditions is quantifiable and predictable.

[0049] As one implementation method, a posterior distribution result is obtained based on a variational Bayesian robust student t-Kalman filter. Specifically, this posterior distribution result serves as the data foundation for the expectation step (E-step) in the expectation-maximization algorithm, directly derived from the output of the aforementioned modified variational Bayesian robust student t-Kalman filter in the current iteration. This posterior distribution not only includes the estimated value of the target state and its error covariance matrix but also encompasses the posterior statistics of auxiliary variables and the posterior distribution parameters of the noise covariance matrix. In incomplete measurement scenarios, even with missing sensor data, the variational Bayesian framework can still infer a complete joint posterior distribution approximation solution using the remaining available measurements. This posterior distribution result carries the system's complete understanding of state uncertainty at the current moment, providing the necessary probability density input for constructing a Q-function containing complete data information, ensuring that the expectation-maximization algorithm still possesses a clear optimization objective under information-constrained conditions.

[0050] Based on the posterior distribution results, and combining the time-series information of missing measurements with the spatial structure constraints of the multi-sensor network, a Q-function for the expectation-maximization algorithm is constructed. Traditional Q-function construction typically relies solely on the actual measurement data received at the current moment. When measurements are missing, the corresponding likelihood term is often ignored or simplified, leading to low information utilization. In this embodiment, however, the Q-function construction explicitly embeds two types of spatiotemporal constraint information. In the time dimension, utilizing the state transition characteristics of a two-state Markov process, the measurement arrival patterns at historical moments are incorporated as prior knowledge into the expectation calculation of the log-likelihood of the complete data, enabling the Q-function to reflect the temporal inertia characteristics of missing measurements. In the spatial dimension, spatial structure constraints of the multi-sensor network are introduced, such as using spatial interpolation techniques like Kriging or Gaussian process regression, to construct a conditional probability model for missing measurements based on measurements from nearby normally functioning sensors. This approach does not simply fill in missing values ​​but transforms spatial correlation into a probability constraint term added to the Q-function. For example, when a sensor loses data due to obstruction, if its spatially adjacent sensors can still provide high-quality observations, the cross-correlation term calculated through the spatial covariance function will form an effective information compensation in the Q function.

[0051] Based on this, the maximum likelihood estimation update model is obtained by differentiating the Q function with respect to the prior inverse scaling matrix and setting the derivative to zero. This step corresponds to the maximization step (M-step) of the expectation-maximization algorithm. Since the Q function already incorporates spatiotemporal constraint information, its partial derivative expression with respect to the prior inverse scaling matrix no longer depends solely on the sum of squared residuals at the current time step, but also includes additional correction terms derived from temporal inertia and spatial correlation. By taking the partial derivatives of this enhanced Q function with respect to the prior inverse scaling matrices of the process noise covariance and the measurement noise covariance, and setting the derivatives to zero, the maximum likelihood estimation update formula incorporating spatiotemporal information gain can be analytically derived.

[0052] As one implementation method, the modified prior inverse scaling matrix is ​​used as the current prior parameter to perform variational Bayesian robust student t-Kalman filter updates. Specifically, this step constitutes a key connection point in the alternating iterative loop. This parameter transfer mechanism ensures that when the filter processes the data at the current time, the noise statistics it relies on have already incorporated historical time series information and the spatial structural constraints of neighboring sensors, thereby effectively overcoming the problem of insufficient or distorted prior information caused by measurement gaps.

[0053] Then, using the updated posterior distribution, the EM correction of the prior inverse scaling matrix is ​​performed again. Specifically, this corresponds to the feedback loop in the alternating iterative loop. After performing one update, the variational Bayesian robust student t-Kalman filter outputs the latest posterior distribution, which includes the mean of the state estimate, the error covariance, and the statistics of auxiliary variables. This posterior distribution reflects the system's latest understanding of the target state under the combined influence of the current prior parameters and actual observations (including missing modes). This updated posterior distribution is then re-input into the expectation-maximization module to construct a new round of Q-functions. Since the posterior distribution is more accurate than the previous one, the conditional expectation of missing data and the spatial interpolation weights calculated based on it will also be optimized, thus enabling the prior inverse scaling matrix obtained in the next EM correction to more closely approximate the real environmental noise characteristics. This two-way interactive mechanism of "VB updating the posterior and EM correcting the prior" forms a positively reinforcing closed-loop optimization loop, enabling the algorithm to continuously mine effective information from limited data under incomplete measurement conditions and gradually improve the compactness of the joint posterior distribution.

[0054] Based on this, variational Bayesian updates and EM prior corrections are iteratively performed alternately. Specifically, such as... Figure 1 As shown, the two steps described above are not executed in isolation, but are organized within a controlled loop structure. At each sampling moment or within each processing window, the algorithm repeatedly switches between the VB filtering step and the EM correction step until a preset convergence condition is met. This alternating iterative strategy mathematically guarantees the non-decreasing nature of the evidence lower bound (ELBO), meaning that each complete VB-EM cycle increases or remains unchanged the system's variational free energy, thus theoretically ensuring the algorithm's convergence and stability. Regarding the determination of iteration termination, this embodiment provides several optional implementation methods. A preferred method is to monitor the change in ELBO between two adjacent iterations, determining convergence when the change is less than a preset threshold; another method is to monitor the relative rate of change of the prior inverse scaling matrix or state estimate; a maximum number of iterations can also be set as a safety net to prevent getting stuck in an infinite loop under extreme abnormal conditions.

[0055] Finally, after iterative convergence, the algorithm outputs the state estimate, error covariance, and optimized noise covariance. Specifically, once the alternating iteration process meets the convergence condition, the algorithm locks the calculation result of the current round as the final output. The three output elements have clear physical meaning and application value: the state estimate provides the optimal inference values ​​for motion parameters such as target position and velocity, serving as direct input for downstream tasks such as navigation and tracking; the error covariance quantifies the uncertainty of the state estimate and can be used for subsequent trajectory association, sensor scheduling, or confidence assessment; the optimized noise covariance reflects the algorithm's adaptive learning result on the statistical characteristics of current environmental noise, and can be used not only for performance monitoring of the current filtering but also as prior information for the filter in the next time step to achieve parameter transfer across time steps. Compared to existing technologies that only output the state estimate, the complete output set provided in this embodiment offers richer information dimensions for upper-level decision-making in multi-sensor systems. For example, in autonomous driving scenarios, the optimized noise covariance can help the planning module determine the reliability of the current positioning result, thereby deciding whether to trigger a degradation strategy or request additional sensor redundancy.

[0056] During the alternating iteration process, the corrected prior inverse scaling matrix is ​​used as the prior parameter for the next round of variational Bayesian robust student t-Kalman filtering, forming a closed-loop iterative optimization of the prior parameters and the posterior distribution. The posterior distribution output by the variational Bayesian robust student t-Kalman filter is not only the result of state estimation, but also the information source for the expectation maximization algorithm to correct the prior parameters; and the prior inverse scaling matrix corrected by expectation maximization under spatiotemporal constraints is immediately injected back into the next round of variational Bayesian inference as a new prior input. This cyclical mechanism of "posterior-driven prior correction and prior-guided posterior update" enables the algorithm to continuously self-calibrate in each iteration, forming a positively reinforcing optimization loop.

[0057] In embodiments of the present invention, such as Figure 2 Furthermore, an enhanced robust state estimation device based on spatiotemporal constraint expectation maximization is provided, comprising:

[0058] Measurement Missing Measurement Modeling Unit 21 is used to construct an incomplete measurement model based on a two-state Markov process, perform time-inertia modeling for multi-sensor measurement missingness, and calculate the measurement arrival probability; including:

[0059] The state modeling module is used to model the arrival and absence of measurements from multiple sensors in various dimensions as a two-state Markov process and determine the state transition matrix.

[0060] The observation building module is used to introduce measurement indicator variables and construct system observation equations with missing measurements using the Hadamard product form;

[0061] The probability solving module is used to solve the measurement arrival probability of the sensor based on the state transition matrix and the observation equation of the system in this application.

[0062] Error lower bound derivation unit 22 is used to combine the incomplete measurement model of this application with the measurement arrival probability of this application to derive the posterior Cramer-Rao lower bound under the coupling condition of incomplete measurement and heavy-tailed noise, so as to quantify the state estimate of the error lower bound.

[0063] The filter construction unit 23 is used to use the posterior Cramerlow lower bound of this application as the accuracy constraint, adopt the inverse Wishart distribution as the conjugate prior of the noise covariance, realize the adaptive estimation of the noise covariance through moment matching, and construct a modified variational Bayesian robust student t-Kalman filter based on the incomplete measurement model of this application.

[0064] Prior matrix correction unit 24 is used to correct the prior inverse scaling matrix of the noise covariance based on the variational Bayesian robust student t-Kalman filter of this application; specifically including:

[0065] The acquisition module is used to obtain the posterior distribution results based on the variational Bayesian robust student t-Kalman filter of this application;

[0066] The construction module is used to construct the Q function of the expectation maximization algorithm based on the posterior distribution results of this application, combined with the time series information of missing measurements and the spatial structure constraints of the multi-sensor system.

[0067] The derivation module is used to differentiate the Q-function of this application with respect to the prior inverse scaling matrix and set it to zero to obtain the maximum likelihood estimation update model;

[0068] The correction module is used to update the prior inverse scaling matrix of the process noise covariance and the measurement noise covariance of the model correction using the maximum likelihood estimation.

[0069] The iterative operation output unit 25 is used to update the variational Bayesian robust student t-Kalman filter of this application, and alternately iterates with the modified prior inverse scaling matrix, outputting the state estimate, error covariance, and optimized noise covariance. During the alternating iteration process, the modified prior inverse scaling matrix is ​​used as the prior parameter of the next round of variational Bayesian robust student t-Kalman filter, forming a closed-loop iterative optimization of the prior parameters and the posterior distribution.

[0070] The iterative operation output unit 25 includes: a filtering update module, used to perform variational Bayesian robust student t-Kalman filtering update with the corrected prior inverse scaling matrix as the current prior parameter; a secondary correction module, used to perform EM correction of the prior inverse scaling matrix again using the updated posterior distribution; a cyclic iteration module, used to alternately iterate the variational Bayesian update and the EM prior correction; and a result output module, used to output the state estimate, error covariance, and optimized noise covariance after the iteration converges.

[0071] An enhanced robust state estimation device based on spatiotemporal constraint expectation maximization employs the aforementioned enhanced robust state estimation method based on spatiotemporal constraint expectation maximization. Its principle and process are the same as those of the enhanced robust state estimation method based on spatiotemporal constraint expectation maximization, and will not be repeated here.

[0072] Example:

[0073] This invention provides an enhanced robust state estimation method based on spatiotemporal constraint expectation maximization. First, an incomplete measurement system model based on a Markov process is established;

[0074] Consider the following general nonlinear stochastic system:

[0075]

[0076] in For discrete time, . For system status, For control. For system measurement. Nonlinear function. and Continuously differentiable. Consider and These are Gaussian process noise and heavy-tailed measurement noise, respectively, satisfying... The mean Gaussian distribution and the Student's t-distribution, i.e.

[0077]

[0078] in, and They are PNCM and MNCM, respectively, and both are positive definite matrices. These are the degrees of freedom parameters.

[0079] time The Dimensional measurement using indicator variables It means that, among them Indicates that the measurement has arrived, with a probability of . The specific expression is given in Section 3. This indicates a missing measurement, with a probability of . .when hour, Time MNCM is Otherwise, the position of the missing data in MNCM becomes Missing data corresponds to .therefore, The time measurement indicates the random vector as follows

[0080]

[0081] The incomplete measurements from multiple sensors are modeled as a two-state Markov process, with the transition matrix being:

[0082]

[0083] in Let it be its state space. and The first 3D measurement The probability of loss and the probability of recovery at time t. .make The actual time measurement is That is, for partially missing measurements, the system model... Become

[0084]

[0085] in This represents the Hadamard product. In practical measurement systems, the arrival of data from a sensor is often related to the data received in the previous moment. Its inertia makes it easy to maintain the data reception status from the previous moment, or at least avoid frequent switching, such as in cases of continuous occlusion. Therefore, compared to Bernoulli missing data models based on independent and identically distributed data, partially incomplete measurement models based on two-state Markov processes can provide a more accurate mathematical representation.

[0086] At the signal transmission level, the probability of missing measurement data may be related to the signal-to-noise ratio, and consequently, the communication distance; specifically, the probability of missing data increases with increasing distance. Furthermore, target occlusion, sensor failure, and deliberate data optimization or censoring to conserve sensor power can also lead to incomplete measurements. Therefore, it is necessary to study the properties of state estimation under incomplete measurements and methods to improve its performance.

[0087] PCRLB under incomplete measurements and heavy-tailed noise

[0088] To quantify the lower bound of the estimation error under the coupled effect of incomplete measurement and heavy-tailed measurement noise, we will derive the PCRLB under the Markov model when incomplete measurement and heavy-tailed noise coexist.

[0089] Because the derivation of traditional PCRLB relies on the assumption of regularity, i.e., that all state variables are continuously distributed, and in the case of incomplete measurements, the missing data variables are discrete 0-1 variables, traditional PCRLB cannot be directly applied. PCRLB is defined as state... unbiased estimator The lower bound of the variance, i.e.

[0090]

[0091] in The Fisher Information Matrix (FIM) is defined as follows:

[0092]

[0093] in For the second-order partial derivative operator with respect to the state, The joint PDF can be decomposed into the product of the likelihood PDF and the state transition PDF, i.e.

[0094]

[0095] FIM can then be decomposed into two parts.

[0096]

[0097] By decomposing, a priori information based on target dynamics and data information derived from measurement were distinguished. The prior information portion of FIM is defined as follows:

[0098]

[0099] In a nonlinear system with additive Gaussian noise, the state transition PDF is:

[0100]

[0101] For the system state model, perform a first-order Taylor expansion. ,in Let be the Jacobian matrix for the state transition. The linearized state transition PDF is approximately:

[0102]

[0103] Based on the results of the prior information section regarding FIM,

[0104]

[0105] in:

[0106]

[0107] The formula Substitution and combined achievable

[0108]

[0109] Lemma of matrix inversion , can be obtained

[0110]

[0111] Therefore, the prior information part of FIM It can be represented as

[0112]

[0113] Mode middle, The measurement information portion of FIM is defined as follows:

[0114]

[0115] Under incomplete measurement conditions, the measurement information portion is:

[0116]

[0117] Since the missing data from different measurement sensors follows an independent Markov process, the conditional PDF can be decomposed into:

[0118]

[0119] Take the logarithm of both sides and substitute it into the expression We can obtain:

[0120]

[0121] Measurement missing When it does not contribute to the measurement information, the above equation becomes

[0122]

[0123] Next consideration ,make Then by formula Its recursive relation can be obtained.

[0124]

[0125] In special circumstances, if It is a time-invariant matrix, i.e. Then the above equation has a closed-form solution.

[0126]

[0127] in The probability of arrival is measured at the initial time. When hour, Convergence to steady state probability .

[0128] In system model In this context, considering that measurements are obtained from different sensors, meaning that measurements in each dimension are independent of each other, measurement noise... The PDF is the product of the independent Student's t-distributions in each dimension, i.e.

[0129]

[0130] in, and The first Each sensor measures the scale parameters and degrees of freedom parameters of the noise. The likelihood PDF is then used. It can be represented as

[0131]

[0132] in, For the first dimensional measurement function, Let represent the gamma function. Since it is not an exponential family distribution, we can directly substitute it into... It is quite complex, so its Gaussian scaling mixture representation is adopted, and auxiliary variables are introduced. ,but

[0133]

[0134] in For shape parameters The rate parameter is Gamma PDF. Given auxiliary variables. Under these conditions, the conditional joint distribution is the product of independent Gaussian distributions:

[0135]

[0136] Substitute it into the formula achievable

[0137]

[0138] in For the first Dimensional measurement function with respect to The Jacobian matrix is ​​denoted as The above formula applies to Expected value

[0139]

[0140] in satisfy That is, its prior expectation Next, we calculate the posterior distribution. The expected value. According to Bayes' theorem, the posterior distribution is proportional to the product of the likelihood and the prior:

[0141]

[0142] Substituting the PDFs of the Gaussian and gamma distributions into the above equation:

[0143]

[0144] The above formula can be simplified to:

[0145]

[0146] Observing the above formula, it is precisely a kernel with a gamma distribution: Therefore, the posterior distribution is also a gamma distribution:

[0147]

[0148] Where the posterior shape parameter and posterior rate parameter They are respectively

[0149]

[0150] Therefore, posterior expectation

[0151]

[0152] The residual square term in the denominator It measures the degree of anomaly in the current measurement.

[0153] make for Time of the first Dimensional measurement residuals, comprehensive , , and Then, PCRLB under incomplete measurement and heavy-tailed noise conditions can be expressed as:

[0154]

[0155] Remark 1: The derived PCRLB quantifies the impact of incomplete measurements from some sensors and heavy-tailed noise on the accuracy of target state estimation. If all sensors have no measurement gaps and no measurement outliers, the measurement noise strictly follows a zero-mean Gaussian distribution, i.e. and , then the formula It degenerates into traditional PCRLB. In special cases, if the residuals of all sensors are so small that... Then by formula It can be seen that by leveraging the high reliability of robust identification measurements, thereby increasing the posterior weight (i.e., the information contribution of the corresponding measurement to the FIM), the probability model based on Student's t may provide a more compact PCRLB. This is especially relevant when some sensors do not perform complete measurements. When some sensors have abnormal measurement values, the formula is used. It can be known Both of these scenarios reduce the weight of the corresponding sensor measurements, i.e., their contribution to state estimation, thus reducing the measurement information portion of the FIM and increasing PCRLB, reflecting the additional uncertainty in state estimation caused by incomplete measurements or measurement anomalies.

[0156] Remark 2: The derived PCRLB can be extended to the case where each measurement sensor has multiple measurement channels. In this case, the measurement noise of each sensor is modeled as a multivariate Student's t distribution. Similarly, using Gaussian scale mixture representation, similar FIM and PCRLB can be derived.

[0157] Enhanced Robust State Estimation Based on Spatiotemporal Constraints (EM)

[0158] The existing VB-RSTKF uses the IW distribution as the prior conjugate distribution of the prediction error covariance matrix (PECM) and the MNCM, which is an effective method for handling heavy-tailed noise. However, its prediction error covariance depends on the nominal PNCM, which leads to a decrease in filtering accuracy when the prior information is inaccurate. To address this issue, we directly model the PNCM and MNCM as IW distributions and use the moment matching method to achieve closed-loop estimation of the PNCM and MNCM without prior information. In the case of incomplete measurements, we derive a modified VB-RSTKF, demonstrating that the existing VB posterior update framework does not fully utilize measurement information. Therefore, we propose an enhanced robust filter based on spatiotemporal constraint expectation maximization.

[0159] Corrected VB-RSTKF under incomplete measurement:

[0160] In nonlinear stochastic systems In the middle, the state prediction PDF satisfies

[0161]

[0162] in and These are the predicted state and PECM, respectively.

[0163] For PNCM and MNCM To perform adaptive estimation and ensure that the posterior PDF has the same form as the prior PDF, the inverse Wishart (IW) distribution is chosen as its conjugate prior distribution.

[0164]

[0165]

[0166] in The degree of freedom parameter is The inverse scaling matrix is The IW PDF, for A symmetric positive definite matrix, whose specific form is:

[0167]

[0168] in and These represent determinant and trace operations, respectively. .

[0169] Also using a heuristic method

[29] Through the forgetting factor ,use Posterior distribution at time 1 and Parameter update The prior parameters at time, i.e.

[0170]

[0171]

[0172] At this point, the calculation methods for the predicted state and PECM are as follows:

[0173]

[0174] in Here is the state transition matrix. .

[0175] The above are predictions of system state, PNCM, and MNCM, unaffected by incomplete measurements. Next, we will derive the VB posterior update in the case of missing measurements.

[0176] In the case of complete measurement, by formula Based on auxiliary random variables The likelihood PDF can be measured. Represented as a mixture of an infinite number of Gaussian distributions,

[0177]

[0178] In the case of incomplete measurement, Divided into and Two parts. For the measurement to be reached, the corresponding measurement function and MNCM are respectively and , For the missing measurement, the corresponding measurement function and MNCM are respectively and .at this time The time-likelihood PDF is based solely on arrival measurements and is rewritten as follows:

[0179]

[0180] based on The likelihood of a PDF can be expressed in Gaussian form.

[0181]

[0182] make To estimate its joint posterior distribution The approximate posterior distribution of each component can be calculated using the VB method.

[0183] The joint posterior PDF can be decomposed into a product of approximate posterior PDFs, and because It does not contribute to posterior updates, therefore...

[0184]

[0185] Based on existing VB methods, the estimation method involves minimizing the KL divergence between the approximate posterior PDF and the true posterior PDF, i.e.

[0186]

[0187] in,

[0188]

[0189] for and The KL divergence between two distributions measures how close they are. The optimal solution of the above equation satisfies...

[0190]

[0191] Joint distribution It can be broken down into

[0192]

[0193] The formula , and Substituting into the above equation, we get:

[0194]

[0195] Taking the logarithm of the above equation, we employ the moment matching method in the current Bayesian iteration, i.e., using point estimation. replace .but It can be represented as

[0196]

[0197] in To reach the dimension of the measurement, let the dimension of the missing measurement be . , .

[0198] Solving the equation using a fixed-point iteration method First, let , will Substitution , can be obtained in the first In the next iteration

[0199]

[0200] in:

[0201]

[0202] From the formula , Its parameters are updated to

[0203]

[0204] Next, let , will Substitution , can be obtained in the first In the next iteration

[0205]

[0206] in

[0207]

[0208] From the formula , Its parameters are updated to

[0209]

[0210] Similarly, let , will Substitution , can be obtained in the first In the next iteration

[0211]

[0212] in:

[0213]

[0214] From the formula , Its parameters are updated to

[0215]

[0216] Right now ,in Only when the measurement is reached... Partial measurement updates have been performed.

[0217] In update No. After the posterior distribution of the nth iteration, it can be seen that the corrected MNCM and PECM, as well as the updated gain, are...

[0218]

[0219] because Since some time-based measurements are unavailable, state updates are performed only based on available measurements, and the gain becomes...

[0220]

[0221] in, and It contains only the measurement matrix and the elements in the MNCM that correspond to the dimensions reaching the measurement. According to the standard EKF process, the following can be obtained: The parameters are updated to

[0222]

[0223] Right now .

[0224] Remark 3: From the formula , and It can be seen that, in the case of missing measurements, state prediction and The posterior update is consistent with the complete measurement, while the auxiliary variables... MNCM and status The posterior update is based solely on the arrived measurements, and this method of handling missing measurements does not fully utilize or even ignores the effective information contained in the missing variables. This paper proposes a strategy to improve the accuracy of incomplete measurement state estimation based on the EM algorithm, utilizing the time series and spatial structure information contained in the missing data and inferring the statistical probability of the missing data rather than making explicit predictions.

[0225] Enhanced robust filtering based on EM:

[0226] consider , The update formula for the posterior inverse scaling matrix , In this process, the prior inverse scaling matrix is ​​calculated based on the posterior inverse scaling matrix estimated only from the arrival measurements at the previous time step, and its inaccuracy affects the state estimation accuracy. Therefore, a Q-function is constructed based on the time series information constraints of the missing measurements and the spatial structure information constraints of the corresponding sensors. Then, the EM algorithm is used to achieve maximum likelihood estimation of the prior inverse scaling matrix, ultimately improving the estimation accuracy of the posterior inverse scaling matrix and the state.

[0227] make In the EM algorithm The maximum likelihood estimate is

[0228]

[0229] The Q function Let be the expectation of the conditional distribution of the log-likelihood function of the complete data with respect to the missing measures, auxiliary variables, and states. Indicates the first Parameter estimation for the next iteration.

[0230] The log-likelihood function of complete data can be decomposed into:

[0231]

[0232] Due to measurement Missing Not directly obtainable, considering measurement likelihood The system already includes time-series information about the states. Furthermore, to utilize potential spatial information, we model the posterior distribution of missing measurements using a kriging method based on spatial structure constraints. The kriging method models spatiotemporal random fields through a covariance function and can be used to describe the spatial correlation of measurements in sensor networks.

[37] .therefore,

[0233]

[0234] in,

[0235]

[0236] Therefore, the joint distribution can be written as:

[0237]

[0238] Taking the logarithm, we get:

[0239]

[0240] Without loss of generality, we assume that the noise of the missing sensor and the noise of the arriving sensor are uncorrelated, i.e. Then the above equation becomes

[0241]

[0242] Similarly, using fixed-point iteration... The update method remains unchanged. , will Substitution , can be obtained in the first In the next iteration

[0243]

[0244] in:

[0245]

[0246] From the formula , Its parameters are updated to

[0247]

[0248] Similarly, let , will Substitution , can be obtained in the first In the next iteration

[0249]

[0250] in The calculation method is the same However, the parameters are corrected to the formula. From the formula , Its parameters are updated to

[0251]

[0252] In update No. After the posterior distribution, the updated methods for the corrected MNCM and PECM are the same. ,in The gain update is corrected to...

[0253]

[0254] According to the standard EKF process, we can obtain... The posterior parameters are updated to

[0255]

[0256] Based on the aforementioned posterior distribution, the Q-function in the expectation step (E-step) of the EM algorithm is first constructed, using the equation... Only retain with The relevant terms are:

[0257]

[0258] The above formulas respectively for and Find the derivative and set it to zero.

[0259]

[0260] therefore, and In the The update formula in the next iteration is:

[0261]

[0262] Correction and replace and Subsequent VB iterations are then performed to reduce the impact of incomplete measurements on the accuracy of prior parameters and improve the state estimation accuracy. Thus, VB provides the posterior distribution for EM, and EM provides more accurate prior parameters for subsequent VB iterations. In each iteration, the two processes complement and alternate. The final state of the target can then be estimated. Error covariance And the parameters of PNCM and MNCM.

[0263] In summary, the proposed enhanced robust filtering (STEMERF) algorithm based on spatiotemporal constraint expectation maximization is shown in Table 1, and the detailed steps are shown in Algorithm 1.

[0264] Table 1 Flowchart of Algorithm 1

[0265] Remark 4: Regarding the selection of initial parameters,

[0266]

[0267] This makes , This is the nominal PNCM. Similarly, for the initial parameters of the MNCM,

[0268]

[0269] in and These are all algorithm hyperparameters.

[0270] Algorithm Performance Analysis

[0271] This application performs a performance analysis on the proposed algorithm, including convergence analysis and computational complexity analysis, and makes a theoretical comparison with existing methods.

[0272] Convergence analysis

[0273] First, define the lower bound of evidence (ELBO):

[0274]

[0275] Regarding the convergence of the proposed algorithm, the following theorem holds.

[0276] Theorem 1: In the alternating VB-EM iteration of Algorithm 1, ELBO is non-decreasing, i.e.

[0277]

[0278] in Indicates the first Variational posterior distribution of the VB iteration. Indicates the first Prior parameter estimation for each EM iteration.

[0279] prove: The posterior distribution is obtained by minimizing the KL divergence using the VB method, as shown in equation [equation missing]. and ,

[0280]

[0281] From the formula Japanese style ,

[0282]

[0283] because Since it is a constant, therefore, the first In the next iteration, given prior parameters In the case of the KL divergence minimization problem Equivalent to ELBO maximization, i.e.

[0284]

[0285] therefore, .

[0286] From the formula In the In this iteration, the Q function can be written as

[0287]

[0288] Decomposition of joint distribution The above formula can be written as

[0289]

[0290] Because of the given Down and Irrelevant, and The above formula can be written as

[0291]

[0292] From the formula and ,

[0293]

[0294] Therefore, in the first In each EM iteration, maximizing the Q function is equivalent to maximizing ELBO, i.e.

[0295]

[0296] therefore, In summary, we can obtain the formula. .

[0297] The existing VB-RSTKF ignores the influence of incomplete measurements on state estimation and estimates the joint posterior distribution only through VB iteration, i.e., fixing the prior parameters. This is a sub-method of the proposed method, and its... The ELBO of the next iteration is Another sub-method is to estimate the posterior distribution of the state solely through the EM method, i.e., fixing the posterior distributions of PNCM and MNCM, then its 1st... The ELBO of the next iteration is Regarding the performance of the proposed algorithm compared to existing methods and sub-methods, the following inferences can be drawn.

[0298] Inference 1: The ELBO of the proposed method is no less than that of any sub-method and the existing VB-RSTKF, i.e.

[0299]

[0300] Proof: By equation It can be known

[0301]

[0302] In addition, by formula It can be known

[0303]

[0304] Therefore, the conclusion Established.

[0305] Remark 5: From formula ,because Therefore, ELBO has an upper bound. Theorem 1 shows that ELBO is non-decreasing during the alternating iterations of VB and EM in the proposed algorithm, thus the algorithm converges. Furthermore, Corollary 1 shows that, under incomplete measurement, through posterior distribution updates and adaptive estimation of prior parameters, the ELBO of the proposed algorithm in each iteration is no less than that of any sub-method and existing methods, thereby providing a more accurate joint posterior distribution and higher state estimation accuracy.

[0306] Computational complexity analysis:

[0307] Complexity analysis primarily measures the number of floating-point operations, with a focus on the state dimension. Measurement dimensions and number of iterations Impact on computational burden.

[0308] The calculation is based on prior parameters, the predicted state, and the covariance of the prediction error, i.e., equation [equation missing]. , and The computational complexity of time updates is From the formula , , , , , It can be seen that in each iteration of the variational measurement update, PNCM Auxiliary variables and MNCM The computational complexity of updating the posterior distribution parameters is as follows: From the formula and It can be seen that the complexity of PECM update and gain calculation is O(n). .

[0309] From the formula The computational complexity of updating the posterior state and error covariance is O(n). From the formula The computational complexity of prior parameter correction based on EM is... .In summary, The total computational complexity of the proposed algorithm after the second iteration is:

[0310]

[0311] For the existing VB-RSTKF, when some measurements are missing, the computational complexity of inferring the joint posterior distribution using only available measurements and the VB method is O(n log n).

[0312]

[0313] Remark 6: The proposed algorithm introduces spatiotemporal modeling of missing measurements and the EM algorithm, which increases overhead, but the overall computational complexity is on par with existing robust filtering algorithms, mainly affected by the state dimension. Measurement dimensions and number of iterations The algorithm achieves joint modeling and estimation of incomplete measurements and heavy-tailed noise within an acceptable computational cost, improving the accuracy of state and parameter estimation under incomplete measurements.

[0314] Under the same measurement missing rate and noise conditions, the proposed algorithm outperforms other algorithms. This is because the EM algorithm based on missing measurements improves the accuracy of parameter and state estimation, and enhances robustness to missing measurements and heavy-tailed noise. When the measurement missing rate increases, other algorithms experience a significant performance drop because they only utilize available measurements from the remaining sensors, and the RMSE curve fluctuates considerably when the measurement missing rate is severe. SF-RNF, employing a multi-sensor sequential fusion strategy, exhibits better robustness to missing measurements than the other three comparative algorithms, while the multi-model interaction mechanism of PIMM-FEIOKF also makes its performance superior to MCIOKF, which is based solely on the maximum correlation entropy criterion. For the proposed STEMERF, the modeling mechanism for missing measurements and the iterative correction of noise parameters make the algorithm more robust and stable; therefore, when the measurement missing rate increases, the performance drop is smaller and the RMSE curve converges more smoothly.

[0315] Table 2. ARMSE of the estimated state for each algorithm

[0316] In addition, to evaluate and compare the computational efficiency of each algorithm, the average single-step running time of each algorithm was calculated, as shown in Table 3. The hardware environment was an Intel Core i9-12900H processor with a main frequency of 2.50GHz.

[0317] Table 3 Computation time per step of algorithms

[0318] Table 3 shows that KFTCM has the highest computational efficiency, but the true noise covariance is usually unknown in real-world scenarios. MCIOKF's iteration count depends on the kernel bandwidth, and each iteration has low complexity, thus its computational efficiency is high. PIMM-FEIOKF runs two IMM filters in parallel and incorporates two different information entropy criteria, while SF-RNF involves multi-sensor sequential fusion; both have high computational complexity. Due to the proposed EM-based prior parameter iterative correction, STEMERF's computational efficiency is slightly lower than RSTKF, but within the same order of magnitude, and does not affect the algorithm's real-time performance. This is consistent with theoretical analysis; the proposed algorithm improves estimation accuracy and robustness at the cost of acceptable computational complexity.

[0319] Furthermore, to evaluate and optimize parameters and Forgetting factors and number of iterations The impact on the performance of the proposed algorithm, In the case of, set respectively , ,as well as .

[0320] The proposed algorithm is applicable to different... , and It exhibits high estimation accuracy and relatively consistent estimation performance. With increasing variational Bayes iteration count... As increases, ARMSE generally shows a downward trend. (From formula...) and , and The prior strength of the IW distribution's degrees of freedom parameters affects the algorithm's performance; increasing it slightly improves the algorithm's performance. (From equation...) and , Inertia in noise prediction is controlled. Since the noise does not change drastically during the experiment, the estimation accuracy mainly depends on robustness to measurement gaps and heavy-tailed noise, which is determined by the algorithm's variational measurement updates and EM-based parameter corrections. Therefore, the parameters controlling the noise prediction... As the value increases, ARMSE decreases slightly, but the algorithm performance remains basically the same.

[0321] In summary, the proposed STEMERF integrates the EM algorithm based on missing measurement spatiotemporal constraints into a variational Bayesian noise adaptive framework, possessing robustness against missing measurements and heavy-tailed noise, and requiring no fine-tuning in practical measurement system deployments. This provides support for the algorithm's transferability and effectiveness in complex dynamic measurement systems.

[0322] To address existing problems, this invention proposes a moment-matching approximation method that utilizes the IW distribution as the conjugate prior for both the PNCM and MNCM to achieve closed-loop posterior estimation of PNCM and MNCM. A modified VB-RSTKF for incomplete measurements is derived, and theoretical analysis reveals the information bottleneck of existing VB update frameworks when handling incomplete measurements. To further address this issue, a Q-function is constructed based on the time-series information of missing measurements and the spatial structure information of multiple sensors. The EM algorithm is then used to correct the prior scale matrices of the estimated PNCM and MNCM, thereby improving state estimation accuracy and enhancing the robustness of the multi-sensor positioning system in complex measurement environments. The proposed algorithm achieves higher ELBO at the cost of an acceptable increase in computational complexity. This invention effectively improves the state estimation accuracy of multi-sensor systems in complex measurement environments or communication-constrained scenarios while ensuring real-time performance, and exhibits relatively consistent estimation performance across different algorithm parameters.

[0323] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for enhanced robust state estimation based on spatiotemporal constraint expectation maximization, characterized in that, include: An incomplete measurement model based on a two-state Markov process is constructed to model the time inertia of multi-sensor measurement gaps and calculate the measurement arrival probability. By combining the incomplete measurement model with the measurement arrival probability, the posterior Cramer-Rao lower bound under the condition of incomplete measurement coupled with heavy-tailed noise is derived to quantify the lower bound of the state estimation error. Using the posterior Cramerlow lower bound as the accuracy constraint, the inverse Wishart distribution is adopted as the conjugate prior of the noise covariance. Adaptive estimation of the noise covariance is achieved through moment matching, and a modified variational Bayesian robust student t-Kalman filter is constructed based on the incomplete measurement model. Based on the variational Bayesian robust student t-Kalman filter, the prior inverse scaling matrix of the noise covariance is corrected; The variational Bayesian robust student t-Kalman filter is updated and iterated alternately with the modified prior inverse scaling matrix to output the state estimate, error covariance, and optimized noise covariance.

2. The method for enhanced robust state estimation based on spatiotemporal constraint expectation maximization according to claim 1, characterized in that, The construction of an incomplete measurement model based on a two-state Markov process, performing time-inertia modeling for multi-sensor measurement gaps, and calculating the measurement arrival probability includes: The arrival and absence of measurements from multiple sensors in various dimensions are modeled as a two-state Markov process, and the state transition matrix is ​​determined. By introducing measurement indicator variables, the system observation equations with measurement missing are constructed using the Hadamard product form; Based on the state transition matrix and the system observation equation, the measurement arrival probability of the sensor is solved.

3. The method for enhanced robust state estimation based on spatiotemporal constraint expectation maximization according to claim 1, characterized in that, The step of correcting the prior inverse scaling matrix of the noise covariance based on the variational Bayesian robust student t-Kalman filter includes: Based on the variational Bayesian robust student t-Kalman filter, the posterior distribution result is obtained; Based on the posterior distribution results, and combined with the time series information of missing measurements and the spatial structure constraints of the multi-sensor system, the Q function of the expectation maximization algorithm is constructed. The maximum likelihood estimation update model is obtained by taking the derivative of the Q function with respect to the prior inverse scaling matrix and setting it to zero. The prior inverse scaling matrix of the process noise covariance and measurement noise covariance is updated using the maximum likelihood estimation method.

4. The method for enhanced robust state estimation based on spatiotemporal constraint expectation maximization according to claim 3, characterized in that, The update of the variational Bayesian robust student t-Kalman filter, iterated alternately with the modified prior inverse scaling matrix, outputs state estimates, error covariance, and optimized noise covariance, including: Using the corrected prior inverse scaling matrix as the current prior parameter, perform variational Bayesian robust student t-Kalman filter update; Using the updated posterior distribution, the EM correction of the prior inverse scaling matrix is ​​performed again; The variational Bayes update and EM prior correction are iterated alternately. After the iteration converges, the output states are estimated, the error covariance is calculated, and the optimized noise covariance is calculated.

5. The method for enhanced robust state estimation based on spatiotemporal constraint expectation maximization according to claim 4, characterized in that, During the alternating iteration process, the corrected prior inverse scaling matrix is ​​used as the prior parameter for the next round of variational Bayesian robust student t-Kalman filtering, forming a closed-loop iterative optimization of the prior parameters and the posterior distribution.

6. A device for enhanced robust state estimation based on spatiotemporal constraint expectation maximization, characterized in that, include: The measurement missing modeling unit is used to construct an incomplete measurement model based on a two-state Markov process, perform time inertial modeling of multi-sensor measurement missing, and calculate the measurement arrival probability. The error lower bound derivation unit is used to combine the incomplete measurement model with the measurement arrival probability to derive the posterior Cramer-Rao lower bound under the coupling condition of incomplete measurement and heavy-tailed noise, so as to quantify the state estimate of the error lower bound. The filter construction unit is used to use the posterior Cramerlow lower bound as the accuracy constraint, adopt the inverse Wishart distribution as the conjugate prior of the noise covariance, realize the adaptive estimation of the noise covariance through moment matching, and construct a modified variational Bayesian robust student t-Kalman filter based on the incomplete measurement model. The prior matrix correction unit is used to correct the prior inverse scaling matrix of the noise covariance based on the variational Bayesian robust student t-Kalman filter. The iterative operation output unit is used to update the variational Bayesian robust student t-Kalman filter and alternately iterate with the modified prior inverse scaling matrix to output the state estimate, error covariance and optimized noise covariance.

7. The apparatus for enhanced robust state estimation based on spatiotemporal constraint expectation maximization according to claim 6, characterized in that, The measurement missing modeling unit includes: The state modeling module is used to model the arrival and absence of measurements from multiple sensors in various dimensions as a two-state Markov process and determine the state transition matrix. The observation building module is used to introduce measurement indicator variables and construct system observation equations with missing measurements using the Hadamard product form; The probability solving module is used to solve for the measurement arrival probability of the sensor based on the state transition matrix and the system observation equation.

8. The apparatus for enhanced robust state estimation based on spatiotemporal constraint expectation maximization according to claim 6, characterized in that, The prior matrix correction unit includes: The acquisition module is used to obtain the posterior distribution result based on the variational Bayesian robust student t-Kalman filter; The construction module is used to construct the Q function of the expectation maximization algorithm based on the posterior distribution results, combined with the time series information of missing measurements and the spatial structure constraints of the multi-sensor system. The derivation module is used to differentiate the Q function with respect to the prior inverse scaling matrix and set it to zero to obtain the maximum likelihood estimation update model; The correction module is used to update the prior inverse scaling matrix of the process noise covariance and the measurement noise covariance of the model correction using the maximum likelihood estimation.

9. The apparatus for enhanced robust state estimation based on spatiotemporal constraint expectation maximization according to claim 8, characterized in that, The iterative operation output unit includes: The filter update module is used to perform variational Bayesian robust student t-Kalman filter update with the corrected prior inverse scaling matrix as the current prior parameter; The secondary correction module is used to perform EM correction on the prior inverse scaling matrix again using the updated posterior distribution; The iterative loop module is used to alternately iterate the variational Bayes update and the EM prior correction. The results output module is used to output the state estimate, error covariance, and optimized noise covariance after iterative convergence.

10. The apparatus for enhanced robust state estimation based on spatiotemporal constraint expectation maximization according to claim 9, characterized in that, During the alternating iteration process, the corrected prior inverse scaling matrix is ​​used as the prior parameter for the next round of variational Bayesian robust student t-Kalman filtering, forming a closed-loop iterative optimization of the prior parameters and the posterior distribution.

Images