An intelligent special robot system for combined perception of attitude, position and track in extreme working conditions

Abstract

The application belongs to the technical field of artificial intelligence, state estimation and multi-source time series data fusion, and particularly relates to a joint perception system for attitude, motion and environment of special robots in extreme working conditions. The joint perception system comprises an observation factor construction module, an observability evolution network, a counter-fact checking module, a neural factor graph joint estimation module and a residual feedback module. The observation factor construction module encodes attitude, motion and environment observation data into observation factors. The observability evolution network analyzes constraint direction, strength and residual change according to state components. The counter-fact checking module generates a credibility correction amount through a reserved branch and a shielding branch. The neural factor graph joint estimation module reorganizes attitude, motion and environment constraints and outputs a joint state. The residual feedback module uses state residuals to correct degradation identification parameters, identifies implicit degradation in continuous output, suppresses the cross-state propagation of error information, and maintains the continuity and stability of the solution of the attitude, motion and environment state in the presence of asynchronous observation, local missing and constraint conflicts.

Description

Technical Field

[0001] This invention belongs to the fields of artificial intelligence, state estimation and multi-source time series data fusion technology, specifically a joint perception system for attitude and trajectory of an embodied intelligent special robot under extreme working conditions. Background Technology

[0002] Existing attitude and trajectory perception schemes for special robots operating in extreme conditions typically involve attitude perception data, motion perception data, and environmental relative observation data in state calculation. The system first performs time synchronization, outlier removal, and noise smoothing on various input data. Then, it assigns fusion weights based on pre-defined measurement noise, data completeness, or single-frame confidence, and solves for heading, pitch, roll, spatial position, velocity, and trajectory curvature using extended Kalman filtering, sliding window optimization, or factor graphs. Some schemes reduce the corresponding weight when a certain type of observation is missing, maintaining short-term output using the previous state, kinematic model, or other sensing sources. Other schemes establish separate attitude estimation and position estimation links, then combine them through coordinate transformation or result-level correction. Such implementations usually assume that observation errors can be described by a fixed noise model, or that confidence drops can be directly identified through data amplitude, missing measurement ratio, and residual magnitude. The relationship between attitude and trajectory states is mainly used for result correction, not as a joint constraint for judging the observability changes of state components and the source of anomalies.

[0003] Under conditions of smoke and dust obscuring, strong vibration, continuous impact, rapid attitude changes, or local observation reference drift, the sensing source may maintain continuous output, but its data may have undergone directional shifts, time delay accumulation, or local distortion. Since the output format remains intact, fusion methods relying on fixed noise parameters, missing measurement criteria, or single residual magnitudes are insufficient to promptly identify such implicit degradation. Erroneous observations continue to inject information into the corresponding state components and propagate to other state components via the coupling relationship between attitude and track. When the attitude and position links are processed separately, track curvature, displacement continuity, attitude change rate, and velocity continuity cannot be mutually verified in the same solution process. The system also cannot determine which type of state constraint is currently missing, or whether removing a certain observation would actually make the joint state more stable. The main technical problem is that existing solutions cannot identify observable capabilities and distinguish the true contribution of erroneous observations by state components when multi-source observations are continuously outputting but their reliability has undergone implicit changes. This leads to the continuous fusion of abnormal information, error propagation across states, and cumulative drift or convergence failure in the joint attitude and track solution under extreme conditions. Summary of the Invention

[0004] The purpose of this invention is to provide a joint perception system for attitude and trajectory of an embodied intelligent special robot under extreme working conditions, which can solve the problems mentioned in the background art.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows: An extreme working condition embodied intelligent special robot attitude and trajectory joint perception system includes an observation factor construction module, an observability evolution network, a counterfactual verification module, a neural factor graph joint estimation module, and a residual feedback module; The observation factor construction module encodes attitude perception data, motion perception data, and environmental relative observation data into observation factors with time correlation and uncertainty identifiers. The observability evolution network generates state component-level observability representations based on the constraint relationship between observation factors within a continuous time window on attitude and track state components. The counterfactual verification module constructs observation factor shielding branches and forms a credibility correction quantity; The neural factor graph joint estimation module reorganizes the observation constraints based on the observability representation and the confidence correction amount, and outputs the joint attitude and trajectory state. The residual feedback module feeds back the joint state residuals to the observation factor construction module and the observability evolution network.

[0006] Preferably, the observation factor construction module establishes a joint state vector including heading, pitch, roll, angular motion trend, spatial position, motion speed and track curvature, and configures source identifier, observation subspace, acquisition time, effective interval, delay state and uncertainty code for each type of input data; The observation factor construction module divides the causal time window according to the control action timestamp, maps the attitude perception data, motion perception data and environmental relative observation data in the same time window into state constraint vectors respectively, generates degradation features based on the residual change rate, cross-source correlation and missing measurement persistence of adjacent time windows, and writes the state constraint vector and the degradation features into the node attributes and edge attributes of the observation factor.

[0007] Preferably, the observability evolution network includes a state constraint projection layer, a temporal correlation layer, and an observability aggregation layer; The state constraint projection layer projects each observation factor to the state component corresponding to the joint state vector, forming a directed constraint graph between the observation factor and the state component. The temporal correlation layer calculates the direction change, intensity change and residual correlation change of the constraints received by the same state component along a continuous time window, and forms the temporal embedding of the state component. The observability aggregation layer performs graph message passing on the temporal embedding of the state components, and outputs the observability level of each state component, the main sources of constraints, and the constraint conflict relationships, which constitute the state component-level observability representation.

[0008] Preferably, the counterfactual verification module establishes a reserved branch and a masked branch for each observation factor to be evaluated, and calls the same neural factor graph parameters in the two branches to generate the corresponding short-term joint state sequence; The counterfactual verification module calculates the cross-time continuity, motion constraint violation, subsequent observation interpretation residual, and state uncertainty expansion of the two joint state sequences, and encodes the difference between the shielded branch and the retained branch as an observation contribution vector. The observation contribution vector and the constraint conflict relationship in the observability representation are jointly reasoned to generate the credibility correction amount of the corresponding observation factor and the information injection coefficient that acts on the specific state component.

[0009] Preferably, the observation factor construction module establishes a variable delay alignment graph for input data collected at different times within the same causal time window. The variable delay alignment graph uses the control action time and the joint state prediction time as anchor points, and the acquisition time, transmission delay, and adjacent observation change trends of the input data as edge constraints. The observation factor construction module obtains the state mapping interval of each input data relative to the anchor point time through time delay latent variable inference, writes the state mapping interval and the uncertainty code together into the observation factor, and retains the input data exceeding the mapping interval as cross-window delay factor, which is associated and referenced by the state constraint graph in the subsequent time window.

[0010] Preferably, the observability evolution network constructs a local observable subgraph based on the observability level of each state component and the main source of constraint, sets the state component with decreasing observability level as the target node, and sets the state component with kinematic coupling relationship with the target node and whose observability level meets the constraint conditions as the support node. The observable evolution network, combined with the variable time delay alignment map, determines the effective time correspondence between the target node and the support node, and selects cross-state constraint paths according to the constraint direction complementarity. The heading constraint path is formed by track curvature and displacement continuity, and the position and track transfer constraint path is formed by attitude change rate and velocity continuity.

[0011] Preferably, the counterfactual verification module is configured according to the observation factor, observation source, and locally observable sub-factor. Figure 3 Nested shielding branches are established at each level. First, a single observation factor is shielded to obtain the local contribution vector. Then, the set of observation factors formed by the same observation source within a continuous time window is shielded to obtain the source contribution vector. Finally, the constraint path connected to the target node is shielded to obtain the path contribution vector. The counterfact verification module aligns the local contribution vector, the source contribution vector, and the path contribution vector according to their corresponding state components using tensors, and generates implicit degradation identifiers, error propagation directions, and fractional confidence corrections based on the consistency of contributions among the three levels.

