Autonomous driving positioning integrity fault detection identification adaptive evaluation method

Abstract

The application provides an automatic driving positioning integrity fault detection and recognition adaptive evaluation method, and relates to the technical field of vehicle control.The application realizes real-time estimation and constraint of observation error and state uncertainty by fusing GNSS and IMU and introducing error state Kalman filtering.The application introduces residual error driven detection-recognition-adaptation, can realize real-time identification, elimination and weight correction of abnormal observation in a multi-dimensional observation space, and through dynamic threshold adjustment and uncertainty quantization mechanism, makes the filtering process be able to automatically adjust the observation noise model according to the environmental change, and ensures that the system maintains stable convergence and high-precision performance in multiple scenes.The application adopts optimal subset selection and white residual error construction in the ESKF updating link, effectively reduces the influence of abnormal observation dimension on state estimation covariance.Through the integrity evaluation method combining the protection level and the alarm limit value, whether the positioning error meets the safety requirement can be judged.

Description

Technical Field

[0001] This invention relates to the field of vehicle control technology, specifically to an adaptive evaluation method for detecting and identifying faults in the positioning integrity of autonomous driving systems. Background Technology

[0002] Vehicle positioning services based on global navigation satellite systems (such as GPS and BeiDou) play a crucial role in fields such as autonomous driving, intelligent transportation, and logistics management. To ensure the reliability and safety of the positioning system in complex and dynamic environments, it must provide high accuracy, strong robustness, and real-time response capabilities. For autonomous vehicle applications, the integrity of the positioning system is one of the core issues ensuring driving safety. However, complex urban environments may present problems such as occlusion, multipath effects, and signal interference, leading to increased positioning error (PE), thereby threatening vehicle driving safety.

[0003] Currently, common methods for improving positioning integrity include Fault Detection and Exclusion (FDE). FDE primarily ensures the system's positioning accuracy by eliminating signals or sensor data affected by faults. Residual models and particle filters have been introduced to improve the FDE process for vehicle positioning integrity assessment. Dynamic Detection, Identification, and Adaptation (DIA) can dynamically identify fault types and adjust system model parameters in real time to improve system adaptability. Traditional FDE methods perform well in single-fault scenarios, but their effectiveness may be limited when facing multiple faults or complex dynamic environments. Therefore, combining the advantages of FDE and DIA, through the synergistic work of fault elimination and adaptive adjustment, can significantly improve the integrity of vehicle positioning systems in complex environments. Positioning integrity methods based on the combination of FDE and DIA can simultaneously address signal interference and sensor faults from multiple sources, such as navigation satellite signal obstruction, lidar obstruction by rain or fog, or degraded camera image quality. This approach typically relies on advanced filtering algorithms such as Kalman filtering (KF) and multi-sensor data fusion technology to ensure that the system can quickly recover and adapt to new environmental conditions after the fault is eliminated.

[0004] Furthermore, a key metric for vehicle positioning integrity is the Protection Level (PL), which is the fault tolerance defined by the probability distribution and confidence interval of vehicle positioning errors. In the approach combining FDE and DIA, real-time fault detection, identification of impact sources, and dynamic adjustment of the Protection Level calculation can improve the positioning system's adaptability to multi-fault environments. However, due to the diversity of fault types and the complexity of Protection Level calculation in dynamic environments, maintaining high-precision Protection Levels while ensuring real-time performance remains a critical technical challenge. Summary of the Invention

[0005] To address the shortcomings of existing technologies, the present invention aims to propose an adaptive evaluation method for detecting and identifying faults in the positioning integrity of autonomous driving systems, comprising:

[0006] Data is collected through the Global Navigation Satellite System (GNSS), Inertial Measurement Unit (IMU), and wheel odometers installed in autonomous vehicles. k Positioning data at time -1 and k Positioning data at any given time, including position, velocity, attitude angle, acceleration, angular velocity, and wheel speed;

[0007] Based on the error state Kalman filter (ESKF), k Positioning data at time -1 and k The location data at any given time is processed to obtain... k Prediction at time -1 k Error state covariance at time 1 , k Prediction at time -1 k State vector at time step Kalman gain , k Time Error State Covariance and k State vector at time step ;

[0008] based on and Calculate residuals and residual covariance GNSS measurement vector at time k Divide the observation into multiple observation dimensions according to the coordinate system, set an adaptive detection threshold for each observation dimension, perform fault detection on all observation dimensions, and determine multiple candidate observation dimensions.

[0009] Identify and determine the fault observation dimension from all candidate observation dimensions;

[0010] Based on fault observation dimensions and Kalman gain The error covariance matrix is ​​updated to obtain the updated error state covariance. The position, velocity, and attitude angles are corrected to obtain the final position estimate after fusion of observations. Final velocity estimation after fusion of observations Final attitude estimation after fusion of observations ;

[0011] Based on the updated error state covariance The positioning integrity capability of autonomous vehicles is evaluated to obtain evaluation results. The evaluation results indicate that the autonomous vehicle has positioning integrity capability that meets application requirements, or that the autonomous vehicle does not have positioning integrity capability that meets application requirements.

[0012] Optionally, based on the error state Kalman filter (ESKF), for k Positioning data at time -1 and k The location data at any given time is processed to obtain... k Prediction at time -1 k Error state covariance at time 1 , k Prediction at time -1 k State vector at time step Kalman gain , k Time Error State Covariance and k State vector at time step ,include:

[0013] based on k The positioning data at time -1 and the positioning data at time k are used to construct... k The state vector at time -1 and k State vector at time step , k The state vector at time -1 and k State vector at time step Represented as:

[0014] ;

[0015] ;

[0016] in, for k The position at time -1 for k Location at any given moment for k The velocity at time -1 for k The speed of time for k The attitude angle at time -1 for k Attitude angle at any moment For the zero bias term of the accelerometer, This is the zero bias term of the gyroscope. T Indicates matrix transpose;

[0017] Based on inertial navigation and the ESKF algorithm, the autonomous vehicle is defined as... k The state error vector at time -1 and k State error vector at time step , k The state error vector at time -1 and k State error vector at time step Represented as:

[0018] ;

[0019] ;

[0020] in, express k Position error at time -1 express k Position error at any given time; express k The velocity error at time -1 express k The speed error at any moment, express k Attitude error at time -1 , for k The roll angle at time -1 for k The pitch angle at time -1 for k Yaw angle at time -1 express k Attitude error at any given moment; The zero bias error of the gyroscope in a coordinate system with the vehicle's center of gravity as the origin; The zero bias error of the accelerometer in a coordinate system with the vehicle's center of gravity as the origin;

[0021] according to k The state error vector at time -1 and k State error vector at time step An error state propagation model is constructed, which is expressed as follows:

[0022] ;

[0023] in, This represents the prediction of the attitude angle error at time k-1 for time k. This represents the Earth's rotation rate in the navigation coordinate system. , Let be the direction cosine matrix, where , , Represented as:

[0024] ;

[0025] ;

[0026] ;

[0027] Based on the state error vector An error state linearization model is constructed, which is expressed as follows:

[0028] ;

[0029] ;

[0030] ;

[0031] in, Indicates speed error, This represents the prediction of the velocity error at time k-1 from that at time k. For the comparison of IMU, Indicates the force relative to the north direction. Indicates the force in the east direction. Represents the specific force in the vertical direction. This indicates the relative force error of the IMU. This represents the prediction of the position error at time k-1 from that at time k. This represents the state prediction at time k-1 for time k; Here is the state transition matrix. The noise transfer matrix, It is the process noise vector matrix;

[0032] Construct a GNSS observation model, which is represented as follows:

[0033] ;

[0034] in, yes k GNSS observation model at time 10:00 It is the state vector at time k. For the observation matrix, The first 3 rows are the identity matrix, and the last 12 rows are the zero matrix. The noise coupling matrix is... GNSS measurement noise with zero mean Gaussian distribution. This indicates GNSS measurement noise in the north direction. This indicates GNSS measurement noise in the east direction. Indicates GNSS measurement noise in the vertical direction;

[0035] Error State Kalman Filter (ESKF) is used to predict the state vector at time k and the error state covariance at time k, thus obtaining the predicted state vector at time k-1. Error state covariance at time k predicted at time k-1 Specifically, this is achieved through the following formula:

[0036] ;

[0037] ;

[0038] in, yes k The state vector at time -1 It is a nonlinear state propagation function driven by IMU data. Acceleration and angular velocity measured by the inertial measurement unit (IMU) This represents the discrete noise transition matrix at time k-1. This represents the discrete state transition matrix at time k-1. express k Error state covariance at time -1 It is the process noise covariance;

[0039] Based on GNSS observation models, and Update Kalman gain Error state covariance at time k and the state vector at time k Specifically, this is achieved through the following formula:

[0040] ;

[0041] ;

[0042] ;

[0043] in, It is an observation model The Jacobian matrix relative to the error state vector at time k, Represents the discretized observation matrix. For the GNSS measurement noise covariance matrix, Let be the GNSS measurement vector at time k. The GNSS measurement vector is obtained by processing the position, velocity, and attitude angle of the GNSS measurements. The observation function is predicted at time k-1.

[0044] Optionally, the state transition matrix Represented as:

[0045] ;

[0046] ;

[0047] ;

[0048] in, Represents a 3x3 zero matrix. This represents a 3x3 identity matrix. and For simplification Matrix;

[0049] Among them, the noise transfer matrix Represented as:

[0050] ;

[0051] in, Represents a 6x3 zero matrix;

