Robust carrier state estimation method and apparatus based on adaptive window factor graph

Abstract

The embodiment of the application provides a kind of based on adaptive window factor graph's carrier state robust estimation method and device, method includes: obtaining the observation data of multiple state parameters that first sensor collects in recent history time range and the observation data of at least one state parameter that at least one second sensor respectively collects;First sensor's observation data and the observation data of all second sensor are merged into observation data set according to time stamp and state parameter;Each observation data set is marked as valid data set or abnormal data set based on multi-channel anomaly detection method;Establish the window boundary optimization model considering abnormal data distribution, solve window boundary optimization model, divide the recent history time range into at least one sub sliding window;Traditional factor graph optimization method based on Gaussian assumption is used to process each observation data set in sub sliding window, and the state estimation result of each state parameter at all time stamps is obtained.

Description

Technical Field

[0001] This invention relates to the field of navigation and positioning technology, and in particular to a robust estimation method and apparatus for carrier state based on adaptive window factor graph. Background Technology

[0002] Unmanned vehicles (UAVs) are widely used in scientific research, military reconnaissance, and disaster relief due to their low risk, high efficiency, flexibility, and environmental adaptability. Precise positioning is a prerequisite for UAVs to reach their destinations efficiently and safely. However, UAVs face extremely complex mission environments. Specifically, clouds, fog, and sandstorms weaken the observation capabilities of optical sensors by disrupting lighting conditions and reducing visibility. Simultaneously, sandy or soft ground can easily cause wheel slippage, further reducing the accuracy of odometer readings. High-density urban areas experience signal attenuation and multipath effects, exacerbating positioning errors in satellite navigation systems. These factors collectively lead to uncertainty in perception. Furthermore, the platform constraints of UAVs (such as size, weight, and power consumption) significantly limit the number and selection of navigation sensors. Therefore, observational data exhibits both sparsity and high variability. In addition, the limited computing resources of UAVs struggle to meet the real-time requirements of complex estimation algorithms, and the resulting computational delays jeopardize vehicle safety in uncertain environments. Therefore, under resource-constrained conditions, lightweight robust state estimation methods are crucial for ensuring the safety, accuracy, and real-time performance of vehicle operation.

[0003] In the process of developing this invention, the applicant discovered at least the following problems in the prior art:

[0004] Current factor graph optimization methods based on sliding windows exhibit insufficient robustness when faced with outlier observations. Summary of the Invention

[0005] This invention provides a robust estimation method and apparatus for carrier state based on an adaptive window factor graph, which at least solves the problem of insufficient robustness of current sliding window-based factor graph optimization methods when facing anomalous observations.

[0006] To achieve the above objectives, in one aspect, embodiments of the present invention provide a robust estimation method for carrier states based on an adaptive window factor graph, comprising:

[0007] Acquire observation data of multiple state parameters collected by a first sensor within the most recent historical time range, and observation data of at least one state parameter collected by at least one second sensor.

[0008] The observation data of the first sensor and all the observation data of the second sensor corresponding to the same timestamp and the same state parameter are merged into the observation dataset of the state parameter under the timestamp, so as to obtain the observation dataset of each state parameter under each timestamp in the most recent historical time range.

[0009] For each time stamp, the validity of the observation dataset for each state parameter is determined, and each observation dataset is marked as a valid dataset or an abnormal dataset.

[0010] Based on the anomaly determination results of all observation datasets within the most recent historical time range, a window boundary optimization model considering the distribution of abnormal data is established. With the goal of minimizing the total prediction error, the window boundary optimization model is solved, and the most recent historical time range is divided into at least one sub-sliding window.

[0011] For each sub-sliding window, the traditional factor graph optimization method based on the Gaussian assumption is used to process the observation dataset of each state parameter at all timestamps within the sub-sliding window, and the estimation results of each state parameter at all timestamps within the sub-sliding window are obtained.

[0012] On the other hand, embodiments of the present invention provide a robust estimation device for carrier state based on an adaptive window factor graph, comprising:

[0013] The observation data acquisition unit is used to acquire observation data of multiple state parameters collected by the first sensor within the most recent historical time range and observation data of at least one state parameter collected by at least one second sensor.

[0014] The dataset merging unit is used to merge the observation data of the first sensor and all the observation data of the second sensor corresponding to the same timestamp and the same state parameter into an observation dataset of the state parameter under the timestamp, so as to obtain the observation dataset of each state parameter under each timestamp in the most recent historical time range.

[0015] The dataset anomaly labeling unit is used to determine the validity of the observation dataset for each state parameter at each timestamp, and to label each observation dataset as a valid dataset or an anomalous dataset.

[0016] The window partitioning unit is used to establish a window boundary optimization model that considers the distribution of abnormal data based on the anomaly determination results of all observation datasets within the recent historical time range. With the goal of minimizing the total prediction error, the window boundary optimization model is solved to divide the recent historical time range into at least one sub-sliding window.

[0017] The state estimation unit is used to process the observation dataset of each state parameter at all timestamps within each sub-sliding window using a traditional factor graph optimization method based on the Gaussian assumption, and to obtain the estimation results of each state parameter at all timestamps within the sub-sliding window.

[0018] The above technical solution has the following beneficial effects: It generates an observation dataset by organizing observation data collected from multiple sensors according to timestamps and state parameters; it marks the observation dataset as a valid dataset or an abnormal dataset based on a multi-channel anomaly detection method, transforming the detection object from discrete observations into a holistic observation dataset, thus simplifying the anomaly identification process; based on the anomaly detection results of the dataset, with the goal of minimizing the total prediction error and the sub-sliding window length as the optimization variable, it establishes a window boundary optimization model based on the distribution of abnormal data. This model comprehensively considers factors such as anomaly magnitude, cumulative effect, and the temporal distribution of multiple anomaly observations. Compared with the Fixed Sliding Window Factor Graph Optimization Method (FSWFGO) and the Variance-based Adaptive Sliding Window Factor Graph Optimization Method (VASWFGO), the Adaptive Sliding Window Factor Graph Optimization Method (ADASWFGO) considering the distribution of abnormal data in this invention can achieve higher estimation accuracy. Attached Figure Description

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

[0020] Figure 1 This is a flowchart of a robust estimation method for carrier state based on an adaptive window factor graph, one of the embodiments of the present invention;

[0021] Figure 2 This is an architecture diagram of a carrier state robust estimation device based on an adaptive window factor graph, one of the embodiments of the present invention;

[0022] Figure 3This is one embodiment of the factor graph optimization fusion framework based on an adaptive sliding window.

[0023] Figure 4 This is a schematic diagram of the fusion of observation information from multiple sensors in one embodiment of the present invention;

[0024] Figure 5 This is a factor graph structure model of a typical integrated navigation system, one of the embodiments of the present invention;

[0025] Figure 6 This is a flowchart of a multi-channel anomaly detection method according to one embodiment of the present invention;

[0026] Figure 7 This is a schematic diagram of a sliding window according to one embodiment of the present invention;

[0027] Figure 8 This is a comparative schematic diagram of a fixed sliding window, a variance-based adaptive window, and an adaptive sliding window that considers the distribution of abnormal data in one of the embodiments of the present invention.

[0028] Figure 9 This is a flowchart of an adaptive robust sliding window design considering abnormal data distribution, which is one embodiment of the present invention.

[0029] Figure 10 Here are the flowcharts for the FSWFGO, VASWFGO, and ADASWFGO algorithms;

[0030] Figure 11 This is a simulation environment of the two-dimensional planar motion trajectory of the carrier in one of the embodiments of the present invention;

[0031] Figure 12 This is a comparison result between the true value of the state parameter and the noise observation value in one embodiment of the present invention;

[0032] Figure 13 This is a comparison of the estimation errors of position parameters using different methods;

[0033] Figure 14 This is a comparison of the estimation errors of velocity parameters using different methods;

[0034] Figure 15 It is the root mean square error of the state parameter estimation results under the five methods;

[0035] Figure 16 It is the cumulative distribution function curve of the position estimation error under the five methods;

[0036] Figure 17 It shows the cumulative distribution function curves of velocity estimation errors under the five methods;

[0037] Figure 18This is a radar chart showing the root mean square error of state estimation versus running time for five methods in 100 random simulation tests. Detailed Implementation

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

[0039] On the one hand, such as Figure 1 As shown, this embodiment of the invention provides a robust estimation method for carrier states based on an adaptive window factor graph, including:

[0040] Step S10: Obtain observation data of multiple state parameters collected by the first sensor within the most recent historical time range and observation data of at least one state parameter collected by at least one second sensor.

[0041] Step S11: Merge the observation data of the first sensor and all the observation data of the second sensor corresponding to the same timestamp and the same state parameter into an observation dataset of the state parameter under the timestamp, so as to obtain the observation dataset of each state parameter under each timestamp in the most recent historical time range.

[0042] Step S12: Determine the validity of the observation dataset for each state parameter at each timestamp, and mark each observation dataset as a valid dataset or an abnormal dataset.