[0012] Preferably, the neural factor graph joint estimation module sets the joint state vector as a state node, sets the observed factors, cross-state constraint paths and motion constraints corresponding to control actions as factor nodes, and adjusts the message passing direction between each factor node and the state node according to the observability representation of the state component level. The neural factor graph joint estimation module writes the fractional confidence correction into the gated variable of the corresponding message and writes the latent degradation identifier into the reserved variable of the factor node. By alternately executing state node update, factor node residual update and uncertainty propagation, a joint attitude and trajectory state sequence with state component confidence intervals is formed.

[0013] Preferably, the residual feedback module extracts observation reconstruction residuals, cross-state constraint residuals, counterfactual branch differences, and confidence interval coverage states from the joint attitude and track state sequence, and establishes a residual memory map according to the observation source and state components; The residual feedback module selects time windows where the observable levels continuously satisfy the set relationship, the residual directions of multiple observation sources are consistent, and the counterfactual branches are not marked with implicit degradation as self-supervised time windows. The residual memory graph within the self-supervised time window is converted into degradation labels and constraint strength labels, and the degradation feature encoding parameters of the observation factor construction module and the graph message passing parameters of the observability evolution network are corrected respectively by a separate update method.

[0014] Preferably, the neural factor graph joint estimation module establishes a short-term state update layer and a cross-window state smoothing layer. The short-term state update layer generates candidate joint states based on the locally observable subgraph, the fractional confidence correction amount, and the current time window observation factors. The cross-window state smoothing layer retrieves the historical constraint chain corresponding to the candidate joint states based on the residual memory graph and performs reverse message passing on a preset number of state nodes before the current time window. The observability evolution network regenerates the observability representation based on the constraint residuals after the reverse message passing. The counterfactual verification module reconstructs nested masked branches for the observation factors that have undergone weight reversal. The residual feedback module saves the state nodes and observation factor versions before and after the update.

[0015] Compared with the prior art, the beneficial effects of the present invention are as follows: 1. By incorporating heading, pitch, roll, angular motion trends, spatial position, velocity, and track curvature into the same joint state space, and encoding various inputs as observation factors with temporal correlation, uncertainty, and degradation characteristics, the observability evolution network can analyze the continuous changes in constraint direction, constraint strength, and residual correlation according to state components, determining which states still have sufficient constraints and which states are losing reliable observations. The counterfactual verification module establishes retained and masked branches for the observations to be evaluated, comparing the cross-time continuity of the two branches, the degree of motion constraint violation, and the subsequent observation interpretation of residuals and uncertainty expansion, forming a credibility correction quantity acting on specific state components. The neural factor graph reorganizes the message transmission direction and information injection intensity accordingly, causing track curvature and displacement continuity to constrain heading drift, and attitude change rate and velocity continuity to limit position and track abrupt changes. This allows for the identification of hidden degradation sources while the observation data remains output, blocking the propagation of erroneous information along the joint state coupling relationship, and maintaining the continuity, interpretability, and solution convergence of the attitude and track state sequence.

[0016] 2. By introducing a variable-delay alignment graph during the construction of observation factors, cross-time-window associations can be established using control action timing, state prediction timing, acquisition timing, and transmission delay. This allows delayed data to participate in subsequent solutions as cross-window factors, reducing the possibility of asynchronous observations being incorrectly matched to the current state. The locally observable subgraph selects cross-state constraint paths based on the kinematic coupling relationship between the target state component and the supporting state component, ensuring that constraint compensation revolves around the actual missing state information and avoiding uniform weighting of all observation sources. Nested masked branches can also form contribution judgments from three levels: single observation, continuous source, and constraint path, distinguishing between local anomalies, source-related degradation, and error propagation paths. The residual memory graph saves the observation reconstruction residuals, cross-state constraint residuals, counterfactual branch differences, and confidence interval coverage states by source and state component, and updates the degradation feature encoding and graph message passing parameters using time windows that satisfy multi-source consistency. The cross-window state smoothing layer then performs reverse message passing based on the updated constraint residuals to backtrack and correct previous state nodes, reducing the continuous impact of delay mismatches, local degradation, and historical error residues on subsequent joint sensing results. Attached Figure Description

[0017] Figure 1 A flowchart of the overall process for joint and reliable perception of attitude and trajectory; Figure 2 The response diagrams are for attitude angle and planar position components. Figure 3 The response diagrams are for altitude, velocity, and track curvature components. Figure 4 The residual response diagram is the observation of the attitude and motion source of the aircraft. Figure 5Environmental residual response diagram relative to external constraint sources; Figure 6 For the convergence graph of the joint sequence objective cost; Figure 7 A flowchart for variable delay alignment, locally observable reconstruction, and nested counterfactual verification. Figure 8 A smoothed attitude angle and planar position component map across the window; Figure 9 This is a smoothed graph of altitude, velocity, and trajectory curvature components across the window. Detailed Implementation

[0018] In one embodiment, an extreme-condition embodied intelligent special robot attitude and trajectory joint perception system includes an observation factor construction module, an observability evolution network, a counterfactual verification module, a neural factor graph joint estimation module, and a residual feedback module. The observation factor construction module encodes attitude perception data, motion perception data, and environmental relative observation data into observation factors with time correlation and uncertainty identifiers. The observability evolution network generates state component-level observability representations based on the constraint relationships between observation factors and attitude and trajectory state components within a continuous time window. The counterfactual verification module constructs observation factor masking branches and forms a confidence correction quantity. The neural factor graph joint estimation module reorganizes the observation constraints based on the observability representation and confidence correction quantity, outputting the joint attitude and trajectory state. The residual feedback module feeds back the joint state residual to the observation factor construction module and the observability evolution network. In this embodiment, please refer to... Figure 1 Each module operates under a unified discrete-time reference. The observation factor construction module receives attitude perception data, motion perception data, and environmental relative observation data obtained during the robot's operation. It does not limit the specific hardware form of the data source, but establishes observation categories based on the data constraints on heading, pitch, roll, position, speed, or track curvature. All observations carry the acquisition time, arrival time, source identifier, constrainable state components, and uncertainty description before entering the joint estimation, avoiding binary processing based solely on the data source for overall reliability or overall unreliability.

[0019] The attitude sensing data may include triaxial angular rate and triaxial specific force sampling data output by MEMS (Micro-Electro-Mechanical Systems) inertial measurement units. Such MEMS inertial devices are prone to zero-bias asymptotic drift, scale factor offset and high-frequency vibration noise coupling under extreme conditions such as strong vibration and continuous impact. This manifests as a complete and continuous data output form but an implicit deviation in the measurement truth value, which can serve as a typical observation source for implicit degradation identification and adaptive correction in this system.

[0020] In this embodiment, joint sensing processing uses continuous time windows as the basic computation interval. Each time window covers multiple adjacent state moments. The observation factor construction module first maps the original input to the corresponding state moment according to the acquisition time, and then retains the cross-window reference relationship according to the data arrival order. For continuous inputs from the same source, time edges are formed; for inputs from different sources but constraining the same state component, lateral consistency edges are formed; and for inputs that can jointly constrain the attitude and track coupling relationship, cross-state edges are formed. The observability evolution network does not directly output a unified weight from a certain source, but instead judges one by one which observation constraints each state component is currently constrained, whether different constraints provide information in the same direction, and whether there is a continuous conflict between constraints. The counterfactual verification module replicates the current joint estimation process without changing the original input, and forms pairwise inference results by retaining or masking the observations to be evaluated. The neural factor graph joint estimation module writes the observability changes and counterfactual contributions into the message passing process. The residual feedback module saves the difference between the current estimate and the original observation, and uses the residual fragments confirmed by multi-source consistency for subsequent degradation feature encoding and constraint propagation parameter correction.

[0021] The joint state at time k is represented as: ; in, This represents the joint attitude and trajectory state at the k-th state time. , and These represent the heading, pitch, and roll states, respectively. , and These represent the angular motion trends in the three orientation directions, , and Represents the position components in a unified reference coordinate system. , and Indicates the velocity in the corresponding direction. Indicates the current track curvature state, symbol This represents the vector transpose; for example, when a robot maintains a slow change in position but a continuous rotation in heading within a continuous time window, the change in the position component is small and... and Continuous variation, track curvature The system updates its attitude along with the heading and displacement direction, enabling it to distinguish between in-situ attitude changes and turning motions involving spatial transfer.