[0052] For the state transition matrix and noise transfer matrix Discretization yields the discrete state transition matrix and the discrete noise transition matrix. and discrete noise transfer matrix Represented as:

[0053] ;

[0054] ;

[0055] in, This represents a 15x15 identity matrix. This represents the state estimate at time k-1. Represents the velocity at time k; It is a fixed time interval.

[0056] Optional, based on and Calculate residuals and residual covariance Specifically, this is achieved through the following formula:

[0057] ;

[0058] ;

[0059] in, For the GNSS measurement noise covariance matrix, It is an observation model The Jacobian matrix relative to the error state vector at time k.

[0060] Optionally, the GNSS measurement vector at time k... The observation dimensions are divided according to a coordinate system. An adaptive detection threshold is set for each observation dimension. Fault detection is performed on all observation dimensions to determine multiple candidate observation dimensions, including:

[0061] The GNSS measurement vector at time k is divided into multiple observation dimensions according to the coordinate system. The residuals of each observation dimension are normalized to construct the statistical detection quantity, which is achieved through the following formula:

[0062] ;

[0063] ;

[0064] ;

[0065] in, T i For the first i The statistics after normalizing the residuals of each observation dimension for The Middle i The residuals of each observation dimension Indicates the first i The mean of a Gaussian distribution in each observation dimension. Indicates the first i The variance of a Gaussian distribution in each observation dimension. For residual covariance The i One diagonal element;

[0066] For each observation dimension L The normalized residual sequence over time is used for moving statistics to calculate the mean and standard deviation, specifically through the following formula:

[0067] ;

[0068] ;

[0069] in, Represents the k-th time. i The mean of each observation dimension, express j Time of the first i The normalized statistics of individual observations This represents the standard deviation of the i-th observation dimension at time k;

[0070] Introducing an uncertainty measure index Specifically, it is calculated using the following formula:

[0071] ;

[0072] in, This represents the error state covariance at time 0. It is the Frobenius norm;

[0073] Based on uncertainty measure index Set dynamic scaling factor The dynamic scaling factor Calculated using the following formula:

[0074] ;

[0075] in, As the baseline scaling factor, These are sensitivity adjustment parameters;

[0076] Based on dynamic scaling factor Calculate the adaptive detection threshold Specifically, this is achieved through the following formula:

[0077] ;

[0078] exist Greater than the adaptive detection threshold In the case of the first i Each observation dimension is used as a candidate observation dimension. Less than or equal to the adaptive detection threshold In the case of, it means the first i Since there are no faults in the observation dimension, multiple candidate observation dimensions are obtained.

[0079] Optionally, identify and determine the fault observation dimensions from all candidate observation dimensions, including:

[0080] Based on all candidate observation dimensions, multiple candidate fault subsets are generated. All candidate fault subsets form a candidate fault subset set. The candidate fault subset is the case where one or more candidate observation dimensions have a fault.

[0081] For each candidate fault subset, in the GNSS measurement vector In the process, data corresponding to candidate observation dimensions that contain faults are obtained from the candidate fault subset, thus obtaining subset observations; The vectors corresponding to the candidate observation dimensions containing faults in the candidate fault subset are obtained to obtain the subset observation matrix; The vectors corresponding to the candidate observation dimensions that contain faults in the candidate fault subset are obtained from the data, and the subset noise covariance matrix is ​​obtained.

[0082] The subset observations are whitened to obtain the whitened observations, which is achieved using the following formula:

[0083] ;

[0084] in, Denotes the subset observations of the a-th candidate fault subset. For observations after whitening, Let be the subset noise covariance matrix of the a-th candidate fault subset. It follows a multivariate normal distribution with a mean of 0 and a covariance matrix equal to the identity matrix. ;

[0085] The cost function for each candidate fault subset is calculated using the following formula:

[0086] ;

[0087] in, This represents the a-th candidate fault subset. Let represent the cost function for the a-th candidate fault subset. It is a constant. It is the dimension number of the candidate fault subset. These are the weighting coefficients. For drift trend identification function, Used to determine the first d Does the dimensional observation exhibit significant drift?

[0088] Among all the cost functions corresponding to the candidate fault subsets, the candidate fault subset with the smallest cost function is selected as the optimal subset, and the candidate observation dimension containing the fault in the optimal subset is the fault observation dimension.

[0089] Optionally, based on the fault observation dimension and Kalman gain The error covariance matrix is ​​updated to obtain the updated error state covariance. ,include:

[0090] exist After removing the data corresponding to the fault observation dimension, the target observation is obtained. ,exist After removing the data corresponding to the fault observation dimension, the target observation matrix is ​​obtained. ,exist After removing the data corresponding to the fault observation dimension, the target noise covariance matrix is ​​obtained. ;

[0091] The error covariance matrix is ​​updated based on the Kalman gain to obtain the updated error state covariance. Specifically, this is achieved through the following formula:

[0092] ;

[0093] ;

[0094] ;

[0095] ;

[0096] in, Indicates the target residual covariance. This indicates the updated Kalman gain. This represents the updated state error vector.

[0097] Optionally, the position, velocity, and attitude angles can be corrected to obtain the final position estimate after fusion of observations. Final velocity estimation after fusion of observations Final attitude estimation after fusion of observations ,include:

[0098] The predicted position at time k is obtained by using an additive correction method. The predicted velocity at time k is corrected to obtain the final position estimate after fusion of observations. Final velocity estimate after fusion of observations Specifically, this is achieved through the following formula:

[0099] ;

[0100] ;

[0101] in, This is the position error correction amount calculated by the filter. The speed error correction amount calculated by the filter;

[0102] The method of multiplication correction, for the predicted attitude angle at time k. After making corrections, the final attitude estimate after fusion of observations is obtained. Specifically, this is achieved through the following formula:

[0103] ;

[0104] ;

[0105] in, It is the small-angle attitude error calculated by the filter. This is the attitude error correction amount. It is quaternion multiplication.

[0106] Optionally, based on the updated error state covariance The positioning integrity capability of autonomous vehicles is evaluated, and the evaluation results include:

[0107] Based on the updated error state covariance The horizontal protection level (HPL) and vertical protection level (VPL) are calculated using the following formulas:

[0108] ;

[0109] ;

[0110] in, This is based on an expansion factor related to the confidence level in the horizontal direction. It is an expansion factor related to the confidence level in the vertical direction. It is the projection vector of the horizontal error. It is the projection vector of the vertical direction error;

[0111] Based on the updated error state covariance Calculate the protection level along the track direction Protection level in the transverse direction Specifically, it is calculated using the following formula:

[0112] ;

[0113] ;

[0114] in, It is a unit vector along the orbital direction. It is a unit vector in the horizontal direction. The confidence expansion factor along the track direction. The confidence expansion factor in the transverse direction;

[0115] Determine HPL, VPL, and Whether the alarm limit conditions are met, the alarm limit conditions include multiple conditions, in HPL, VPL, and When all the alarm limit conditions are met simultaneously, it indicates that the autonomous vehicle possesses the positioning integrity capability to meet application requirements, in HPL, VPL, and If at least one of the alarm limit conditions is not met, it indicates that the autonomous vehicle does not have the positioning integrity capability to meet the application requirements.

[0116] Optionally, the warning limit condition is expressed as:

[0117] ;

[0118] ;

[0119] ;

[0120] ;

[0121] in, This represents the HPL threshold in the global coordinate system. This represents the VPL threshold in the global coordinate system. This represents the threshold value for autonomous vehicles along the track direction. This indicates the threshold value for the lateral direction of an autonomous vehicle.

[0122] The beneficial effects of adopting the above technical solution are as follows:

[0123] 1. Compared with traditional single-sensor positioning methods, this invention achieves real-time estimation and constraint of observation errors and state uncertainties by fusing GNSS and IMU information and introducing the Error State Kalman Filter (ESKF) framework. This effectively suppresses the interference of anomalies such as signal jumps, delays and occlusions on the positioning results, and significantly improves the robustness and reliability of the system in complex environments.

[0124] 2. Fault detection and dynamic adaptation. This invention introduces residual-driven detection-identification-adaptation (DIA), which enables real-time identification, elimination, and weight correction of abnormal observations in a multi-dimensional observation space. Through dynamic threshold adjustment and uncertainty quantification mechanisms, the filtering process can automatically adjust the observation noise model according to environmental changes, ensuring that the system maintains stable convergence and high-precision performance in multiple scenarios.

[0125] 3. Improve system stability and accuracy continuity. This invention employs an optimal observation subset selection and whitening residual construction method in the ESKF update stage, which effectively reduces the impact of abnormal observation dimensions on the state estimation covariance, ensuring that the filter can maintain continuous and smooth positioning output even in GNSS degradation, signal loss, or strong noise environments.

[0126] 4. Establish an integrity quantification and alarm mechanism. By combining the integrity assessment method of Protection Level (PL) and Alert Limit (AL), this invention can calculate the safety limits in the horizontal, vertical, and along-rail and transverse directions in real time based on the filtered output covariance matrix, and determine whether the positioning error meets the safety requirements, thereby realizing the integrity monitoring and risk quantification of the navigation system.

[0127] 5. Enhanced adaptability and safety in multi-fault scenarios. This invention comprehensively considers multi-source sensor failure modes and environmental uncertainties, supports the identification and handling of various abnormal conditions such as occlusion, drift, and multipath interference, and realizes a closed-loop integrity assurance structure from "anomaly detection - state estimation - safety verification", which significantly improves the safety and mission continuity of autonomous vehicles in complex dynamic environments. Attached Figure Description