[0043] Step S13: Based on the anomaly determination results of all observation datasets within the most recent historical time range, establish a window boundary optimization model that considers the distribution of abnormal data. With the goal of minimizing the total prediction error, solve the window boundary optimization model and divide the most recent historical time range into at least one sub-sliding window.

[0044] Step S14: For each sub-sliding window, the traditional factor graph optimization method based on the Gaussian assumption is used to process the observation dataset of each state parameter at all timestamps within the sub-sliding window, obtaining the estimation results of each state parameter at all timestamps within the sub-sliding window. The most recent historical time range is set according to the specific scenario and task requirements, typically on the order of minutes, such as 5 minutes.

[0045] The embodiments of this invention have the following technical effects: By organizing observation data collected from multiple sensors according to timestamps and state parameters, an observation dataset is generated; each observation dataset is marked as a valid dataset or an abnormal dataset based on a multi-channel anomaly detection method. This method transforms the detection object from discrete observations into a holistic observation dataset, simplifying the anomaly identification process; based on the anomaly detection results of the dataset, with the goal of minimizing the total prediction error and the sub-sliding window length as the optimization variable, a window boundary optimization model based on the distribution of abnormal data is established. This model comprehensively considers factors such as anomaly magnitude, cumulative effect, and the temporal distribution of multiple anomaly observations. Compared with the fixed sliding window factor graph optimization method (FSWFGO) and the variance-based adaptive sliding window factor graph optimization method (VASWFGO), the adaptive sliding window factor graph optimization method (ADASWFGO) considering the distribution of abnormal data in this invention can achieve higher estimation accuracy.

[0046] Furthermore, the validity of the observation datasets for each state parameter at each timestamp is determined, and each observation dataset is marked as a valid dataset or an abnormal dataset, including:

[0047] For each observation dataset of each state parameter at each timestamp, if the validity determination results of the observation dataset are all valid by the three methods of thresholding based on prior knowledge, statistical method based on multi-source data, and incremental method based on time series, the observation dataset is marked as a valid dataset; otherwise, the observation dataset is marked as an abnormal dataset.

[0048] The embodiments of the present invention have the following technical effects: by using three methods—thresholding based on prior knowledge, statistical methods based on multi-source data, and incremental methods based on time series—to determine the validity of the observed dataset, the invention achieves low computational complexity and high accuracy in detecting abnormal datasets, thus avoiding the risk of anomaly identification errors in the fixed sliding window factor graph optimization method (FSWFGO).

[0049] Furthermore, the thresholding method based on prior knowledge includes:

[0050] For each time stamp, determine whether the observed data in the observation dataset for each state parameter is within a reasonable range of values ​​corresponding to the state parameter.

[0051] If all observation data in the observation dataset falls within a reasonable range of values ​​for the parameter, the observation dataset is considered valid; otherwise, the observation dataset is considered abnormal.

[0052] Furthermore, the statistical method based on multi-source data includes:

[0053] For timestamp t iThe observation dataset of the s-th state parameter The observed dataset is considered valid if the test statistic G satisfies the following validity condition; otherwise, it is considered abnormal:

[0054]

[0055] In the formula, σ is the mean of the observed data; T is the standard deviation of the observed data; c Indicates n degrees of freedom d -2. The significance level is 0.025 / n d The critical value of the t-distribution; n d The number of observation data.

[0056] Furthermore, the state parameters include position parameters and velocity parameters, and the corresponding observation data are position observations and velocity observations, respectively; the time-series-based incremental method includes:

[0057] Let z p(i-1) and z pi These represent timestamps t and t, respectively. i-1 and t i Position observation at any of the X, Y, or Z directions, z vi Represents timestamp t i Velocity observation in the same direction as the location observation, when z p(i-1) z pi and z vi When the three observation data satisfy the following validity condition, z pi and z vi The dataset to which it belongs is considered valid; otherwise, z pi and z vi The associated observation dataset was determined to be an anomaly:

[0058]

[0059] In the formula, δ pv This is a preset threshold, the value of which is determined based on the specific task scenario and accuracy requirements.

[0060] Furthermore, based on the anomaly detection results of all observation datasets within the most recent historical timeframe, a window boundary optimization model considering the distribution of anomalous data is established. The window boundary optimization model is solved, dividing the most recent historical timeframe into at least one sub-sliding window, including:

[0061] The window boundary optimization model is as follows:

[0062]

[0063] In the formula, l = [l1, l2, ..., li ,…,l r ] represents the optimization variable; Let be the length of the i-th sub-sliding window; Indicates the number of child sliding windows; n e The number of anomalous datasets; l min Δ is the minimum length of the sub-sliding window; L is the window length used for sub-sliding window boundary optimization, i.e., the length of the most recent history time range; κ Let be the range of the κ-th outlier dataset within the most recent historical timeframe, representing the degree of outlier in the outlier dataset; d κ g is the timestamp index of the κ-th anomalous dataset; κ1 (·) represents the impact of the κth outlier dataset on the state estimation accuracy when the sub-sliding window length is l and the κth outlier dataset is located at the endpoint timestamp of the sub-sliding window; g κ2 (·) represents the impact of the κ-th outlier dataset on the state estimation accuracy when the sub-sliding window length is l and the timestamp is within the sub-sliding window; e(κ) is the index of the κ-th outlier dataset in all observation datasets within a window of length L; η κ1 η is the estimation error introduced when the κ-th outlier dataset is at the end timestamp of the sub-sliding window; κ2 The estimation error introduced when the κth outlier dataset is located at a timestamp inside the sub-sliding window; The estimation error introduced by the κth outlier dataset to the jth state parameter within the sub-sliding window when the κth outlier dataset is at the end timestamp of the sub-sliding window; The estimation error introduced by the κth outlier dataset to the jth state parameter within the sub-sliding window when the κth outlier dataset is located at the timestamp inside the sub-sliding window; β is the timestamp index of the left boundary of the i-th sub-sliding window; i This is the right boundary timestamp index of the i-th sub-sliding window.

[0064] On the other hand, such as Figure 2 As shown, this embodiment of the invention provides a robust estimation device for carrier state based on an adaptive window factor graph, comprising:

[0065] The observation data acquisition unit 200 is used to acquire observation data of multiple state parameters collected by the first sensor within the most recent historical time range and observation data of at least one state parameter collected by at least one second sensor.

[0066] The dataset merging unit 201 is used to merge the observation data of the first sensor and all the observation data of the second sensor corresponding to the same timestamp and the same state parameter into the observation dataset of the state parameter under the timestamp, so as to obtain the observation dataset of each state parameter under each timestamp in the most recent historical time range.

[0067] The dataset anomaly marking unit 202 is used to determine the validity of the observation dataset for each state parameter at each timestamp, and to mark each observation dataset as a valid dataset or an abnormal dataset.

[0068] The window partitioning unit 203 is used to establish a window boundary optimization model that considers the distribution of abnormal data based on the anomaly determination results of all observation datasets within the recent historical time range, with the goal of minimizing the total prediction error, and solve the window boundary optimization model to divide the recent historical time range into at least one sub-sliding window.

[0069] The state estimation unit 204 is used to process the observation dataset of each state parameter at all timestamps within each sub-sliding window using a traditional factor graph optimization method based on the Gaussian assumption, and to obtain the estimation results of each state parameter at all timestamps within the sub-sliding window.

[0070] The embodiments of this invention have the following technical effects: By organizing observation data collected from multiple sensors according to timestamps and state parameters, an observation dataset is generated; each observation dataset is marked as a valid dataset or an abnormal dataset based on a multi-channel anomaly detection method. This method transforms the detection object from discrete observations into a holistic observation dataset, simplifying the anomaly identification process; based on the anomaly detection results of the dataset, with the goal of minimizing the total prediction error and the sub-sliding window length as the optimization variable, a window boundary optimization model based on the distribution of abnormal data is established. This model comprehensively considers factors such as anomaly magnitude, cumulative effect, and the temporal distribution of multiple anomaly observations. Compared with the fixed sliding window factor graph optimization method (FSWFGO) and the variance-based adaptive sliding window factor graph optimization method (VASWFGO), the adaptive sliding window factor graph optimization method (ADASWFGO) considering the distribution of abnormal data in this invention can achieve higher estimation accuracy.

[0071] Furthermore, the dataset anomaly labeling unit 202 is used to mark the observation dataset as a valid dataset when all three methods—thresholding based on prior knowledge, statistical method based on multi-source data, and incremental method based on time series—are valid for each state parameter observation dataset at each timestamp; otherwise, the observation dataset is marked as an anomalous dataset.

[0072] Furthermore, the window partitioning unit includes: the window boundary optimization model is formula (3).

[0073] The technical solutions of the present invention will be described in detail below with reference to specific application examples. For technical details not described in the implementation process, please refer to the relevant descriptions above.