[0022] Please refer to Figure 2 and Figure 3 The horizontal axis represents the sampling window stage, labeled with Chinese characters for start, middle, and end segments. The vertical axis represents the stage determined by the joint state vector. The expanded state values ​​are labeled with the names of each state component on the right. Figure 2 The attitude angle and planar position component response diagrams centrally display yaw angle, pitch angle, roll angle, forward position, and lateral position, etc. The relationships between the elements are as follows: the forward position continuously increases from 0.002750 to 17.289906, and the lateral position increases from 1.999833 to 5.877890, indicating that the robot maintains forward displacement accompanied by lateral offset within a continuous time window. The yaw angle gradually increases from near zero to 1.660910, reflecting the synchronous accumulation of heading changes with displacement. The pitch and roll angles oscillate slightly around zero, indicating that although attitude disturbances exist, they are within the estimable range. Figure 3 The response diagrams for altitude, velocity, and track curvature components centrally display elements such as altitude, forward velocity, lateral velocity, vertical velocity, and path curvature. Altitude fluctuates, first increasing and then decreasing; forward velocity generally decreases; lateral velocity initially increases and then stabilizes; vertical velocity varies around zero; and path curvature peaks locally before declining. This indicates that velocity decomposition and curvature constraints jointly characterize the turning and trajectory adjustment processes. These two types of components together constitute a unified state baseline for subsequent observation residual construction, state component observability inference, neural factor graph message passing, and cross-window smoothing.

[0023] Specifically, the observation factor construction module generates an observation record for the i-th type of observation at the k-th state time. The observation record includes at least the observed value, acquisition time, arrival time, source identifier, corresponding state component index, available duration interval, and uncertainty matrix. Then, observation factors are generated based on the mapping relationship between the observed value and the current predicted state. For data that directly describes attitude changes, the observation factor connects the corresponding attitude state node and the angular motion trend node at adjacent time points. For data that describes relative displacement or relative changes in the environment, the observation factor connects the position node, velocity node, and relevant attitude node. For data that can reflect changes in motion direction, the observation factor connects both the heading node and the track curvature node. When the observation only covers a part of the joint state, the uncovered state components are not written into the constraint edges of the corresponding observation factor to avoid spreading irrelevant uncertainties to other state components through uniform weights.

[0024] The residual of the i-th observation factor at the k-th state time is expressed as:

[0025] ; in, Represents the observation reconstruction residuals, This represents the observation vector of the i-th observation factor. This represents the observation function that maps the joint state and time delay to the corresponding observation space. Indicates the observation delay. Indicates the time when the data arrives at the joint estimation module. This indicates the actual time of data acquisition; for example, when an observation arrives after the state time, When a positive value is taken, the observation function uses this positive value to backtrack the joint state to the acquisition time along the state transition relationship before calculating the residual. If an observation is formed in advance in the buffer but participates in the solution later, the actual acquisition time is still used instead of the time of participation in the solution, so that the residual corresponds to the real time position.

[0026] Please refer to Figure 4 and Figure 5 The horizontal axis represents the sampling window stage, labeled with Chinese terms for the start, middle, and end stages. The vertical axis represents the observed residual amplitude. The right-hand caption indicates the source of the observation. Figure 4 The residual response diagrams for attitude and motion sources show the residual changes of inertial sources, odometer sources, magnetic heading sources, and control bus sources within a continuous time window. The peak value of the inertial source is 0.347577, indicating that the attitude link crosses the handling reference line in a local window. The odometer, magnetic heading, and control bus are generally close to or below the observation reference line, indicating that motion prediction and heading constraints still have good continuity. Figure 5 The residual response plots for environmental sources relative to external constraints show the residual changes for relative location, depth, visual, and distance anchor sources. The relative location source reaches 0.494589 near window 29, the visual source reaches 0.503576, and the distance anchor source exceeds the treatment reference line for 21 windows, indicating that these sources... There is a concentrated deviation in the alignment or spatial constraint mapping.

[0027] In a preferred embodiment, the observation factor construction module establishes a joint state vector including heading, pitch, roll, angular motion trend, spatial position, motion speed, and track curvature, and configures a source identifier, observation subspace, acquisition time, effective interval, delay state, and uncertainty code for each type of input data. The observation factor construction module divides causal time windows according to the control action timestamp, maps the attitude perception data, motion perception data, and environmental relative observation data within the same time window into state constraint vectors respectively, generates degradation features based on the residual change rate, cross-source correlation, and missing measurement persistence of adjacent time windows, and writes the state constraint vector and degradation features together into the node attributes and edge attributes of the observation factor. The control action timestamp is only used to determine the causal boundary of the robot's motion state change and does not require changing the robot's original control logic. The starting point of the causal time window is set at the moment when the control quantity changes or the joint state prediction begins, and the ending point is set at the moment when the next control quantity changes or the preset state update boundary. The inputs within the window are sorted according to their actual acquisition time. The same input can constrain multiple state components at the same time, but the residuals and uncertainties are saved separately for each state component.

[0028] Preferably, the state constraint vector is obtained by encoding the local change direction of the joint state through the observation value and the corresponding observation function. The encoding result includes the constraint state component index, constraint direction, constraint magnitude, and constraint confidence interval. The residual change rate is constructed by the difference in the residual mean and the difference in residual dispersion of the same observation source within adjacent time windows. The cross-source correlation is constructed by the consistency of the change direction of different sources constraining the same state component after time alignment. The duration of missing data is constructed according to the interval from the last valid acquisition time of the source to the current state time. When a source continues to output but the residual direction is biased to the same side for a long time, the degradation feature records the directional shift and does not judge it as random noise. When the residual magnitude of a source is small but the change direction of other sources is continuously opposite, the degradation feature records the cross-source conflict. When the source has intermittent output, the degradation feature records the duration of missing data and maintains the residual trend before the interruption, so that subsequent observability inference can distinguish between random fluctuations, systematic drift, and complete missing data.

[0029] For MEMS inertial measurement observations, the degradation features also introduce two additional feature dimensions: vibration frequency band residual coupling degree and temperature-related drift amount. These are used to distinguish between instantaneous shock disturbances under extreme conditions and the continuous performance degradation of the MEMS device itself, avoiding misjudging short-term vibration interference as device failure and improving the accuracy of degradation identification.

[0030] The observability of the j-th state component at the k-th state time is expressed as: ; in, This represents the observability of the j-th state component. This represents the set of observation factors constraining the j-th state component within the current time window. This represents the constraint strength of the i-th observation factor on the j-th state component. This represents the degradation amount obtained from the degradation feature, and its value ranges from 0 to 1. This represents the time interval between the time of data collection corresponding to the observed factor and the current state time. This represents the time-degradation coefficient of the j-th state component. Let represent the constraint conflict between the i-th observation factor and the q-th observation factor for the same state component. This represents the conflict suppression coefficient; for example, when both observations are constrained in their headings and change in the same direction, When the value approaches 0, and two observations point to opposite heading changes, the conflict term increases, and even if both observations are continuously output, the observability of the heading state will decrease.

[0031] In one embodiment, the observability evolution network includes a state constraint projection layer, a temporal correlation layer, and an observability aggregation layer. The state constraint projection layer projects each observation factor to the state component corresponding to the joint state vector, forming a directed constraint graph between the observation factor and the state component. The temporal correlation layer calculates the direction change, intensity change, and residual correlation change of the constraints received by the same state component along a continuous time window, and forms a temporal embedding of the state component. The observability aggregation layer performs graph message passing on the temporal embedding of the state component, and outputs the observability level of each state component, the main source of constraints, and the state component-level observability representation composed of constraint conflict relationships. The state constraint projection layer establishes directed edges according to the state component index recorded by the observation factor. The edge direction points from the observation factor to the constrained state node, and the edge attributes are written with constraint strength, degradation amount, time interval, and uncertainty. The state node attributes are written with the estimated value of the previous time step, the amount of state change, and the confidence interval.