[0128] Figure 1 This is a flowchart illustrating an adaptive evaluation method for detecting and identifying positioning integrity faults in autonomous driving, as described in an embodiment of the present invention.

[0129] Figure 2 This is a flowchart illustrating another adaptive evaluation method for detecting and identifying positioning integrity faults in autonomous driving according to an embodiment of the present invention.

[0130] Figure 3 This is a flowchart illustrating another adaptive evaluation method for detecting and identifying positioning integrity faults in autonomous driving, as described in an embodiment of the present invention.

[0131] Figure 4 This is a schematic diagram of the fault detection, identification, elimination, and adaptation process in an embodiment of the present invention;

[0132] Figure 5 This is a schematic diagram of the residual generation strategy in an embodiment of the present invention;

[0133] Figure 6 This is a schematic diagram of the candidate hypothesis generation strategy in an embodiment of the present invention;

[0134] Figure 7 This is a schematic diagram of the fault detection, identification, and troubleshooting workflow in an embodiment of the present invention;

[0135] Figure 8This is a schematic diagram of the alarm workflow in an embodiment of the present invention;

[0136] Figure 9 The following are schematic diagrams of simulation results in the embodiments of the present invention, wherein (a) is a schematic diagram of residual and detection threshold, (b) is a schematic diagram of fault detection results, and (c) is a schematic diagram of estimated state and true state. Detailed Implementation

[0137] The specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples. The following examples are for illustrative purposes only and are not intended to limit the scope of the invention.

[0138] To address the problems of existing technologies, this invention proposes an integrity enhancement method combining Fault Detection and Elimination (FDE) and Dynamic Detection, Identification, and Adaptation (DIA) to address the integrity issue of vehicle positioning systems in complex dynamic environments. This method can detect faults in real time, identify fault types, and dynamically adjust the system model and parameters, ensuring that the vehicle maintains high accuracy and reliability throughout the positioning service. Furthermore, this invention enhances the system's adaptability to multi-source signal interference and faults through multi-sensor data fusion technology, thereby improving the stability of the vehicle positioning service and ensuring vehicle driving safety in dynamic scenarios.

[0139] The adaptive evaluation method for fault detection and identification of autonomous driving positioning integrity provided by this invention first performs data acquisition and preprocessing. Specifically, it collects positioning-related data, vehicle operating status (speed, acceleration, angle), and sensor (IMU, GNSS) information from multiple sources (such as GPS, IMU, LiDAR, cameras, etc.). The raw data is preprocessed to remove noise and obvious outliers, providing high-quality input for subsequent processing. The second step is state prediction, specifically, state propagation based on the Error State Kalman Filter (ESKF) model. An error state equation including position, speed, attitude, and sensor bias is established based on the vehicle kinematics equations and IMU measurement information. The third step is fault detection. Specifically, by comparing the predicted values ​​of the filter model with the actual sensor observations, the residuals and their covariance matrix are calculated. The residuals are compared with a set threshold value; if the value exceeds the threshold, an anomaly is determined; if it is within the threshold, the observation is considered normal. The fourth step is fault identification and elimination. Specifically, the detected abnormal signals are further classified to identify the source and type of the anomaly. Construct a multidimensional candidate hypothesis set, calculate the significance statistic for each dimension's residuals, and determine whether the anomaly is unidimensional or multidimensional. Identify the source of the fault through statistical and significance level assessments, providing a basis for subsequent elimination and correction. Process the identified anomalous observations or affected data, including:

[0140] 1) Remove faulty data: Directly remove unreliable observations from the filter update;

[0141] 2) Weight adjustment: Reduce the fusion weight of some affected data;

[0142] 3) Data substitution: Filling in anomalous observations with redundant sensors or historical prediction results.

[0143] This step ensures the validity and statistical consistency of the filtered input observation set.

[0144] The fifth step is Dynamic Adaptation (DIA). Specifically, this involves dynamically updating system model parameters, such as the process noise covariance matrix, measurement noise matrix, and state transition matrix, based on the fault identification results. This step automatically adjusts the filter gain according to environmental changes or system uncertainties, achieving model self-adaptation and numerical stability maintenance, ensuring the system remains robust during abnormal periods.

[0145] The sixth step involves integrity assessment and result output, specifically, outputting reliable state estimates such as vehicle position, speed, and attitude. The corrected state estimates and covariance matrix are calculated, the system's protection level is determined, and compared with the navigation system's alarm values.

[0146] The results of detection, identification, elimination, and adaptation are fed back to the system to optimize the next step of fault detection and handling strategy.

[0147] This invention provides an adaptive evaluation method for detecting and identifying faults in the positioning integrity of autonomous driving systems, combined with... Figure 1 , Figure 2 and Figure 3 This may include the following steps:

[0148] Step 1: Collect data using the Global Navigation Satellite System (GNSS), Inertial Measurement Unit (IMU), and wheel odometers installed in the autonomous vehicle. k Positioning data at time -1 and k The positioning data at any given time includes position, velocity, attitude angles, acceleration, angular velocity, and wheel speed. The attitude angles include roll angle, pitch angle, and yaw angle.

[0149] Specifically, the position, speed, and attitude angles of the autonomous vehicle are measured using the Global Navigation Satellite System (GNSS), including roll angle, pitch angle, and yaw angle. The acceleration and angular velocity of the autonomous vehicle are measured using an inertial measurement unit (IMU), and the wheel speed of the autonomous vehicle is collected using a wheel odometer.

[0150] Step 1 aims to acquire raw data from multiple sensors and perform unified time synchronization, format conversion, and noise suppression to establish high-quality input observations for the filter. The system collects positioning-related data through various sensors installed on the vehicle, primarily including: Global Navigation Satellite System (GNSS): providing the vehicle's three-dimensional position, velocity, and heading information; Inertial Measurement Unit (IMU): measuring the vehicle's acceleration and angular velocity for short-term attitude and velocity prediction; Wheel odometers or vehicle speed sensors: providing vehicle linear velocity information to assist IMU integration correction; and a time synchronization module: using GPS time or a high-precision clock for unified timestamp calibration to ensure the temporal consistency of data from all sensors. The acquired raw data includes position (latitude, longitude, altitude), velocity components, and attitude angles (roll, pitch, yaw).

[0151] Furthermore, due to the different sampling frequencies of the sensors, the data is aligned using timestamps. Interpolation methods are used to synchronize IMU and GNSS data to a unified time axis, and the measurement results of each sensor are unified to the ENU coordinate system to ensure consistency of the mathematical model. To reduce high-frequency noise and abrupt interference, the preprocessing operations included in this step are: filtering and noise reduction: low-pass filtering or moving average smoothing is applied to the IMU acceleration and angular velocity signals; anomaly detection: abrupt changes, sudden increases or decreases, and long-term saturation data (i.e., data exceeding the format range) are identified and marked; interpolation and compensation: time interpolation compensation is performed on missing or delayed data; normalization and unit unification: all observations are converted to unified physical units (m, m / s, rad). The processed data is organized into a standardized input format for continuous integration in step 2. The purpose of step 2 is to use the high-frequency measurement information provided by the inertial measurement unit (IMU) to achieve dynamic prediction and covariance propagation of the vehicle state through error-state Kalman filter (ESKF), providing prior state estimates for subsequent residual detection and integrity assessment.

[0152] Step 2: Based on the Error State Kalman Filter (ESKF), perform... k Positioning data at time -1 and k The location data at any given time is processed to obtain... k Prediction at time -1 k Error state covariance at time 1 , k Prediction at time -1 k State vector at time step Kalman gain , k Time Error State Covariance and k State vector at time step ;

[0153] Step 2.1: Based on k The positioning data at time -1 and the positioning data at time k are used to construct... k The state vector at time -1 and k State vector at time step , k The state vector at time -1 and k State vector at time step Represented as:

[0154] ;

[0155] ;

[0156] in, for k The position at time -1 for k Location at any given moment for k The velocity at time -1 for k The speed of time for k The attitude angle at time -1 for k Attitude angle at any moment For the zero bias term of the accelerometer, This is the zero bias term of the gyroscope. T Indicates matrix transpose;

[0157] Step 2.2: Define the autonomous vehicle's... k The state error vector at time -1 and k State error vector at time step , k The state error vector at time -1 and k State error vector at time step Represented as:

[0158] ;

[0159] ;

[0160] in, express k Position error at time -1 express k The position error at any given time, where the position error includes the position error in the north direction. Eastward position error Vertical position error ; express k The velocity error at time -1 express k The velocity error at any given time includes the velocity error in the north direction. Eastward velocity error Vertical velocity error , express k Attitude error at time -1 , for k The roll angle at time -1 fork The pitch angle at time -1 for k Yaw angle at time -1 express k Attitude error at any given moment; This represents the zero-bias error of the gyroscope in a coordinate system with the vehicle's center of gravity as the origin. , Indicates gyroscope x Axis offset, Indicates gyroscope y Axis offset, Indicates gyroscope z Axis offset; The zero bias error of the accelerometer in a coordinate system with the vehicle's center of gravity as the origin. , Indicates the accelerometer x Axis offset, Indicates the accelerometer y Axis offset, Indicates the accelerometer z Axis offset;

[0161] The IMU provides acceleration and angular velocity measurements at each sampling time for state propagation.

[0162] Error State Kalman Filter (ESKF) uses an error state propagation model to characterize the evolution of position, velocity, and attitude errors over time.