[0074] The inventors conducted an in-depth analysis of existing technologies: there are currently three main methods for state estimation: Kalman filtering, factor graph optimization, and learning-based methods. While the last method can achieve high-precision state estimation, it faces challenges such as complex model training and insufficient generalization ability, making it difficult to deploy in state estimation scenarios with limited airborne resources.

[0075] Kalman filtering, as the optimal recursive estimator for linear Gaussian systems, can effectively suppress observation noise and achieve asymptotic convergence of state estimation in stationary dynamic systems. However, in time-varying systems, it has several limitations when dealing with highly variable data: (1) The Markov assumption leads to insufficient utilization of historical information, making it difficult to correct the bias introduced by historical abnormal observations in a timely manner, which may lead to estimation divergence; (2) Sensitivity to initial estimation and covariance matrix; (3) Insufficient dynamic adaptability of pre-configured noise covariance tuning strategy; (4) The inherent contradiction between synchronous measurement framework and asynchronous data stream makes the alignment process inevitably introduce phase lag, thereby deteriorating the estimation performance.

[0076] To overcome the shortcomings of the Kalman filter method, the factor graph optimization method provides a scalable dynamic system probabilistic modeling framework. Its core advantage is its plug-and-play nature. By iteratively optimizing the joint posterior probability of the state and observations from the initial timestamp to the current timestamp, it fully utilizes historical information to achieve state estimation, theoretically achieving better estimation accuracy than traditional filtering methods. However, this method suffers from a drawback: as the number of variables and factor nodes increases over time, the computational complexity continuously increases, making it difficult to guarantee real-time performance. To address this issue, a sliding window is proposed based on the factor graph framework. It only retains data within the most recent few timestamps (i.e., data within the window) while discarding data outside the window. This ensures that the global optimization process of the factor graph only occurs within the most recently generated timestamps, thus limiting the size of the factor graph and reducing computational complexity. Depending on whether the window length is dynamically variable, sliding windows are divided into fixed sliding windows and dynamic sliding windows. The fixed sliding window strategy transforms the global state estimation problem into a local optimization problem within a fixed time length, while traditional factor graph optimization methods based on the Gaussian assumption are highly sensitive to outliers. When multiple anomalous observations occur within a window, their errors propagate through the kinematics and observation constraints in the factor graph, leading to severe deviations in state estimation and even causing the optimization process to fail completely. In contrast, the dynamic sliding window mechanism can adaptively adjust the window length based on observation uncertainties or anomalous detection results, achieving a dynamic balance between estimation robustness and computational efficiency.

[0077] The existing FSWFGO and VASWFGO have the following problems:

[0078] Limitations of FSWFGO: While a fixed sliding window improves efficiency through local optimization, it sacrifices the utilization of historical information outside the window. In dynamic environments, the limited information content makes it difficult to effectively suppress the impact of anomalous observations, further amplifying FSWFGO's inherent sensitivity to anomalous observations. This method inherently exposes a trade-off between efficiency and robustness: a shorter window limits the number of available observations, potentially leading to decreased accuracy; a longer window, while improving accuracy, increases computational cost. Therefore, window length becomes a key trade-off between estimation accuracy and computational efficiency. A typical solution to this problem is to dynamically adjust observation weights based on anomaly detection results to improve FSWFGO performance. The effectiveness of such methods relies on accurate and reliable anomaly detection results. However, sparse, highly variable observations rarely strictly follow a Gaussian distribution, violating the assumptions of traditional anomaly detection methods and leading to decreased detection reliability.

[0079] Limitations of VASWFGO: VASWFGO dynamically adjusts the window length based on the variance of observations within the window. When the variance of observations within the window exceeds a preset threshold, the window expands to include more observations, thereby reducing the variance. Essentially, this method effectively suppresses the impact of outlier observations on estimation accuracy within a fixed window. However, it ignores the impact of the temporal distribution of outlier observations on estimation accuracy. In particular, when outlier data is at the endpoint timestamp of the adaptive sliding window, its estimation accuracy may be lower than that of FSWFGO.

[0080] In complex and ever-changing operating environments, navigation sensors are susceptible to interference from uncertainties, leading to abnormal observations and severely impacting the accuracy of state estimation. In resource-constrained applications, to balance computational efficiency and estimation accuracy, and addressing the limitations of FSWFGO and VASWFGO, this invention proposes a robust carrier state estimation method based on an adaptive window factor graph. This method first analyzes the impact of the temporal distribution of abnormal observations within the sliding window on estimation accuracy. Then, based on the analysis, it comprehensively considers factors such as the magnitude of abnormalities, the superposition effect of multiple abnormal observations, and the temporal distribution of abnormal observations. Under the constraint of limited computational resources, with the goal of minimizing the total prediction error, and using sub-sliding windows as optimization variables, an adaptive sliding window boundary model is established. Finally, for each optimal sub-sliding window, a traditional factor graph optimization method based on the Gaussian assumption is used to achieve state estimation. Experimental results show that, in complex environments, compared to FSWFGO and VASWFGO, the method ADASWFGO of this invention has better state estimation accuracy, verifying the theoretical advantages and engineering applicability of this method in suppressing the impact of abnormal observations.

[0081] 1. Fusion Framework Based on Factor Graph Optimization Method

[0082] In complex and ever-changing environments, embodiments of the present invention achieve robust estimation of position and velocity based on an adaptive factor graph fusion framework. Figure 3 This paper describes a factor graph optimization fusion framework based on an adaptive sliding window. The first step in state estimation involves spatially synchronizing observations from multiple sensors, including inertial measurement units (IMUs), odometry (ODs), and visual odometry (VOs), to eliminate benchmark discrepancies and ensure spatial consistency across sensor data.

[0083] The second step is to organize the observation datasets of each state parameter at different timestamps based on the observation output frequency of each sensor. For example... Figure 4 As shown, rectangles represent IMU observations; triangles represent OD observations; and hexagons represent VO observations. Arrows indicate the fusion process. IMUT OD and T VO The sampling periods for IMU, OD, and VO are respectively; T R The sampling period for all three sensors is defined as t = T. Because the IMU's sampling frequency is significantly higher than that of the other sensors, multiple timestamps exist, and its state estimation relies solely on IMU observations. OD At that time, the IMU and OD achieve velocity observation fusion; when t=T VO At t=nT, the IMU and VO achieve position observation fusion; R At that time, the IMU, OD and VO achieve observation fusion of the same state parameters.

[0084] The third step involves classifying the observed datasets using a multi-channel anomaly detection method, labeling each dataset as either a valid or anomaly dataset. The fourth step is to establish a window boundary optimization model that considers the distribution of anomaly data, aiming to minimize the total prediction error within the sliding window, with the sub-sliding window length as the optimization variable, and solving for the optimal sub-sliding window length. The fifth step is to use a traditional factor graph optimization method based on the Gaussian assumption to estimate the state for each optimal sub-sliding window.

[0085] This invention primarily addresses navigation scenarios on flat roads, where state variables include three-dimensional position parameters and three-dimensional velocity parameters. The IMU / OD / VO integrated navigation system is widely used in the field of unmanned vehicle state estimation. Specifically, the IMU provides velocity and position increments between adjacent timestamps, the VO provides position observations, and the OD provides velocity observations. Figure 5 The factor graph structure of this integrated navigation system is shown. (Timestamp t) i The state variable at that location is represented as:

[0086]

[0087] In the formula, p represents the position parameter; v represents the velocity parameter; i represents the current timestamp index; and the superscripts x, y, and z of the state parameter represent the X, Y, and Z directions in the navigation coordinate system, respectively.

[0088] All states are obtained by optimizing the mathematical model of the traditional factor graph based on the Gaussian assumption, i.e.:

[0089]

[0090] In the formula, X = [x τ ,…,x τ+l ] indicates the sliding window starting from timestamp t τ to t τ+l The state vector; Γ represents the estimation result of X; l represents the length of the sliding window; Γ is the prior covariance matrix at the initial timestamp; R is the covariance matrix of VO / OD observations within the sliding window; Λ is the covariance matrix of IMU observations within the sliding window; γ is the prior information of x0 at the initial timestamp t0; j and k are timestamp indices; f(·) is the observation model of the IMU; H k (·) represents the timestamp t k Observational model for VO / OD; Let A be the square of the Mahalanobis norm.

[0091] By solving the nonlinear least squares problem equation (5) using the Gauss-Newton method, the optimal estimates of each state parameter under all timestamps within the sliding window can be obtained.

[0092] 2. Anomaly Detection Methods

[0093] A multi-channel anomaly detection method is established by employing a threshold method based on prior knowledge, a statistical method based on multi-source data, and an incremental method based on time series data. This method identifies abnormal sensor outputs, such as... Figure 6 As shown. For timestamp t i The observation dataset Y at the s-th state parameter si If any anomaly detection method detects an anomaly in the dataset, then the dataset is considered an anomaly dataset; otherwise, it is a valid dataset.

[0094] (1) Threshold method based on prior knowledge

