Filtering freeze detection recovery method and system based on innovation norm autocorrelation

Abstract

The application discloses a filter freezing detection and recovery method and system based on innovation norm autocorrelation, and belongs to the technical field of nonlinear state estimation and adaptive filtering. In view of the adaptive freezing problem caused by serious underestimation of process noise covariance Q in a variational Bayesian process noise adaptive filter, the application appends an innovation vector to a sliding window buffer at each filter recursion time, calculates a lag-1 autocorrelation coefficient of an innovation norm sequence, determines that a freezing event occurs when the absolute value of the coefficient is greater than a preset threshold and a freezing flag is not set, executes a Q matrix multiplicative reset and sets the freezing flag, and resets the freezing flag when a continuous normal frame count reaches a preset reset frame number. The application also provides a system for implementing the method. The application can be applied to nonlinear system state estimation of motion tracking, vehicle positioning, intelligent manufacturing dynamic parameter identification and other systems containing unknown time-varying process noise, can effectively detect and recover a freezing state, and improves filtering precision and robustness.

Description

Technical Field

[0001] This invention relates to the field of nonlinear state estimation and adaptive filtering, and to industrial Internet of Things (IIoT) information sensing, sensing technology services, and industrial information and data processing services. More specifically, this invention relates to a method and system for filtering, freezing detection, and recovery based on innovation norm autocorrelation. Background Technology

[0002] Volatile Kalman filters and their nonlinear extensions are widely used in state estimation fields such as industrial workpiece tracking, vehicle positioning, and dynamic parameter identification in intelligent manufacturing. Their basic assumption is that the process noise covariance matrix Q is known a priori. However, in practical engineering, Q is often impossible to obtain precisely and may exhibit time-varying characteristics due to changes in target motion modes, environmental conditions, or model simplification. When Q is severely underestimated, the filter over-relys on inaccurate dynamic model predictions, leading to a significant decrease in state estimation accuracy or even divergence.

[0003] To address the aforementioned issues, existing research has incorporated variational Bayesian inference into the Kalman filtering framework. This involves introducing a conjugate prior distribution onto the unknown noise covariance matrix, simultaneously updating the state and noise statistics during the filtering process. For example, Chinese patent CN108763167A, "An Adaptive Filtering Method Based on Variational Bayesian Principles," introduces latent variables and uses an inverse Wissaud distribution to model the process noise covariance a priori. Within the variational Bayesian framework, iterative mechanisms are used to jointly optimize the target state, process noise covariance, and latent variables to improve target tracking accuracy. This method effectively enhances the accuracy of state estimation through iterative optimization.

[0004] In the engineering implementation of the variational Bayesian Q-adaptive method, to avoid Q-estimate divergence, a safety strategy combining normalized innovation squared gating and dead-zone protection is typically introduced. This strategy controls the triggering of Q-updates by comparing the matching ratio of the empirical innovation covariance to the theoretical innovation covariance within the sliding window: when the ratio is within a preset dead zone, Q-updates are suppressed to prevent over-adaptation; only when the ratio exceeds the dead zone is an exponentially weighted recursive update performed with a NIS-gated learning rate. This design effectively maintains filter stability when the initial Q-estimate is relatively accurate.

[0005] However, in practical engineering applications, designers often set Q to a relatively small value initially to avoid overfitting to process noise, due to conservative considerations. In this case, a failure mode known as adaptive freezing is observed: Q estimation almost stagnates at the initial underestimation during long-term recursion, the filter actually operates in a severely mismatched state, but the VB-Q adaptive mechanism is never effectively triggered. This phenomenon arises from the following mechanism: when Q is underestimated, the state prediction covariance collapses, leading to an underestimation of the theoretical innovation covariance; at the same time, the increase in the actual innovation amplitude causes the empirical innovation covariance to expand accordingly. The result of the synchronous scaling of the two is that the matching ratio can still be maintained within the dead zone under various mismatch conditions. More specifically, in application scenarios dominated by measurement noise, the growth trend of the suppressed term in the theoretical innovation covariance and the empirical innovation covariance is approximately proportional, making it difficult for the ratio of their traces to escape the dead zone, thus triggering zero VB-Q updates. This phenomenon is particularly prominent in scenarios with relatively large measurement noise or weak system observability.

[0006] To address the aforementioned issues, existing technologies have attempted to estimate noise statistics using the autocorrelation function of the innovation sequence. See the *Journal of East China University of Science and Technology*, 2004, for an improved adaptive estimation method for filter noise models. This method directly estimates the noise covariance matrices Q and R by calculating the autocorrelation and cross-correlation of each component of the innovation vector. Filter fault detection methods based on innovation whitening tests, such as the chi-square test and sequential probability ratio test, have been widely used to detect whether filters are diverging or whether measurements are abnormal. For example, Chinese patent CN122172229A describes an adaptive filtering method based on sliding window innovation evaluation and joint constraints. This method uses parallel calculation of theoretical innovation covariance and sliding window empirical innovation covariance, dynamically quantifies their mismatch to generate a real-time risk assessment index for signal anomalies, and adjusts the measurement noise covariance accordingly to suppress the influence of abnormal signals.

[0007] However, none of the above methods can solve the problem of freezing detection in VB-Q adaptive mechanisms under severe Q underestimation scenarios. Noise estimation methods based on innovation autocorrelation use ACF as an estimation tool for the noise covariance matrix rather than a diagnostic tool for the frozen state. Their computational objects and functional purposes are different from the problems faced by those skilled in the art. Fault detection methods based on innovation whitening tests can determine whether the filter has statistical inconsistencies, but cannot distinguish whether the inconsistency is a frozen state caused by Q underestimation or a temporary high NIS caused by target motion, because NIS may show abnormalities in both cases. The gating and dead-zone protection introduced in existing VB-Q adaptive methods, while solving the over-adaptation problem, constitute a bottleneck that causes the adaptation to fail when the initial Q setting is severely mismatched. Therefore, there is a lack of a reliable method for detecting the frozen state of VB-Q adaptive mechanisms in the existing technology. Summary of the Invention

[0008] One object of the present invention is to solve at least the above-mentioned problems and to provide at least the advantages that will be described later.

[0009] To achieve these objectives and other advantages according to the present invention, a method for filtering freeze detection recovery based on novel norm autocorrelation is provided, comprising:

[0010] S1. At each filtering iteration, obtain the current frame information vector output after the measurement update of the capacitive Kalman filter, and append the current frame information vector to the sliding window buffer. When the length of the sliding window buffer exceeds the preset window length K, remove the earliest information vector that entered, so that the sliding window buffer always keeps the information vector of the most recent K frames.

[0011] S2. When the sliding window buffer accumulates to K frames of innovation vectors, extract the Euclidean norm of each frame's innovation vector from the sliding window buffer, construct a sequence of innovation norms of length K, and calculate the lag-1 autocorrelation coefficient of the innovation norm sequence. ;