[0163] Step 2.3: Due to IMU sensor noise and bias, the error is accumulated through integration. Specifically, according to... k The state error vector at time -1 and k State error vector at time step An error state propagation model is constructed, which is expressed as follows:

[0164] ;

[0165] in, This represents the prediction of the attitude angle error at time k-1 for time k. This represents the Earth's rotation rate in the navigation coordinate system. , Lat Indicates the latitude of autonomous vehicles. It is a constant. The angular velocity error is caused by gyroscope deviation. , This is the direction cosine matrix, whose function is to transform the vector from the IMU main frame to the navigation frame. , , Represented as:

[0166] ;

[0167] ;

[0168] ;

[0169] Step 2.4: In the acceleration error propagation model, the propagation of velocity error is driven by both attitude and acceleration measurement errors, while position error is obtained by integrating the velocity error. After unifying the propagation equations for attitude, velocity, and position errors, a complete linearized error state model is established. Specifically, based on the state error vector... An error state linearization model is constructed, which is expressed as follows:

[0170] ;

[0171] ;

[0172] ;

[0173] in, Indicates speed error, This represents the prediction of the velocity error at time k-1 from that at time k. Specific Force for IMU Indicates the force relative to the north direction. Indicates the force in the east direction. Represents the specific force in the vertical direction. This indicates the relative force error of the IMU. This represents the prediction of the position error at time k-1 from that at time k. This represents the state prediction at time k-1 for time k; The state transition matrix is ​​the Jacobian matrix that characterizes the coupling relationships between variables in the state vector. ， The noise transition matrix is ​​the noise input matrix associated with the noise. It is the process noise vector matrix;

[0174] Wherein, the state transition matrix Represented as:

[0175] ;

[0176] ;

[0177] ;

[0178] in, Represents a 3x3 zero matrix. This represents a 3x3 identity matrix. and For simplification Matrix;

[0179] Among them, the noise transfer matrix Represented as:

[0180] ;

[0181] in, Represents a 6x3 zero matrix;

[0182] To achieve recursive filtering under discrete-time conditions, the continuous-time error state must be discretized over a fixed time interval Δt. Based on prior state estimation, a first-order Taylor expansion is used to approximate the state transition matrix and the noise transition matrix.

[0183] Specifically, regarding the state transition matrix and noise transfer matrix Discretization yields the discrete state transition matrix and the discrete noise transition matrix. and discrete noise transfer matrix Represented as:

[0184] ;

[0185] ;

[0186] in, This represents a 15x15 identity matrix. This represents the state estimate at time k-1. Represents the velocity at time k; For fixed time intervals;

[0187] This discretization method allows for the discretization of sampling periods. This step effectively characterizes the propagation characteristics of system process noise and realizes discrete-time updates of the state covariance. This step is a key component of the error state Kalman filter (ESKF) prediction stage, ensuring the real-time feasibility and numerical stability of the continuous-time model in digital implementation.

[0188] Step 2.5: Construct a GNSS observation model, which is represented as follows:

[0189] ;

[0190] in, yes k GNSS observation model at time 10:00 It is the state vector at time k. For the observation matrix, The first 3 rows are the identity matrix, and the last 12 rows are the zero matrix. It is used to extract the position component from the state vector. The noise coupling matrix is... GNSS measurement noise with zero mean Gaussian distribution. This indicates GNSS measurement noise in the north direction. This indicates GNSS measurement noise in the east direction. Indicates GNSS measurement noise in the vertical direction;

[0191] Step 2.6: Using Error State Kalman Filter (ESKF), predict the state vector at time k and the error state covariance at time k to obtain the predicted state vector at time k-1. Error state covariance at time k predicted at time k-1 Specifically, this is achieved through the following formula:

[0192] ;

[0193] ;

[0194] in, yes k The state vector at time -1 It is a nonlinear state propagation function driven by IMU data. Acceleration and angular velocity measured by the inertial measurement unit (IMU) express k Discrete noise transition matrix at time -1, express k The discrete state transition matrix at time -1 express k Error state covariance at time -1 It is the process noise covariance;

[0195] Step 2.7: Based on the GNSS observation model, and Update Kalman gain , k Time Error State Covariance and k State vector at time step Specifically, this is achieved through the following formula:

[0196] ;

[0197] ;

[0198] ;

[0199] in, It is an observation model The Jacobian matrix relative to the error state vector at time k, Represents the discretized observation matrix. For the GNSS measurement noise covariance matrix, Let be the GNSS measurement vector at time k. The GNSS measurement vector is obtained by processing the position, velocity, and attitude angle of the GNSS measurements. The observation function predicted at time k-1;

[0200] By calculating the Kalman gain The filter adaptively determines the correction magnitude of GNSS measurements to the system's prediction results. This gain is determined by the prior error covariance. Covariance of measurement noise The common decision is that when system prediction uncertainty increases, the filter increases its confidence in GNSS measurements; conversely, when GNSS noise is high, the filter relies more on IMU-based state predictions. Therefore, no manual weighting is required; the fusion ratio is automatically calculated by the statistical model. Updated error covariance. This describes the uncertainty of the current state estimate and serves as prior information input to the prediction step at the next moment, thereby ensuring the continuous recursion of the filter over the time series.

[0201] Step 3: Based on and Calculate residuals and residual covariance GNSS measurement vector at time k The observation dimensions are divided according to a coordinate system. An adaptive detection threshold is set for each observation dimension. Fault detection is performed on all observation dimensions to determine multiple candidate observation dimensions. Figure 4 , Figure 5 and Figure 6 Specifically, it includes the following steps:

[0202] Step 3.1: Calculate the residuals and residual covariance Specifically, this is achieved through the following formula:

[0203] ;

[0204] ;

[0205] in, For the GNSS measurement noise covariance matrix, It is an observation model The Jacobian matrix relative to the error state vector at time k;

[0206] In GNSS / IMU fusion systems, observations typically consist of multiple components with different physical meanings, such as GNSS three-dimensional position, GNSS velocity, and heading angle. Each component can be considered an independent observation dimension in the coordinate system. Since the noise characteristics, dimensions, and statistical distributions of different dimensions are not consistent, in order to accurately assess whether anomalies exist in each observation component during the fusion process, it is necessary to perform dimension-by-dimensional statistical tests on the residuals of each dimension. Therefore, in this invention, GNSS observations are divided into multiple dimensions according to all components.

[0207] Step 3.2: Divide the GNSS measurement vector at time k into multiple observation dimensions according to the coordinate system, normalize the residuals of each observation dimension, and construct the statistical detection quantity. This is achieved through the following formula:

[0208] ;

[0209] ;

[0210] ;

[0211] in, T i For the first i The statistics obtained by normalizing the residuals of each observation dimension, under the condition that the system has no anomalies. It follows a Gaussian distribution with zero mean. for The Middle i The residuals of each observation dimension Indicates the first i The mean of a Gaussian distribution in each observation dimension. Indicates the first i The variance of a Gaussian distribution in each observation dimension. For residual covariance The i One diagonal element;

[0212] In traditional methods, a uniform fixed threshold is usually used. Perform anomaly detection when The GNSS observation value marked as the current time may be abnormal, and less than or equal to is normal. However, this method has the following shortcomings in multidimensional systems: (1) the noise variance of each observation dimension is different, and a uniform threshold will lead to false detection or false negative detection; (2) the system uncertainty changes dynamically with the motion state and environmental conditions, and a fixed threshold cannot accurately reflect the current system confidence; (3) the static threshold cannot adapt to environmental changes, which reduces the practicality of the detection algorithm. To this end, this invention introduces a dimension-adaptive dynamic threshold mechanism, combined with Figure 7 This is achieved through the following steps.

[0213] Step 3.3: For each observation dimension in L The normalized residual sequence over time is used for moving statistics to calculate the mean and standard deviation, specifically through the following formula:

[0214] ;

[0215] ;

[0216] in, Represents the k-th time. i The mean of each observation dimension, express j Time of the first i The normalized statistics of individual observations This represents the standard deviation of the i-th observation dimension at time k;

[0217] Step 3.4: Introduce an uncertainty measure index Specifically, it is calculated using the following formula:

[0218] ;

[0219] in, This represents the error state covariance at time 0. For Frobenius norm, when When the value is less than 1, it indicates an increase in system uncertainty; conversely, a value less than 1 indicates convergence and system stability.

[0220] Step 3.5: Based on the uncertainty measure index Set dynamic scaling factor The dynamic scaling factor is used to balance sensitivity and robustness. Calculated using the following formula:

[0221] ;

[0222] in, As the baseline scaling factor, These are sensitivity adjustment parameters;

[0223] Step 3.6: Based on dynamic scaling factor Calculate the adaptive detection threshold Specifically, this is achieved through the following formula:

[0224] ;

[0225] Step 3.7: In Greater than the adaptive detection threshold In the case of the first i Each observation dimension is used as a candidate observation dimension. Less than or equal to the adaptive detection threshold In the case of, it means the first i Since there are no faults in any of the observation dimensions, multiple candidate observation dimensions are obtained.

[0226] Through the above mechanism, this invention can jointly adjust the detection threshold based on the statistical changes of the residual signal and the real-time level of system uncertainty during the filtering process, thereby maintaining detection sensitivity and stability under different environments and noise conditions. This mechanism constitutes the core part of the Error State Kalman Filter and Detection-Recognition-Adaptation (ESKF-DIA) framework, enabling context-aware adaptive removal of abnormal observation data, significantly improving the integrity and robustness of system localization.