[0095] In practical engineering applications, all types of state parameters have feasible value ranges that conform to physical laws. Let the effective value range of the s-th state parameter be [χ... low ,χ up For timestamp t i The observation dataset of the s-th state parameter When all observations satisfy:

[0096] χ low ≤y u ≤χ up (6)

[0097] If the observed dataset is valid, it is considered valid; otherwise, it is considered abnormal.

[0098] (2) Grubbs' Test

[0099] For timestamp t i The observation dataset of the s-th state parameter The dataset is considered valid if the test statistic G meets the following validity condition; otherwise, it is considered abnormal:

[0100]

[0101] In the formula, σ is the mean of the observed data; T is the standard deviation of the observed data; c Indicates n degrees of freedom d -2. The significance level is 0.025 / n d The critical value of the t-distribution; n d The number of observation data.

[0102] (3) Incremental method based on time series

[0103] Let z p(i-1) and z pi These represent timestamps t and t, respectively. i-1 and t i Position observation at any of the X, Y, or Z directions, z vi Represents timestamp t i Velocity observation in the same direction as the location observation, when z p(i-1) z pi and z vi When the three observation data satisfy the following validity condition, z pi and z vi The dataset in question is considered valid; otherwise, it is considered invalid.

[0104]

[0105] In the formula, δ pv This is a preset threshold, the value of which is determined based on the specific task scenario and accuracy requirements.

[0106] 3. Spatiotemporal Propagation Modeling and Analysis of Observation Errors in State Estimation

[0107] 3.1 Optimal Solution of State Estimation

[0108] In an IMU / OD / VO integrated navigation system, each state parameter at each timestamp corresponds to a dynamic observation from the IMU and an external observation from the OD or VO. Without loss of generality, we will use X-axis position and X-axis velocity estimation as an example; the processing of Y / Z-axis state parameters is similar. Let N represent the number of timestamps within the sliding window, then the X-axis position observation from the VO and the X-axis velocity observation from the OD constitute the observation vector z. p =[z p1 ,z p2 ,…,z pN ] T and z v =[z v1 ,z v2 ,…,z vN ] TThe observation vector of an IMU is defined as...

[0109] Further assuming that the observation noise of each sensor is independent, the observation noise matrices for both position and velocity are diagonal matrices. In this case, the traditional factor graph optimization model can be expressed as follows:

[0110]

[0111] In the formula, This represents a vector of position and velocity estimates based on N timestamps; pi is the timestamp t. i Location estimate at; v i For timestamp t i The estimated velocity at that location; This represents an observation vector consisting of VO and OD observations at N timestamps; Represents the noise covariance matrix of OD and VO; Let C represent the noise covariance matrix of the IMU; C is an upper double diagonal matrix, and its specific expression is as follows.

[0112]

[0113] Objective function Expand into a quadratic form:

[0114]

[0115] right about Taking the derivative and setting it to zero, we get:

[0116]

[0117] Rearrange the above equations into a system of linear equations:

[0118]

[0119] set up The above formula can be rewritten as:

[0120]

[0121] Clearly, due to the invertibility of matrix H, this estimate can be expressed as:

[0122]

[0123] 3.2 Mechanism of the impact of anomalous observations on estimation accuracy

[0124] The relationship between position and velocity is introduced into the optimization problem through kinematic constraints. Specifically, assuming a time interval of Δt, the position and velocity of adjacent time stamps satisfy:

[0125]

[0126] In the formula, This is the location observation for the (i+1)th timestamp.

[0127] Suppose a sliding window contains 2k+1 timestamps, then there are X-axis position observations. The X-axis velocity observation vector is Suppose the location observation at the (k+1)th timestamp is an outlier. The error is Let the true value be but All other observation data are accurate; the error of the observation at position (k+1) is... The estimation results will be propagated to other locations via velocity information. The estimation process for state variables in the Y and Z directions is similar to that in the X direction, and will not be elaborated upon here.

[0128] 3.3 Time decay characteristics of abnormal effects

[0129] When the e-th observation in the observation vector z is an anomaly, the estimator It can be represented as:

[0130]

[0131] In the formula, z' represents the observation vector containing anomalous observations; e e yes The e-th standard basis vector in the space, i.e., the e-th component, is 1, and the rest of the components are 0.

[0132] From the above equation, we can see that Δ e This introduces errors into the state estimation at N timestamps within the window. An anomaly propagation matrix F is defined to characterize the impact of anomaly observations on the system state at N timestamps, and is expressed as:

[0133]

[0134] To facilitate the analysis of the properties of F, we first establish the structural characteristics of matrix M. Then, we construct the inverse matrix M of F from it. -1 We will rigorously deduce the attributes.

[0135] (1) Properties of M

[0136] From the above equation, we can deduce that M is a pentagonal matrix. Its specific expression is:

[0137]

[0138] Furthermore, M is a strictly diagonally dominant and asymmetric matrix. A detailed analysis follows:

[0139] 1) Asymmetry

[0140] because and Σ O Both are diagonal matrices, and the transpose of matrix M is:

[0141]

[0142] However, due to differences in the measurement accuracy of sensors, Σ O It is a nonscalar diagonal matrix. Therefore, we can obtain:

[0143]

[0144] Therefore, M≠M T M is an asymmetric matrix.

[0145] 2) Strict diagonal dominance

[0146] Given Δt < 1, σ oa <1, σ sh <1, a = 1, 2, ..., n, h = 1, 2, ..., n-2, for each row index r, M satisfies:

[0147]

[0148] Therefore, M satisfies the condition of diagonal dominance.

[0149] (2)M -1 Attenuation characteristics and attenuation rate

[0150] Theorem 1: Let M be a strictly diagonal dominant matrix with half-bandwidth b. The inverse matrix M... -1 The (i,j)th element decays exponentially with increasing distance |ij| between it and the (j,j)th element on the main diagonal, i.e., there exist constants 0 < λ < 1 and E > 0 such that:

[0151] |(M -1 ) i,j |≤E·λ |i-j| (twenty one)

[0152] Proof: Decompose M into a diagonal matrix D and a pentagonal matrix N with diagonal elements of 0:

[0153] M = D + N (22)

[0154] In the formula, D = diag(diag(M)).

[0155] Since all diagonal elements of N are 0, M is a strictly diagonally dominated matrix, that is:

[0156]

[0157] Therefore, matrix D -1 The infinite norm of N satisfies:

[0158]

[0159] The above equation satisfies the convergence condition of the Neumann series, therefore the inverse matrix M is M -1 It can be expanded as follows:

[0160]

[0161] According to the properties of matrix norm, we have:

[0162] ||M -1 ||=||D -1 -(D -1 N)D -1 +(D -1 N) 2 D -1 +…+(D -1 N) k D -1 ||

[0163] ≤||D -1 ||+||(D -1 N)D -1 ||+…+||(D -1 N) k D -1 ||

[0164] ≤||D -1 ||+||D -1 N||||D -1 ||+…+||D -1 N|| k ||D -1 || (26)

[0165] For any element (M) -1 ) i,j Let i,j = 1, 2, ..., n, and their absolute values ​​satisfy:

[0166]

[0167] Furthermore, according to Theorem 2, the absolute value of the (i,j)th element of matrix M-1 satisfies:

[0168]

[0169] Since ρ=||D -1 N|| ∞ <1, The series decays exponentially with increasing exponent k. The series summation in the above equation can be further simplified to:

[0170]

[0171] From the above equation, we can see that matrix M -1 The (i,j)th element varies with the non-diagonal distance The value decreases exponentially with increasing M. This is because matrix M... -1 The condition for elements (i,j) to remain non-zero requires the existence of a higher-order term (D). -1 N) m ,and However, as the exponent k increases, the norm of these higher-order terms decays sharply.

[0172] Q.E.D.

[0173] For a pentagonal matrix D -1 The power propagation characteristics of N are analyzed as follows.

[0174] Theorem 2: (D- 1 N) k As the power k increases, non-zero elements gradually spread away from the main diagonal. However, (D-1N) k Propagate the non-zero padding elements to a position b·k from the diagonal. Equivalently, (D-1N) k The condition for the (i,j)th element to be non-zero is b·k≥|ij|.

[0175] Proof: This theorem can be proved by mathematical induction.

[0176] Basic example (k=1): (D -1 N) 1 Non-zero elements only appear in adjacent positions on the main diagonal, i.e., |ij| = 1.

[0177] Assume (k = m): For the element in the j-th column, (D -1 N) m The non-zero element in the matrix appears at the position |ij| = bm, and (D -1 N) m The positions of non-zero elements in the equation satisfy |ij|≤bm.

[0178] Recursion (k = m + 1):

[0179]

[0180] To prove (D) -1 N)m+1 The (i,j)th element is nonzero only when |ij|≤b(m+1).

[0181] According to the inductive hypothesis, ((D) -1 N) m ) i,r Non-zero only when |ir|≤bm, (D -1 N) r,j It is non-zero only when |rj|≤b, therefore:

[0182] |ij|=|i-r+rj|≤|ir|+|rj|≤b(m+1) (31)