[0012] S3, lagged autocorrelation coefficient The absolute value is compared with a preset threshold τ, when Furthermore, if the freeze flag is not set, a freeze event is determined to have occurred, and a multiplicative reset of the noise covariance matrix is ​​performed. , where κ>1 is the multiplicative reset factor, and the sliding window buffer is cleared and the freeze flag is set;

[0013] S4. When the freeze flag is set, determine the hysteresis autocorrelation coefficient of the current frame in each frame.

[0014] Does it meet the requirements? If the condition is met, the consecutive normal frame count will be incremented. If the consecutive normal frame count reaches the preset reset frame number N, the count will be incremented. reset If any frame freezes, the freeze flag will be reset and the continuous normal frame counter will be reset to zero; if any frame freezes... Then the continuous normal frame counter will be reset to zero.

[0015] Preferably, the autocorrelation coefficient with lag 1 is... Calculate using the following formula:

[0016]

[0017] Where, γ i Let i be the information vector of the i-th frame in the sliding window buffer. Let be the Euclidean norm of the information vector of the i-th frame. Let be the mean of the new norm sequence.

[0018] Preferably, the preset threshold τ and the preset window length K satisfy a statistically significant relationship:

[0019]

[0020] in, τ represents the standard normal two-sided quantile, α is the significance level for detection, and for K=20, τ=0.3.

[0021] Preferably, before step S2, a whitening test is included as a preliminary screening step, specifically including:

[0022] Calculate the paired average inner product of the innovation vectors of all adjacent frames in the sliding window buffer. If the paired average inner product is less than the whitening test threshold, the current innovation sequence is determined to satisfy the white noise hypothesis, and step S2 is skipped directly to step S4 without performing freeze detection. Step S2 is only performed when the paired average inner product is greater than or equal to the whitening test threshold, where the whitening test threshold is equal to... .

[0023] Preferably, the paired average inner product is calculated according to the following formula:

[0024]

[0025] Among them, c k Let be the whitening test statistic at time k. Let γ be the new information vector. i transpose, γ i+1 It is the information vector of the (i+1)th frame in the sliding window buffer.

[0026] Preferably, each frame also includes a variational Bayesian process noise covariance continuous adaptive update step, specifically including:

[0027] Calculate the empirical innovation covariance matrix S of the innovation vector within the current sliding window. emp And obtain the theoretical information covariance matrix S calculated recursively by Kalman filtering. k , ,in, H is the state prediction covariance matrix. k For the measurement matrix, R k The measurement noise covariance matrix;

[0028] Calculate the scalar matching ratio r between the empirical innovation covariance matrix and the theoretical innovation covariance matrix. k , ,in Represents the trace of a matrix;

[0029] The preset dead zone range is [0.7, 1.3]. When the matching ratio rk When the noise exceeds the preset dead zone, the process noise covariance matrix is ​​updated using the following exponentially weighted recursive formula. :

[0030]

[0031] Among them, effective learning rate η0 is the base learning rate. This is the exponential moving average of the normalized squared innovation. Let p be the p-quantile of the chi-square distribution with m degrees of freedom;

[0032] When the matching ratio r k When within the preset dead zone range, the process noise covariance matrix remains unchanged, i.e. .

[0033] Preferably, the multiplicative reset factor κ = 3.0, the preset window length K = 20, and the preset reset frame number N are set. reset =10.

[0034] Preferably, the preset damping index is set to 0.2.

[0035] This invention provides a filtered freeze detection and recovery system based on innovation norm autocorrelation, used to implement the filtered freeze detection and recovery method, comprising:

[0036] The innovation buffer maintenance module is used to obtain the current frame innovation vector output after the measurement update of the volumetric Kalman filter at each filtering recursion time, and append the current frame innovation vector to the sliding window buffer. When the length of the sliding window buffer exceeds the preset window length K, the earliest entering innovation vector is removed, so that the sliding window buffer always keeps the innovation vector of the most recent K frames.

[0037] The autocorrelation coefficient calculation module is used to extract the Euclidean norms of each frame's innovation vector from the sliding window buffer when the accumulated innovation vectors of the sliding window buffer reach K frames, construct an innovation norm sequence of length K, and calculate the lag-1 autocorrelation coefficient of the innovation norm sequence. ;

[0038] The freeze determination and reset module is used to freeze the autocorrelation coefficient with 1 lag. The absolute value is compared with a preset threshold τ, when Furthermore, a freeze event is determined to have occurred when the freeze flag is not set, and a multiplicative reset of the noise covariance matrix is ​​performed during the execution process. , where κ>1 is the multiplicative reset factor, and the sliding window buffer is cleared and the freeze flag is set;

[0039] The hysteresis reset state machine module is used to determine the hysteresis autocorrelation coefficient of the current frame in each frame when the freeze flag is set. Does it meet the requirements? If the condition is met, the consecutive normal frame count will be incremented until the consecutive normal frame count reaches the preset reset frame number N. reset The freeze flag will be reset and the continuous normal frame counter will be reset to zero; if any frame shows a freeze flag, the freeze flag will be reset and the continuous normal frame counter will be reset to zero. Then the continuous normal frame counter will be reset to zero.

[0040] The present invention has at least the following beneficial effects:

[0041] First, this invention effectively solves the technical problem that existing VB-Q adaptive mechanisms cannot identify adaptive frozen states when the process noise covariance Q is severely underestimated by introducing the innovation norm lag 1 autocorrelation coefficient as a statistical detection quantity for the frozen state. Unlike traditional NIS gating, which can only detect abnormal innovation amplitudes but cannot distinguish between normal high NIS and frozen state high NIS, this invention utilizes the essential change in the correlation of the innovation sequence under Q mismatch conditions to directly capture the negative autocorrelation characteristics caused by overshoot-compensation, providing a dedicated detection index with clear statistical significance for the frozen state. This fills the technical gap in existing adaptive filtering for frozen state identification and can be applied to the fields of industrial IoT information sensing and sensing technology services and industrial information and data processing services.

[0042] Secondly, this invention establishes a hierarchical statistical detection framework based on whitening test and lag 1 autocorrelation coefficient calculation, which solves the technical problem that existing methods cannot effectively distinguish the causes of filter mismatch. The whitening test, as a pre-screening step, uses the inner product of adjacent innovation vectors to determine whether the innovation sequence deviates from the white noise assumption, avoiding redundant calculations when the filter is working normally and greatly reducing detection overhead. The complete lag 1 autocorrelation coefficient calculation is only initiated after non-white noise features are detected, forming a progressive diagnostic logic from anomaly pre-detection to freeze confirmation, ensuring that the detection results have both statistical reliability and computational efficiency.

[0043] Third, this invention solves the technical problem that existing VB-Q adaptive filters cannot autonomously recover from a frozen state through the coordinated operation of a multiplicative reset mechanism and a hysteresis reset state machine. Unlike continuous VB-Q adaptive filters, which can only perform incremental Q updates and whose updates completely stagnate under dead zone protection, this invention performs a multiplicative amplification reset of the Q matrix after detecting a frozen event, breaking through the dead zone barrier in a discrete jump manner to allow the filter to re-enter the adaptive range. At the same time, it implements an asymmetric recovery mechanism of single-frame triggering and continuous confirmation through a hysteresis reset state machine, effectively suppressing repeated false triggers caused by transient fluctuations after reset, and constructing a complete closed loop of detection-reset-confirmation-recovery technology to ensure that the frozen state can be reliably released and will not frequently malfunction.