[0032] In this embodiment, the temporal association layer maintains a fixed-length temporal embedding cache for each state component. The cache does not store the original observations, but stores the constraint direction distribution, constraint strength distribution, source composition, residual mean, residual direction persistence, and conflict amount of the state component in each time window. When a new time window arrives, the current constraint statistics are differentially encoded with the adjacent time windows in the cache. If the constraint sources decrease but the remaining sources remain the same, the constraint sparsity change is recorded. If the number of sources does not decrease but the conflict amount continues to increase, the implicit reliable change is recorded. If the main constraint direction changes abruptly and the joint motion state cannot explain the change, the constraint direction is recorded as abnormal. The observability aggregation layer transmits information along the path from the observation factor to the state node, from the state node to the coupled state node, and from the coupled state node back to the target state node. It determines the sources that can directly constrain the target state component as the main constraint sources and the state components that indirectly provide constraints through kinematic coupling as support sources.

[0033] Furthermore, the observability representation of state components is expressed using a combination of hierarchical numerical values ​​and structured relationships. The hierarchical numerical values ​​are determined by the interval of observability, and the structured relationships include the set of primary constraint sources, the set of backup constraint sources, conflict source pairs, cross-state support paths, and unobservable directions. When the heading state is directly observable but the track curvature, displacement direction, and attitude change rate still have consistent constraints, the heading state is marked as insufficiently observable but can be supported by cross-state paths. When the position state is jointly constrained by multiple relative environmental observations and these observations are continuously biased towards the same erroneous direction, cross-source consistency alone cannot identify anomalies. The observability evolution network further calls the counterfactual verification results to determine the explanatory power of the entire set of sources for subsequent states. When a state component has no direct constraints and the support path is also interrupted due to coupled state degradation, the corresponding unobservable direction is written into the joint estimation module, restricting the state component from receiving correction messages without source support.

[0034] In one embodiment, the counterfactual verification module establishes a retained branch and a masked branch for each observation factor to be evaluated, and calls the same neural factor graph parameters in both branches to generate the corresponding short-term joint state sequence. The counterfactual verification module calculates the cross-time continuity, motion constraint violation, subsequent observation interpretation residual, and state uncertainty expansion of the two joint state sequences respectively, and encodes the difference between the masked branch and the retained branch as an observation contribution vector. The observation contribution vector is jointly inferred with the constraint conflict relationship in the observability representation to generate the credibility correction amount of the corresponding observation factor and the information injection coefficient acting on the specific state component. The retained branch uses all candidate observation factors in the current time window, while the masked branch only removes the message edge between the observation factor to be evaluated and the state node. The two branches share the same initial joint state, state transition constraints, training parameters, and number of iterations, so that the branch difference comes from the existence or non-existence of the observation to be evaluated.

[0035] The counterfactual contribution of the observed factor i to the state component j is expressed as: ; in, This represents the counterfactual contribution of the observed factor to be evaluated to the corresponding state component. and These represent the time-series discontinuities of the masked branch and the retained branch, respectively. and These represent the motion constraint violations of the two branches, respectively. and These represent the residuals from the interpretation of subsequent observations by the two branches, respectively. and These represent the expansion of the state uncertainty in the two branches, respectively. to This represents the combination coefficient of the corresponding evaluation item; when all evaluation quantities decrease after the observations are masked. A negative value indicates that the observation being evaluated is disrupting the consistency of the corresponding state component. When the evaluation quantities increase after the observations are masked, A positive value indicates that the observation provides an irreplaceable constraint.

[0036] Specifically, the cross-time discontinuity is calculated from the deviation between changes between adjacent states and state transition constraints; the motion constraint violation is calculated from the degree of inconsistency between attitude changes, velocity changes, displacement direction, and track curvature; the subsequent observation interpretation residual is calculated from the reconstruction error of the short-time joint state sequence for observations after the current time window; the state uncertainty expansion is calculated from the expansion of the confidence interval of each state component relative to the branch start point; the confidence correction is not directly used as a unified weight for the observation source, but is generated separately for each state component connected by the observation factor; when the same observation provides a positive contribution to the heading but a negative contribution to the position, the information injection coefficient allows the retention of its message to the heading node and the suppression of its message to the position node; when jointly reasoning the observation contribution vector and the constraint conflict relationship, if the observation is in the conflict source pair and the counterfactual contribution is consistently negative, it is marked as implicit degradation; if the observation is in the conflict source pair but the contribution is positive, the observation is retained and the other source in the conflict pair continues to be evaluated by masking.

[0037] In one embodiment, the neural factor graph joint estimation module reorganizes observation constraints based on observability representation and confidence correction. Specifically, it sets each state component in the joint state vector as a state node and sets the observation factors, adjacent time-state transition relationships, attitude and track coupling relationships, and motion constraints corresponding to control actions as factor nodes. The message strength from the observation factor to the state node is jointly determined by the original uncertainty, observability, and information injection coefficient. The state transition factor maintains the continuous propagation of adjacent states when external observations degrade. The attitude and track coupling factor associates track curvature with heading changes and attitude change rate with position transfer direction. When the observability of a state component is insufficient, the joint estimation module prohibits correction messages from negative contribution observations from directly overwriting the state prediction value and selects supporting state nodes from the locally observable subgraph to form alternative constraints. When a state component has multiple mutually consistent direct observations, the supporting path is only used for consistency verification and does not repeatedly increase the information strength.

[0038] The joint state sequence is obtained using the following objective function: ; in, Let X represent the sequence of joint states to be determined, and let X represent the set of variables consisting of all joint states within the time window. This represents the information injection coefficient obtained from observability representation and counterfactual verification. This indicates the observation residuals in the weight matrix. The weighted quadratic quantity below, f represents the quantity based on the previous state and control action. Obtain the state transition function for the current state. This indicates the combined attitude and track state at the previous state. Let g represent the state transition constraint matrix, and g represent the coupling constraint function between the attitude state and the track state. This represents the coupling constraint matrix; for example, when an observation is determined to have a latent degradation only with respect to the position component, the corresponding position message... The injection coefficient for the heading component decreases, while the injection coefficient for the same observation remains unchanged.

[0039] Please refer to Figure 6 , Figure 6 The horizontal axis represents the solution round stage, and the vertical axis represents the cost in the joint objective function. The curves correspond to the observation residual cost, state transition cost, attitude and track coupling cost, and total cost, respectively. Figure 6 correspond The iterative solution process, in which The correlation terms dominate the observational explanation of error. Constraining the continuity of adjacent states, This constrains the consistency between attitude and trajectory. Figure 6As can be seen, the observation residual cost decreased from 48.327875 to 38.898347, and the total cost decreased from 49.829979 to 40.400802, indicating that the information injection coefficient and the state update direction enabled the objective function to continuously converge. The state transition cost remained at approximately 1.388, and the coupling cost remained around 0.113 to 0.115, indicating that the motion continuity and attitude-track coupling relationship were not disrupted while reducing the observation error.

[0040] In a preferred embodiment, please refer to Figure 7 The observation factor construction module establishes a variable delay alignment graph for input data collected at different times within the same causal time window. The variable delay alignment graph uses the control action time and the joint state prediction time as anchor points, and the acquisition time, transmission delay, and adjacent observation change trends of the input data as edge constraints. The observation factor construction module obtains the state mapping interval of each input data relative to the anchor point time through delay latent variable inference, and writes the state mapping interval and uncertainty coding together into the observation factor. Input data exceeding the mapping interval is retained as cross-window delay factors, which are associated and referenced by the state constraint graph in subsequent time windows. The nodes in the variable delay alignment graph include anchor point state nodes, observation acquisition nodes, and data arrival nodes. The edge between the acquisition node and the arrival node records the transmission delay, the edge between the acquisition node and the anchor point state node records the state propagation interval, and the edge between adjacent acquisition nodes records the observation change trend. The delay latent variable is obtained by jointly inferring the transmission delay, the continuity of the change trend, and the current state prediction.