[0227] This step aims to detect and identify anomalous measurements in GNSS observations in real time during the filter update process, ensuring the reliability and integrity of the system's positioning results. Based on statistical analysis of the filter residuals, this step achieves anomaly detection and type identification through dynamic threshold adaptation and dimensional significance determination.

[0228] After anomaly detection, this step further analyzes and classifies the detected abnormal signals to identify the source, dimension, and type of the fault. This step constructs a multidimensional candidate hypothesis set and performs statistical tests on the significance of the residuals to determine whether the anomaly originates from a single measurement dimension, multiple observation combinations, or a systematic bias, providing a basis for subsequent fault troubleshooting and dynamic correction.

[0229] Step 4: Identify and determine the fault observation dimension from all candidate observation dimensions;

[0230] Step 4.1: Based on all candidate observation dimensions, generate multiple candidate fault subsets (i.e. Figure 6 The candidate hypothesis set in the data (the set of candidate hypotheses), and all candidate fault subsets constitute the candidate fault subset set, wherein the candidate fault subset is the case where one or more candidate observation dimensions have a fault;

[0231] At time k, the observation vectors obtained by the system from the GNSS have i independent observation dimensions. To identify potential anomalies in the multidimensional observations, this invention determines the number of observation dimensions with the potential for anomalies at the current time based on the single-dimensional anomaly detection results obtained in step 3. Based on the aforementioned m suspicious observation dimensions, this invention constructs all possible combinations of fault dimensions using a power set combination method, forming a subset of candidate faults: , where set Each subset in Each represents a possible combination hypothesis of fault dimensions, meaning that the observation dimensions included in this subset are abnormal at the current moment. The way this subset is constructed can cover all single-dimensional, arbitrary multi-dimensional combination anomalies, and cases where all dimensions are faulty simultaneously, thus ensuring the integrity of localization under abnormal conditions.

[0232] The following example illustrates the candidate fault subset. Assume m=3, meaning step 3 detected three potentially faulty dimensions. However, it's possible that all three dimensions have issues, or that a serious problem in one dimension causes anomalies in the other two. Therefore, there are 2^m - 1 possible fault combinations from these three dimensions. Taking m=3, i.e., anomalies in dimensions 1, 2, and 3, as an example, the corresponding subsets are M{1} where only dimension 1 is faulty, M{2} where only dimension 2 is faulty, ..., M{7} where all three dimensions are faulty. This subset construction method covers all cases of single-dimensional, arbitrary multi-dimensional combination anomalies, and simultaneous faults in all dimensions.

[0233] Step 4.2: For each candidate fault subset, in the GNSS measurement vector In the process, data corresponding to candidate observation dimensions that contain faults are obtained from the candidate fault subset, thus obtaining subset observations; The vectors corresponding to the candidate observation dimensions containing faults in the candidate fault subset are obtained to obtain the subset observation matrix; The vectors corresponding to the candidate observation dimensions that contain faults in the candidate fault subset are obtained from the data, and the subset noise covariance matrix is ​​obtained.

[0234] To determine the candidate subsets that best match the system's observation characteristics and are most likely to represent the current anomaly, this invention constructs a corresponding cost evaluation index for each candidate subset. This cost index comprehensively measures factors such as observation consistency under the assumptions of the candidate subset, the number of observation dimensions removed, and potential drift-type anomalies, thereby achieving a reliable ranking of the candidate subsets. This is specifically achieved through the following steps:

[0235] Step 4.3: Whiten the subset observations to obtain the whitened observations, specifically achieved using the following formula:

[0236] ;

[0237] in, Denotes the subset observations of the a-th candidate fault subset. For observations after whitening, Let be the subset noise covariance matrix of the a-th candidate fault subset. It follows a multivariate normal distribution with a mean of 0 and a covariance matrix equal to the identity matrix. ;

[0238] Step 4.4: Calculate the cost function for each candidate fault subset, specifically using the following formula:

[0239] ;

[0240] in, Indicates the first a A subset of candidate faults Let represent the cost function for the a-th candidate fault subset. It is a constant. It is the dimension number of the candidate fault subset. These are the weighting coefficients. For drift trend identification function, Used to determine the first d Does the dimensional observation exhibit significant drift?

[0241] The first item is the residual consistency measure, which calculates the overall error energy and measures the degree of agreement between the observed residuals and the assumptions of the candidate subset. The smaller the residuals of a candidate subset, the better the assumption can explain the current observation results, the higher the consistency, and the lower the corresponding cost.

[0242] In the second item This is a constant used to limit the removal of weights from too many dimensions. This is the number of dimensions contained in the subset (i.e., the number of dimensions that are hypothesized to be removed). The purpose of this term is to suppress the phenomenon of "over-removal" in the evaluation process of candidate subsets. The larger the number of dimensions contained in a candidate subset, the more observation information is hypothesized to be removed from the subset, thereby reducing the effectiveness of the system in utilizing observation information. The corresponding cost term should be increased to ensure that the final selected candidate subset achieves abnormal removal while maintaining sufficient observation information as much as possible.

[0243] The third item is the penalty for drift-related anomalies. The drift trend identification function is used to determine the first d Does the observation exhibit significant drift (e.g., continuous bias, non-Gaussian trend)? This is its weighting factor. The purpose of this is to impose an additional penalty on a candidate subset when it contains observation dimensions that are marked as potentially at risk of drift, in order to avoid mistaking normal observations of the slowly varying drift class as faulty observations.

[0244] Step 4.5: Among the cost functions corresponding to all candidate fault subsets, obtain the candidate fault subset with the smallest cost function and take it as the optimal subset (i.e., ...). Figure 6 (as a significant assumption in the model), the candidate observation dimension in the optimal subset containing the fault is the fault observation dimension;

[0245] This invention combines the aforementioned three types of indicators in a weighted manner to form a comprehensive cost for candidate subsets. Specifically, the residual consistency index serves as the primary criterion, the dimensionality removal quantity penalty is used to limit the removal of excessive dimensions, and the drift anomaly penalty is used to reduce the risk of erroneously removing drift dimensions. By calculating this comprehensive cost for each candidate subset and sorting them in ascending order of cost, the optimal candidate subset can be obtained, which represents the dimensionality combination hypothesis that best matches the current observed anomaly pattern.

[0246] ;

[0247] in, Indicates the first i A subset of candidate faults;

[0248] The optimal subset The corresponding observation dimensions are considered reliable and valid data, while the remaining dimensions are considered anomalies and discarded. Ultimately, the filter updates its state only based on the optimal set of retained observation dimensions, thereby achieving fault identification and adaptive observation selection from multi-dimensional sensor information, ensuring that the system still has stable and reliable positioning estimation capabilities even when abnormal measurements exist.

[0249] By calculating the comprehensive cost of each candidate subset and sorting them in ascending order of cost, the optimal candidate subset M can be obtained, which is the set of fault dimensions that best matches the current observation anomaly pattern. The optimal subset M represents the observation dimensions most likely to be abnormal. Therefore, in the subsequent filtering update, the corresponding observation dimensions in M ​​are considered abnormal and removed, while the remaining observation dimensions not included in M ​​are retained as reliable and valid data to participate in the state update.

[0250] In this invention, to achieve adaptive updating and dynamic response of the filter after anomaly identification, a dynamic adaptation and filter integration method based on uncertainty perception and statistical threshold adjustment is proposed. After completing fault identification and selecting the optimal set of retained observation dimensions, this method reconstructs the filter's observation update steps, enabling it to simultaneously possess statistical dynamism and covariance self-adjustment capabilities, thereby achieving a balance between filtering accuracy and system robustness.

[0251] Step 5: Based on the fault observation dimension and Kalman gain The error covariance matrix is ​​updated to obtain the updated error state covariance. The position, velocity, and attitude angles are corrected to obtain the final position estimate after fusion of observations. Final velocity estimation after fusion of observations Final attitude estimation after fusion of observations ;

[0252] After completing the model reconstruction, this invention calculates the residual vector based on the difference between the observations of the optimal subset and the predicted observations. To eliminate the influence of differences in noise levels across different observation dimensions, the residuals are normalized according to the noise covariance matrix corresponding to the optimal subset, giving them a unified statistical standard and ensuring the stability of the filtering update process. Using the normalized residuals, the reconstructed observation matrix, and the prediction covariance, the Kalman gain for this optimal subset is calculated. The Kalman gain is used to determine the degree of correction of the predicted state by the current measurement information, achieving a weighted fusion between prediction and measurement. The error covariance matrix is ​​updated according to the Kalman gain, enabling the system to obtain a new, statistically consistent expression of state uncertainty after fusing the optimal subset measurement information. The updated covariance matrix serves as the prior covariance input for the state prediction at the next time step, specifically implemented through the following steps.

[0253] Step 5.1: In After removing the data corresponding to the fault observation dimension, the target observation is obtained. ,exist After removing the data corresponding to the fault observation dimension, the target observation matrix is ​​obtained. ,exist After removing the data corresponding to the fault observation dimension, the target noise covariance matrix is ​​obtained. ;

[0254] Step 5.2: Update the error covariance matrix based on the Kalman gain to obtain the updated error state covariance. Specifically, this is achieved through the following formula:

[0255] ;

[0256] ;

[0257] ;

[0258] ;

[0259] in, Indicates the target residual covariance. This indicates the updated Kalman gain. This represents the updated state error vector;

[0260] For state variables such as position and velocity that belong to linear space, an additive correction method is used, which adds the predicted state to the corresponding error correction amount to obtain the updated linear state variables. This method directly incorporates the influence of observational information on these linear variables into the prediction results, achieving accurate correction of position and velocity.

[0261] Secondly, for state variables such as attitude quaternions, which belong to nonlinear spaces, this invention employs a multiplicative update method for correction. Specifically, small attitude increments in the error state are transformed into equivalent error quaternions, which are then multiplied and combined with the predicted attitude quaternions to obtain the updated attitude state. This method maintains the physical continuity and orthogonality of the attitude, does not violate the normality of the rotation matrix or quaternions, and ensures the geometric consistency of the attitude update.

[0262] By employing additive and multiplicative updates for linear and nonlinear state variables respectively, this invention achieves comprehensive and accurate correction of the system state. The updated state constitutes the final output of the filter at the current moment and serves as the initial state for the next prediction cycle, continuing to participate in subsequent filtering processes, thereby achieving continuous and stable operation of the system.

[0263] Step 5.3: Use an additive correction method to adjust the predicted position at time k. The predicted velocity at time k is corrected to obtain the final position estimate after fusion of observations. Final velocity estimate after fusion of observations Specifically, this is achieved through the following formula:

[0264] ;

[0265] ;

[0266] in, This is the position error correction amount calculated by the filter. The speed error correction amount calculated by the filter;

[0267] Step 5.4: Adjust the multiplication correction method for the predicted attitude angle at time k. After making corrections, the final attitude estimate after fusion of observations is obtained. Specifically, this is achieved through the following formula:

[0268] ;

[0269] ;

[0270] in, It is the small-angle attitude error calculated by the filter. This is the attitude error correction amount. It is quaternion multiplication;

[0271] In summary, steps 1-5 of this invention enable reliable screening and fusion of multi-source observation information in complex environments. By jointly and adaptively adjusting the prediction model and the observation model, the final output system state has higher accuracy, robustness and completeness, which can effectively improve the positioning and navigation performance of unmanned vehicles in confined environments.

[0272] Step 6: Based on the updated error state covariance The positioning integrity capability of autonomous vehicles is evaluated to obtain evaluation results. The evaluation results indicate that the autonomous vehicle has positioning integrity capability that meets application requirements, or that the autonomous vehicle does not have positioning integrity capability that meets application requirements.

[0273] The calculation of the Protection Level (PL) is a crucial part of the integrity assessment of a navigation system. It is used to determine the confidence limits of position errors and ensure the reliability of the navigation system under safety requirements. The following are the details of the Protection Level calculation:

[0274] There are two levels of protection:

[0275] Horizontal Protection Level (HPL) indicates the horizontal position error of the coverage area;

[0276] Vertical Protection Level (VPL) indicates the coverage of vertical position error.

[0277] The goal of protection level calculation is to ensure that the position error does not exceed the navigation system's alarm limit (AL).

[0278] PL calculation and navigation state covariance matrix It is related to the residual covariance matrix.

[0279] Step 6.1: Combining Figure 8 Based on the updated error state covariance The horizontal protection level (HPL) and vertical protection level (VPL) are calculated using the following formulas:

[0280] ;

[0281] ;

[0282] in, This is based on an expansion factor related to the confidence level in the horizontal direction. It is an expansion factor related to the confidence level in the vertical direction. It is the projection vector of the horizontal error. It is the projection vector of the vertical direction error;

[0283] When the error distribution is non-Gaussian or has a large bias, using Student's t-distribution can improve robustness:

[0284] ;

[0285] in, It is the protection level for a given confidence level α. It is the expansion factor of Student's t-distribution, which is determined by the degrees of freedom. And the confidence level α is determined, It is the largest eigenvalue of the state covariance matrix, corresponding to the most unfavorable case in the principal direction of the system error. This form ensures the robustness and interpretability of the protection level calculation even in the presence of model bias or anomalous noise.

[0286] In vehicle or moving object navigation scenarios, this invention further extends the protection level to calculations along the track and across the track to meet dynamic navigation trajectory constraints:

[0287] Step 6.2: Based on the updated error state covariance Calculate the protection level along the track direction Protection level in the transverse direction Specifically, it is calculated using the following formula:

[0288] ;

[0289] ;

[0290] in, It is a unit vector along the orbital direction. It is a unit vector in the horizontal direction. The confidence expansion factor along the track direction. The confidence expansion factor in the transverse direction;

[0291] The two sets of formulas above are essentially the same, both based on the covariance matrix. The confidence limits for calculating the projection variance in a certain direction are distinguished by the fact that HPL / VPL uses global coordinate directions (ENU coordinate system). / The vehicle's local coordinate system is used. These two coordinate systems can be transformed into each other using a rotation transformation matrix, thus representing different coordinate expressions within the same integrity assessment framework. Therefore, the calculated PL needs to be compared with the navigation system's warning limit value AL.

[0292] Step 6.3: Determine HPL, VPL, and Whether the alarm limit conditions are met, the alarm limit conditions include multiple conditions, in HPL, VPL, and When all the alarm limit conditions are met simultaneously, it indicates that the autonomous vehicle possesses the positioning integrity capability to meet application requirements, in HPL, VPL, and If at least one of the alarm limit conditions is not met, it indicates that the autonomous vehicle does not have the positioning integrity capability to meet the application requirements.

[0293] The alarm limit condition is expressed as follows:

[0294] ;

[0295] ;

[0296] ;

[0297] ;

[0298] in, This represents the HPL threshold in the global coordinate system. This represents the VPL threshold in the global coordinate system. This represents the threshold value for autonomous vehicles along the track direction. This represents the threshold value in the lateral direction of an autonomous vehicle.

[0299] Here is an example to illustrate step 6:

[0300] Assume the state covariance matrix of the navigation system is:

[0301] ;

[0302] Projection direction vector:

[0303] ;

[0304] ;

[0305] The horizontal protection level can be calculated. HPLApproximately 5.86m, vertical protection level ( VPL It is approximately equal to 2.38m.

[0306] Calculate PL based on Student's t-distribution:

[0307] ;

[0308] After eigenvalue decomposition Maximum eigenvalue =0.52, calculate PL:

[0309] ;

[0310] Calculated HPL , VPL PL can be compared with the pre-set AL to determine whether the system meets the integrity requirements.

[0311] Figure 9 The simulation results of the technical solution of this invention are shown in Figure 1. (a) is a schematic diagram of the residual and detection threshold, (b) is a schematic diagram of the fault detection results, and (c) is a schematic diagram of the estimated state and the actual state. The upper part of the figure shows the change of the observed residual and its adaptive integrity monitoring threshold over time, where the blue curve represents the actual residual and the red dashed line represents the integrity detection threshold obtained by dynamic adjustment based on the system covariance and residual statistics. Experiments show that the residuals are stably within the threshold range during the normal observation phase, but the residuals significantly exceed the limit at about 300 s, indicating the existence of potential observation dimension anomalies. The middle part of the figure shows the anomaly judgment results output by the fault detection and identification module of this invention, which can accurately give the fault trigger signal at the time of anomaly occurrence and maintain zero false alarms during the normal phase, demonstrating the sensitivity and reliability of the method in ensuring integrity. The lower part of the figure shows the comparison between the updated state estimation trajectory and the true trajectory. Even when an anomaly occurs, the present invention can still maintain the continuity and accuracy of the state estimation by adaptively eliminating abnormal observations and selecting the optimal subset of observations for filtering and updating, without divergence or abrupt changes. This verifies that the present invention still has a high integrity positioning capability under observation failure conditions.

[0312] Combination Figure 2 Based on the technical solution provided by this invention, a system can be constructed, which consists of the following modules:

[0313] Sensor modules: including GNSS receivers, IMUs, visual sensors, lidar, etc. (including ultrasonic sensors, millimeter-wave radar, etc.) are optional and are used to collect multi-source data related to positioning.

[0314] Data processing module: Performs synchronization, filtering, and normalization processing on raw sensor data through data preprocessing.

[0315] DIA module:

[0316] Detection Unit: Detects abnormal data in real time using methods such as residual analysis and Kalman filtering.

[0317] Identification Unit: Based on a fault mode library and machine learning models, it identifies the source of anomalies.

[0318] Adaptation Unit: Adjusts the parameters of the positioning algorithm or switches to an alternative model to compensate for the impact of faults on the positioning results.

[0319] Integrity assessment module: Based on the error between the positioning result and the actual location, it generates a positioning integrity index and assesses the reliability of the positioning through a probabilistic risk model.

[0320] The key technical point of this invention is:

[0321] 1. An anomaly detection and elimination method based on Error State Kalman Filtering (ESKF) and multi-source observation residual analysis is proposed. By constructing an error state model and process noise propagation matrix, the method can identify and eliminate anomalies in GNSS / IMU observation data in real time, effectively suppressing the filter divergence problem caused by abnormal observations and improving the accuracy and stability of the system positioning results.

[0322] 2. A residual-driven detection-identification-adaptation (DIA) module was designed to achieve adaptive detection and dynamic adjustment for multiple types of fault modes in complex dynamic environments. This module, through sliding window statistics and uncertainty factor adjustment mechanisms, can update the detection threshold in real time according to changes in the system state covariance, achieving a dynamic balance between detection sensitivity and robustness, thereby ensuring high reliability and adaptability of the filtering model under various scenario conditions.

[0323] 3. An adaptive fusion update mechanism combining uncertainty quantification and dynamic threshold adjustment was constructed, which calculates the system uncertainty index. And introduce a dynamic scaling factor The observation noise matrix and threshold are adaptively scaled, which effectively improves the robustness and numerical stability of the system under signal degradation or abrupt changes.

[0324] 4. A robustness assessment method combining Protection Level (PL) and Alert Limit (AL) is proposed. Based on the filter output covariance matrix, the horizontal (HPL), vertical (VPL), and rail-along and transverse protection levels are calculated to quantitatively assess whether the positioning error meets safety requirements. This method achieves closed-loop robustness verification from filtering results to safety alarms, providing reliable safety redundancy for the mission execution of autonomous vehicles.

[0325] 5. An integrated closed-loop integrity assurance framework of ESKF–DIA–PL was constructed, realizing the collaborative operation of the three-layer progressive structure of "anomaly detection - state estimation - integrity verification". This enables the system to maintain the consistency of positioning accuracy and integrity assessment under complex scenarios such as GNSS signal degradation, obstruction and sensor failure, and significantly improves the robustness and safety of the autonomous driving navigation system.

[0326] Compared with existing technologies, the advantages of this invention are:

[0327] 1. Improve system robustness and reliability. Compared with traditional single-sensor positioning methods, this invention achieves real-time estimation and constraint of observation errors and state uncertainties by fusing GNSS and IMU information and introducing the Error State Kalman Filter (ESKF) framework. This effectively suppresses the interference of anomalies such as signal jumps, delays, and occlusions on the positioning results, and significantly improves the robustness and reliability of the system in complex environments.

[0328] 2. Fault detection and dynamic adaptation are achieved. This invention introduces a residual-driven detection-identification-adaptation (DIA) module, which can identify, eliminate, and correct the weights of abnormal observations in real time within a multi-dimensional observation space. Through dynamic threshold adjustment and uncertainty quantification mechanisms, the filtering process can automatically adjust the observation noise model according to environmental changes, ensuring that the system maintains stable convergence and high-precision performance in multiple scenarios.

[0329] 3. Improve system stability and accuracy continuity. This invention employs an optimal observation subset selection and whitening residual construction method in the ESKF update stage, which effectively reduces the impact of abnormal observation dimensions on the state estimation covariance, ensuring that the filter can maintain continuous and smooth positioning output even in GNSS degradation, signal loss, or strong noise environments.

[0330] 4. Establish an integrity quantification and alarm mechanism. By combining the integrity assessment method of Protection Level (PL) and Alert Limit (AL), this invention can calculate the safety limits in the horizontal, vertical, and along-rail and transverse directions in real time based on the filtered output covariance matrix, and determine whether the positioning error meets the safety requirements, thereby realizing the integrity monitoring and risk quantification of the navigation system.

[0331] 5. Enhanced adaptability and safety in multi-fault scenarios. This invention comprehensively considers multi-source sensor failure modes and environmental uncertainties, supports the identification and handling of various abnormal conditions such as occlusion, drift, and multipath interference, and realizes a closed-loop integrity assurance structure from "anomaly detection - state estimation - safety verification", which significantly improves the safety and mission continuity of autonomous vehicles in complex dynamic environments.

[0332] The present invention also provides a vehicle positioning integrity monitoring system based on the dynamic detection, identification and adaptation (DIA) method, which is used to realize dynamic fault detection and adaptive optimization of multi-source data in the vehicle positioning process, thereby improving the positioning accuracy and integrity of the system.

[0333] The system includes a signal acquisition module, a fault detection module, a fault identification module, an adaptive module, an integrity assessment module, and a result output module.

[0334] Signal acquisition module: The signal acquisition module is used to receive data input from multiple sources, including GNSS signals, IMU data, vehicle speed information, and various observation data from vehicle-mounted cameras.

[0335] This module preprocesses the collected multi-source data, including data denoising, synchronization, and format conversion, to ensure the reliability and accuracy of the input data.

[0336] Fault Detection Module: This module takes preprocessed multi-source data as input, calculates the residuals using Kalman filtering, and generates a test statistic. A dynamic threshold adjustment mechanism is employed to adjust the fault detection threshold in real time based on the noise characteristics and system status of different scenarios. If the residual statistic exceeds the detection threshold, the fault detection module generates a fault alarm and transmits the relevant data to the fault identification module.

[0337] Fault Identification Module: Based on residual significance analysis and a multi-hypothesis approach, the fault identification module generates possible fault source hypotheses (single or multiple faults). It uses various statistical methods (such as least squares and Mahalanobis distance) to screen significant hypotheses and identify specific fault sources. This module can distinguish between different types of faults (such as sensor drift, signal loss, or excessive noise), providing a basis for subsequent adaptive processing.

[0338] Adaptive Module: The adaptive module dynamically adjusts the filter model and parameters based on the fault identification results: updating the state covariance matrix and the measurement noise covariance matrix. It selects the most suitable filtering algorithm based on the fault mode (e.g., switching to particle filtering). The module reduces the impact of faults through iterative optimization, ensuring the system's positioning stability and robustness.

[0339] Integrity Assessment Module: Based on dynamic fault detection and adaptively adjusted data, the integrity assessment module calculates the protection level (PL) in real time. It evaluates the confidence boundary of the positioning error using extended Mahalanobis distance or Student's t-distribution. By comparing the protection level with the alarm limit (AL), it determines whether the system positioning results meet safety requirements.

[0340] The results output module outputs the corrected vehicle position, speed, attitude, and other key positioning parameters. It also provides detailed records of fault detection, identification, and troubleshooting to facilitate subsequent system optimization and analysis.

[0341] This invention is applicable to various scenarios (such as occlusion, multipath effect, and harsh environment) and is of great significance to fields such as autonomous driving and navigation systems.

[0342] The above description is merely a preferred embodiment of this disclosure and an explanation of the technical principles employed. Those skilled in the art should understand that the scope of the invention involved in the embodiments of this disclosure is not limited to technical solutions formed by specific combinations of the above-described technical features, but should also cover other technical solutions formed by arbitrary combinations of the above-described technical features or their equivalents without departing from the above-described inventive concept. For example, technical solutions formed by substituting the above-described features with (but not limited to) technical features with similar functions disclosed in the embodiments of this disclosure.

Claims

1. An adaptive evaluation method for detecting and identifying faults in the positioning integrity of autonomous driving systems, characterized in that: include: Data is collected through the Global Navigation Satellite System (GNSS), Inertial Measurement Unit (IMU), and wheel odometers installed in autonomous vehicles. k Positioning data at time -1 and k Positioning data at any given time, including position, velocity, attitude angle, acceleration, angular velocity, and wheel speed; Based on the error state Kalman filter (ESKF), k Positioning data at time -1 and k The location data at any given time is processed to obtain... k Prediction at time -1 k Error state covariance at time 1 , k Prediction at time -1 k State vector at time step Kalman gain , k Time Error State Covariance and k State vector at time step ; based on and Calculate residuals and residual covariance GNSS measurement vector at time k Divide the observation into multiple observation dimensions according to the coordinate system, set an adaptive detection threshold for each observation dimension, perform fault detection on all observation dimensions, and determine multiple candidate observation dimensions. Among them, based on and Calculate residuals and residual covariance Specifically, this is achieved through the following formula: ； ； in, For the GNSS measurement noise covariance matrix, It is an observation model The Jacobian matrix relative to the error state vector at time k; Wherein, the GNSS measurement vector at time k The observation dimensions are divided according to a coordinate system. An adaptive detection threshold is set for each observation dimension. Fault detection is performed on all observation dimensions to determine multiple candidate observation dimensions, including: The GNSS measurement vector at time k is divided into multiple observation dimensions according to the coordinate system. The residuals of each observation dimension are normalized to construct the statistical detection quantity, which is achieved through the following formula: ； ； ； in, T i For the first i The statistics after normalizing the residuals of each observation dimension for The Middle i The residuals of each observation dimension Indicates the first i The mean of a Gaussian distribution in each observation dimension. Indicates the first i The variance of a Gaussian distribution in each observation dimension. For residual covariance The i One diagonal element; For each observation dimension L Moving statistics are performed on the normalized residual sequence over a period of time to calculate the mean and standard deviation, specifically using the following formula: ； ； in, Represents the k-th time. i The mean of each observation dimension, express j Time of the first i The normalized statistics of individual observations This represents the standard deviation of the i-th observation dimension at time k; Introducing an uncertainty measure index Specifically, it is calculated using the following formula: ； in, This represents the error state covariance at time 0. It is the Frobenius norm; Based on uncertainty measure index Set dynamic scaling factor The dynamic scaling factor Calculated using the following formula: ； in, As the baseline scaling factor, These are sensitivity adjustment parameters; Based on dynamic scaling factor Calculate the adaptive detection threshold Specifically, this is achieved through the following formula: ； exist Greater than the adaptive detection threshold In the case of the first i Each observation dimension is used as a candidate observation dimension. Less than or equal to the adaptive detection threshold In the case of, it means the first i Since there are no faults in any of the observation dimensions, multiple candidate observation dimensions are obtained. Identify and determine the fault observation dimensions from all candidate observation dimensions, including: Based on all candidate observation dimensions, multiple candidate fault subsets are generated. All candidate fault subsets form a candidate fault subset set. The candidate fault subset is the case where one or more candidate observation dimensions have a fault. For each candidate fault subset, in the GNSS measurement vector In the process, data corresponding to candidate observation dimensions that contain faults are obtained from the candidate fault subset, thus obtaining subset observations; The vectors corresponding to the candidate observation dimensions containing faults in the candidate fault subset are obtained to obtain the subset observation matrix; The vectors corresponding to the candidate observation dimensions that contain faults in the candidate fault subset are obtained from the data, and the subset noise covariance matrix is ​​obtained. The subset observations are whitened to obtain the whitened observations, which is achieved using the following formula: ； in, Denotes the subset observations of the a-th candidate fault subset. For the observations after whitening, Let be the subset noise covariance matrix of the a-th candidate fault subset. It follows a multivariate normal distribution with a mean of 0 and a covariance matrix equal to the identity matrix. ; The cost function for each candidate fault subset is calculated using the following formula: ； in, This represents the a-th candidate fault subset. Let represent the cost function for the a-th candidate fault subset. It is a constant. It is the dimension number of the candidate fault subset. These are the weighting coefficients. For drift trend identification function, Used to determine the first d Does the dimensional observation exhibit significant drift? Among all the cost functions corresponding to the candidate fault subsets, the candidate fault subset with the smallest cost function is obtained and taken as the optimal subset. The candidate observation dimension with faults in the optimal subset is the fault observation dimension. Based on fault observation dimensions and Kalman gain The error covariance matrix is ​​updated to obtain the updated error state covariance. The position, velocity, and attitude angles are corrected to obtain the final position estimate after fusion of observations. Final velocity estimation after fusion of observations Final attitude estimation after fusion of observations ; Based on the updated error state covariance The positioning integrity capability of autonomous vehicles is evaluated to obtain evaluation results. The evaluation results indicate that the autonomous vehicle has positioning integrity capability that meets application requirements, or that the autonomous vehicle does not have positioning integrity capability that meets application requirements.

2. The adaptive evaluation method for detecting and identifying faults in the positioning integrity of autonomous driving according to claim 1, characterized in that, Based on the error state Kalman filter (ESKF), k Positioning data at time -1 and k The location data at any given time is processed to obtain... k Prediction at time -1 k Error state covariance at time 1 , k Prediction at time -1 k State vector at time step Kalman gain , k Time Error State Covariance and k State vector at time step ,include: based on k The positioning data at time -1 and the positioning data at time k are used to construct... k The state vector at time -1 and k State vector at time step , k The state vector at time -1 and k State vector at time step Represented as: ； ； in, for k The position at time -1 for k Location at any given moment for k The velocity at time -1 for k The speed of time, for k The attitude angle at time -1 for k Attitude angle at any moment For the zero bias term of the accelerometer, This is the zero bias term of the gyroscope. T Indicates matrix transpose; Based on inertial navigation and the ESKF algorithm, the autonomous vehicle is defined as... k The state error vector at time -1 and k State error vector at time step , k The state error vector at time -1 and k State error vector at time step Represented as: ； ； in, express k Position error at time -1 express k Position error at any given time; express k The velocity error at time -1 express k The speed error at any moment, express k Attitude error at time -1 , for k The roll angle at time -1 for k The pitch angle at time -1 for k Yaw angle at time -1 express k Attitude error at any given moment; The zero bias error of the gyroscope in a coordinate system with the vehicle's center of gravity as the origin; The zero bias error of the accelerometer in a coordinate system with the vehicle's center of gravity as the origin; according to k The state error vector at time -1 and k State error vector at time step An error state propagation model is constructed, which is expressed as follows: ； in, This represents the prediction of the attitude angle error at time k-1 for time k. This represents the Earth's rotation rate in the navigation coordinate system. , Let be the direction cosine matrix, where , , Represented as: ； ； ； Based on the state error vector An error state linearization model is constructed, which is expressed as follows: ； ； ； in, Indicates speed error, This represents the prediction of the velocity error at time k-1 from that at time k. For the comparison of IMU, Indicates the force relative to the north direction. Indicates the force in the east direction. Represents the specific force in the vertical direction. This indicates the specific force error of the IMU. This represents the prediction of the position error at time k-1 from that at time k. This represents the state prediction at time k-1 for time k; Here is the state transition matrix. The noise transfer matrix, It is the process noise vector matrix; Construct a GNSS observation model, which is represented as follows: ； in, yes k GNSS observation model at time 10:00 It is the state vector at time k. For the observation matrix, The first 3 rows are the identity matrix, and the last 12 rows are the zero matrix. The noise coupling matrix is... GNSS measurement noise with zero mean Gaussian distribution. This indicates GNSS measurement noise in the north direction. This indicates GNSS measurement noise in the east direction. Indicates GNSS measurement noise in the vertical direction; Error State Kalman Filter (ESKF) is used to predict the state vector at time k and the error state covariance at time k, thus obtaining the predicted state vector at time k-1. Error state covariance at time k predicted at time k-1 Specifically, this is achieved through the following formula: ； ； in, yes k The state vector at time -1 It is a nonlinear state propagation function driven by IMU data. Acceleration and angular velocity measured by the inertial measurement unit (IMU) This represents the discrete noise transition matrix at time k-1. This represents the discrete state transition matrix at time k-1. express k Error state covariance at time -1 It is the process noise covariance; Based on GNSS observation models, and Update Kalman gain Error state covariance at time k and the state vector at time k Specifically, this is achieved through the following formula: ； ； ； in, It is an observation model The Jacobian matrix relative to the error state vector at time k, Represents the discretized observation matrix. For the GNSS measurement noise covariance matrix, Let be the GNSS measurement vector at time k. The GNSS measurement vector is obtained by processing the position, velocity, and attitude angle of the GNSS measurements. The observation function is predicted at time k-1.

3. The adaptive evaluation method for detecting and identifying faults in the positioning integrity of autonomous driving according to claim 2, characterized in that, The state transition matrix Represented as: ； ； ； in, Represents a 3x3 zero matrix. This represents a 3x3 identity matrix. and For simplification Matrix; Among them, the noise transfer matrix Represented as: ； in, Represents a 6x3 zero matrix; For the state transition matrix and noise transfer matrix Discretization yields the discrete state transition matrix and the discrete noise transition matrix. and discrete noise transfer matrix Represented as: ； ； in, This represents a 15x15 identity matrix. This represents the state estimate at time k-1. Represents the velocity at time k; It is a fixed time interval.

4. The adaptive evaluation method for detecting and identifying faults in the positioning integrity of autonomous driving according to claim 1, characterized in that, Based on fault observation dimensions and Kalman gain The error covariance matrix is ​​updated to obtain the updated error state covariance. ,include: exist After removing the data corresponding to the fault observation dimension, the target observation is obtained. ,exist After removing the data corresponding to the fault observation dimension, the target observation matrix is ​​obtained. ,exist After removing the data corresponding to the fault observation dimension, the target noise covariance matrix is ​​obtained. ; The error covariance matrix is ​​updated based on the Kalman gain to obtain the updated error state covariance. Specifically, this is achieved through the following formula: ； ； ； ； in, Indicates the target residual covariance. This indicates the updated Kalman gain. This represents the updated state error vector.

5. The adaptive evaluation method for detecting and identifying faults in the positioning integrity of autonomous driving according to claim 4, characterized in that, The position, velocity, and attitude angles are corrected to obtain the final position estimate after fusion of observations. Final velocity estimation after fusion of observations Final attitude estimation after fusion of observations ,include: The predicted position at time k is obtained by using an additive correction method. The predicted velocity at time k is corrected to obtain the final position estimate after fusion of observations. Final velocity estimate after fusion of observations Specifically, this is achieved through the following formula: ； ； in, This is the position error correction amount calculated by the filter. The speed error correction amount calculated by the filter; The method of multiplication correction, for the predicted attitude angle at time k. After making corrections, the final attitude estimate after fusion of observations is obtained. Specifically, this is achieved through the following formula: ； ； in, It is the small-angle attitude error calculated by the filter. This is the attitude error correction amount. It is quaternion multiplication.

6. The adaptive evaluation method for detecting and identifying faults in the positioning integrity of autonomous driving according to claim 1, characterized in that, Based on the updated error state covariance The positioning integrity capability of autonomous vehicles is evaluated, and the evaluation results include: Based on the updated error state covariance The horizontal protection level (HPL) and vertical protection level (VPL) are calculated using the following formulas: ； ； in, This is based on an expansion factor related to the confidence level in the horizontal direction. It is an expansion factor related to the confidence level in the vertical direction. It is the projection vector of the horizontal direction error. It is the projection vector of the vertical direction error; Based on the updated error state covariance Calculate the protection level along the track direction Protection level in the transverse direction Specifically, it is calculated using the following formula: ； ； in, It is a unit vector along the orbital direction. It is a unit vector in the horizontal direction. The confidence expansion factor along the track direction. The confidence expansion factor in the transverse direction; Determine HPL, VPL, and Whether the alarm limit conditions are met, the alarm limit conditions include multiple conditions, in HPL, VPL, and When all the alarm limit conditions are met simultaneously, it indicates that the autonomous vehicle possesses the positioning integrity capability to meet application requirements, in HPL, VPL, and If at least one of the alarm limit conditions is not met, it indicates that the autonomous vehicle does not have the positioning integrity capability to meet the application requirements.

7. The adaptive evaluation method for detecting and identifying faults in the positioning integrity of autonomous driving according to claim 6, characterized in that, The alarm limit condition is expressed as follows: ； ； ； ； in, Indicates the HPL threshold for global coordinate system orientation. This represents the VPL threshold in the global coordinate system direction. This represents the threshold value for autonomous vehicles along the track direction. This indicates the threshold value for the lateral direction of an autonomous vehicle.

Images