[0044] Fourth, the filter freeze detection and recovery system provided by this invention implements the above method in the form of functional modules through the structured design of the information buffer maintenance module, the autocorrelation coefficient calculation module, the freeze determination and reset module, and the hysteresis reset state machine module. Each module corresponds to the core steps such as sliding window buffer maintenance, hysteresis 1 autocorrelation coefficient calculation, freeze event determination and Q multiplicative reset, and continuous normal frame counting and freeze flag reset, forming a complete signal processing link from data buffering, feature extraction, state decision to reset confirmation, providing a clear and reproducible technical solution for engineering deployment.

[0045] Other advantages, objectives and features of the present invention will become apparent in part from the following description, and in part from those skilled in the art through study and practice of the invention. Attached Figure Description

[0046] Figure 1 The flowchart of the ACF-VB-Q autocorrelation coefficient freezing detection and mitigation algorithm is shown in one of the technical solutions of the present invention.

[0047] Figure 2 This is a NIS saturated thermogram of one of the technical solutions of the present invention;

[0048] Figure 3 This is a heatmap of the effective learning rate decay of one of the technical solutions of the present invention;

[0049] Figure 4 This is a scatter plot of the convergence time scaling law of one of the technical solutions of the present invention;

[0050] Figure 5 This is a bar chart comparing the ACF freeze relief performance of one of the technical solutions of the present invention;

[0051] Figure 6 This is a double logarithmic scatter plot of FI and M, representing one of the technical solutions of the present invention.

[0052] Figure 7 This is a stacked histogram showing the ratio of steady-state Q-estimate to the true value for one of the technical solutions of the present invention.

[0053] Figure 8 This is one of the technical solutions of the present invention. Time series evolution curve;

[0054] Figure 9 This is a timing curve of the position error of one of the technical solutions of the present invention;

[0055] Figure 10 This is a learning rate sensitivity analysis and comparison diagram of three-way algorithms for one of the technical solutions of the present invention. Detailed Implementation

[0056] The present invention will now be described in further detail with reference to examples, so that those skilled in the art can implement it based on the description.

[0057] It should be noted that, unless otherwise specified, the experimental methods described in the following implementation plan are all conventional methods, and the reagents and materials described are all commercially available unless otherwise specified.

[0058] like Figures 1 to 10 As shown, this invention provides a method for filtering, freezing detection, and recovery based on innovation norm autocorrelation, which can serve the fields of industrial artificial intelligence network system services and industrial Internet of Things information sensing and perception technology services, and includes the following steps:

[0059] S1, Standard Kalman Filter Prediction-Update Cycle: A capacitive Kalman filter (CKF) is used to perform standard time update (prediction) and measurement update (correction) steps to obtain the state estimate at the current time. and new information vector ;

[0060] S2. Calculation and conditional update of the noise covariance ratio in variational Bayesian processes; calculation of the scalar matching ratio between the empirical innovation covariance matrix and the theoretical innovation covariance matrix:

[0061]

[0062] If r k If the range exceeds the preset dead zone, such as [0.7, 1.3], the process noise covariance matrix is ​​updated using a variational Bayesian update mechanism. That is, the noise covariance of the VB-Q mechanism update process:

[0063]

[0064]

[0065] Where η0 is the basic learning rate. This is the exponential moving average of the normalized squared innovation. Let p be the p-quantile of the chi-square distribution with m degrees of freedom;

[0066] S3. Maintain the information buffer and set the current information vector γ. k Add to the sliding window buffer, retaining the K most recent updates; K=20 is recommended.

[0067]

[0068] Preferably, a whitening check gate is included. Before performing 1-hysteresis autocorrelation calculation, a whitening check is preferably performed first. The whitening check is an autocorrelation freeze detection, i.e., a recommended pre-step for ACF freeze detection. In embedded deployment scenarios with limited computing resources, the whitening check can be skipped by configuring a switch to directly enter the subsequent complete autocorrelation calculation process. At this time, the computational overhead of ACF freeze detection increases by about 5 to 20 times, depending on the Q accuracy, but the freeze detection function is not affected. For normal deployment, it is recommended to enable the whitening check to significantly reduce the average computational load. The complete autocorrelation calculation process is only entered when the innovation sequence does not meet the white noise assumption.

[0069] The testing method calculates the paired mean inner product of all adjacent information pairs (lag 1) in the buffer, using the same time scale (lag 1) as the autocorrelation detection measurement in subsequent steps, thus providing statistical pre-screening for autocorrelation detection.

[0070]

[0071] Where γ i and γ i+1 For continuous pairs of innovation vectors indexed in the buffer, the correlation of vectors with a measurement lag of 1 is tested. This is similar to the correlation test at the same time scale used in the subsequent autocorrelation test. In the case of general random vectors, the vectors are not linearly correlated, meaning that the pairwise mean inner product being close to zero does not strictly imply that their norm is not autocorrelated. This implied relationship requires the assumption that "the innovation vector approximately follows a joint Gaussian distribution" to hold. The independence of each component of a Gaussian random vector is equivalent to no correlation. Under the normal operating conditions of Kalman filtering near steady state, the Gaussianity assumption of the innovation vector is a reasonable approximation. The innovation is generated by a linear / linearized combination of Gaussian process noise and Gaussian measurement noise. Therefore, in practice, the inference chain of no correlation in vectors ⇒ no autocorrelation in norm holds true under most normal operating conditions. This pre-test constitutes a statistical pre-screening for autocorrelation freeze detection, rather than an absolute logical guarantee.

[0072] like , If there is no significant linear correlation between adjacent innovations, the innovation sequence is determined to satisfy the white noise assumption. At this point, the filter is in a statistically consistent state, and the probability of freezing is extremely low. Steps S2-S3 are skipped directly, and freeze detection is not performed. This significantly reduces unnecessary ACF computation overhead when Q is correct; in actual tests, approximately 80%-95% of frames are skipped. Freeze detection is only performed when the whitening test fails. Only when the presence of non-white noise correlation in the innovation sequence is indicated will the complete ACF calculation process begin;

[0073] Under the null hypothesis of white noise, the expectation of the inner product of adjacent innovations is zero, and the innovations at different times are independent, with variances... ,in Let the true covariance matrix of the innovation vector be a conservative approximation. Using the normalized new information estimate, we get Standard deviation For typical measurement dimensions m=3 and K=20, the standard deviation is approximately 0.397, the threshold is 0.459, and the standard deviation is approximately 1.16 standard deviations. The one-sided p-value is approximately 0.12, achieving a balance between false alarm rate and detection sensitivity. When the measurement dimensions differ significantly, the coefficients can be adjusted or a dimension-normalized version can be used. The recommended approach is to use [the specified method / technology]. This is consistent with the ACF detection window, and the threshold is approximately 0.459 at this time.