[0183] Therefore, (D) -1 N) m+1 The non-zero elements satisfy |ij|≤b(m+1).

[0184] Q.E.D.

[0185] This section demonstrates that anomalous observations only affect the accuracy of state estimation for locally adjacent timestamps, and that this effect decays exponentially with increasing time intervals. This conclusion provides a basis for determining the minimum window length l. min It provides a theoretical basis.

[0186] 3.4 Sensitivity Analysis of Temporal Location of Anomaly Observations in a Sliding Window

[0187] Figure 7 Two equal-length sliding windows (N) are shown. max =6). The eight timestamps are divided into two sliding windows. The first window is marked with a solid-line rectangle, and the timestamp interval is [t]. i -2,t i +3]. The second window is marked with a dashed rectangle, and the timestamp interval is [t]. i ,t i +5]. Timestamp t i The anomaly observation was located at the third timestamp of the first sliding window, while it was located at the first timestamp of the second sliding window. Despite the differences in window parameters, M was observed because the sensor configuration was identical. -1 The values ​​remain unchanged. However, the impact of anomalous observations on the state estimates of the remaining timestamps depends on their temporal position within the two windows. Therefore, subsequent analyses will focus on comparing M... -1 The numerical values ​​of the (1,1)th and (3,3)th elements in the matrix.

[0188] Theorem 3: The impact of outlier observations on estimation accuracy exhibits a marginal effect; that is, outlier observations at the end timestamps of the sliding window have a more significant impact on estimation accuracy than those at timestamps inside the window. Define the timestamp set of the two sliding windows as T1 = {t...}i-2-α ,…,t N+i-α-3} and T2={t i ,…,t i+N-1}, α=0,1,…,N-6. Assume the anomalous observation ε is located at timestamp t. i At this point, the timestamp is both the initial endpoint timestamp of T2 and an internal timestamp of T1. Let η1 and η2 represent the influence coefficients of anomalous observations on the state estimation in windows T1 and T2, respectively, then:

[0189]

[0190] Proof: For any row in matrix M with an odd or even index, the diagonal element at the boundary position is strictly smaller than the diagonal element inside the non-boundary position. That is:

[0191]

[0192] In addition, besides the four diagonal elements in the first and last rows ((M) 1,1 (M) 2,2 (M) n-1,n-1 and (M) n,n In addition, in matrix M, all remaining diagonal elements in odd-numbered rows are equal, and all remaining diagonal elements in even-numbered rows are also equal. Equivalently, there exist constants ξ and... Make:

[0193]

[0194] Therefore, in (M) 1,1 <(M) 3,3 Given this premise, the key to the proof lies in:

[0195] (M -1 ) 1,1 >(M -1 ) 3,3 (35)

[0196] Combining the properties of strictly diagonally dominant matrices, M -1 diagonal element (M) -1 ) i,i It can be represented as:

[0197] (M -1 ) i,i =det(M ii In equation (36), M is... ii It is the principal submatrix obtained by removing the i-th row and i-th column from matrix M.

[0198] Since matrix M is strictly row-diagonally dominated, its principal submatrix M0 iiIt also exhibits diagonal dominance. According to Ostrowski's theorem, for a strictly diagonally dominant matrix, its lower bound on the determinant satisfies:

[0199]

[0200] Since all diagonal elements of matrix M are positive, its determinant must be positive. Because the off-diagonal elements of a strictly diagonally dominant matrix M are extremely small, on the order of approximately 10-1. -4 Substituting equation (37) into equation (36), we get:

[0201]

[0202] Given (M) 1,1 <(M) 3,3 ,therefore

[0203]

[0204] Then, when the elements of matrix M satisfy (M) 1,1 <(M) 3,3 At that time, (M) -1 ) 3,3 The lower bound is less than (M) -1 ) 1,1 The lower bound.

[0205] According to Ostrowski's theorem, for a strictly diagonally dominated matrix M, the diagonal elements of its inverse matrix satisfy an upper bound:

[0206] (M -1 ) 1,1 ≤((M) 1,1 -∑ i≠j |(M) i,j |) -1 ≈((M) 1,1 ) -1 (41)

[0207] (M -1 ) 3,3 ≤((M) 3,3 -∑ i≠j |(M) i,j |) -1 ≈((M) 3,3 ) -1 (42)

[0208] Clearly, when the elements of matrix M satisfy (M) 1,1 <(M) 3,3 At that time, (M) -1 ) 1,1 The upper bound is greater than (M) -1 ) 3,3 The upper boundary.

[0209] (M -1 ) 1,1 With (M) -1 ) 3,3 The upper and lower bounds indicate that as the diagonal dominance of the original matrix increases, the bounds of the diagonal elements of its inverse matrix exhibit a monotonically decreasing trend.

[0210] Q.E.D.

[0211] 4 Adaptive Robust Sliding Window Design Method

[0212] 4.1 Adaptive Sliding Window Factor Graph Optimization Method Based on Abnormal Data Distribution

[0213] The adaptive sliding window method is described in detail below. A comparison of the embodiments of this invention with the fixed sliding window (Scheme 1) and the variance-based adaptive sliding window (Scheme 2) is shown below. Figure 8 As shown in the diagram. Based on the aforementioned analysis of anomaly impacts, it is known that the accuracy of state estimation within a sliding window is influenced by multiple factors, including the temporal characteristics and degree of anomaly at the anomaly observation points. Specifically, these factors include: first, the temporal position of a single anomaly observation within the sliding window; second, the temporal distribution pattern of multiple anomaly observations; and third, the deviation magnitude of each anomaly observation. To minimize the total prediction error, this embodiment of the invention proposes an adaptive sliding window method that considers the impact of anomaly observations, the flowchart of which is shown in the diagram. Figure 9 As shown, the core idea is as follows: based on the judgment results of the observation dataset, the sliding window is divided into two categories: effective sliding window (excluding abnormal datasets) and abnormal sliding window (including abnormal datasets). For effective sliding windows, the traditional factor graph optimization method based on the Gaussian assumption is directly used for state estimation; for abnormal sliding windows, considering the temporal distribution and magnitude of abnormal observations, a boundary optimization model is established with the goal of minimizing the total prediction error within the sliding window. The optimal sub-sliding windows after partitioning are obtained by solving the model, and then the traditional factor graph optimization method based on the Gaussian assumption is used for state estimation for each sub-sliding window.

[0214] The specific steps are as follows:

[0215] Step 1: Parameter settings. (1) Maximum window length l max This parameter defines the upper limit of the algorithm's data processing capacity. Its value is determined by both computational resource constraints and algorithm complexity, aiming to ensure that the algorithm meets real-time requirements. Its value must ensure that the algorithm's computational resource requirement C(l) is met. max (The resource budget C allocated by the system for each update instance shall not exceed the maximum resource budget C) max That is, C(l) max )≤C max (2) Minimum window length l minThis parameter ensures that the estimation accuracy of the sliding window is not lower than the lower limit requirement. Based on the attenuation characteristics of the impact of anomalous observations in Theorem 1, under a specific sensor configuration, l min Determined by the influence coefficient threshold. (3) Window length L used for sub-sliding window optimization, L≥l max This parameter is designed to extend the window length, thereby constructing a time-range constraint framework for sub-window adaptive boundary optimization.

[0216] Step 2: Dataset Anomaly Detection. Construct a dataset for estimation, whose corresponding timestamps satisfy: {t i |t k -t i ≤L}, where t k This is the current timestamp. Furthermore, using a multi-channel anomaly detection method, anomalies are detected in the dataset for each state parameter at each timestamp, enabling the classification of the observed dataset and thus determining the temporal distribution of the anomaly dataset.

[0217] Step 3: Establish and solve the sub-sliding window boundary optimization model. According to Theorem 3, the impact of anomaly observations on state estimation is time-series location sensitive; that is, the impact of anomaly observations on estimation accuracy is significantly greater when they are at endpoint timestamps than when they are at non-endpoint timestamps, and there is no significant difference in estimation accuracy when they are at any non-endpoint timestamp. Therefore, for each anomaly dataset, its corresponding sub-objective function is a two-piece function. For n timestamps within L timestamps... e The number of abnormal datasets will form 2n e A piecewise function. The window boundary optimization model can be expressed as formula (3).

[0218] 4.2 Validity Analysis of the Solution

[0219] The window boundary optimization problem requires proving the existence of a global solution and verifying its optimality, while uniqueness is not a necessary condition in this framework. Equation (3) is a discrete optimization problem, which belongs to the integer programming problem. The existence of the optimal solution is guaranteed when the following three conditions are met:

[0220] Condition 1: The feasible set Q is non-empty. (Based on the constraints...) It can be seen that Q contains at least one feasible solution Q = l min ·1 r Therefore

[0221] Condition 2: Lower bound of the objective function. That is, there exists a constant W such that for all l∈Q, f(l)≥W. Since the objective function is a piecewise constant function with a finite number of segments, its finite range guarantees the existence of the lower bound.