[0041] Preferably, the state mapping interval does not use a single time point, but uses the start mapping time and end mapping time to represent the range of states that the observation may correspond to. When the observation changes slowly and the transmission delay is stable, the mapping interval shrinks to a time close to a single state. When the observation changes drastically or the transmission delay fluctuates, the mapping interval covers multiple adjacent state nodes. The joint estimation module connects the observation factors to the candidate state nodes in the mapping interval and allocates the initial correlation amount according to the degree of matching between each candidate state and the observation change trend. During the iteration process, the correlation amount is reallocated according to the reconstruction residual. When the observation arrival time is later than the end of the current time window, the observation factor is stored in the cache as a cross-window delay factor. The historical state node version corresponding to its acquisition time is also retained. When the cross-window delay factor is called in subsequent time windows, the newly arrived observation is connected to the historical state node and a local backtracking is triggered, instead of directly mismatching the delayed observation to the current state node.

[0042] The candidate state association quantity corresponding to the time-delay latent variable is represented as follows: ; in, Let represent the correlation between the i-th observation factor and the candidate state time r. This represents the candidate state time corresponding to time r. This represents the candidate state time corresponding to time q. This represents the set of candidate state times within the state mapping interval. This represents the difference between the observed trend of change and the trend of change of the candidate states. This represents the amount of prediction discrepancy when the observations reconstruct the candidate state. This represents the difference between the observed trend of change of the candidate state at time q and the trend of change of the candidate state. This represents the difference in predictions when the observation at time q reconstructs the candidate state. , and These represent the constraint coefficients for time interval, trend of change, and prediction difference, respectively. When the candidate state time is close to the actual acquisition time and the trend of change is consistent with the observation, the correlation quantity increases. When the candidate states are close in time but the direction of change is opposite, the trend difference term suppresses the corresponding correlation quantity.

[0043] In one embodiment, the observability evolution network constructs a locally observable subgraph based on the observability level of each state component and the main source of constraint. State components with decreasing observability levels are set as target nodes, and state components with kinematic coupling to the target nodes and whose observability levels meet the constraint conditions are set as support nodes. The observability evolution network combines a variable time delay alignment graph to determine the effective time correspondence between target nodes and support nodes, and selects cross-state constraint paths according to the complementarity of constraint directions. The heading constraint path is formed by track curvature and displacement continuity, and the position and track transfer constraint path is formed by attitude change rate and velocity continuity. The locally observable subgraph only retains nodes and factors that have direct constraints or kinematic coupling constraints with the target node, avoiding the introduction of irrelevant information from the entire joint state graph into the local recovery process. Support nodes must satisfy the following conditions: their observability level is in a stable range, the main source of constraint does not have a continuous negative contribution, and the coupling relationship with the target node is valid in the current motion state.

[0044] Specifically, when a heading node becomes a target node, the locally observable subgraph extracts track curvature nodes, horizontal position change nodes, corresponding velocity direction nodes, and angular motion trend nodes. Based on the tangential direction formed by continuous displacement and track curvature, the heading change range is deduced, and the angular motion trend is used to limit the heading change per unit time. When a position node becomes a target node, the locally observable subgraph extracts velocity nodes, attitude change nodes, control action constraints, and adjacent position nodes. The displacement range is given by velocity continuity, the displacement direction is limited by the coupling of attitude change and motion direction, and the support state and target state are determined to be in the same actual time period by using a variable time delay alignment map. When a track curvature node becomes a target node, the locally observable subgraph uses continuous heading changes, adjacent displacement directions, and velocity magnitudes to form curvature constraints. If the displacement is close to a stationary state, the inference of curvature from the displacement direction is paused, and the angular motion trend constraints are retained.

[0045] The choice of a path across state constraints is expressed as: ; in, Indicate candidate path The selection factor for the target state component j This indicates the complementarity between the constraint directions provided by the candidate path and the missing constraint directions of the target node. The normalized combination value representing the observability of support nodes within the path. This represents the normalized combination value of observed conflicts within the path. It represents the time inconsistency of nodes within the path after variable time delay alignment and has a value range of 0 to 1. For example, when the direct observation of the heading degrades while the displacement direction, track curvature, and angular motion trend are all stable, the corresponding cross-state path has a high degree of complementarity and a low amount of conflict, and the path selection amount increases. When the actual corresponding time of the support node and the target node is inconsistent, the time inconsistency inhibits the path from participating in the joint estimation.

[0046] In one embodiment, the counterfactual verification module is configured according to the observation factor, observation source, and locally observable sub-factor. Figure 3Nested shielding branches are established at each level. First, a single observation factor is shielded to obtain a local contribution vector. Then, the set of observation factors formed by the same observation source within a continuous time window is shielded to obtain a source contribution vector. Subsequently, the constraint path connected to the target node is shielded to obtain a path contribution vector. The counterfactual verification module aligns the local contribution vector, source contribution vector, and path contribution vector according to the corresponding state components and generates implicit degradation markers, error propagation directions, and fractional confidence corrections based on the contribution consistency among the three levels. The single observation factor layer is used to identify occasional anomalies, the observation source layer is used to identify the overall drift of continuous sources, and the locally observable sublayer is used to identify cases where erroneous observations are transmitted to other state components through coupled paths. The three levels use the same joint state origin and state transition constraints and perform short-time inference for different shielding ranges.

[0047] In this embodiment, the local contribution vector records the contribution value according to the state components connected by a single observation factor, the source contribution vector records the contribution value according to the state change after all observation factors from the same source are removed within a continuous time window, and the path contribution vector records the contribution value according to the state change after the complete constraint path between the target node and the support node is removed. The tensor alignment process uses the state component index and the state time index as a unified dimension, and sets the contribution position that does not involve a certain state component as a no-contribution marker instead of a value of zero, so as to avoid unrelated states being misjudged as neutral contributions. When the local contribution is occasionally negative while the source contribution and path contribution remain positive, the corresponding anomaly is recorded as a local perturbation. When the local contribution, source contribution, and path contribution are all negative within a continuous time window, a latent degradation marker is generated. When the source contribution is negative but some local contributions are positive, positive contribution observations are retained and negative contribution observations are split from the source set.

[0048] Furthermore, the error propagation direction is determined by the sequential relationship of state residual changes under different shielding levels. If the heading residual decreases first after shielding a certain observation source, and subsequently causes a decrease in the position and track curvature residuals, then the heading node is recorded as the propagation starting point, and the position node and track curvature node are recorded as the propagation arrival node. The neural factor graph joint estimation module reduces the message intensity propagated from the heading node to subsequent nodes through the coupling factor accordingly. If the target node residual decreases but the supporting node residual increases after shielding the constraint path, it indicates that the path direction setting is not suitable for the current state, and the local observable subgraph reselects an alternative path. The fractional confidence correction is stored separately according to the observation source, state component, and time window, so that the same source has different information injection states at different time periods and different state components, instead of covering all its observation content through fixed weights at the source level.

[0049] In one embodiment, the neural factor graph joint estimation module sets the joint state vector as state nodes, and the motion constraints corresponding to the observed factors, cross-state constraint paths, and control actions as factor nodes. It adjusts the message transmission direction between each factor node and the state node based on the observability representation of the state components. The neural factor graph joint estimation module writes the fractional confidence correction amount into the gating variables of the corresponding messages and the implicit degradation identifier into the retained variables of the factor nodes. By alternately executing state node updates, factor node residual updates, and uncertainty propagation, a joint attitude and trajectory state sequence with confidence intervals for the state components is formed. The message transmission direction is determined based on whether the factors provide direct constraints, cross-state support, or conflict information to the state components. Direct constraint messages are transmitted from the observed factors to the corresponding state nodes, cross-state support messages are transmitted from the support nodes through the coupling factors to the target nodes, and conflict information is transmitted from the target nodes back to the factor nodes that generated the conflict to update the retained variables.

