A factor graph robust marginalization positioning method based on heterogeneous prior

Abstract

The application relates to a factor graph robust edge localization method based on heterogeneous prior, and belongs to the technical field of multi-source information fusion and navigation and positioning, and specifically as follows: firstly, during the sliding window promotion of the factor graph, the mean square residual of a node to be edged is extracted in the window residence period to obtain a historical comprehensive observation confidence; secondly, the fault state of the node to be edged is judged, and a heterogeneous prior information matrix containing a Gaussian distribution prior or a heavy-tailed distribution prior is constructed; then, the heterogeneous prior information matrix is taken as the input of back-end optimization solving; finally, the iterative solution of the global state of the system is completed in combination with an adaptive factor, the robust update of the carrier pose is realized, and the final positioning result is output. The application realizes time domain soft isolation of slowly varying faults and reversible correction of misjudged nodes, can effectively inhibit the time sequence transmission of slowly varying faults in the prior matrix on the premise of controlling the calculation overhead, and further improves the robustness and precision of system fusion positioning in a complex environment.

Description

Technical Field

[0001] This invention belongs to the field of multi-source information fusion and navigation positioning technology, specifically involving a factor graph robust edge positioning method based on heterogeneous priors. Background Technology

[0002] With the application of mobile platform technologies such as autonomous driving and drones, the fusion of multiple sensor sources, including Global Navigation Satellite System (GNSS) and Inertial Measurement Unit (IMU), has become a common solution for ensuring continuous positioning of these platforms. Currently, multi-source fusion architectures based on Factor Graph Optimization (FGO) exhibit certain advantages in handling nonlinear observations and are widely used in navigation and positioning systems. To balance estimation accuracy and computational efficiency, this architecture often employs a sliding window mechanism and utilizes edge detection operations to transform state nodes that move out of the window into Gaussian prior information matrices, thus preserving historical observation constraints.

[0003] However, in complex application scenarios such as urban canyons, sensors are susceptible to interference from multipath effects and non-line-of-sight (NLOS) signals, leading to significant biases in the observation data. This is particularly true for slowly varying faults, where the incremental bias is highly concealed and often difficult for real-time detection mechanisms to capture in a single epoch. The standard factor graph marginalization mechanism is based on the ideal assumption that the observation error follows a Gaussian distribution. If a node containing slowly varying fault bias is marginalized without effective identification, the fault information will be integrated and solidified in the prior information matrix.

[0004] Existing research has attempted to mitigate the impact of anomalous observations by introducing robust estimation, such as configuring robust kernel functions for active measurement residuals during the optimization phase to adjust their weights. While these methods are effective in handling measurement anomalies within the current window, they have limitations when dealing with marginalized prior constraints. Because traditional kernel functions primarily operate on current measurement factors, they fail to effectively intervene in the generation mechanism of prior factors. If a slowly varying fault node has been transformed into a standard Gaussian prior matrix, its quadratic cost function will be sensitive to residual errors. These errors will propagate temporally throughout the system as the sliding window progresses, affecting the accuracy of subsequent state calculations and even causing a continuous degradation in positioning accuracy.

[0005] Therefore, how to effectively evaluate the node state during the edge-up stage, improve the single Gaussian prior construction method, and propose a heterogeneous prior construction mechanism that can achieve temporal isolation and dynamic weight adjustment for slowly changing faults has become an important research direction for improving the reliability of multi-source fusion positioning in complex environments. Summary of the Invention

[0006] The purpose of this invention is to address the technical problem in existing factor graph multi-source fusion localization algorithms where slowly varying faults are easily solidified into standard Gaussian priors during the marginalization stage, leading to continuous contamination of the prior information matrix and causing slow divergence in system localization. This invention provides a factor graph robust marginalization localization method based on heterogeneous priors. By introducing historical residence period state assessment and heterogeneous prior construction mechanisms during the marginalization stage, this method cuts off the temporal propagation of fault deviations at the source of information transmission, effectively improving the reliability of system state estimation results in complex environments.

[0007] To achieve the above objectives, the technical solution of the present invention is as follows:

[0008] The factor graph robust marginalization localization method based on heterogeneous priors suppresses the impact of slowly changing faults on navigation results by constructing a robust graph optimization framework with historical state evaluation and prior equivalent reconstruction. The method includes the following steps:

[0009] S1, Sliding window construction and historical residual extraction: Multi-source sensor observation data of the mobile carrier are acquired to construct a factor graph sliding window optimization model. As the sliding window advances and nodes face marginalization, the historical residual sequence of the nodes to be marginalized during the window residence period is extracted, and their mean square residuals are calculated.

[0010] S2, Node confidence assessment and anomaly determination: Using the obtained mean square residuals, the historical comprehensive observation confidence of the node to be marginalized is calculated, and the reliability of its observation status is determined based on the confidence level to identify whether there is a slowly changing fault bias.

[0011] S3, Heterogeneous prior information matrix construction: Based on the anomaly determination results, the nodes to be marginalized are processed by traffic splitting: For normal nodes with a confidence level meeting a preset threshold, standard marginalization is performed and a Gaussian distributed prior information matrix is ​​constructed; for slowly varying fault nodes with a confidence level not meeting the preset threshold, an anti-pollution mechanism is triggered, and a heavy-tailed distributed prior information matrix is ​​constructed.

[0012] S4, iterative reweighted equivalent mapping, inputs the generated heterogeneous prior information matrix into the back-end optimization solver. For the heavy-tailed prior information matrix, the IRLS architecture is introduced, and its cost function characteristics are used to map it equivalently into factors with dynamic adaptive weights.

[0013] S5, Global State Iteration and Localization Output: Combining the transformed dynamic adaptive weight factor and other effective measurement factors within the current sliding window, a global objective cost function is constructed. The iterative optimization of the system state is completed by minimizing this cost function, updating the carrier pose and outputting the final localization result.

[0014] Specifically, step S1 includes:

[0015] S11 Measurement residual vector calculation and normalization, to obtain the mobile carrier in the first... The actual measurement vector of the sensor in each epoch. Characterizing the first Based on the actual observed characteristics of the external environment of the carrier, the predicted system state vector values ​​representing the current three-dimensional position, velocity, and attitude of the carrier are extracted. Its components are represented as In the formula For the first The three-dimensional position vector of each epoch carrier It is a three-dimensional velocity vector. It is a quaternion attitude vector. The sensor bias vector is used in the sensor's nonlinear measurement model. Predicted values Mapped to the sensor's measurement space, and the single-epoch measurement residual vector is calculated. , Let the actual sensor measurement vector at the k-th epoch be introduced. The covariance matrix of the sensor measurement noise corresponding to each epoch. The measurement residual vector is normalized using Mahalanobis distance to obtain the dimensionless scalar squared residual. :

[0016]

[0017] In the formula, For the first The measurement residual vector of each epoch. for The transpose of , For the measurement noise covariance matrix The inverse matrix,

[0018] S12 Window residence period sequence construction and mean squared residual extraction: As the mobile carrier moves over time, step S11 is continuously executed, and the calculated single-epoch scalar squared residuals are extracted. The historical data queue of the factor graph sliding window is dynamically pushed in. When the number of nodes in the sliding window reaches the preset maximum capacity threshold, the system's edge detection mechanism is triggered. The state node with the oldest timestamp in the window is selected as the node to be edged out. The lifetime of the node to be edged out in the factor graph sliding window is tracked, and its status within the window is extracted. scalar squared residual sequence under consecutive historical epochs Calculate the historical mean square residuals of the nodes to be marginalized. :

[0019]

[0020] In the formula, The total number of historical epochs of the nodes to be marginalized within the sliding window. Let be the scalar squared residual corresponding to the i-th epoch.