[0222] Condition 3: Infimum reachability. Since Q is a finite discrete set with cardinality |Q|<∞, traversing and searching all l∈Q allows finding the minimum value, thus ensuring the existence of an optimal solution.

[0223] In summary, this optimization problem has both feasible solutions and a global optimal solution. The specific solution method will be described in the next section.

[0224] 4.3 Feasible Solution Enumeration Method

[0225] As can be seen from the objective function, all variables l1,l2,…,l r satisfy:

[0226]

[0227] These three constraints transform the multivariable optimization problem into a standard integer combination problem, significantly restricting the feasible solution space. Based on the Stars and Bars Theorem with dual constraints, the window length L for sub-sliding window boundary optimization is decomposed into an ordered sum of r positive integers. Therefore, different permutations of the same elements will produce completely different solutions. Under the inequality constraints and equality constraints, the total number of feasible solutions of the objective function is given by the binomial coefficient:

[0228]

[0229] The computational complexity of the enumeration method is discussed below.

[0230] 1) Due to the symmetry and unimodality of the binomial coefficient, the solution space size U reaches its maximum value under the following conditions:

[0231] r*=(L+1) / (l min +1) (45)

[0232] Under these conditions, the maximum solution space size is approximately:

[0233]

[0234] 2) When 1≤r≤r*, based on Stirling's approximation, Taylor series expansion and the assumption of r<<L, the solution space size U is approximately:

[0235] U≈(L-r(l min -1)) r-1 / (r-1)! (47)

[0236] 3) When r*<r≤r max , the solution space size decreases gradually.

[0237] In summary, with parameter L fixed, the solution space size U exhibits a unimodal function as a function of r. It first increases and then decreases with r, and at r = r... * The peak value is achieved by constraining L to a small value and strategically increasing l. min This can effectively compress the range of values ​​for U to meet real-time calculation constraints.

[0238] 4.4 Comparison of the Complexity of Several Algorithms

[0239] The state estimation steps based on FSWFGO, VASWFGO, and ADASWFGO are as follows: Figure 10 As shown. The algorithm's complexity is mainly reflected in steps 3 and 4, which involve floating-point operations required for matrix and vector multiplication. A detailed computational complexity analysis of different methods is as follows:

[0240] (1) FSWFGO: This state estimation model contains one variable node and two independent velocity and position observation factors at each time point. Due to the Jacobian matrix... J is a banded sparse matrix with bandwidth b = 5. When b << N, T J and J T The computational complexity of r is O(N), and the computational complexity of the equation solver based on Cholesky decomposition is also O(N). Therefore, the computational complexity of FSWFGO with a sliding window length of N simplifies to O(N).

[0241] (2) VASWFGO: Considering the complete calculation process of the window length N, ignoring constant terms and low-order minor terms, the computational complexity of VASWFGO is in the range of O((Nl). min )·l min ), O((Nl min )·((l max -l min )·l max ).

[0242] (3)ADASWFGO: For computational complexity details, see Section 4.3.

[0243] (4) Summary of the complexity of several methods: Compared with FSWFGO, VASWFGO improves the estimation accuracy by introducing a variance-based adaptive windowing method in the local optimization framework, but also increases the computational cost. Furthermore, ADASWFGO proposes an adaptive windowing adjustment mechanism based on the distribution of anomalous data. This method dynamically optimizes the boundaries of the sub-sliding window to adaptively suppress the influence of anomalous observations in non-stationary environments, thereby obtaining more accurate estimation results. Although ADASWFGO has the highest computational complexity among the three, it still maintains real-time estimation capabilities based on its efficient optimization strategy.

[0244] 5. Simulation Experiment

[0245] To evaluate the effectiveness of the ADSWFGO method, this invention presents simulation experiments on a typical integrated navigation system (IMU / OD / VO) for Mars rovers. Based on simulated observation datasets, and with known reference ground truth, validation experiments can quantitatively assess the estimation performance of methods such as FSWFGO, VASWFGO, and ADASWFGO by constructing diverse and representative anomaly scenarios, thereby verifying the robustness of the proposed method under complex anomaly conditions.

[0246] 5.1 Data Description and Processing

[0247] Step 1: Preprocessing of raw observation data from the sensor. (1) IMU: The raw output is three-dimensional linear acceleration (X-axis acceleration, Y-axis acceleration, Z-axis acceleration), unit: m / s². 2 (1) OD: The original output is pulse count. The three-dimensional position (unit: m) and three-dimensional velocity (unit: m / s) in the navigation coordinate system are obtained by kinematic integration, and a total of six state parameters are observed. (2) OD: The original output is pulse count. The three-dimensional velocity (X-axis velocity, Y-axis velocity, Z-axis velocity) in the navigation coordinate system is obtained by pulse, vehicle parameters (such as wheel radius, wheelbase) and coordinate transformation (vehicle body coordinate system - navigation coordinate system), and the unit is m / s. A total of three state parameters are observed. (3) VO: The original output is sequence image. The three-dimensional position (X-axis position, Y-axis position, Z-axis position) in the navigation coordinate system is obtained by image calculation, and the unit is m. A total of three state parameters are observed.

[0248] Step two: Constructing the dataset. The state parameters for this simulation are three-dimensional position and three-dimensional velocity, namely X-axis position, Y-axis position, Z-axis position, X-axis velocity, Y-axis velocity, and Z-axis velocity, for a total of six parameters. Based on the state parameter categories, an observation dataset for each of the six state parameters at each time stamp is constructed.

[0249] Step 3: Dataset classification and labeling. Based on... Figure 6 The dataset classification method marks the observation dataset of each state parameter at each timestamp as a valid dataset or an abnormal dataset, thereby obtaining the distribution of abnormal data within the sliding window.

[0250] Step 4: Adaptive sliding window boundary optimization. Establish a window boundary optimization model that considers outlier data distributions, and solve for the optimal sub-sliding window.

[0251] Step 5, State estimation. Based on the divided sub-sliding windows, the corresponding observation dataset is determined, and then the state estimation results for the corresponding timestamps are obtained using the traditional factor graph optimization method based on the Gaussian assumption.

[0252] 5.2 Simulation Conditions

[0253] (1) Sensor parameters

[0254] In this simulation experiment, the navigation system performs state estimation by fusing observation data from three sensors: IMU, OD, and VO. The data acquisition frequency and key performance parameters of each sensor are set as follows:

[0255] IMU (First Sensor):

[0256] 1) Data acquisition frequency: The IMU, as the core high-frequency sensor, has a data output frequency of 100Hz, providing continuous information on the carrier's angular and velocity increments; 2) Performance parameters: The IMU's accelerometer zero bias is set to ±100μgal. This parameter can be calibrated and simulated using a precision turntable and gravity reference device to characterize the inherent systematic error of the sensor.

[0257] OD (one of the second sensors):

[0258] 1) Data acquisition frequency: The output frequency of the odometer is set to 10Hz to provide mid-frequency vehicle speed observation information; 2) Performance parameters: Its initial scale factor is configured to 0.013m / P. This parameter can be obtained by calibrating the pulse count (P) of the wheel encoder with the actual travel distance (m) through a wheel range test on a standard baseline field of known length.

[0259] VO (one of the second sensors):

[0260] 1) Data acquisition frequency: The visual odometry output frequency is set to 2Hz to provide low-frequency position observation; 2) Performance parameters: The image frame rate is not less than 30FPS, and the image resolution is higher than 1024×1024 pixels. These parameters ensure that sufficient visual feature points can be stably extracted and matched between consecutive frames, thereby guaranteeing the accuracy of pose estimation. Detailed specifications of each simulated navigation source are shown in Table 1.

[0261] Table 1. Parameters of each navigation source in the simulation experiment.

[0262]

[0263]

[0264] (2) Operation trajectory and status information

[0265] The simulation was conducted on a flat area of ​​the Martian surface, and the rover's motion was modeled as two-dimensional planar motion. The trajectory starting point was set on the Martian surface (25.1°N, 109.7°E). In the East-North-Sky navigation coordinate system, the initial position was (0,0) cm, and the initial velocity was (0,0) cm / s. Based on the Perseverance rover's speed of 4.2 cm / s on a flat, solid surface, the experimental rover's speed was controlled to be within 5 cm / s. To simulate the rover's realistic motion characteristics, three basic motion modes were designed for the simulation: constant speed, acceleration, and deceleration. The simulation duration was set to 15 minutes. The simulated distance was approximately 20.97 m. Figure 11 The simulated trajectory was displayed.

[0266] (3) Experimental Design

[0267] In complex environments, both OD and VO exhibit significant observation degradation. Specifically, in sandy or soft soil conditions, wheels are prone to slippage and spinning, leading to a substantial decrease in speed observation accuracy. In complex scenarios with repetitive or missing features, visual odometry is prone to feature mismatches, resulting in anomalous position observations. In particular, when multiple sensors (such as VO and OD) experience simultaneous observation degradation, it leads to a severe deterioration in state estimation performance. To systematically evaluate the robustness of each method in handling anomalous observations, time-varying anomalous modes of navigation sensors were simulated, and a hybrid observation scenario including normal and anomalous phases was designed. In the normal phase, the observations of each sensor follow a Gaussian noise distribution.