[0050] The message from the a-th factor node to the j-th state node in the (n+1)-th iteration is represented as follows: ; in, This represents the correction message sent by the factor node to the state node. This represents the gate variable corresponding to the confidence correction amount for the quantization. Indicates that the factor node retains variables. This represents the local mapping matrix of factor nodes to state components. This represents the constrained observations carried by the factor nodes. Represents the set of adjacent states based on factors. The function for calculating constrained predicted values. This represents the estimated value of the adjacent state nodes at the nth iteration; when the factor is marked as implicitly degenerate, Factor records are reduced but not deleted. When the counterfactual check reconfirms that the factor has been restored to credibility, the retained variables can be added back and message passing can be restored.

[0051] Preferably, when updating state nodes, all incoming messages are normalized according to their corresponding state component confidence intervals. Factor messages with high uncertainty only change the state distribution range without directly causing large state shifts. Cross-state support messages from local observable subgraphs only enter the main update channel when the observability of the target node is insufficient. When updating factor node residuals, the interpretation difference of the current state on the coupling relationship between observation, state transition, and attitude and track is recalculated, and the residual direction is fed back to the observability evolution network. Uncertainty propagation synchronously updates the state node confidence intervals along the edge of the factor graph. When direct observations decrease but support paths are stable, the confidence interval expands according to the uncertainty of the support path. When direct observations and support paths continue to conflict, the confidence interval simultaneously contains two candidate state directions and triggers a new counterfactual shielding branch.

[0052] In one embodiment, the residual feedback module extracts observation reconstruction residuals, cross-state constraint residuals, counterfactual branch differences, and confidence interval coverage states from the joint attitude and track state sequence, and establishes a residual memory graph according to the observation source and state components. The residual feedback module selects time windows where the observability level continuously satisfies the set relationship, the residual directions of multiple observation sources are consistent, and the counterfactual branches are not marked with implicit degradation as self-supervised time windows. The residual memory graph within the self-supervised time window is converted into degradation labels and constraint strength labels, and the degradation feature encoding parameters of the observation factor construction module and the graph message passing parameters of the observability evolution network are corrected respectively using a separate update method. The nodes in the residual memory graph correspond to the observation source, state component, and time window. The edges between the source node and the state component node store the observation reconstruction residuals, the edges between the state component nodes store the cross-state constraint residuals, and the edges between the time window nodes store the persistence of the residual direction and the changes in the counterfactual branch differences.

[0053] In this embodiment, the selection of self-supervised time windows does not rely on external manual annotation, but rather establishes constraints based on the internal consistency of the joint state. The stable observability level within multiple consecutive time windows indicates that the state constraints have not been suddenly interrupted. The consistent direction of residuals from multiple sources indicates that there is no continuous conflict in the interpretation of state changes by different observations. The unlabeled implicit degradation of counterfactual branches indicates that the state interpretation capability will not improve in the reverse direction after removing any major observation. Within the time windows that meet the above conditions, segments where the observation reconstruction residuals slowly increase over time and the source contribution changes from positive to negative are converted into progressive degradation labels. Segments where the residuals suddenly increase but recover immediately afterward are converted into short-term perturbation labels. Segments where the main constraint sources of a certain state component decrease but the cross-state support path remains stable are converted into constraint migration labels. The degradation feature encoding parameters are updated only according to the degradation labels, and the observability graph message passing parameters are updated only according to the constraint strength labels, avoiding the mutual cancellation of the two types of parameters under the same residual driving.

[0054] The update representation of the b-th memory node in the residual memory graph is as follows: ; in, This represents the residual memory vector of the b-th memory node at state k. This represents the residual memory vector at the previous state time. This represents the residual encoding of observation reconstruction. This represents the encoding of cross-state constraint residuals. This indicates the counterfactual branch difference encoding. This indicates the confidence interval coverage status code. to The memory vector represents the retention coefficient and the writing coefficient of the corresponding information. When the observation reconstruction residual increases in the short term but the counterfactual difference does not indicate that the observation has a negative contribution, the memory vector retains the short-term disturbance record. When the observation reconstruction residual and the counterfactual negative contribution accumulate continuously in the same direction, the memory vector forms a progressively degenerate trajectory.

[0055] In a preferred embodiment, the neural factor graph joint estimation module establishes a short-term state update layer and a cross-window state smoothing layer. The short-term state update layer generates candidate joint states based on the local observable subgraph, the fractional confidence correction, and the observation factors of the current time window. The cross-window state smoothing layer retrieves the historical constraint chains corresponding to the candidate joint states based on the residual memory graph and performs reverse message passing on a preset number of state nodes before the current time window. The observability evolution network regenerates the observability representation based on the constraint residuals after reverse message passing. The counterfactual verification module reconstructs nested masked branches for observation factors that have undergone weight reversal. The residual feedback module saves the state nodes and observation factor versions before and after the update. The short-term state update layer only processes the current time window and the adjacent historical time windows to ensure that the current joint state can be formed after the arrival of new observations. The cross-window state smoothing layer is activated when delayed observations, confidence reversals, or inconsistencies in historical constraint chains are detected, and the backtracking range is limited to state nodes that have a propagation relationship with the current residual.

[0056] Specifically, residual memory graph retrieval uses the attitude change pattern, track change pattern, main constraint source, and error propagation direction of the candidate joint state as query conditions, and returns historical constraint chains connected to the current state node that contain the same source or the same cross-state path. Reverse message passing starts from the current state node and propagates the residual correction formed by the new observation to the corresponding historical state node along the historical constraint chain. For each state transition factor, the continuity between the historical state and its adjacent states is recalculated, and for each attitude and track coupling factor, the consistency between attitude change and track change is recalculated. If the reverse correction causes the observation reconstruction residual of an earlier state node to continue to decrease, it continues to propagate forward. If the reverse correction causes multiple originally consistent observation sources to conflict simultaneously, it stops propagation and uses the stopping position as the backtracking boundary. The backtracking range is determined by the residual propagation relationship and does not use a fixed length to cover all historical states.

[0057] The smoothing of cross-window states is performed using the following constraints: ; in, This represents the smoothed joint state sequence from historical state a to current state b. It represents the constraint quantity composed of the continuity between adjacent states and the consistency of attitude and track coupling. This represents a residual memory subplot within the corresponding time range. This represents the amount of explanation provided by the state sequence for the discrepancies between historical observations and counterfactual differences in the residual memory graph. This represents the set of state nodes and observation factor versions stored within this time range. This indicates the deviation of the updated state from the constraints of the original version. When delayed observations can explain historical residuals without disrupting the continuity of subsequent states, the smoothing result is corrected in the direction of delayed observations. When delayed observations continue to conflict with multiple confirmed sources, the version deviation restricts their rewriting of historical states.

[0058] Please refer to Figure 8 and Figure 9 The horizontal axis represents the sampling window stage, labeled with Chinese terms for the start, middle, and end segments. The vertical axis represents the state value after smoothing across the window. The right-hand caption indicates the name of the state component. Figure 8 This is a smoothed attitude angle and planar position component map across the window, corresponding to... The components of the yaw angle, pitch angle, roll angle, forward position, and lateral position are as follows: the forward position and lateral position continue to increase, and the yaw angle gradually rises to 1.660910, indicating that the robot's displacement propulsion and heading accumulation trend are preserved after the window smoothing. The pitch angle and roll angle fluctuate slightly around zero, indicating that the smoothing constraint does not introduce additional attitude jumps. Figure 9The smoothed altitude, velocity, and trajectory curvature components are shown across windows, displaying components such as altitude, forward velocity, lateral velocity, vertical velocity, and path curvature. The altitude initially rises and then falls, the forward velocity decreases overall, the lateral velocity initially rises and then stabilizes, the vertical velocity fluctuates near zero, and the path curvature peaks at 0.044159 near window 19 before falling back, indicating that the trajectory turning and velocity decomposition are stably preserved during the smoothing process.