[0074] S4. Calculation of the lag-1 autocorrelation coefficient of the innovation norm: When the buffer length is ≥ K, calculate the lag-1 autocorrelation coefficient of the innovation norm. :

[0075]

[0076] in, For the new information vector The Euclidean norm, This represents the mean of the innovation norm within the sliding window buffer.

[0077] When the Kalman filter uses the correct Q, the innovation sequence constitutes white noise with a lag-1 autocorrelation coefficient. The information should be close to zero. When Q is underestimated, the filter overcorrects, introducing a negative hysteresis autocorrelation of 1 into the innovation norm. Large innovation leads to overcorrection, and the next step generates a compensating innovation in the opposite direction, forming an overshoot-compensation negative correlation pattern. When Q is overestimated, the prediction covariance... Expansion, high Kalman gain, excessive trust in measurements by the filter and underestimation of dynamic model predictions, high gain causes the state estimate to over-follow noisy measurements. Random noise in the current frame's measurements enters the state estimate through high gain, causing the predicted value of the next frame to deviate from the true state. This results in the next frame's innovation deviating in the same direction as the current frame's innovation, either both being positive or both being negative, forming a positive hysteresis 1 autocorrelation in the innovation norm sequence. This mechanism is symmetrical to the negative autocorrelation and overshoot-compensation when Q is underestimated: Q underestimation → low gain → overcorrection → overshoot-compensation → negative correlation; Q overestimation → high gain → noise following → same-direction deviation → positive correlation. In engineering, a two-sided test is used ( Simultaneously capturing two mismatch modes and prioritizing detection sensitivity is a statistical characteristic unique to the frozen state, which contrasts sharply with the case when Q is correct.

[0078] S5. Freeze determination and Q reset based on autocorrelation threshold, where the preset threshold τ and the sliding window size K satisfy a statistically significant relationship: ,in The two-sided quantile of the standard normal distribution is given, and α is the significance level for detection. Based on the statistical property that the autocorrelation coefficient of the lag 1 sample of white noise sequence approximately follows a normal distribution with a mean of zero and a variance of 1 / K, it should be noted that the classical derivation of Bartlett's formula assumes that the sequence itself is independent and identically distributed Gaussian white noise. In this invention, the ACF calculation object is the innovation norm sequence, not the innovation vector itself, and its distribution is chi-square correlation. Therefore, the variance is an approximate result, which is sufficient to distinguish white noise from correlated sequences in engineering practice. The selection of τ has a clear statistical significance reference meaning. For the recommended value K=20, τ=0.3, corresponding to z approximately equal to 1.34, a two-sided p-value approximately equal to 0.18, a confidence level of approximately 82%, and a one-sided p-value approximately 0.09, which is sufficient to distinguish white noise from correlated sequences in statistical practice. In engineering, a two-sided test is used to simultaneously capture the innovation oscillation caused by positive autocorrelation and Q overestimation, and the innovation overshoot-compensation caused by negative autocorrelation and Q underestimation, prioritizing detection sensitivity. Under the recommended configuration, a two-sided test is used. When Q is correct, approximately 18% of the frames meet the triggering condition. The seemingly high false alarm rate is effectively suppressed by the hysteresis reset state machine in step S4. After a single false alarm triggers a Q reset, the sliding window buffer is immediately cleared, followed by N consecutive... reset =The freeze flag remains set for 10 frames, even if the condition is met again. It will not trigger resets repeatedly; in actual tests, when Q is correct, the average reset interval is much longer than the tracking duration, but it has no substantial impact on tracking accuracy. When M=1×, the ACF-enhanced RMSE is basically the same as the original VB-Q. Further quantifying the cumulative effect, in a typical 200-frame tracking scenario, with an independent trigger rate of 18% / frame, it is expected that approximately 36 frames will meet the requirement. However, the delayed reset state machine means that a single false detection triggering a reset requires 10 consecutive frames. Only then can it be reset, and even if a false detection occurs during this period, it will not be triggered again; the actual expected number of resets is much less than 36, with typical experimental values ​​of 1 to 3 times. After each reset, the sliding window buffer is cleared and new information is re-accumulated, which delays the next trigger. The asymmetric protection strategy of preferring false detection to missed detection is far less costly than the divergence risk caused by a missed real freeze in safety-critical applications due to the occasional invalid reset.

[0079] When the absolute value of the 1-hysteresis autocorrelation coefficient exceeds the preset threshold τ, a freeze event is determined to have occurred.

[0080] The statement declared the freezing of the event;

[0081] After the freeze event is triggered, Q-multiplicative reset is performed:

[0082]

[0083] Where κ>1 is the multiplicative reset factor, and κ=3.0 is recommended;

[0084] The sliding window buffer is cleared immediately after a reset to prevent stale autocorrelation estimates from triggering repeated resets.

[0085] A hysteresis-reset state machine is introduced to prevent repeated triggering caused by transient autocorrelation fluctuations. A hysteresis-reset mechanism is then implemented when a hysteresis-reset state machine is detected. After performing a Q reset, the freeze flag is set; when the freeze flag is set for N consecutive times... reset Frame average satisfies When the freeze flag is reset, ensure that ACF detection is upgraded from single-frame triggering and never recoverable to continuous confirmation and reliable detection-recovery closed loop with delayed reset.

[0086] Specifically, at each recursive time k, after completing the CKF measurement update, the method of this invention is executed according to the following steps:

[0087] Continuous adaptive update of noise covariance in variational Bayesian process:

[0088] Obtain the innovation vector γ of the current frame k The scalar matching ratio r between the empirical innovation covariance matrix and the theoretical innovation covariance matrix is ​​calculated as follows. k , S emp S is the empirical covariance matrix of the innovation vector within the sliding window. k Let r be the theoretical information covariance matrix obtained recursively from Kalman filtering. k If the value exceeds the preset dead zone range [0.7, 1.3], then the VB-Q process noise covariance matrix will be updated.

[0089]

[0090] Among them, effective learning rate η0 is the base learning rate. This is the exponential moving average of the normalized squared innovation. Let r be the p-quantile of a chi-square distribution with m degrees of freedom; if r k If within the dead zone, then maintain The continuous adaptive update and subsequent freeze detection and recovery steps run in parallel without interfering with each other;

[0091] The core steps of freeze detection and recovery include:

[0092] S1. Inspiration Buffer Maintenance: Maintain the inspiration vector γ of the current frame. kAppend to a sliding window buffer of length K. If the buffer length exceeds K, remove the earliest entering information vector so that the buffer always keeps the information vector of the most recent K frames.

[0093] S2. Calculation of lag-1 autocorrelation coefficient and pre-whitening screening: When the sliding window buffer accumulates to K frames, a whitening test is first performed, calculating the paired average inner product of the innovation vectors of all adjacent frames in the buffer:

[0094]