[0268] During the anomaly phase, sensor observations exhibit the following characteristics: 1) Four anomaly time periods (A, B, C, and D) are set. Anomaly observations within each time period correspond to different types of state parameters to simulate diverse fault scenarios; 2) The degree of anomaly degradation at each stage shows a dynamic evolution from high amplitude to low amplitude; 3) Anomalies exhibit discrete or continuous distribution patterns, with their timestamps following a uniform distribution within the time interval; 4) Within each of the A, B, C, and D anomaly time periods, for each anomaly parameter, the proportion of corresponding anomaly observations is approximately 20%. This scheme effectively simulates the spatiotemporal differences in degradation within a multi-sensor system by constructing a non-stationary, non-uniform composite fault mode. Specific details are shown in Table 2.

[0269] Table 2 Anomaly Observation Design Scheme

[0270]

[0271] (4) Comparison methods and performance evaluation indicators

[0272] Given the limited computing resources of the Mars rover platform and the real-time requirements of the mission, the fixed window length of FSWFGO is set to 10, 20, and 30, respectively denoted as FSWFGO_10, FSWFGO_20, and FSWFGO_30. Furthermore, these three FSWFGO methods, VASWFGO, and the proposed ADASWFGO are compared to evaluate the state estimation accuracy and computational complexity of different methods. The algorithm evaluation framework consists of two parts: accuracy and complexity. Accuracy is evaluated using the Cumulative Distribution Function (CDF) and the Root Mean Square Error (RMSE). Complexity is quantified using actual runtime.

[0273] 5.3 Performance Comparison of Different Methods in Position and Velocity Estimation

[0274] This experimental scheme designs synchronous anomalous perturbations in VO and OD to induce composite position and velocity errors in state estimation, thereby evaluating the robustness of the algorithm in multi-source sensing degradation scenarios. Figure 12 By comparing with the true reference values, the temporal distribution of abnormal observations of the four state parameters is visualized.

[0275] Figure 13 and Figure 14 The estimation errors of four state parameters under five methods are presented. Overall, ADASWFGO exhibits lower estimation errors compared to the other four methods, demonstrating its stronger robustness in handling anomalous observations. Furthermore, as... Figure 14 As shown, during the D time period (651-800 seconds), although no anomalies were observed in velocity observations, anomalies in position observations affected and reduced the accuracy of velocity estimation through kinematic constraints. This phenomenon verifies the theoretical conclusions regarding the error propagation mechanism.

[0276] Figure 15 The root mean square error (RMSE) of the estimation results for four state parameters under different methods is shown in Table 3. ADASWFGO significantly outperforms all benchmark methods (FSWFGO_10, FSWFGO_20, FSWFGO_30, VASWFGO) in state estimation accuracy, with average relative reductions in RMSE as follows: X-axis position 8.42%, Y-axis position 12.38%, X-axis velocity 20.65%, and Y-axis velocity 22.79%.

[0277] Table 3. Root mean square error of state parameter estimation results under different methods

[0278]

[0279] Figure 16 and Figure 17 The cumulative distribution function curves of position estimation error and velocity estimation error are shown separately. For the X-axis position, at a 90% confidence level, ADASWFGO has the smallest estimation error at 13.48 cm, while the estimation errors of FSWFGO_10, FSWFGO_20, FSWFGO_30, and VASWFGO are 17.68 cm, 15.76 cm, 14.87 cm, and 16.29 cm, respectively. For the Y-axis position, at a 90% confidence level, ADASWFGO has the smallest estimation error at 13.75 cm, while the estimation errors of FSWFGO_10, FSWFGO_20, FSWFGO_30, and VASWFGO are 18.41 cm, 16.26 cm, 14.14 cm, and 18.41 cm, respectively. For the X-axis velocity, at a 90% confidence level, ADASWFGO had the smallest estimation error of 3.05 cm / s, while the estimation errors of FSWFGO_10, FSWFGO_20, FSWFGO_30, and VASWFGO were 4.55 cm / s, 3.83 cm / s, 3.49 cm / s, and 4.29 cm / s, respectively. For the Y-axis velocity, at a 90% confidence level, ADASWFGO had the smallest estimation error of 3.13 cm / s, while the estimation errors of FSWFGO_10, FSWFGO_20, FSWFGO_30, and VASWFGO were 4.71 cm / s, 4.02 cm / s, 3.59 cm / s, and 4.27 cm / s, respectively.

[0280] To improve the reliability of the results, 100 Monte Carlo simulations were performed in this embodiment of the invention. Considering the differences in parameter units and numerical magnitudes, L2 norm normalization was used to standardize the average root mean square error of the four parameters. The results are as follows: Figure 18 As shown, in complex and anomalous scenarios, ADASWFGO significantly reduces the root mean square error of state estimation compared to the baseline method. Specifically, the RMSE of X-axis and Y-axis positions is reduced by an average of 8.74% and 8.72%, respectively, and the RMSE of X-axis and Y-axis velocities is reduced by an average of 19.25% and 19.31%, respectively.

[0281] Similarly, the average runtime after L2 norm normalization is as follows: Figure 18 As shown, VASWFGO has the longest runtime (18.28ms), while FSWFGO_20 has the shortest runtime (11.62ms). Compared to VASWFGO and FSWFGO_10, ADASWFGO has a runtime of 14.07ms, representing a reduction in computational latency of 23.07% and 10.69%, respectively. This indicates that while ensuring high estimation accuracy, the computational efficiency meets the real-time requirements of practical engineering.

[0282] This invention proposes a state estimation method based on adaptive sliding window factor graph optimization. First, it theoretically analyzes the impact mechanism of the temporal distribution of anomalous observations within the sliding window on the accuracy of state estimation. Then, it proposes a multi-channel anomaly detection method at the dataset level, achieving classification of the observation dataset and temporal localization of the anomalous observation dataset. Based on this, an adaptive sliding window boundary optimization model considering the temporal distribution of anomalous data is constructed, and the global optimal solution is efficiently obtained using the star-and-stripes theorem. This method effectively overcomes the robustness limitations of fixed-length sliding windows and variance-based adaptive sliding windows in existing factor graph optimization methods. Compared to the former, the core advantage of the proposed method is that anomaly identification focuses on the observation dataset level rather than the data level, effectively avoiding the risk of misjudgment caused by empirical distribution distortion in small sample scenarios. Compared to the latter, the proposed method effectively suppresses the significant impact of the temporal distribution of anomalous data within the sliding window on the estimation accuracy. Experimental results show that in complex scenarios with random disturbances, the proposed method reduces the root mean square error by an average of about 14.01% compared with the two mainstream sliding window methods, significantly improving the accuracy of state estimation.

[0283] The embodiments of this invention have the following technical effects: By organizing observation data collected from multiple sensors according to timestamps and state parameters, an observation dataset is generated; each observation dataset is marked as a valid dataset or an abnormal dataset based on a multi-channel anomaly detection method. This method transforms the detection object from discrete observations into a holistic observation dataset, simplifying the anomaly identification process; based on the anomaly detection results of the dataset, with the goal of minimizing the total prediction error and the sub-sliding window length as the optimization variable, a window boundary optimization model based on the distribution of abnormal data is established. This model comprehensively considers factors such as anomaly magnitude, cumulative effect, and the temporal distribution of multiple anomaly observations. Compared with the fixed sliding window factor graph optimization method and the variance-based adaptive sliding window factor graph optimization method, the adaptive sliding window factor graph optimization method considering the distribution of abnormal data in this invention can achieve higher estimation accuracy.