[0059] In this embodiment, after cross-window smoothing, the observability evolution network rereads the observation residuals, cross-state residuals, and source components of each historical state node and recalculates the observability representation at the state component level. If an observation factor has its information injection coefficient reduced due to high residuals in the initial solution, but its residuals decrease after cross-window smoothing while the residuals from other sources increase, then a weight reversal is determined. The counterfactual verification module uses the updated historical states as a common starting point to re-establish three levels of shielded branches: single observation factor, observation source, and constraint path. The re-verified confidence correction is written to the new version of the corresponding observation factor. The old version remains read-only and is bound to the state node at the time the version was generated. The residual feedback module saves the correspondence between the state nodes, observation factors, observability representations, and counterfactual contributions before and after the update, so that subsequent backtracking can determine the constraint source on which each state change is based.

[0060] Furthermore, during system operation, observation reception, observation factor construction, observability inference, counterfactual verification, neural factor graph update, and residual feedback are executed cyclically according to state time. No single source's output is used as the basis for state credibility at any stage. When new data arrives, the observation factor construction module first determines its true acquisition time and state mapping interval. The observability evolution network then determines whether there are observation gaps or constraint conflicts in the state components constrained by the new data. The counterfactual verification module only establishes shielded branches for observations that may change the joint state interpretation relationship. The neural factor graph joint estimation module applies the confirmed information injection coefficients to the corresponding state components. The residual feedback module writes the updated residuals into the memory graph. When observations remain normal, the counterfactual branch can reuse the contribution trend of the previous time window and recalculate only for sources of change. When observations degrade, cross-window delays occur, or state uncertainty expands, the system expands the counterfactual verification range and initiates local observable subgraphs and cross-window smoothing processing.

[0061] In a preferred embodiment, attitude perception data, motion perception data, and environmental relative observation data are allowed to have different sampling frequencies and different data dimensions. The observation factor construction module does not perform uniform frequency resampling, but instead retains the actual acquisition time for each data point and connects the corresponding state nodes according to the state mapping interval. High-frequency data can form multiple observation factors at the same state time. Multiple factors are merged into a source subgraph according to their time position and residual correlation. When low-frequency data spans multiple state times, it forms an interval constraint factor. The interval constraint factor connects the starting state node, the ending state node, and the state components directly related to it within the interval. When the observable evolution network calculates the constraint strength according to the independent information provided by the factors, it does not directly increase the constraint strength by the number of factors from the same source, thus avoiding high-frequency repetitive data suppressing low-frequency data with independent constraint directions.

[0062] Preferably, joint state initialization does not require all state components to have direct observations simultaneously. Initial state nodes are formed first from state components that can be determined by current observations. State components that cannot be directly determined form an initial distribution with a relatively wide confidence interval based on motion constraints and adjacent state components. The observability evolution network marks state components that lack direct observations in the initialization phase as states to be supported, and selects support paths that can explain their change direction from the attitude and track coupling relationship. The counterfactual verification module does not perform overall masking on the only observation that lacks alternative constraints in the initialization phase. Instead, it generates candidate states by reducing the injection intensity and retains a relatively wide confidence interval. After subsequent observations arrive, the retained branches and masked branches are compared. When subsequent observations are consistent with the candidate states, the corresponding confidence interval is shrunk. When subsequent observations negate the candidate states, the initialization state is corrected by backtracking through the cross-window state smoothing layer.

[0063] In this embodiment, when extreme conditions cause multiple observation sources to degrade simultaneously, the observability evolution network identifies the remaining constraints one by one according to the state components. If a state component still has direct observations but there are continuous conflicts between observations, the counterfactual verification module prioritizes identifying the negative contribution source. If a state component lacks direct observations but has a stable cross-state path, the joint estimation module maintains state continuity through support nodes. If both direct observations and cross-state support are insufficient, the joint estimation module maintains the predicted distribution given by the state transition function and expands the confidence interval, without forcibly using observations of other uncoupled state components for compensation. When a new observation is recovered, the recovered observation first determines the actual corresponding state time through a variable time delay alignment map, and then the counterfactual verification module judges whether it can explain the joint state changes during the missing period. Recovered observations that satisfy time consistency and motion consistency enter cross-window smoothing, while recovered observations that do not satisfy the constraints are retained as factors to be verified.

[0064] In typical extreme operating conditions, when the MEMS inertial measurement unit experiences pitch and roll rate zero-bias drift due to continuous strong vibration, the implicit degradation identification and cross-state compensation process of this system is triggered.

[0065] Specifically, when a heading observation experiences directional drift while position and velocity observations remain continuous, the state constraint projection layer continuously connects the drift observation to the heading node. The temporal correlation layer records that the residual direction is biased to the same side for a long time. The cross-source conflict relationship shows that the heading change is inconsistent with the inference of displacement direction and track curvature. The counterfactual shielding branch removes the drift observation and obtains a lower cross-time discontinuity and coupling constraint violation. The fractional quantization confidence correction reduces the information injection of the drift observation to the heading node. The local observable subgraph calls displacement direction, track curvature, and angular motion trend to form a heading support path. When the same observation can still provide usable information for other state components, the information injection coefficients for the corresponding other state components remain unchanged, and error propagation is isolated according to the state components.

[0066] Specifically, in cases where the relative environmental observation arrives late and the robot's attitude changes during this period, the variable time delay alignment map connects the relative environmental observation to the historical state mapping interval based on the actual acquisition time. The observation change trend and the candidate historical state change trend jointly determine the correlation quantity, and the delayed observation does not directly correct the current state. The cross-window state smoothing layer retrieves the historical constraint chain connected to the source of the relative environmental observation from the current residual memory map, and propagates the correction in reverse along the position node, velocity node, and attitude coupling node. If the corrected historical state can simultaneously explain the delayed observation, subsequent displacement, and current attitude, the updated version is saved and the observability representation of the relevant time window is recalculated. If the correction causes multiple subsequent confirmed observations to have residuals in the same direction, the information injection of the delayed observation is restricted and the original historical state version is retained.

[0067] Specifically, when the robot rapidly changes its posture and multiple observation residuals increase simultaneously, the system does not directly determine all sources of degradation based on the residual magnitude. Instead, it divides the causal time window by controlling the action timestamp, writes the expected state changes caused by the control action into the state transition factor, and reconstructs the posture and trajectory changes in the observation function at the corresponding acquisition time. Residual changes that can be explained by the coupling relationship between control action and attitude and trajectory are recorded as motion state changes and are not included in the implicit degradation label. Sources that cannot be explained by state transition and cause the subsequent observation to explain the decrease in residuals after masking are written into the negative contribution label. The observability evolution network updates the observability of each state component according to the residual constraints, and the joint estimation module suppresses observation corrections that are inconsistent with the motion causal relationship while maintaining the real rapid posture changes.

[0068] In a preferred embodiment, the residual feedback module uses constrained incremental updates to correct model parameters. Degenerate feature encoding parameters are updated only when the number of self-supervised time windows meets the continuity requirement, and graph message passing parameters are updated only when the main constraint sources and supporting paths of the same state component can be confirmed. Before each update, the original parameter version and the corresponding residual memory map range are saved. After the update, the adjacent time windows that did not participate in the parameter correction are used for verification. If the updated parameters cause the observation reconstruction residual to decrease but the cross-state constraint residual continues to increase, the graph message passing parameter update is canceled and the degenerate feature encoding parameters are retained. If the updated parameters cause the counterfactual negative contribution observation to regain higher information injection and the subsequent interpretation residual increases, the degenerate feature encoding parameter update is canceled. The parameter version, state node version, and observation factor version jointly establish a reference relationship to ensure that the inference conditions used in the corresponding time window can be reproduced when backtracking across windows.

[0069] In this embodiment, the output of the joint attitude and trajectory state includes the estimated value of each state component, confidence interval, main constraint source, supporting constraint path, and current degradation identifier. The output process only selects the latest confirmed version of the current state node and does not use candidate states in the masked branch as the formal output. When a state component is in a state where direct observation is insufficient but cross-state support exists, the output retains the supporting path identifier and the corresponding confidence interval. When a state component lacks both direct observation and available supporting path, the output retains the state transition prediction value and the expanded confidence interval. After cross-window smoothing modification of the historical state, the current state is re-solved through the updated historical constraint chain to form a new version. The old version is used for residual comparison and counterfactual verification and no longer participates in subsequent master state propagation.