[0095] like The current information sequence is determined to satisfy the white noise assumption. At this time, the filter is in a statistically consistent state and the probability of freezing is extremely low. The subsequent freeze detection and reset steps (autocorrelation coefficient calculation in S2 and S3 and S4) are skipped directly, and no freeze detection action is performed.

[0096] Only when If the presence of non-white noise correlation in the innovation sequence is confirmed, then the complete autocorrelation coefficient calculation proceeds: the Euclidean norm of each frame's innovation vector is extracted from the sliding window buffer, a innovation norm sequence of length K is constructed, and its lag-1 autocorrelation coefficient is calculated. The formula is:

[0097]

[0098] in, Let be the Euclidean norm of the information vector of the i-th frame. This represents the mean of the norm of the information within the window;

[0099] S3, Freeze Detection and Q Multiplicative Reset: [This will...] The absolute value of K is compared with a preset threshold τ, and τ satisfies K. A freeze event is determined to have occurred when both of the following conditions are met simultaneously: And with the freeze flag in an unset state, after a freeze event is triggered, the multiplicative reset of the process noise covariance matrix is ​​performed:

[0100]

[0101] Where κ>1 is the multiplicative reset factor, the sliding window buffer is cleared, the freeze flag is set, and the continuous normal frame counter is reset to zero.

[0102] S4, Hysteresis Reset State Machine: When the freeze flag is set, monitor each frame... The recovery status, if the current frame satisfies The consecutive normal frame count will be incremented; once the consecutive normal frame count reaches the preset reset frame number N, the count will be incremented. resetIf the freeze flag is reset, the continuous normal frame counter will be reset to zero. Afterward, ACF detection will return to normal alert status, the freeze flag will be reset, and if the freeze flag is detected again in the next frame... Then Q can be triggered again to reset;

[0103] If any frame reappears during the period of accumulating consecutive normal frames... If the condition is met, the continuous normal frame counter will be immediately reset to zero and accumulation will restart. The asymmetric set / reset condition requires only one frame to be satisfied before setting. Reset requires N consecutive steps. reset Frame average satisfies This constitutes a reliable hysteresis reset state machine, ensuring that the detector will not be falsely triggered by transient autocorrelation fluctuations during the transient recovery period after Q reset.

[0104] like Figure 1 The diagram shows the flowchart of the ACF-VB-Q autocorrelation coefficient freeze detection and mitigation algorithm, which fully demonstrates the two-layer adaptive architecture proposed in this invention that uses the innovation autocorrelation function as the freeze diagnostic quantity. The process is divided into four main stages from top to bottom:

[0105] The first stage involves obtaining the innovation sequence stream from the CKF measurement update and inputting it into the ACF estimation module.

[0106] The second stage involves ACF estimation and decision-making, calculating the lag-1 autocorrelation coefficient of the new norm sequence. Compare it with a preset threshold τ: when When the state is frozen, the left branch is marked in red with "Frozen detected! Suppression triggered," and the multiplicative reset of the noise covariance matrix is ​​performed. Clear the sliding window buffer and force the base learning rate η0 to a lower value for elastic perturbation recovery; when When the condition is normal, the right branch is marked in green as "normal operating standard VB-Q", and a convergence check is performed, with NIS monitoring and η0 gradual increase strategy.

[0107] In the third stage, the adaptive rate controller merges the left and right branches. In the frozen state, η0 takes a lower value to ensure a stable transition after Q reset. In the normal state, η0 is continuously adjusted by the NIS gated learning rate. At the same time, the η0 gradual increase strategy gradually increases the response sensitivity of η0 after the continuous normal frame count meets the condition, accelerating the switch from conservative mode to full-speed adaptive mode.

[0108] In the fourth stage, the state is updated, and the state estimate is output after adaptive Q-correction. Covariance ;

[0109] Figure 1The clearly defined left and right branches and subsequent merging structure intuitively demonstrate the positive interactive complementary relationship between ACF discrete reset and continuous VB-Q adaptation. The two are uniformly scheduled through the adaptive rate controller, forming a complete two-layer architecture of conservative continuous adaptation and discrete large mismatch correction.

[0110] Figures 2 to 4 A three-panel plot is used to visualize the frozen chain, showing NIS saturation, effective learning rate decay, and convergence time scaling law across nine mismatch levels. Figure 2 The NIS saturation heatmap shows the horizontal axis as the time step (0-180 frames) and the vertical axis as the Q mismatch level (M) (1× to 500×). Color depth represents the NIS value (0-25). The low-M region (M≤5×) shows a light yellow low-temperature band, indicating good filter consistency. When M≥50×, a deep red saturation band appears, and the NIS remains high (≥20), indicating severe filter inconsistency. Surprisingly, VB-Q did not trigger an update. (See...) Figure 3 This is precisely the core characteristic of "adaptive freezing": NIS can detect inconsistencies, but cannot drive Q-correction; Figure 3 The effective learning rate decay heatmap shows the color intensity as representing the effective learning rate η, ranging from 0.01 to 0.05. The lower temperature band is light blue, with η ≈ 0.01 to 0.015, covering almost all time steps and mismatch levels. The effective learning rate is compressed to an extremely low value (approximately 0.01 to 0.015) by NIS gating, and Q-updates almost cease. This indicates that NIS saturation did not trigger gating learning rate recovery because the NIS gating... The operation is saturated at 1.0 in the high value range of NIS, but η is still limited by the upper limit of the basic learning rate η0. The theoretical maximum value is 0.05. The range of 0.01 to 0.05 shown in the heatmap corresponds to the actual range of η after gating modulation. The matching ratio r is truncated within the preset dead zone range [0.7, 1.3] due to dead zone protection, resulting in Q not being updated at all. Figure 4 This is a scatter plot with convergence time scaling law, where the horizontal axis is M and the logarithmic coordinates are 10. 0 ~10 3 The vertical axis represents the number of convergence steps, from 0 to 160. Purple dots indicate the measured average number of convergence frames at each M level. The black dashed line represents the O(lnM) least squares fit for this data. This scaling law verifies that the convergence time of VB-Q without ACF increases only logarithmically with M. However, convergence is contingent on r escaping the dead zone. Figure 3 It has been proven that η is completely suppressed under high M, and r never escapes the dead zone range [0.7, 1.3]. Therefore Figure 4 The convergence time actually corresponds to the theoretical scaling law rather than the measured value. In actual measurements, it has never converged. This comparison itself reveals the fundamental contradiction of the freezing phenomenon.

[0111] Figure 5The bar chart compares the ACF freeze mitigation performance, showing the RMSE for different preset threshold τ configurations at M=1×, M=50×, and M=200×. The horizontal axis represents three mismatch levels, each containing four preset ACF threshold configurations: τ=0.2, 0.3, 0.4, and 0.5, distinguished by different colors. The vertical axis represents the positional RMSE; the dark blue bar (τ=0.2) shows an RMSE of approximately 28 at M=50×. The green bar with τ=0.3 showed an improvement of 34.3% in RMSE to approximately 23.5m at M=50×, representing a 45.0% improvement. Among all thresholds, M=50× showed the greatest improvement, and M=1× yielded the best performance with an RMSE of approximately 11.4m, compared to approximately 11.6m without ACF enhancement. The orange bar with τ=0.4 showed an improvement of approximately 25m at M=50×, decreasing to approximately 39m at M=200×, representing a 20.4% improvement. The red bar with τ=0.5 showed an improvement of approximately 28m at M=50×, and approximately 38m at M=200×.