[0284] It should be understood that the specific order or hierarchy of steps in the disclosed process is an example of an exemplary method. Based on design preferences, it should be understood that the specific order or hierarchy of steps in the process can be rearranged without departing from the scope of this disclosure. The appended method claims provide elements of various steps in an exemplary order and are not intended to limit us to the specific order or hierarchy stated therein. In the detailed description above, various features are combined together in a single embodiment to simplify this disclosure. This method of disclosure should not be construed as reflecting an intention that an embodiment of the claimed subject matter requires more features than are explicitly stated in each claim. Rather, as reflected in the appended claims, the invention is in a state with fewer features than all the features of a single disclosed embodiment. Therefore, the appended claims are hereby explicitly incorporated into the detailed description, with each claim representing a separate preferred embodiment of the invention. The disclosed embodiments have been described above to enable any person skilled in the art to make or use the invention. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein can also be applied to other embodiments without departing from the spirit and scope of this disclosure. Therefore, this disclosure is not limited to the embodiments given herein, but is consistent with the broadest scope of the principles and novel features disclosed in this application. The foregoing description includes examples of one or more embodiments. Of course, it is impossible to describe all possible combinations of components or methods in order to describe the above embodiments, but those skilled in the art should recognize that further combinations and arrangements of the various embodiments are possible. Therefore, the embodiments described herein are intended to cover all such changes, modifications, and variations falling within the scope of the appended claims. Furthermore, the term "comprising" as used in the specification or claims is used in a manner similar to the term "including". Additionally, the use of any term "or" in the specification of the claims is intended to mean "non-exclusive or". The specific embodiments described above further illustrate the purpose, technical solutions, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are 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 robust estimation method for carrier state based on adaptive window factor graphs, characterized in that, include: Acquire observation data of multiple state parameters collected by a first sensor within the most recent historical time range, and observation data of at least one state parameter collected by at least one second sensor. The observation data of the first sensor and all the observation data of the second sensor corresponding to the same timestamp and the same state parameter are merged into the observation dataset of the state parameter under the timestamp, so as to obtain the observation dataset of each state parameter under each timestamp in the most recent historical time range. For each time stamp, the validity of the observation dataset for each state parameter is determined, and each observation dataset is marked as a valid dataset or an abnormal dataset. Based on the anomaly determination results of all observation datasets within the most recent historical time range, a window boundary optimization model considering the distribution of abnormal data is established. With the goal of minimizing the total prediction error, the window boundary optimization model is solved, and the most recent historical time range is divided into at least one sub-sliding window. For each sub-sliding window, the traditional factor graph optimization method based on the Gaussian assumption is used to process the observation dataset of each state parameter at all timestamps within the sub-sliding window, and the estimation results of each state parameter at all timestamps within the sub-sliding window are obtained.

2. The robust estimation method for carrier state based on adaptive window factor graph as described in claim 1, characterized in that, For each time stamp, the validity of the observation dataset for each state parameter is determined, and each observation dataset is marked as a valid dataset or an abnormal dataset, including: For each observation dataset of each state parameter at each timestamp, if the validity determination results of the observation dataset are all valid by the three methods of thresholding based on prior knowledge, statistical method based on multi-source data, and incremental method based on time series, the observation dataset is marked as a valid dataset; otherwise, the observation dataset is marked as an abnormal dataset.

3. The robust estimation method for carrier state based on adaptive window factor graph as described in claim 2, characterized in that, The threshold method based on prior knowledge includes: For each time stamp, determine whether the observed data in the observation dataset for each state parameter is within a reasonable range of values ​​corresponding to the state parameter. If all observation data in the observation dataset falls within a reasonable range of values ​​for the parameter, the observation dataset is deemed valid; otherwise, the observation dataset is deemed abnormal.

4. The robust estimation method for carrier state based on adaptive window factor graph as described in claim 2, characterized in that, The statistical method based on multi-source data includes: For timestamp t i The observation dataset of the s-th state parameter The observed dataset is considered valid if the test statistic G satisfies the following validity condition; otherwise, it is considered abnormal: In the formula, σ is the mean of the observed data; T is the standard deviation of the observed data; c Indicates n degrees of freedom d -2. The significance level is 0.025 / n d The critical value of the t-distribution; n d The number of observation data.

5. The robust estimation method for carrier state based on adaptive window factor graph as described in claim 2, characterized in that, The state parameters include position parameters and velocity parameters, and the corresponding observation data are position observations and velocity observations, respectively. The time-series-based incremental method includes: Let z p(i-1) and z pi These represent timestamps t and t, respectively. i-1 and t i Position observation at any of the X, Y, or Z directions, z vi Represents timestamp t i Velocity observation in the same direction as the location observation, when z p(i-1) z pi and z vi When the three observation data satisfy the following validity condition, z pi and z vi The observation dataset to which it belongs is considered valid; otherwise, z pi and z vi The associated observation dataset was determined to be an anomaly: In the formula, δ pv This is a preset threshold, the value of which is determined based on the specific task scenario and accuracy requirements.

6. The robust estimation method for carrier state based on adaptive window factor graph as described in claim 1, characterized in that, Based on the anomaly detection results of all observation datasets within the most recent historical timeframe, a window boundary optimization model considering the distribution of anomalous data is established. The window boundary optimization model is solved, dividing the most recent historical timeframe into at least one sub-sliding window, including: The window boundary optimization model is as follows: In the formula, l = [l1, l2, ..., l i ,…,l r ] represents the optimization variable; Let be the length of the i-th sub-sliding window; Indicates the number of child sliding windows; n e The number of anomalous datasets; l min Δ is the minimum length of the sub-sliding window; L is the window length used for sub-sliding window boundary optimization, i.e., the length of the most recent history time range; κ Let be the range of the κ-th outlier dataset within the most recent historical timeframe, representing the degree of outlier in the outlier dataset; d κ g is the timestamp index of the κ-th anomalous dataset; κ1 (·) represents the impact of the κth outlier dataset on the state estimation accuracy when the sub-sliding window length is l and the κth outlier dataset is located at the endpoint timestamp of the sub-sliding window; g κ2 (·) represents the impact of the κ-th outlier dataset on the state estimation accuracy when the sub-sliding window length is l and the timestamp is within the sub-sliding window; e(κ) is the index of the κ-th outlier dataset in all observation datasets within a window of length L; η κ1 η is the estimation error introduced when the κ-th outlier dataset is at the end timestamp of the sub-sliding window; κ2 The estimation error introduced when the κth outlier dataset is located at a timestamp inside the sub-sliding window; The estimation error introduced by the κth outlier dataset to the jth state parameter within the sub-sliding window when the κth outlier dataset is at the end timestamp of the sub-sliding window; The estimation error introduced by the κth outlier dataset to the jth state parameter within the sub-sliding window when the κth outlier dataset is located at the timestamp inside the sub-sliding window; β is the timestamp index of the left boundary of the i-th sub-sliding window; i This is the right boundary timestamp index of the i-th sub-sliding window.

7. A robust estimation device for carrier state based on adaptive window factor graph, characterized in that, include: The observation data acquisition unit is used to acquire observation data of multiple state parameters collected by the first sensor within the most recent historical time range and observation data of at least one state parameter collected by at least one second sensor. The dataset merging unit is used to merge the observation data of the first sensor and all the observation data of the second sensor corresponding to the same timestamp and the same state parameter into an observation dataset of the state parameter under the timestamp, so as to obtain the observation dataset of each state parameter under each timestamp in the most recent historical time range. The dataset anomaly labeling unit is used to determine the validity of the observation dataset for each state parameter at each timestamp, and to label each observation dataset as a valid dataset or an anomalous dataset. The window partitioning unit is used to establish a window boundary optimization model that considers the distribution of abnormal data based on the anomaly determination results of all observation datasets within the recent historical time range. With the goal of minimizing the total prediction error, the window boundary optimization model is solved to divide the recent historical time range into at least one sub-sliding window. The state estimation unit is used to process the observation dataset of each state parameter at all timestamps within each sub-sliding window using a traditional factor graph optimization method based on the Gaussian assumption, and to obtain the estimation results of each state parameter at all timestamps within the sub-sliding window.

8. The robust estimation device for carrier state based on adaptive window factor graph as described in claim 7, characterized in that, The dataset anomaly labeling unit is used to mark the observation dataset as a valid dataset when all three methods—thresholding based on prior knowledge, statistical method based on multi-source data, and incremental method based on time series—are valid in determining the validity of the observation dataset; otherwise, the observation dataset is marked as an anomalous dataset.

9. The robust estimation device for carrier state based on adaptive window factor graph as described in claim 7, characterized in that, Window division units include: The window boundary optimization model is as follows: In the formula, l = [l1, l2, ..., l i ,…,l r ] represents the optimization variable; Let be the length of the i-th sub-sliding window; Indicates the number of child sliding windows; n e The number of anomalous datasets; l min Δ is the minimum length of the sub-sliding window; L is the window length used for sub-sliding window boundary optimization, i.e., the length of the most recent history time range; κ Let be the range of the κ-th outlier dataset within the most recent historical timeframe, representing the degree of outlier in the outlier dataset; d κ g is the timestamp index of the κ-th anomalous dataset; κ1 (·) represents the impact of the κth outlier dataset on the state estimation accuracy when the sub-sliding window length is l and the κth outlier dataset is located at the endpoint timestamp of the sub-sliding window; g κ2 (·) represents the impact of the κ-th outlier dataset on the state estimation accuracy when the sub-sliding window length is l and the timestamp is within the sub-sliding window; e(κ) is the index of the κ-th outlier dataset in all observation datasets within a window of length L; η κ1 η is the estimation error introduced when the κ-th outlier dataset is at the end timestamp of the sub-sliding window; κ2 The estimation error introduced when the κth outlier dataset is located at a timestamp inside the sub-sliding window; The estimation error introduced by the κth outlier dataset to the jth state parameter within the sub-sliding window when the κth outlier dataset is at the end timestamp of the sub-sliding window; The estimation error introduced by the κth outlier dataset to the jth state parameter within the sub-sliding window when the κth outlier dataset is located at the timestamp inside the sub-sliding window; β is the timestamp index of the left boundary of the i-th sub-sliding window; i This is the right boundary timestamp index of the i-th sub-sliding window.

Images