[0070] Furthermore, the observation factor construction module, observability evolution network, counterfactual verification module, neural factor graph joint estimation module, and residual feedback module maintain consistent referencing through state time index, observation factor index, and version index. Any observation factor can be traced back to the original observation record, the corresponding state mapping interval, the connected state components, observability representation, counterfactual contribution, information injection coefficient, and residual memory node. Any joint state node can be traced back to the observation factors, state transition factors, cross-state constraint paths, and parameter versions involved in the solution. When the system detects an anomaly in the state result, it can reconstruct the retained branch and masked branch within the specified time window based on the above indexes to verify that the anomaly comes from the observation source, time delay mapping, cross-state path, or parameter update, and regenerate the joint attitude and trajectory state sequence within the limited backtracking range.

Claims

1. A joint perception system for attitude and trajectory of an embodied intelligent special robot under extreme working conditions, characterized in that, It includes an observation factor construction module, an observability evolution network, a counterfactual verification module, a neural factor graph joint estimation module, and a residual feedback module; The observation factor construction module encodes attitude perception data, motion perception data, and environmental relative observation data into observation factors with time correlation and uncertainty identifiers. The observability evolution network generates state component-level observability representations based on the constraint relationship between observation factors within a continuous time window on attitude and track state components. The counterfactual verification module constructs observation factor shielding branches and forms a credibility correction quantity; The neural factor graph joint estimation module reorganizes the observation constraints based on the observability representation and the confidence correction amount, and outputs the joint attitude and trajectory state. The residual feedback module feeds back the joint state residuals to the observation factor construction module and the observability evolution network.

2. The attitude and trajectory joint perception system for an embodied intelligent special robot under extreme working conditions according to claim 1, characterized in that, The observation factor construction module establishes a joint state vector that includes heading, pitch, roll, angular motion trend, spatial position, motion speed and track curvature, and configures source identifier, observation subspace, acquisition time, effective interval, delay state and uncertainty code for each type of input data; The observation factor construction module divides the causal time window according to the control action timestamp, maps the attitude perception data, motion perception data and environmental relative observation data in the same time window into state constraint vectors respectively, generates degradation features based on the residual change rate, cross-source correlation and missing measurement persistence of adjacent time windows, and writes the state constraint vector and the degradation features into the node attributes and edge attributes of the observation factor.

3. The attitude and trajectory joint perception system for an embodied intelligent special robot under extreme working conditions according to claim 1, characterized in that, The observability evolution network includes a state-constrained projection layer, a temporal correlation layer, and an observability aggregation layer; The state constraint projection layer projects each observation factor to the state component corresponding to the joint state vector, forming a directed constraint graph between the observation factor and the state component. The temporal correlation layer calculates the direction change, intensity change and residual correlation change of the constraints received by the same state component along a continuous time window, and forms the temporal embedding of the state component. The observability aggregation layer performs graph message passing on the temporal embedding of the state components, and outputs the observability level of each state component, the main sources of constraints, and the constraint conflict relationships, which constitute the state component-level observability representation.

4. The extreme working condition embodied intelligent special robot attitude and trajectory joint perception system according to claim 1, characterized in that, The counterfactual verification module establishes a reserved branch and a masked branch for each observation factor to be evaluated, and calls the same neural factor graph parameters in the two branches to generate the corresponding short-term joint state sequence. The counterfactual verification module calculates the cross-time continuity, motion constraint violation, subsequent observation interpretation residual, and state uncertainty expansion of the two joint state sequences, and encodes the difference between the shielded branch and the retained branch as an observation contribution vector. The observation contribution vector and the constraint conflict relationship in the observability representation are jointly reasoned to generate the credibility correction amount of the corresponding observation factor and the information injection coefficient that acts on the specific state component.

5. The attitude and trajectory joint perception system for an embodied intelligent special robot under extreme working conditions according to claim 2, characterized in that, The observation factor construction module establishes a variable delay alignment graph for input data that are collected at different times within the same causal time window. The variable delay alignment graph uses the control action time and the joint state prediction time as anchor points, and the collection time, transmission delay and adjacent observation change trends of the input data as edge constraints. The observation factor construction module obtains the state mapping interval of each input data relative to the anchor point time through time delay latent variable inference, writes the state mapping interval and the uncertainty code together into the observation factor, and retains the input data exceeding the mapping interval as cross-window delay factor, which is associated and referenced by the state constraint graph in the subsequent time window.

6. The attitude and trajectory joint perception system for an embodied intelligent special robot under extreme working conditions according to claim 3, characterized in that, The observable evolution network constructs a local observable subgraph based on the observability level of each state component and the main source of constraint. The state component whose observability level decreases is set as the target node, and the state component that has a kinematic coupling relationship with the target node and whose observability level satisfies the constraint condition is set as the support node. The observable evolution network, combined with the variable time delay alignment map, determines the effective time correspondence between the target node and the support node, and selects cross-state constraint paths according to the constraint direction complementarity. The heading constraint path is formed by track curvature and displacement continuity, and the position and track transfer constraint path is formed by attitude change rate and velocity continuity.

7. The attitude and trajectory joint perception system for an embodied intelligent special robot under extreme working conditions according to claim 4, characterized in that, The counterfact verification module establishes nested shielding branches according to three levels: observation factor, observation source, and locally observable subgraph. First, it shields a single observation factor to obtain a local contribution vector. Then, it shields the set of observation factors formed by the same observation source within a continuous time window to obtain a source contribution vector. Finally, it shields the constraint path connected to the target node to obtain a path contribution vector. The counterfact verification module aligns the local contribution vector, the source contribution vector, and the path contribution vector according to their corresponding state components using tensors, and generates implicit degradation identifiers, error propagation directions, and fractional confidence corrections based on the consistency of contributions among the three levels.

8. The attitude and trajectory joint perception system for an embodied intelligent special robot under extreme working conditions according to claim 7, characterized in that, The neural factor graph joint estimation module sets the joint state vector as a state node, sets the observed factors, cross-state constraint paths and motion constraints corresponding to control actions as factor nodes, and adjusts the message passing direction between each factor node and the state node according to the observability representation of the state component level. The neural factor graph joint estimation module writes the fractional confidence correction into the gated variable of the corresponding message and writes the latent degradation identifier into the reserved variable of the factor node. By alternately executing state node update, factor node residual update and uncertainty propagation, a joint attitude and trajectory state sequence with state component confidence intervals is formed.

9. The attitude and trajectory joint perception system for an embodied intelligent special robot under extreme working conditions according to claim 1, characterized in that, The residual feedback module extracts observation reconstruction residuals, cross-state constraint residuals, counterfactual branch differences, and confidence interval coverage states from the joint attitude and track state sequence, and establishes a residual memory map according to the observation source and state components. The residual feedback module selects time windows where the observable levels continuously satisfy the set relationship, the residual directions of multiple observation sources are consistent, and the counterfactual branches are not marked with implicit degradation as self-supervised time windows. The residual memory graph within the self-supervised time window is converted into degradation labels and constraint strength labels, and the degradation feature encoding parameters of the observation factor construction module and the graph message passing parameters of the observability evolution network are corrected respectively by a separate update method.

10. The attitude and trajectory joint perception system for an embodied intelligent special robot under extreme working conditions according to claim 8, characterized in that, The neural factor graph joint estimation module establishes a short-term state update layer and a cross-window state smoothing layer. The short-term state update layer generates candidate joint states based on the local observable subgraph, the fractional confidence correction amount, and the current time window observation factors. The cross-window state smoothing layer retrieves the historical constraint chain corresponding to the candidate joint states based on the residual memory graph and performs reverse message passing on a preset number of state nodes before the current time window. The observability evolution network regenerates the observability representation based on the constraint residuals after the reverse message passing. The counterfactual verification module reconstructs nested masked branches for the observation factors that have undergone weight reversal. The residual feedback module saves the state nodes and observation factor versions before and after the update.

Images