[0112] Increasing τ from 0.2 to 0.3 results in a jump in improvement from 34.3% to 45.0% for M=50×, because τ=0.2 is too sensitive, triggering too many frames for multiplicative resets, leading to frequent Q jumps and filter oscillations. Increasing τ from 0.3 to 0.5 results in a decrease in improvement from 45.0% to approximately 34.5% for M=50×, because excessively large τ misses true freezes, causing Q recovery to be delayed. At M=200×, the improvement for all thresholds is lower than at M=50×, because at 200× mismatch, a single multiplicative reset factor κ=3.0 only amplifies Q by a factor of 3, and it takes longer to get from 200× to the correct value. With approximately 5 resets, plus the accumulation interval of the sliding window buffer with a preset window length of K=20 frames, the recovery speed is limited by the detection frequency of the autocorrelation coefficient with hysteresis of 1; when M=1×, the RMSE of each τ is in the range of 11.3~11.5 m, with a difference of <2%, verifying the zero-cost characteristic of ACF with basically flat performance under the correct model; the experimental conditions are 30 Monte Carlo runs, N=200 frames, dt=1.0 s, sinusoidal motion, state dimension n=6, and measurement dimension m=3;

[0113] Figures 6 to 7 To characterize the frozen index FI using a dual-panel plot, Figure 6 This is a double logarithmic scatter plot of FI and M, with M on the horizontal axis and logarithmic coordinates of 10. 0 ~10 2+ The vertical axis represents FI, the freeze index, and the logarithmic scale is 10. −2 ~10 2 The solid magenta line and circles mark the measured FI values ​​for each M level, while the gray dashed diagonal line indicates ∝M. 1 / 2 Reference scaling law; FI is defined as continuous N freeze Intra-frame The relative change is less than 10 −6When M is low, the region is considered frozen. A higher FI value indicates a greater degree of freezing. At low M, FI < 0.01 indicates a shallow frozen region; at M ≥ 50×, FI jumps to approximately 0.1~10, indicating a deep frozen region. 1 / 2 The scaling law was well verified by the measured data. The physical meaning of the scaling law is that the degree of freezing FI increases with the mismatch ratio M at a square root rate, rather than a nonlinear rate. This indicates that the damping compression mechanism of VB-Q has a certain buffering effect on extreme mismatch, but is not enough to prevent freezing.

[0114] Figure 7 This is a stacked bar chart of the ratio of steady-state Q estimates to true values. The horizontal axis represents the M level (1×, 10×, 50×, 200×, 500×), and the vertical axis represents the ratio of steady-state Q estimates to true values ​​(0~1.0). Orange bars represent CACKF (no ACF), and purple bars represent ACF-CACKF. The chart is marked with horizontal dashed lines. The ideal recovery line; the orange CACKF ratio is ≈1.0 when M=1×, indicating correct initialization. The ratio drops sharply as M increases, reaching ≈0.02 when M=50×, recovering only 2% of the true value, and ≈0.002 when M=500×, recovering 0.2%, confirming that Q almost stagnates at the initial undervaluation under severe freeze; the purple ACF-CACKF ratio recovers to ≈1.0 when M=10×, indicating full recovery, ≈0.98 when M=50×, almost full recovery, ≈0.92 when M=200×, and ≈0.85 when M=500×, verifying the decisive contribution of ACF discrete multiplicative reset to Q recovery under large mismatch; the difference between the purple and orange bars visually demonstrates the mitigating effect of ACF on freeze. At M=50×, ACF increases the Q recovery rate from 2% to 98%, an improvement of approximately 49 times, and at M=500×, it increases it from 0.2% to 85%, an improvement of approximately 425 times;

[0115] Figures 8 to 9 A two-panel plot of the ACF-triggered motion pattern is used to illustrate the complete recovery process, with an example configuration of M=50×, preset threshold τ=0.3, and multiplicative reset factor κ=3.0. Figure 8 for The time series evolution curve, with the horizontal axis representing time steps 0 to 200 and the vertical axis representing time steps 0 to 200. 0~175, the solid orange line represents CACKF No ACF, the solid purple line represents ACF-CACKF. The green dashed vertical line marks the ACF trigger time; CACKF The level remained close to zero throughout. ≈0~2, no effective Q update within 200 frames, classic frozen state;

[0116] ACF-CACKF For the first approximately 45 frames, the level remains low, indicating the accumulation period of the sliding window buffer and the ACF detection delay. Around frame 45, indicated by the green dashed vertical line, the ACF first detects a hysteresis 1 autocorrelation coefficient that satisfies the condition. And triggers a multiplicative reset, The value jumps from approximately 0 to approximately 80, and the sliding window buffer is cleared after the reset. Approximately 20 frames later, at frame 65, the ACF triggers a second reset. After surging to approximately 170, it plummeted back, Q reset → Kalman gain drastic change → innovation amplitude oscillation → ACF detection transient fluctuations; subsequently, it underwent several minor resets and adjustments. It stabilizes in the range of 5-10 after about 100 frames; finally, it reaches a steady state. Approaching, but not entirely, the actual process noise covariance matrix. Because the ACF discrete multiplicative reset is limited by the amplification factor of κ=3.0 and the preset reset frame number N. reset The frame's delayed reset constraint requires multiple resets to accumulate before full recovery can be achieved;

[0117] Figure 9 The position error time-series curve is shown, with the vertical axis representing the position error. The orange solid line represents the CACKF position error, and the purple solid line represents the ACF-CACKF position error. The CACKF error remains in the range of 25~45m throughout the process. Due to severe Q freezing, the estimation is severely biased. The ACF-CACKF error is consistent with the CACKF before ACF triggering, at ~45m. After the first reset, it drops sharply to ~15m. Although there are fluctuations thereafter, the drastic changes in Q cause Kalman gain oscillations, but the overall error level of ~10~20m is much lower than that of CACKF ~25~45m. The dual panel clearly shows the complete closed-loop process of detection → reset → recovery → stabilization, verifying the end-to-end effectiveness of the ACF mechanism from detection to recovery.