[0021] In practical applications of multi-source fusion positioning for mobile carriers, the dimensions of different sensors vary greatly, and slowly varying faults are highly concealed. The S1 method of this invention eliminates the dimensional barriers between heterogeneous sensors through Mahalanobis distance normalization and further extracts the historical mean square residuals of node dwell times based on a factor graph sliding window. This achieves a leap from single-epoch transient assessment to time-series panoramic assessment, accurately capturing concealed slowly varying faults that are difficult to detect with single-variable detection. Simultaneously, this preserves a complete historical data structure foundation for subsequent confidence determination and heterogeneous prior construction, significantly enhancing the system's ability to cope with complex fault modes.

[0022] Specifically, step S2 includes:

[0023] S21 Heterogeneous mapping and historical comprehensive observation confidence calculation, based on the mean square residual obtained in S12. A Welsch robust kernel function weight mapping model with heavy-tailed characteristics is introduced to calculate the historical comprehensive observation confidence of the node to be marginalized. The calculation formula is as follows:

[0024]

[0025] In the formula, The preset confidence distribution adjustment parameter is used to control the penalty scale of the robust kernel function on the error. Since the residuals have been normalized by Mahalanobis distance in the previous steps, the empirical range of the parameter is set to be [value missing]. ,

[0026] S22 Temporal weighted geometric average equivalent definition, defining the node to be edged in the first... The independent Welsch robustness weights for each epoch are: To establish the confidence level of the aforementioned historical comprehensive observations The mapping logic between independent robust weights is constructed through the following mathematical derivation process to prove that it is equivalent to the geometric mean:

[0027] The mean square residual formula Expanding and substituting into the confidence formula, we get:

[0028]

[0029] Extract the constant coefficients in exponentiation operations to separate the summation operation from the exponentiation mapping:

[0030]

[0031] Using the rules of operation for natural exponential functions, the summation term inside is transformed into a series of polynomial products, resulting in:

[0032]

[0033] Independent weights for each epoch Substituting the definition into the above equation, we finally prove and establish its equivalent mapping relationship as follows:

[0034]

[0035] Through the above derivation and proof, the equivalence between the comprehensive confidence level and the independent weights is established, and the algorithm only needs to rely on a single mean square residual. With the input, it is possible to achieve weight states without saving all historical single-epoch weight states. In this context, a quantitative characterization of the reliability of the comprehensive observation status of nodes can be achieved.

[0036] S23 Slowly varying fault determination based on hypothesis testing: Based on the prior assumption that the sensor measurement residuals follow a Gaussian distribution, and that the corresponding scalar squared residuals follow a chi-square distribution, a significance level is set. With system state degrees of freedom Find or calculate the critical value of the chi-square distribution under the corresponding conditions. The critical value is used to reverse-engineer and set a confidence threshold. The threshold derivation formula is as follows:

[0037]

[0038] The historical comprehensive observation confidence level calculated by S21 With the decision threshold Compare:

[0039] when When the observed state of the node to be marginalized is determined to be reliable and there are no fault values,

[0040] when When a node to be edged is determined to be contaminated, its fault value is identified, and a subsequent anti-contamination isolation mechanism is triggered.

[0041] In multi-source robust estimation, existing techniques rely on single-epoch transient residuals and empirical thresholds, which are insufficient to handle hidden, slowly varying faults. Furthermore, storing massive amounts of historical weights to assess long-term reliability leads to a surge in system overhead. To address these shortcomings, this invention, S2, introduces a comprehensive confidence assessment mechanism based on the Welsch robust kernel function. This mechanism nonlinearly maps the mean square residual to a confidence level and proves its equivalence to the geometric mean of time-independent weights. This allows the system to rigorously quantify node states using only a single mean square residual parameter, completely eliminating the memory burden of storing complete historical sequences. In addition, S2 combines chi-square distribution with back-derived decision thresholds, transforming subjective parameter tuning into objective statistical hypothesis testing. This significantly reduces computational and memory overhead while improving the theoretical rigor and adaptability of fault identification, laying a reliable decision foundation for subsequent heterogeneous prior derivation.

[0042] Specifically, step S3 includes:

[0043] S31 Local graph optimization model linearization, determining nodes to be marginalized. and the set of reserved nodes with which measurement constraints exist. Define the local system state vector as , To preserve the node state vector transpose, State vector of the node to be edged The transpose of, wherein the state components include, but are not limited to, three-dimensional position. 3D velocity Quaternion posture and sensor bias ...

[0044] Using the Gauss-Newton method to analyze the local joint measurement residual vector Perform a first-order Taylor expansion at the current state point to obtain its linearized approximate expression:

[0045]

[0046] in, It is a local Jacobian matrix. The state increment to be solved.

[0047] Introducing the joint measurement noise covariance matrix Construct the minimum objective function

[0048]

[0049] By letting the objective function be The derivative is zero, and the incremental equations of the local system are constructed.

[0050]

[0051] Approximate Hessian matrix With gradient vector The calculation is defined as follows:

[0052]

[0053] In the formula, For the local joint measurement residual vector The value of the linearization point in the current state.

[0054] Based on state variables and The dimension of the state increment The corresponding incremental components are split into retained nodes. Incremental components of the node to be edged The approximate Hessian matrix and gradient vector are then divided into blocks:

[0055]

[0056] in , , , and These are the corresponding state components in the Jacobian matrix J. and Column partitioning ,

[0057] S32 constructs the Gaussian prior for normal nodes. For the normal nodes determined to be uncontaminated in S23, in order to maintain the size of the control factor graph without losing the historical constraint information contained in the nodes to be marginalized, the system uses the standard Schur complement elimination operation to... Marginalization, based on the locally linearized block equations constructed in S31:

[0058]

[0059]

[0060] Due to the need to To remove a state from the system, first extract and solve for the increment of the state to be marginalized from the second equation in the system of equations. :

[0061]

[0062] Then the above Substituting the expression into the first equation of the system of equations, the algebraic substitution completely eliminates the error. The dimension, and extract information about the preserved state. Factoring the common factor, the reconstructed equation is as follows:

[0063]

[0064] At this point, the coefficient matrix extracted from the left side of the equation is the Gaussian prior information matrix. The right side of the equation is the prior gradient vector. :

[0065]

[0066]

[0067] Based on this, define the retention state. scalar equivalent prior residual square for:

[0068]

[0069] Due to the nodes to be edged out Nodes identified as normal Gaussian nodes are marginalized and their results are equivalently constructed as standard quadratic Gaussian prior cost factors in the global objective cost function. :

[0070]

[0071] The Gaussian prior cost factor In subsequent optimization calculations, this will be used as a historical experience constraint on the current carrier state. The solution provides unbiased convergence tension.

[0072] The heavy-tailed prior construction for the S33 fault node addresses the issue of slowly varying faults in S23 that trigger isolation mechanisms. If the standard Gaussian prior assumption is followed, the corresponding quadratic cost function will generate a large penalty gradient for fault deviations, leading to divergence in the global state estimation. Therefore, a Welsch heavy-tailed robust kernel function with flat tails is introduced. A nonlinear kernel mapping is performed on the prior information matrix.

[0073] The system also uses Schur complement elimination to extract the fundamental prior matrix and reuses the scalar equivalent prior residual squares defined in S32. Subsequently, the Welsch function structure was introduced to transform quadratic forms. Nonlinear mapping to heavy-tailed prior cost factor :

[0074]

[0075] In the formula, The preset prior kernel function adjustment parameters are used to control the truncation scale of the fault deviation of the heavy-tailed distribution to the marginalized nodes. Their empirical value range is set as follows: ,