[0118] Figure 10 This is a graph comparing the learning rate sensitivity analysis with the three-way algorithm. The horizontal axis is the basic learning rate η0, and the logarithmic scale is 10. −3 ~10 −1 The vertical axis represents the position RMSE (10~45 m), indicated by a solid purple line (circles marking ACF-CACK of this method), orange dashed horizontal lines (squares marking fixed rates VB-Q), blue dotted horizontal lines (triangles marking VB-UKF), and light pink horizontal bands marking the optimal region (RMSE≈13~17 m). The ACF-CACKF curve exhibits a U-shaped characteristic, η0. −3 When ≈0.001, RMSE≈14.2 m, the basic learning rate is too low, VB-Q converges slowly, but ACF can provide multiplicative reset compensation during freezing, η0 −2.5 At approximately 0.003, the RMSE decreases to a minimum of approximately 13.6 m, achieving synergistic optimality between ACF multiplicative reset and an appropriate VB-Q learning rate, η0.−2 At (0.01), RMSE slightly increases to ≈14.6 m, η0 −1.5 When ≈0.03, it rises to ≈16.2 m, η0 −1 (0.1) rises sharply to ≈22.2 m. The excessively high base learning rate η0 causes Q to deviate from the true value due to unnecessary statistical fluctuations under the correct model. The discrete multiplicative reset of ACF can only partially compensate but cannot completely eliminate the negative impact of excessive VB-Q updates. For fixed-rate VB-Q, the RMSE of the orange dashed horizontal line is always about 38.5 m, which is completely unrelated to the base learning rate η0 because its learning rate is constant and not subject to NIS gating modulation. Under the correct model, continuous over-adaptation leads to severe degradation. The RMSE of the blue dot horizontal line of VB-UKF is always about 4646 m. ​​VB-UKF only adapts to measurement noise R, not process noise Q. When Q is severely underestimated, it cannot respond at all. Its constant high RMSE verifies the fundamental difference between process noise adaptation and measurement noise adaptation. ACF-CACKF is effective in the entire interval [η0]. −3 η0 −1.5 ], that is, 10 −3 ~10 −1.5 Within the range, it significantly outperforms the two baseline methods, with an RMSE difference ≥20 m, and most measured points are located within the optimal region. This verifies that the ACF mechanism is highly robust to the base learning rate η0. When η0 varies within a 30-fold range of 0.001 to 0.03, the RMSE fluctuation is only about 2.6 m, approximately 19%. In engineering, the recommended value η0 = 0.01 can be used, which corresponds to an effective learning rate η of approximately 0.01 to 0.02 after NIS gating modulation without the need for fine tuning.

[0119] Table 1 Recommended Algorithm Parameter Configuration Table

[0120]

[0121] Table 1 lists the six core hyperparameters, recommended values, and functional roles involved in the method of this invention, from top to bottom: preset threshold τ=0.3, innovation norm, lag 1 autocorrelation coefficient. The judgment threshold is based on When K=20, the significance level is approximately 0.3, corresponding to a one-sided significance level. The bilateral measurement is approximately 0.18, and bilateral testing is used in engineering. An asymmetric protection strategy that simultaneously captures both underestimation and overestimation of Q, prioritizing false positives over false negatives, ensures that no freezes are missed; a multiplicative reset factor κ=3.0 is executed each time an ACF is triggered. The magnification factor, from M times mismatch recovery requires The reset process strikes a balance between recovery speed and overshoot risk: if κ is too small, such as in 2.0, more resets are needed, resulting in slow recovery; if κ is too large, such as in 5.0, a single amplification may be excessive, potentially causing Kalman gain oscillations. The preset window length K=20, the length of the sliding window buffer, determines... The estimated variance, approximately 1 / K, and the temporal resolution of freeze detection; if K is too small... Large variance leads to a higher false alarm rate, and an excessively large K increases detection latency; whitening test threshold. The decision threshold for the pre-whitening check can skip approximately 80% to 95% of the complete ACF calculation when Q is correct, significantly reducing the average computational cost; the preset reset frame number N reset =10, after the freeze flag is set, N consecutive N... reset Frame satisfies The condition allows for reset, constituting an asymmetric set / reset condition, effectively preventing repeated false triggering caused by transient autocorrelation fluctuations after Q-multiplicative reset; the base learning rate η0, the base learning rate for continuous VB-Q adaptive learning, serves as the effective learning rate. The upper limit of the joint damping index λ=0.2 ensures that the VB-Q is updated gently under the correct model and does not interfere with the ACF freeze detection. The recommended values ​​in Table 1 have been verified by a comprehensive grid search across nine levels of mismatch scanning from M=1× to 500×, four τ values, and four κ values, and can be directly used as the default configuration for engineering deployment.

[0122] Example 1

[0123] Three-dimensional uniform motion target tracking, linear measurement, including:

[0124] Scene setup: A three-dimensional uniform target moving with sinusoidal acceleration, state dimension n=6 (position and velocity), measurement dimension m=3 (position only, linear measurement); real-world noise covariance matrix Q. true =diag(10,10,10,50,50,50),tr(Q true =180; Measurement noise covariance matrix R=diag(25,25,100), tracking time N=200 steps, dt=1.0 s, Monte Carlo simulation 30 times;

[0125] Q mismatch setting: initial estimate When M∈{1,2,5,10,20,50,100,200,500}, M=1 Correct initialization, when M=500 ;

[0126] ACF parameters: preset window length K=20, preset threshold τ=0.3, multiplicative reset factor κ=3.0, preset reset frame count N reset =10;

[0127] Results: When M=50×, the original CACKF VB-Q had an RMSE of 42.71 m and an FI of 0.02, indicating severe freezing. Based on the novel norm autocorrelation-based filtered freeze detection recovery method, the RMSE of ACF-VB-Q was 23.47 m, an improvement of 45.0%, with an average of 3.7 Q-multiplicative resets. When M=1×, the RMSE of ACF-VB-Q was 11.37 m, which was basically the same as the original 11.56 m.

[0128] Example 2

[0129] Generalization verification of high-speed aerodynamic physical motion patterns includes:

[0130] Scenario Setup: A five-stage high-speed motion pattern is generated under the US 1976 standard atmospheric assumptions. The high-speed motion is Ma≈5.2 → turning and climbing → S-shaped change of direction → sharp turn → rapid descent, including aerodynamic pole curves and atmospheric density decaying exponentially with altitude. The actual process noise covariance matrix Q is... true =diag(10,10,10,500,500,500),tr(Q true =1530, velocity noise amplified 10 times to match high motion, measure noise covariance matrix R and ACF parameters, preset window length K, preset threshold τ, multiplicative reset factor κ, preset reset frame number N reset Same as Example 1;

[0131] Result: When M≥10× Strictly unchanged within 200 steps, zero VB-Q update trigger, confirming that the freeze phenomenon is insensitive to motion type and noise level.

[0132] Example 3

[0133] A scan was performed with a preset threshold τ. Under the settings of Example 1, the preset threshold τ∈{0.2,0.3,0.4,0.5} was scanned, and the multiplicative reset factor κ=3.0 was fixed. The results are as follows:

[0134] Table 2. RMSE and number of multiplicative resets under different parameters.

[0135]

[0136] τ=0.3 is the recommended operating point. The RMSE improvement is greatest when M=50×, and the performance is best when M=1×, with RMSE =11.37 m and 11.56 m without ACF enhancement. The lower threshold τ=0.2 has a weaker improvement when M=50×, with 28.08 m, an improvement of 34.3%, but the number of multiplicative resets is less when M=200×.

[0137] Example 4

[0138] The multiplicative reset factor κ was scanned under the settings of Example 1, with a fixed preset threshold τ=0.3, for the multiplicative reset factor κ∈{2.0,3.0,4.0,5.0}. The results are as follows:

[0139] Table 3 Data Comparison Table

[0140]

[0141] κ=3.0 is the recommended value, which is important for recovery speed. Achieving the optimal engineering balance between multiplicative reset and overshoot risk.

[0142] Although embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the specification and embodiments. They can be applied to various fields suitable for the present invention. Other modifications can be easily made by those skilled in the art. Therefore, without departing from the general concept defined by the claims and their equivalents, the present invention is not limited to the specific details and examples shown and described herein.

Claims

1. A method for frozen detection and recovery based on novel norm autocorrelation filtering, characterized in that, include: S1. At each filtering iteration, obtain the current frame information vector output after the measurement update of the capacitive Kalman filter, and append the current frame information vector to the sliding window buffer. When the length of the sliding window buffer exceeds the preset window length K, remove the earliest information vector that entered, so that the sliding window buffer always keeps the information vector of the most recent K frames. S2. When the sliding window buffer accumulates to K frames of innovation vectors, extract the Euclidean norm of each frame's innovation vector from the sliding window buffer, construct a sequence of innovation norms of length K, and calculate the lag-1 autocorrelation coefficient of the innovation norm sequence. ; S3, lagged autocorrelation coefficient The absolute value is compared with a preset threshold τ, when Furthermore, if the freeze flag is not set, a freeze event is determined to have occurred, and a multiplicative reset of the noise covariance matrix is ​​performed. , where κ>1 is the multiplicative reset factor, and the sliding window buffer is cleared and the freeze flag is set; S4. When the freeze flag is set, determine the hysteresis autocorrelation coefficient of the current frame in each frame. Does it meet the requirements? If the condition is met, the consecutive normal frame count will be incremented. If the consecutive normal frame count reaches the preset reset frame number N, the count will be incremented. reset If any frame freezes, the freeze flag will be reset and the continuous normal frame counter will be reset to zero; if any frame freezes... Then the continuous normal frame counter will be reset to zero.

2. The method for filtering, freezing detection, and recovery based on innovation norm autocorrelation according to claim 1, characterized in that, Lagged 1 autocorrelation coefficient Calculate using the following formula: Where, γ i Let i be the information vector of the i-th frame in the sliding window buffer. Let be the Euclidean norm of the information vector of the i-th frame. Let be the mean of the new norm sequence.

3. The method for filtering, freezing detection, and recovery based on innovation norm autocorrelation according to claim 1, characterized in that, The preset threshold τ and the preset window length K satisfy a statistically significant relationship: in, τ represents the standard normal two-sided quantile, α is the significance level for detection, and for K=20, τ=0.

3.

4. The method for filtering, freezing detection, and recovery based on innovation norm autocorrelation according to claim 1, characterized in that, Before step S2, a whitening test is included as a preliminary screening step, specifically including: Calculate the paired average inner product of the innovation vectors of all adjacent frames in the sliding window buffer. If the paired average inner product is less than the whitening test threshold, the current innovation sequence is determined to satisfy the white noise hypothesis, and step S2 is skipped directly to step S4 without performing freeze detection. Step S2 is only performed when the paired average inner product is greater than or equal to the whitening test threshold, where the whitening test threshold is equal to... .

5. The method for filtering, freezing detection, and recovery based on innovation norm autocorrelation according to claim 4, characterized in that, The paired mean inner product is calculated using the following formula: Among them, c k Let be the whitening test statistic at time k. Let γ be the new information vector. i transpose, γ i+1 It is the information vector of the (i+1)th frame in the sliding window buffer.

6. The method for filtering, freezing detection, and recovery based on innovation norm autocorrelation according to claim 1, characterized in that, Each frame also includes a variational Bayesian process noise covariance continuous adaptive update step, specifically including: Calculate the empirical innovation covariance matrix S of the innovation vector within the current sliding window. emp And obtain the theoretical information covariance matrix S calculated recursively by Kalman filtering. k , ,in, H is the state prediction covariance matrix. k For the measurement matrix, R k The measurement noise covariance matrix; Calculate the scalar matching ratio r between the empirical innovation covariance matrix and the theoretical innovation covariance matrix. k , ,in Represents the trace of a matrix; The preset dead zone range is [0.7, 1.3]. When the matching ratio r k When the noise exceeds the preset dead zone, the process noise covariance matrix is ​​updated using the following exponentially weighted recursive formula. : Among them, effective learning rate η0 is the base learning rate. This is the exponential moving average of the normalized squared innovation. Let p be the p-quantile of the chi-square distribution with m degrees of freedom; When the matching ratio r k When within the preset dead zone range, the process noise covariance matrix remains unchanged, i.e. .

7. The method for filtering, freezing detection, and recovery based on innovation norm autocorrelation according to claim 1, characterized in that, Multiplicative reset factor κ=3.0, preset window length K=20, preset reset frame count N reset =10.

8. The method for filtering, freezing detection, and recovery based on innovation norm autocorrelation according to claim 6, characterized in that, The preset damping index is set to 0.

2.

9. A filter-based freeze detection and recovery system based on innovation norm autocorrelation, characterized in that, The method for implementing the filtered freeze detection and recovery method according to any one of claims 1 to 8 includes: The innovation buffer maintenance module is used to obtain the current frame innovation vector output after the measurement update of the volumetric Kalman filter at each filtering recursion time, and append the current frame innovation vector to the sliding window buffer. When the length of the sliding window buffer exceeds the preset window length K, the earliest entering innovation vector is removed, so that the sliding window buffer always keeps the innovation vector of the most recent K frames. The autocorrelation coefficient calculation module is used to extract the Euclidean norms of each frame's innovation vector from the sliding window buffer when the accumulated innovation vectors of the sliding window buffer reach K frames, construct an innovation norm sequence of length K, and calculate the lag-1 autocorrelation coefficient of the innovation norm sequence. ; The freeze determination and reset module is used to freeze the autocorrelation coefficient with lag 1. The absolute value is compared with a preset threshold τ, when Furthermore, a freeze event is determined to have occurred when the freeze flag is not set, and the multiplicative reset of the noise covariance matrix is ​​performed during the execution process. , where κ>1 is the multiplicative reset factor, and the sliding window buffer is cleared and the freeze flag is set; The hysteresis reset state machine module is used to determine the hysteresis autocorrelation coefficient of the current frame in each frame when the freeze flag is set. Does it meet the requirements? If the condition is met, the consecutive normal frame count will be incremented until the consecutive normal frame count reaches the preset reset frame number N. reset The freeze flag will be reset and the continuous normal frame counter will be reset to zero; if any frame shows a freeze flag, the freeze flag will be reset and the continuous normal frame counter will be reset to zero. Then the continuous normal frame counter will be reset to zero.

Images