[0076] The robust physical mechanism of the heavy-tailed cost factor can be analyzed by examining its influence function. To provide a rigorous proof, we take the first-order partial derivative of the cost function to obtain its corresponding penalty gradient vector:

[0077]

[0078] Mathematical analysis reveals that the Welsch heavy-tailed kernel function satisfies the following robust statistical boundary properties:

[0079] When the prior residual is small , The cost factor degenerates into a standard Gaussian distribution, preserving the system's normal constraint information.

[0080] When the fault deviation causes a sharp increase in the prior residual The upper limit of the cost function converges to a constant. (Right now Furthermore, the limit of its influence function converges to zero:

[0081]

[0082] The above heavy-tailed prior mechanism can ensure that when faced with huge timing fault disturbances, the prior cost factor has an impact on the current system state variables. The applied residual pulling gradient strictly approaches zero, thus achieving soft isolation of fault timing propagation without disrupting the sparse structure of the local Hessian matrix.

[0083] Standard factor graph marginalization methods rely on the ideal assumption that observation errors follow a Gaussian distribution. When faced with marginalized nodes containing hidden, slowly varying faults, traditional methods forcibly transform them into standard quadratic Gaussian priors. Since the penalty gradient of the Gaussian cost factor grows unbounded with the residual, the fixed historical faults will continuously introduce estimation biases into subsequent state solutions, leading to divergence in system localization results. To address this problem, this invention S3 constructs a heterogeneous prior mechanism based on confidence-based diversion, employing a method that... The heavy-tailed robust kernel function performs a nonlinear mapping on its prior information matrix. This mechanism utilizes the boundary statistics of the heavy-tailed distribution, namely, that its influence function strictly converges to zero when the prior residuals approach infinity. This treatment limits the anomalous gradients imposed on the current state variables by historical fault disturbances, achieving soft isolation of the time-series propagation of slowly varying faults without disrupting the sparse structure of the local Hessian matrix, thereby improving the reliability of the prior information matrix and the robustness of system state estimation.

[0084] Specifically, step S4 includes:

[0085] S41 inputs the prior information matrix of fault nodes containing heavy-tailed distribution generated in S3 into the back-end optimization solver. Since the objective function contains a non-quadratic heavy-tailed kernel function term, it cannot be solved directly using the standard Gauss-Newton method. Therefore, the system introduces an IRLS optimization architecture, which is equivalent to a dynamically weighted Gaussian least squares problem. Before starting the back-end optimization solver, the initial number of iterations is set. The prior cost factor of the heavy-tailed distribution is temporarily equivalent to a standard Gaussian factor, and its dynamic adaptive weights are initialized as follows: ,

[0086] S42 Prior equivalent residual extraction, in the first In the next nonlinear optimization iteration, the incremental estimate based on the current retained state vector and the Gaussian prior information matrix obtained in S32 With the prior gradient vector Calculate the squared scalar equivalent prior residual of the faulty node in the current iteration step. Its equivalent mapping calculation formula is:

[0087]

[0088] S43 IRLS dynamic weight calculation, based on the scalar equivalent prior residual squared extracted in S42. By utilizing the derivative property of the cost function of the heavy-tailed robust kernel function, the dynamic adaptive weights required for the next iteration are calculated. Its update formula is:

[0089]

[0090] In the formula, This is the adjustment parameter for the prior kernel function in S33, used here to control the decay rate of the dynamic adaptive weights as the equivalent prior residual increases.

[0091] S44 Global Hessian Matrix Reconstruction and Iterative Solution, Calculating the Dynamic Adaptive Weights The prior information matrix and prior gradient vector applied to the slowly varying fault node are subjected to an equivalent Gaussian weighted mapping to obtain the first... The adaptive prior matrix of the next iteration With adaptive gradient vector :

[0092]

[0093] .

[0094] In the back-end graph optimization stage of multi-source fusion localization, existing standard nonlinear solvers only support quadratic cost functions based on the Gaussian assumption. Directly introducing non-quadratic heavy-tailed robust priors can easily disrupt the fundamental least-squares framework and cause numerical solution divergence. This invention, S4, effectively resolves this underlying mathematical conflict by introducing an IRLS architecture to map the complex heavy-tailed prior nonlinearity into a Gaussian factor with dynamically adaptive weights. This conversion mechanism allows the system to achieve rigorous decoupling and compatibility between heterogeneous robust priors and the standard graph optimization kernel without requiring intrusive modifications to existing mature back-end optimizers. This not only significantly reduces the engineering deployment difficulty of robust algorithms in actual unmanned navigation systems but also ensures the computational efficiency and global numerical stability of complex optimization models when dealing with severe fault interference at the architectural level.

[0095] Specifically, step S5 includes:

[0096] S51 global objective cost function is jointly constructed to extract the effective multi-source sensor measurement set within the current sliding window. The effective measurement cost factor is defined as the sum of squared Mahalanobis distances of the residuals of each measurement. ,

[0097] The adaptive prior matrix obtained by the equivalent mapping in S44 With adaptive gradient vector As a robust prior cost factor, a global objective cost function is constructed based on the Bayesian maximum a posteriori estimation criterion, which includes robust prior constraints and current local sensor measurement constraints. Its optimization objective is defined as:

[0098]

[0099] In the formula, The set of global system state variables within the sliding window. and The first Actual observed values ​​and prediction models of each measurement factor This is the set of indices for valid multi-source sensor measurement factors within the current sliding window. For set The measurement factor index in

[0100] S52 Manifold space state update based on LM algorithm, using the aforementioned nonlinear optimization algorithm to solve the global objective cost function.

[0101] For the effective multi-source sensor measurement set within the current sliding window Each measurement factor in Find the Jacobian matrix And combined with the corresponding measurement noise covariance matrix With residual Calculate the Hessian matrix of the current local measurement. With gradient vector :

[0102]

[0103]

[0104] In the formula, This is the set of indices of the valid measurement factors within the current sliding window. For the first Individual measurement factors on global state variables Jacobian matrix, For the corresponding measurement residual vector,

[0105] The adaptive prior matrix output in stage S44 and gradient vector By rigorously aligning and combining the aforementioned measurement information with matrix dimensions, a global Hessian matrix that incorporates historical robustness experience and current actual measurements is reconstructed. With global gradient vector :

[0106]

[0107]

[0108] Considering that complex observation environments can easily lead to ill-conditioned matrices or iterative divergence in the pure Gauss-Newton method, the system introduces a damping factor from the LM algorithm on the main diagonal of the global Hessian matrix. Construct linear incremental solution equations with trust region characteristics:

[0109]

[0110] In the formula, To obtain the identity matrix with the same dimensions as the global Hessian matrix, the above equation can be decomposed to find the first identity matrix. System state increment of the next iteration The system state vector is then updated by introducing a generalized addition operation. The system state vector is rigorously updated on the nonlinear manifold space:

[0111]

[0112] In the formula, For the first For the global system state vector in the next iteration, the standard linear addition is used for incremental updates of the Euclidean space state components; for the attitude state components, the attitude error increment is mapped to the Lie group space through an exponential mapping mechanism, and the attitude state update on the nonlinear manifold space is completed using quaternion multiplication.

[0113] S53 Convergence determination and robust positioning results output, calculate the current manifold space state increment. The norm of , and the preset optimization convergence threshold. Make a judgment:

[0114] like If the system has not yet converged, then the number of iterations is set to zero. Return to execution S42 to re-extract the equivalent residual and enter the next round of loop optimization.

[0115] like If the convergence condition is met, stop the global state iteration and start from the optimized global system state variable. In the process, the three-dimensional position, velocity, and attitude information of the mobile carrier in the current epoch are extracted and output as the final highly reliable positioning result, and provide robust state feedback for the subsequent autonomous navigation and motion control of the mobile carrier.

[0116] In complex observation environments such as urban canyons, the degradation of multi-source sensor quality often leads to ill-conditioned global Hessian matrix, causing traditional optimization algorithms to diverge or output non-physically abrupt results. This invention, S5, constructs a global objective function that integrates robust priors and real-time measurements, and employs a Levenberg-Marquardt algorithm with a damping factor for iterative solution in manifold space. This effectively suppresses numerical instability caused by singular matrices. Furthermore, by combining the exponential mapping update mechanism of Lie groups and Lie algebras, the geometric rigor of the 3D attitude is ensured during the nonlinear optimization process. This step guarantees robust convergence of the global state solution under extreme conditions, enabling the system to stably output smooth, continuous, and high-confidence positioning results, thus solving the robustness output problem of multi-source fusion navigation in complex environments.

[0117] Compared with the prior art, the advantages of the present invention are as follows:

[0118] This invention constructs a confidence assessment mechanism based on a temporal panorama, achieving accurate perception of hidden, slowly changing faults and deep optimization of system overhead. Traditional factor graph multi-source fusion methods often rely on single-epoch transient residuals for anomaly detection, which is prone to missing minute, slowly changing faults. Furthermore, for long-term time-series assessments, they often require continuous storage of massive historical weights, leading to a surge in memory overhead. This invention extracts the mean square residual (S1) of the sliding window residence period, using the integral effect to amplify early hidden biases. Simultaneously, through rigorous mathematical derivation, it establishes an equivalent mapping relationship between the comprehensive confidence and the geometric mean of the temporal weights (S2). This mechanism allows the system to rigorously quantify node states with only a single parameter, completely eliminating the memory burden of storing complete historical sequences. Combined with the chi-square distribution to inversely deduce the decision threshold, subjective parameter tuning is transformed into objective hypothesis testing, significantly improving the adaptive capability and theoretical rigor of fault identification.

[0119] This invention proposes a heterogeneous prior construction strategy based on heavy-tailed distribution, fundamentally blocking the cross-domain propagation of long-term fault biases. Traditional marginalization mechanisms forcibly convert nodes removed from the window into standard Gaussian priors, causing unremoved fault biases to be solidified in the prior matrix as the window advances, leading to system localization divergence. This invention performs heterogeneous flow splitting (S3) based on node confidence, and uses a Welsch robust kernel function for nonlinear mapping of nodes determined to be contaminated. This mechanism utilizes the boundary property that the penalty gradient of the heavy-tailed distribution strictly converges to zero when the residual approaches infinity. Without destroying the sparse structure of the Hessian matrix of the system, it mathematically cuts off the abnormal pulling force exerted by historical faults on the current state, achieving soft isolation of fault time-series propagation and effectively protecting the purity of the prior information base.

[0120] This invention achieves seamless decoupling between heterogeneous robust priors and the standard graph optimization kernel, balancing theoretical rigor with ease of engineering deployment. In backend graph optimization, standard nonlinear solvers heavily rely on the Gaussian quadratic assumption. Directly introducing non-quadratic heavy-tailed priors can easily disrupt the fundamental least-squares framework and lead to numerical divergence. This invention introduces the IRLS architecture (S4), which, based on the derivative properties of robust kernel functions, effectively transforms complex heavy-tailed nonlinear mappings into Gaussian factors with dynamically adaptive weights. This transformation mechanism eliminates the need for invasive modifications to existing mature backend optimizers, bridging the underlying mathematical differences between heterogeneous priors and standard solvers, and ensuring computational efficiency and global numerical stability of complex optimization models with robust constraints.

[0121] This invention establishes a globally robust solution framework in manifold space, ensuring reliable convergence of system positioning under complex and extreme conditions. In severely disturbed environments such as urban canyons, the rapid degradation of multi-source observation quality can easily lead to ill-conditioned global Hessian matrix, causing drastic, non-physical jumps in the solution results. This invention constructs a global objective function (S5) by combining adaptive robust priors and real-time measurements, and employs a damped LM algorithm for rigorous iteration in manifold space. This mechanism sets a dynamic safety boundary for the carrier state update, effectively absorbing numerical oscillations caused by singular matrices. Simultaneously, combined with the exponential mapping update mechanism of Lie groups and Lie algebras, it ensures the geometric rigor of the three-dimensional attitude in nonlinear optimization, enabling the system to output smooth, continuous, and high-confidence poses even under extreme conditions.

[0122] In summary, this invention overcomes the limitations of traditional Gaussian edge detection mechanisms and open-loop transient detection. By introducing temporal confidence assessment, heavy-tailed heterogeneous prior construction, and IRLS equivalent solution mechanisms, it systematically solves the technical challenges of missed detection of slowly varying faults and the susceptibility of historical prior matrices to contamination. Its heterogeneous offloading and decoupled solution architecture not only ensures the robustness and numerical stability of the algorithm but also significantly reduces the system memory and computational overhead for long-term robust assessment. Therefore, this method provides outstanding technical advancement and high engineering practical value for highly reliable multi-source fusion autonomous navigation of mobile vehicles such as UAVs and autonomous vehicles in real-world conditions susceptible to interference, such as urban canyons and complex tree-lined roads. Attached Figure Description

[0123] Figure 1 Overall flowchart of the present invention

[0124] Figure 2 The confidence assessment and heterogeneous prior derivation construction flowchart of this invention.

[0125] Figure 3 The flowchart of IRLS iterative solution and convergence output of the present invention. Detailed Implementation

[0126] The specific embodiments of the present invention are described below to facilitate understanding by those skilled in the art. However, it should be understood that the present invention is not limited to the scope of the specific embodiments. For those skilled in the art, various modifications are obvious as long as they fall within the spirit and scope of the present invention as defined and determined by the appended claims. All inventions utilizing the concept of the present invention are protected. To deepen the understanding of the present invention, the solution is described in detail below with reference to the accompanying drawings.

[0127] Example: See Figure 1 , Figure 2 , Figure 3A robust marginalization localization method based on heterogeneous priors using factor graphs includes the following steps: The method introduces a historical comprehensive observation confidence assessment and heterogeneous prior construction mechanism during the factor graph sliding window marginalization stage, and combines it with an IRLS architecture to effectively block the temporal propagation of slowly varying faults in the prior matrix and suppress the impact of observation bias on the localization results. Specifically, it includes the following steps:

[0128] S1, Sliding window construction and historical residual extraction: Multi-source sensor observation data of the mobile carrier are acquired to construct a factor graph sliding window optimization model. As the sliding window advances and nodes face marginalization, the historical residual sequence of the nodes to be marginalized during the window residence period is extracted, and their mean square residuals are calculated.

[0129] S2, Node confidence assessment and anomaly determination: Using the obtained mean square residuals, the historical comprehensive observation confidence of the node to be marginalized is calculated, and the reliability of its observation status is determined based on the confidence level to identify whether there is a slowly changing fault bias.

[0130] S3, Heterogeneous prior information matrix construction: Based on the anomaly determination results, the nodes to be marginalized are processed by traffic splitting: For normal nodes with a confidence level meeting a preset threshold, standard marginalization is performed and a Gaussian distributed prior information matrix is ​​constructed; for slowly varying fault nodes with a confidence level not meeting the preset threshold, an anti-pollution mechanism is triggered, and a heavy-tailed distributed prior information matrix is ​​constructed.

[0131] S4, iterative reweighted equivalent mapping, inputs the generated heterogeneous prior information matrix into the back-end optimization solver. For the heavy-tailed prior information matrix, the IRLS architecture is introduced, and its cost function characteristics are used to map it equivalently into factors with dynamic adaptive weights.

[0132] S5, Global State Iteration and Localization Output: Combining the transformed dynamic adaptive weight factor and other effective measurement factors within the current sliding window, a global objective cost function is constructed. The iterative optimization of the system state is completed by minimizing this cost function, updating the carrier pose and outputting the final localization result.

[0133] Specifically, step S1 includes:

[0134] S11 Measurement Residual Vector Calculation and Normalization, Obtaining the Moving Carrier in the... The actual measurement vector of the sensor in each epoch. Characterizing the first Based on the actual observed characteristics of the external environment of the carrier, the predicted system state vector values ​​representing the current three-dimensional position, velocity, and attitude of the carrier are extracted. Its components are represented as In the formula Let be the three-dimensional position vector of the carrier in the k-th epoch. It is a three-dimensional velocity vector. It is a quaternion attitude vector. The sensor bias vector is used in the sensor's nonlinear measurement model. Predicted values Mapped to the sensor's measurement space, and the single-epoch measurement residual vector is calculated. , Let the actual sensor measurement vector at the k-th epoch be introduced. The covariance matrix of the sensor measurement noise corresponding to each epoch. The measurement residual vector is normalized using Mahalanobis distance to obtain the dimensionless scalar squared residual. :

[0135]

[0136] In the formula, For the first The measurement residual vector of each epoch. for The transpose of , For the measurement noise covariance matrix The inverse matrix,

[0137] S12 window residence period sequence construction and mean squared residual extraction: As the mobile carrier moves over time, step S11 is continuously executed, and the calculated single-epoch scalar squared residuals are extracted. The historical data queue of the factor graph sliding window is dynamically pushed in. When the number of nodes in the sliding window reaches the preset maximum capacity threshold, the system's edge detection mechanism is triggered. The state node with the oldest timestamp in the window is selected as the node to be edged out. The lifetime of the node to be edged out in the factor graph sliding window is tracked, and its status within the window is extracted. scalar squared residual sequence under consecutive historical epochs Calculate the historical mean square residuals of the nodes to be marginalized. :

[0138]

[0139] In the formula, The total number of historical epochs of the nodes to be marginalized within the sliding window. Let be the scalar squared residual corresponding to the i-th epoch.

[0140] Specifically, step S2 includes:

[0141] S21 Heterogeneous Mapping and Historical Integrated Observation Confidence Calculation, Based on the Mean Square Residue Calculated in S12 A Welsch robust kernel function weight mapping model with heavy-tailed characteristics is introduced to calculate the historical comprehensive observation confidence of the node to be marginalized. The calculation formula is as follows:

[0142]

[0143] In the formula, The preset confidence distribution adjustment parameter is used to control the penalty scale of the robust kernel function on the error. Since the residuals have been normalized by Mahalanobis distance in the previous steps, the empirical range of the parameter is set to be [value missing]. ,

[0144] S22 Temporal Weighted Geometric Mean Equivalent Definition, defining the node to be marginalized in the... The independent Welsch robustness weights for each epoch are: To establish the confidence level of the aforementioned historical comprehensive observations The mapping logic between independent robust weights is constructed through the following mathematical derivation process to prove that it is equivalent to the geometric mean:

[0145] The mean square residual formula Expanding and substituting into the confidence formula, we get:

[0146]

[0147] Extract the constant coefficients in exponentiation operations to separate the summation operation from the exponentiation mapping:

[0148]

[0149] Using the rules of operation for natural exponential functions, the summation term inside is transformed into a series of polynomial products, resulting in:

[0150]

[0151] Independent weights for each epoch Substituting the definition into the above equation, we finally prove and establish its equivalent mapping relationship as follows:

[0152]

[0153] Through the above derivation and proof, the equivalence between the comprehensive confidence level and the independent weights is established, and the algorithm only needs to rely on a single mean square residual. With the input, it is possible to achieve weight states without saving all historical single-epoch weight states. In this context, a quantitative characterization of the reliability of the comprehensive observation status of nodes can be achieved.

[0154] S23. Abnormal observation determination based on hypothesis testing: Based on the prior assumption that sensor measurement residuals follow a Gaussian distribution, and that the corresponding scalar squared residuals follow a chi-square distribution, a significance level is set. With system state degrees of freedom Find or calculate the critical value of the chi-square distribution under the corresponding conditions. The critical value is used to reverse-engineer and set a confidence threshold. The threshold derivation formula is as follows:

[0155]

[0156] The historical comprehensive observation confidence level calculated by S21 With the decision threshold Compare:

[0157] when When the observation status of the node to be marginalized is determined to be reliable and there are no abnormal observations,

[0158] when When a node to be marginalized is determined to be contaminated, abnormal observations are identified, and subsequent contamination prevention and isolation mechanisms are triggered.

[0159] Specifically, step S3 includes:

[0160] S31 local graph optimization model linearization, determining nodes to be marginalized. and the set of reserved nodes with which measurement constraints exist. Define the local system state vector as , To preserve the node state vector transpose, State vector of the node to be edged The transpose of, wherein the state components include, but are not limited to, three-dimensional position. 3D velocity Quaternion posture and sensor bias ...

[0161] Using the Gauss-Newton method to analyze the local joint measurement residual vector Perform a first-order Taylor expansion at the current state point to obtain its linearized approximate expression:

[0162]

[0163] in, It is a local Jacobian matrix. The state increment to be solved.

[0164] Introducing the joint measurement noise covariance matrix Construct the minimum objective function

[0165]

[0166] By letting the objective function be The derivative is zero, and the incremental equations of the local system are constructed.

[0167]

[0168] Approximate Hessian matrix With gradient vector The calculation is defined as follows:

[0169]

[0170] In the formula, For the local joint measurement residual vector The value of the linearization point in the current state.

[0171] Based on state variables and The dimension of the state increment The corresponding incremental components are split into retained nodes. Incremental components of the node to be edged The approximate Hessian matrix and gradient vector are then divided into blocks:

[0172]

[0173] in , , , and These are the corresponding state components in the Jacobian matrix J. and Column partitioning ,

[0174] S32 normal nodes are constructed using Gaussian priors. For the normal nodes determined to be uncontaminated in S23, in order to maintain the size of the control factor graph without losing the historical constraint information contained in the nodes to be marginalized, the system uses standard Schur complement elimination operations to... Marginalization, based on the locally linearized block equations constructed in S31:

[0175]

[0176]

[0177] Due to the need to To remove a state from the system, first extract and solve for the increment of the state to be marginalized from the second equation in the system of equations. :

[0178]

[0179] Then the above Substituting the expression into the first equation of the system of equations, the algebraic substitution completely eliminates the error. The dimension, and extract information about the preserved state. Factoring the common factor, the reconstructed equation is as follows:

[0180]

[0181] At this point, the coefficient matrix extracted from the left side of the equation is the Gaussian prior information matrix. The right side of the equation is the prior gradient vector. :

[0182]

[0183]

[0184] Based on this, define the retention state. scalar equivalent prior residual square for:

[0185]

[0186] Due to the nodes to be edged out Nodes identified as normal Gaussian nodes are marginalized and their results are equivalently constructed as standard quadratic Gaussian prior cost factors in the global objective cost function. :

[0187]

[0188] The Gaussian prior cost factor In subsequent optimization calculations, this will be used as a historical experience constraint on the current carrier state. The solution provides unbiased convergence tension.

[0189] The heavy-tailed prior construction for the S33 slowly changing fault node addresses the issue of the S23 slowly changing fault node being identified as contaminated and triggering the anti-contamination isolation mechanism. If the standard Gaussian prior assumption is followed, the corresponding quadratic cost function will generate a large penalty gradient on the abnormal residuals, leading to divergence in the global state estimation. Therefore, a Welsch heavy-tailed robust kernel function with flat tails is introduced. A nonlinear kernel mapping is performed on the prior information matrix.

[0190] The system also uses Schur complement elimination to extract the fundamental prior matrix and reuses the scalar equivalent prior residual squares defined in S32. Subsequently, the Welsch function structure was introduced to transform quadratic forms. Nonlinear mapping to heavy-tailed prior cost factor :

[0191]

[0192] In the formula, The preset prior kernel function adjustment parameters are used to control the truncation scale of the fault deviation of the heavy-tailed distribution to the marginalized nodes. Their empirical value range is set as follows: ,

[0193] The robust physical mechanism of the heavy-tailed cost factor can be analyzed by examining its influence function. To provide a rigorous proof, we take the first-order partial derivative of the cost function to obtain its corresponding penalty gradient vector:

[0194]

[0195] Mathematical analysis reveals that the Welsch heavy-tailed kernel function satisfies the following robust statistical boundary properties:

[0196] When the prior residual is small , The cost factor degenerates into a standard Gaussian distribution, preserving the system's normal constraint information.

[0197] When outlier observations cause a sharp increase in prior residuals The upper limit of the cost function converges to a constant. (Right now Furthermore, the limit of its influence function converges to zero:

[0198]

[0199] The above heavy-tailed prior mechanism can ensure that when faced with huge temporal anomalies, the prior cost factor has an impact on the current system state variables. The applied residual pulling gradient strictly approaches zero, thus achieving soft isolation of anomalous timing propagation without disrupting the sparse structure of the local Hessian matrix.

[0200] Specifically, step S4 includes:

[0201] S41 inputs the anomalous prior information matrix containing the heavy-tailed distribution generated in S3 into the back-end optimization solver. Since the objective function contains a non-quadratic heavy-tailed kernel term, it cannot be solved directly using the standard Gauss-Newton method. Therefore, the system introduces an IRLS optimization architecture, effectively transforming it into a dynamically weighted Gaussian least squares problem. Before starting the back-end optimization solver, the initial number of iterations is set. The prior cost factor of the heavy-tailed distribution is temporarily equivalent to a standard Gaussian factor, and its dynamic adaptive weights are initialized as follows: ,

[0202] S42 Prior Equivalent Residual Extraction, in the... In the next nonlinear optimization iteration, the incremental estimate based on the current retained state vector and the Gaussian prior information matrix obtained in S32 With the prior gradient vector Calculate the squared scalar equivalent prior residual of the slowly varying fault node in the current iteration step. Its equivalent mapping calculation formula is:

[0203]

[0204] S43 IRLS dynamic weight calculation, based on the scalar equivalent prior residual squared extracted in S42. By utilizing the derivative property of the cost function of the heavy-tailed robust kernel function, the dynamic adaptive weights required for the next iteration are calculated. Its update formula is:

[0205]

[0206] In the formula, This is the adjustment parameter for the prior kernel function in S33, used here to control the decay rate of the dynamic adaptive weights as the equivalent prior residual increases.

[0207] S44 Global Hessian Matrix Reconstruction and Iterative Solution, Calculating the Dynamic Adaptive Weights The prior information matrix and prior gradient vector applied to the slowly varying fault node are subjected to an equivalent Gaussian weighted mapping to obtain the first... The adaptive prior matrix of the next iteration With adaptive gradient vector :

[0208]

[0209] .

[0210] Specifically, step S5 includes:

[0211] S51 global objective cost function is jointly constructed to extract the effective multi-source sensor measurement set within the current sliding window. The effective measurement cost factor is defined as the sum of squared Mahalanobis distances of the residuals of each measurement. ,

[0212] The adaptive prior matrix obtained by the equivalent mapping in S44 With adaptive gradient vector As a robust prior cost factor, a global objective cost function is constructed based on the Bayesian maximum a posteriori estimation criterion, which includes robust prior constraints and current local sensor measurement constraints. Its optimization objective is defined as:

[0213]

[0214] In the formula, The set of global system state variables within the sliding window. and The first Actual observed values ​​and prediction models of each measurement factor This is the set of indices for valid multi-source sensor measurement factors within the current sliding window. For set The measurement factor index in

[0215] S52 Manifold space state update based on LM algorithm, using the aforementioned nonlinear optimization algorithm to solve the global objective cost function.

[0216] For the effective multi-source sensor measurement set within the current sliding window Each measurement factor in Find the Jacobian matrix And combined with the corresponding measurement noise covariance matrix With residual Calculate the Hessian matrix of the current local measurement. With gradient vector :

[0217]

[0218]

[0219] In the formula, This is the set of indices of the valid measurement factors within the current sliding window. For the first Individual measurement factors on global state variables Jacobian matrix, For the corresponding measurement residual vector,

[0220] The adaptive prior matrix output in stage S44 and gradient vector By rigorously aligning and combining the aforementioned measurement information with matrix dimensions, a global Hessian matrix that incorporates historical robustness experience and current actual measurements is reconstructed. With global gradient vector :

[0221]

[0222]

[0223] Considering that complex observation environments can easily lead to ill-conditioned matrices or iterative divergence in the pure Gauss-Newton method, the system introduces a damping factor from the LM algorithm on the main diagonal of the global Hessian matrix. Construct linear incremental solution equations with trust region characteristics:

[0224]

[0225] In the formula, To obtain the identity matrix with the same dimensions as the global Hessian matrix, the above equation can be decomposed to find the first identity matrix. System state increment of the next iteration The system state vector is then updated by introducing a generalized addition operation. The system state vector is rigorously updated on the nonlinear manifold space:

[0226]

[0227] In the formula, For the first For the global system state vector in the next iteration, the standard linear addition is used for incremental updates of the Euclidean space state components; for the attitude state components, the attitude error increment is mapped to the Lie group space through an exponential mapping mechanism, and the attitude state update on the nonlinear manifold space is completed using quaternion multiplication.

[0228] S53 convergence determination and robust positioning results output, calculate the current manifold space state increment. The norm of , and the preset optimization convergence threshold. Make a judgment:

[0229] like If the system has not yet converged, then the number of iterations is set to zero. Return to step S42 to re-extract the equivalent residual and enter the next round of loop optimization.

[0230] like If the convergence condition is met, stop the global state iteration and start from the optimized global system state variable. In the process, the three-dimensional position, velocity, and attitude information of the mobile carrier in the current epoch are extracted and output as the final highly reliable positioning result, and provide robust state feedback for the subsequent autonomous navigation and motion control of the mobile carrier.

[0231] This invention proposes a robust factor graph-based edge detection localization method based on heterogeneous priors. Compared with traditional factor graph edge detection mechanisms, this method first performs a comprehensive confidence assessment of nodes based on the mean square residuals of the historical residence period of the sliding window. Then, it constructs a heterogeneous robust prior based on a heavy-tailed distribution for contaminated nodes and uses an IRLS architecture for equivalent optimization to effectively block the temporal propagation of hidden, slowly changing faults in the prior matrix, significantly improving the positioning accuracy and overall reliability of multi-source fusion navigation systems in complex environments.

[0232] It should be noted that, in the specific embodiments of the present invention, the components of the system state vector are illustrated by including three-dimensional position, three-dimensional velocity, attitude, and sensor bias. However, in practical engineering applications, those skilled in the art can flexibly expand the dimensions of the state vector according to the type of sensor used and the fusion requirements. For example, components such as receiver clock error, time synchronization error, or sensor mounting arm error can be added. These conventional state dimension expansions based on the core robust edge computing architecture of the present invention are all within the protection scope of the present invention.

[0233] It should be noted that the above embodiments are not intended to limit the scope of protection of the present invention. Equivalent transformations or substitutions made based on the above technical solutions all fall within the scope of protection of the claims of the present invention.

Claims

1. A factor graph robust marginalization localization method based on heterogeneous priors, characterized in that, By introducing historical comprehensive observation confidence assessment and heterogeneous prior construction mechanism in the factor graph sliding window marginalization stage, and combining it with the IRLS architecture, the temporal propagation of slowly varying faults in the prior matrix is ​​effectively blocked, achieving reversible robust localization of the system. The steps include the following: S1, Sliding window construction and historical residual extraction: Multi-source sensor observation data of the mobile carrier are acquired to construct a factor graph sliding window optimization model. As the sliding window advances and nodes face marginalization, the historical residual sequence of the nodes to be marginalized during the window residence period is extracted, and their mean square residuals are calculated. S2, Node Confidence Assessment and Fault Determination: Using the obtained mean square residuals, the historical comprehensive observation confidence of the node to be marginalized is calculated, and the reliability of its observation status is determined based on the confidence level to identify whether there is a slowly changing fault bias. S3, Heterogeneous prior information matrix construction: Based on the fault determination results, the nodes to be marginalized are processed separately: For normal nodes with a confidence level meeting the preset threshold, standard marginalization is performed and a Gaussian distributed prior information matrix is ​​constructed; for faulty nodes with a confidence level not meeting the preset threshold, an anti-pollution mechanism is triggered, and a heavy-tailed distributed prior information matrix is ​​constructed. S4, iterative reweighted equivalent mapping, inputs the generated heterogeneous prior information matrix into the back-end optimization solver. For the heavy-tailed prior information matrix, the IRLS architecture is introduced, and its cost function characteristics are used to map it equivalently into factors with dynamic adaptive weights. S5, Global State Iteration and Localization Output: Combining the transformed dynamic adaptive weight factor and other effective measurement factors within the current sliding window, a global target cost function is constructed. The iterative optimization solution of the system state is completed by minimizing the cost function, the carrier pose is updated, and the final localization result is output. Specifically, S3 includes: S31 Local graph optimization model linearization, determining nodes to be marginalized. and the set of reserved nodes with which measurement constraints exist. Define the local system state vector as , To preserve the node state vector transpose, State vector of the node to be edged The transpose of, where the state components include three-dimensional position. 3D velocity Quaternion posture and sensor bias , Using the Gauss-Newton method to analyze the local joint measurement residual vector Perform a first-order Taylor expansion at the current state point to obtain its linear approximate expression: in, It is a local Jacobian matrix. The state increment to be solved. Introducing the joint measurement noise covariance matrix Construct the minimum objective function By letting the objective function be The derivative is zero, and the incremental equations of the local system are constructed. Approximate Hessian matrix With gradient vector The calculation is defined as follows: In the formula, For the local joint measurement residual vector The value of the linearization point in the current state. Based on state variables and The dimension of the state increment The corresponding incremental components are split into retained nodes. Incremental components of the node to be edged The approximate Hessian matrix and gradient vector are then divided into blocks: in , , , and These are the corresponding state components in the Jacobian matrix J. and Column partitioning , S32 constructs the Gaussian prior for normal nodes. For the normal nodes determined to be uncontaminated in S23, in order to maintain the size of the control factor graph without losing the historical constraint information contained in the nodes to be marginalized, the system uses the standard Schur complement elimination operation to... Marginalization, based on the locally linearized block equations constructed in S31: Due to the need to To remove a state from the system, first extract and solve for the increment of the state to be marginalized from the second equation in the system of equations. : Then the above Substituting the expression into the first equation of the system of equations, the algebraic substitution completely eliminates the error. The dimension, and extract information about the preserved state. Factoring the common factor, the reconstructed equation is as follows: At this point, the coefficient matrix extracted from the left side of the equation is the Gaussian prior information matrix. The right side of the equation is the prior gradient vector. : Based on this, define the concept of the preserved state. scalar equivalent prior residual square for: Due to the nodes to be edged out Nodes identified as normal Gaussian nodes are marginalized and their results are equivalently constructed as standard quadratic Gaussian prior cost factors in the global objective cost function. : The Gaussian prior cost factor In subsequent optimization calculations, this will be used as a historical experience constraint on the current carrier state. The solution provides unbiased convergence tension. The heavy-tailed prior construction for the S33 fault node addresses the issue of slowly varying faults in S23 that trigger isolation mechanisms. If the standard Gaussian prior assumption is followed, the corresponding quadratic cost function will generate a large penalty gradient for fault deviations, leading to divergence in the global state estimation. Therefore, a Welsch heavy-tailed robust kernel function with flat tails is introduced. A nonlinear kernel mapping is performed on the prior information matrix. The system also utilizes Schur complement elimination to extract the fundamental prior matrix and reuses the scalar equivalent prior residual squares defined in S32. Subsequently, the Welsch function structure was introduced to transform quadratic forms. Nonlinear mapping to heavy-tailed prior cost factor : In the formula, The preset prior kernel function adjustment parameters are used to control the truncation scale of the fault deviation of the heavy-tailed distribution to the marginalized nodes. Their empirical value range is set as follows: , The robust physical mechanism of the heavy-tailed cost factor is analyzed through its influence function. To provide a rigorous proof, we take the first-order partial derivative of the cost function to obtain its corresponding penalty gradient vector: Mathematical analysis reveals that the Welsch heavy-tailed kernel function satisfies the following robust statistical boundary properties: When the prior residual is small , The cost factor degenerates into a standard Gaussian distribution, preserving the system's normal constraint information. When the fault deviation causes a sharp increase in the prior residual The upper limit of the cost function converges to a constant. ,Right now And the limit of its influence function converges to zero: The above heavy-tailed prior mechanism ensures that when faced with large timing fault disturbances, the prior cost factor has a certain impact on the current system state variables. The applied residual pulling gradient strictly approaches zero, thus achieving soft isolation of fault timing propagation without disrupting the sparse structure of the local Hessian matrix.

2. The factor graph robust marginalization localization method based on heterogeneous priors according to claim 1, characterized in that, S1 specifically includes: S11 Measurement residual vector calculation and normalization, to obtain the mobile carrier in the first... The actual measurement vector of the sensor in each epoch. Characterizing the first Based on the actual observed characteristics of the external environment of the carrier, the predicted system state vector values ​​representing the current three-dimensional position, velocity, and attitude of the carrier are extracted. Its components are represented as In the formula For the first The three-dimensional position vector of each epoch carrier It is a three-dimensional velocity vector. It is a quaternion attitude vector. The sensor bias vector is used in the sensor's nonlinear measurement model. Predicted values Mapped to the sensor's measurement space, and the single-epoch measurement residual vector is calculated. , Let the actual sensor measurement vector at the k-th epoch be introduced. The covariance matrix of the sensor measurement noise corresponding to each epoch. The measurement residual vector is normalized by Mahalanobis distance to obtain the dimensionless scalar squared residual. : In the formula, For the first The measurement residual vector of each epoch. for The transpose of , For the measurement noise covariance matrix The inverse matrix, S12 Window residence period sequence construction and mean squared residual extraction: As the mobile carrier moves over time, step S11 is continuously executed, and the calculated single-epoch scalar squared residuals are extracted. The historical data queue of the factor graph sliding window is dynamically pushed in. When the number of nodes in the sliding window reaches the preset maximum capacity threshold, the system's edge detection mechanism is triggered. The state node with the oldest timestamp in the window is selected as the node to be edged out. The lifetime of the node to be edged out in the factor graph sliding window is tracked, and its status within the window is extracted. scalar squared residual sequence under consecutive historical epochs Calculate the historical mean square residual (MSE) of the node to be marginalized, i.e., the Mean Square Error: In the formula, The total number of historical epochs of the nodes to be marginalized within the sliding window. Let be the scalar squared residual corresponding to the i-th epoch.

3. The factor graph robust marginalization localization method based on heterogeneous priors according to claim 2, characterized in that, S2 specifically includes: S21 Heterogeneous mapping and historical comprehensive observation confidence calculation, based on the mean square residual obtained in S12. A Welsch robust kernel function weight mapping model with heavy-tailed characteristics is introduced to calculate the historical comprehensive observation confidence of the node to be marginalized. The calculation formula is as follows: In the formula, The preset confidence distribution adjustment parameter is used to control the penalty scale of the robust kernel function on the error. Since the residuals have been normalized by Mahalanobis distance in the previous steps, the empirical range of the parameter is set to be [value missing]. , S22 Temporal weighted geometric average equivalent definition, defining the node to be edged in the first... The independent Welsch robustness weights for each epoch are: To establish the confidence level of historical comprehensive observations The mapping logic between independent robust weights is constructed through the following mathematical derivation process to prove that it is equivalent to the geometric mean: The mean square residual formula Expanding and substituting into the confidence formula, we get: Extract the constant coefficients in exponentiation operations to separate the summation operation from the exponentiation mapping: Using the rules of natural exponential function, the summation term inside is transformed into a series of polynomial products, resulting in: Independent weights for single epochs Substituting the definition into the above equation, we finally prove and establish its equivalent mapping relationship as follows: Through the above derivation and proof, the equivalence between the comprehensive confidence level and the independent weights is established, and the algorithm only needs to rely on a single mean square residual. With the input, it is possible to achieve weight states without saving all historical single-epoch weight states. In this context, a quantitative characterization of the reliability of the comprehensive observation status of nodes can be achieved. S23 Slowly varying fault determination based on hypothesis testing: Based on the prior assumption that the sensor measurement residuals follow a Gaussian distribution, and that the corresponding scalar squared residuals follow a chi-square distribution, a significance level is set. With system state degrees of freedom Find or calculate the critical value of the chi-square distribution under the corresponding conditions. The critical value is used to reverse-engineer and set a confidence decision threshold. The threshold derivation formula is as follows: The historical comprehensive observation confidence level calculated by S21 With the decision threshold Compare: when When the observed state of the node to be marginalized is determined to be reliable and there are no fault values, when When a node to be edged is determined to be contaminated, its fault value is identified, and a subsequent anti-contamination isolation mechanism is triggered.

4. The factor graph robust marginalization localization method based on heterogeneous priors according to claim 3, characterized in that, S4 specifically includes: S41 inputs the prior information matrix of fault nodes containing heavy-tailed distribution generated in S3 into the back-end optimization solver. The system introduces the IRLS optimization architecture, which is equivalent to a dynamically weighted Gaussian least squares problem. Before the back-end optimization solver starts, the initial number of iterations is set. The prior cost factor of the heavy-tailed distribution is temporarily equivalent to a standard Gaussian factor, and its dynamic adaptive weights are initialized as follows: , S42 Prior equivalent residual extraction, in the first... In the next nonlinear optimization iteration, the incremental estimate based on the current retained state vector and the Gaussian prior information matrix obtained in S32 With the prior gradient vector Calculate the squared scalar equivalent prior residual of the faulty node in the current iteration step. Its equivalent mapping calculation formula is: S43 IRLS dynamic weight calculation, based on the scalar equivalent prior residual squared extracted in S42. By utilizing the derivative property of the cost function of the heavy-tailed robust kernel function, the dynamic adaptive weights required for the next iteration are calculated. Its update formula is: In the formula, This is the adjustment parameter for the prior kernel function in S33, used here to control the decay rate of the dynamic adaptive weights as the equivalent prior residual increases. S44 Global Hessian Matrix Reconstruction and Iterative Solution, Calculating the Dynamic Adaptive Weights The prior information matrix and prior gradient vector applied to the faulty node are subjected to an equivalent Gaussian weighted mapping to obtain the first... The adaptive prior matrix of the next iteration With adaptive gradient vector : 。 5. The factor graph robust marginalization localization method based on heterogeneous priors according to claim 4, characterized in that, S5 specifically includes: S51 global objective cost function is jointly constructed to extract the effective multi-source sensor measurement set within the current sliding window. The effective measurement cost factor is defined as the sum of squared Mahalanobis distances of the residuals of each measurement. , The adaptive prior matrix obtained by the equivalent mapping in S44 With adaptive gradient vector As a robust prior cost factor, a global objective cost function is constructed based on the Bayesian maximum a posteriori estimation criterion, which includes robust prior constraints and current local sensor measurement constraints. Its optimization objective is defined as: In the formula, The set of global system state variables within the sliding window. and The first Actual observed values ​​and prediction models of each measurement factor This is the set of indices for valid multi-source sensor measurement factors within the current sliding window. For set The measurement factor index in S52 Manifold space state update based on the Levenberg-Marquardt (LM) algorithm, employing a nonlinear optimization algorithm to solve the global objective cost function. For the effective multi-source sensor measurement set within the current sliding window Each measurement factor in Find the Jacobian matrix And combined with the corresponding measurement noise covariance matrix With residual Calculate the Hessian matrix of the current local measurement. With gradient vector : In the formula, This is the set of indices of the valid measurement factors within the current sliding window. For the first Individual measurement factors on global state variables Jacobian matrix, For the corresponding measurement residual vector, The adaptive prior matrix output in stage S44 and gradient vector By rigorously aligning and combining the aforementioned measurement information with matrix dimensions, a global Hessian matrix that incorporates historical robustness experience and current actual measurements is reconstructed. With global gradient vector : Considering that complex observation environments can easily lead to ill-conditioned matrices or iterative divergence in the pure Gauss-Newton method, the system introduces a damping factor from the LM algorithm on the main diagonal of the global Hessian matrix. Construct linear incremental solution equations with trust region characteristics: In the formula, To obtain the identity matrix with the same dimensions as the global Hessian matrix, the above equation can be decomposed to find the first identity matrix. System state increment of the next iteration The system state vector is then updated by introducing a generalized addition operation. The system state vector is rigorously updated on the nonlinear manifold space: In the formula, For the first For the global system state vector in the next iteration, the Euclidean space state components are incrementally updated using standard linear addition; for the attitude state components, the attitude error increment is mapped to the Lie group space through an exponential mapping mechanism, and the attitude state update on the nonlinear manifold space is completed using quaternion multiplication. S53 Convergence determination and robust positioning results output, calculate the current manifold space state increment. The norm of , and the preset optimization convergence threshold. Make a judgment: like If the system has not yet converged, then the number of iterations is set to zero. Return to execution S42 to re-extract the equivalent residual and enter the next round of loop optimization. like If the convergence condition is met, stop the global state iteration and start from the optimized global system state variable. In the process, the three-dimensional position, velocity, and attitude information of the mobile carrier in the current epoch are extracted as the final highly reliable positioning result output, and robust state feedback is provided for the subsequent autonomous navigation and motion control of the mobile carrier.

6. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements a factor graph robust marginalization localization method based on heterogeneous priors as described in any one of claims 1 to 5.

7. A computer-readable storage medium storing computer instructions thereon, characterized in that, When the computer instructions are executed by the processor, they implement a factor graph robust marginalization localization method based on heterogeneous priors as described in any one of claims 1-5.

Images