Unmanned cluster adaptive positioning method under abnormal measurement

Abstract

The invention discloses an unmanned cluster self-adaptive positioning method under abnormal measurement, which aims at solving the problems that an unmanned aerial vehicle cluster is easily interfered by GNSS (Global Navigation Satellite System) signals, the measurement information is abnormal and incomplete and the like in a complex environment, an improved distributed consistency Kalman filter (IDCKF) is designed, and by introducing residual statistics and a chi-square test mechanism, the unmanned aerial vehicle cluster self-adaptive positioning method under abnormal measurement is realized. Identifying and eliminating abnormal observation nodes in real time, and fusing neighborhood information to realize adaptive distribution of state estimation weights; in order to further enhance system completeness and abnormal node positioning performance, a federated filtering structure and a sliding window chi-square statistical method are combined to construct an adaptive hybrid filtering framework, and dynamic switching and precision recovery of navigation modes are realized. According to the method, the precision and robustness of unmanned aerial vehicle cluster navigation can be effectively improved under the conditions of GNSS abnormality and measurement information loss, and good adaptability and stability are shown in a multi-node abnormal scene.

Description

Technical Field

[0001] This invention relates to the field of navigation technology, and more specifically to an adaptive hybrid filtering method for UAV swarm navigation under measurement anomalies. Background Technology

[0002] Sensor measurement and navigation technologies have advanced comprehensively with the times, and the application scope of unmanned aerial vehicles (UAVs) is becoming increasingly wide, playing an important role in both military and civilian fields. In the military sphere, unmanned combat aircraft have demonstrated outstanding performance on the modern battlefield, playing an irreplaceable role, especially in unmanned warfare; in the civilian sphere, civilian UAVs can assist in completing corresponding search and rescue missions.

[0003] While a single drone has limited capabilities, drone swarms can allocate tasks more efficiently, offering greater mobility and higher combat effectiveness. However, a swarm navigation and positioning system is crucial for ensuring high-quality mission completion. Currently, the most widely used algorithm in navigation and positioning systems is a combination of Global Navigation Satellite System (GNSS) and Inertial Navigation System (INS). However, in complex environments, GNSS faces various uncertainties and risks. Specifically, electromagnetic interference significantly affects GNSS signal quality, while signal blockage caused by urban canyon effects or battlefield shielding further reduces system positioning accuracy. These factors combined directly impact the navigation reliability of drone swarms, leading to a decrease in mission success rate.

[0004] With the continuous development of data link technology, distributed estimation methods have demonstrated significant advantages in addressing the performance degradation of UAV navigation and positioning in complex wartime electromagnetic interference environments. This method leverages cooperative communication between adjacent nodes in a sensor network, fusing local observation information to achieve consistent global state estimation, thereby effectively improving the system's anti-interference capability. Compared to centralized architectures, distributed filters offer superior scalability and robustness. Furthermore, the introduction of a consensus protocol effectively ensures spatiotemporal coordination among cluster nodes, enhancing the system's collaborative estimation capabilities. However, the presence of unobservable nodes in the UAV swarm topology can cause errors to propagate and spread throughout the network, ultimately affecting the navigation and positioning accuracy of the entire swarm. Summary of the Invention

[0005] The purpose of this invention is to provide an adaptive localization method for unmanned clusters under abnormal measurement conditions.

[0006] The technical solution to achieve the objective of this invention is: an adaptive localization method for unmanned swarms under abnormal measurement conditions, comprising the following steps:

[0007] Step 1: Construct a strapdown inertial navigation system / global navigation satellite system (SINS / GNSS) integrated navigation model, including establishing the SINS error state space equation of the UAV, and constructing the corresponding measurement equation by combining the position and velocity information output by GNSS;

[0008] Step 2: Construct a relative navigation measurement equation based on airborne sensor measurements. The equation includes measurement information on the relative distance and relative velocity between UAV nodes.

[0009] Step 3: Use a sliding window of length L to record the historical results of the chi-square test of the measurement information of the current node, and calculate the anomaly density within the window. The information is the difference between the observed value and the predicted observed value. When the anomaly density is lower than the first preset threshold, the measurement is determined to be normal, and proceed to step 4; when the anomaly density is higher than or equal to the first preset threshold, the measurement is determined to be abnormal, and proceed to step 6.

[0010] Step 4: For each neighboring node of the current node, calculate the mean and standard deviation of the historical chi-square statistic sequence of the state estimation residuals of each neighboring node; if the mean of a neighboring node exceeds the second preset threshold or the standard deviation exceeds the third preset threshold, then the neighboring node is determined to be an abnormal node and removed from the neighbor set of the current node; otherwise, it is determined to be a normal neighboring node.

[0011] Step 5: Based on the set of normal neighbor nodes obtained in Step 4, state estimation fusion is performed using an improved distributed consensus Kalman filter (IDCKF). The IDCKF determines the confidence weight of each neighbor node based on the reciprocal of the prior error covariance matrix of each neighbor node, and performs weighted fusion of the state estimates of the neighbor nodes with the predicted values ​​of the current node to obtain the navigation state estimate of the current node.

[0012] Step 6: When a measurement anomaly is detected, switch to relative navigation mode and adopt a federated filter architecture. For each normal neighbor node obtained in Step 4, use its relative navigation information to establish a corresponding independent sub-filter and complete the measurement update of each sub-filter.

[0013] Step 7: Combine the local state estimation results of each sub-filter in Step 6 with the prediction results of the SINS-based main filter, generate the global optimal state estimate based on the information allocation principle, and output the navigation result of the current UAV node by combining the mechanical arrangement of inertial navigation.

[0014] Further, step 1: Construct a strapdown inertial navigation system / global navigation satellite system (SINS / GNSS) integrated navigation model, including establishing the SINS error state space equation for the UAV, and constructing the corresponding measurement equation by combining the position and velocity information output by GNSS. The specific method is as follows:

[0015] The specific forms of the platform error angle equation, velocity error equation, position error equation, and inertial instrument error equation are as follows:

[0016]

[0017] Among them represents the attitude rotation matrix for converting the vehicle coordinate system to the navigation coordinate system, is the measured value of the accelerometer in the vehicle coordinate system, is the velocity vector in the navigation coordinate system, including , the output of the gyroscope represents the angular velocity of the b coordinate system relative to the inertial i coordinate system, represents the rotation of the n coordinate system relative to the i coordinate system, including the rotation of the navigation system caused by the earth's rotation and the rotation of the n coordinate system due to the curvature of the earth's surface on the earth's surface, that is , is the principal curvature radius of the meridian, is the principal curvature radius of the prime vertical, is the altitude value, is the latitude value, is the longitude value, represents the gravitational acceleration;

[0018] Combining the platform error angle equation, velocity error equation, position error equation, and inertial device error equation, the error equation of the strapdown inertial navigation system is established. Combining the error equation of the strapdown inertial navigation system, the state equation of the integrated navigation system is derived, and the expression form is:

[0019]

[0020] Among them, , , represents the position error, represents the velocity error, represents the attitude error, represents the random zero offset of the accelerometer, represents the random drift of the gyroscope, represents the white noise of the gyro angular velocity measurement information, represents the white noise of the accelerometer specific force measurement information, is the state transition matrix, and the specific expression form is:

[0021]

[0022] Among them, and are zero matrices, It can be determined from the error state equation of SINS. Let be the attitude rotation matrix from the vehicle coordinate system to the navigation coordinate system. The specific form of the system's perturbation matrix is ​​as follows:

[0023]

[0024] The specific form of the state-space equations of a discrete system is shown below:

[0025]

[0026] in, In order to be in The state vector at time t, It is the system's state transition matrix. It is the system's perturbation matrix. Is the system in The noise vector at time step;

[0027] The measurement model based on satellite navigation systems is represented as follows:

[0028]

[0029] in, , For position measurement noise, For speed measurement noise, The measurement matrix is ​​represented as follows:

[0030]

[0031] The specific form of the discrete observation equation is shown below:

[0032]

[0033] in, Is The observed value at time, Is The observation matrix at time, Is Observational noise at any given moment.

[0034] Further, step 2: Construct a relative navigation measurement equation based on airborne sensor measurements, wherein the equation includes measurement information on the relative distance and relative velocity between UAV nodes, wherein:

[0035] Adjacent nodes The specific representation of the drone computational model is shown below:

[0036]

[0037] in, For nodes The calculated value of the eastward velocity. For nodes The calculated value of the northbound velocity, For nodes The calculated value of the celestial velocity. For nodes The true value of the eastward velocity, For nodes The true value of the northbound velocity, For nodes The true value of the upward velocity, For nodes The eastward velocity error, For nodes The northbound velocity error, For nodes The upward velocity error;

[0038] Current node The specific representation of the drone computational model is shown below:

[0039]

[0040] Therefore, the specific form of the relative velocity calculation model is as follows:

[0041]

[0042] in The relative calculation speed is in the eastward direction. The relative calculation speed is for the northward direction. The relative calculation speed of the sky direction, This is the true value of the eastward relative velocity. This is the true value of the northward relative velocity. This represents the true value of the relative velocity towards the sky.

[0043] Adjacent nodes The specific representation of calculated position coordinates in the Earth-centered and Earth-fixed coordinate system is as follows:

[0044]

[0045] in, , , ; For the truth value of height, For the true value of latitude, For the truth value of height, , , For the corresponding related error, The radius of curvature of the Earth's geoid. The flattening of the Earth's ellipsoid;

[0046] Adjacent nodes The specific representation of the true position coordinates in the Earth-centered Earth-fixed coordinate system is as follows:

[0047]

[0048] By calculating the Jacobian matrix of the actual location coordinates, the neighboring nodes are derived. The specific manifestation of the coordinate error term of the UAV in the Earth-centered, Earth-fixed coordinate system:

[0049]

[0050] Among them, the Jacobian matrix The specific manifestations are as follows:

[0051]

[0052]

[0053] Current drone node The error term in the geocentric-fixed coordinate system is derived through the Jacobian matrix, and its specific form is as follows:

[0054]

[0055] Therefore, the current node and neighboring nodes The specific form of the relative distance calculation value is as follows:

[0056]

[0057] in, The attitude rotation matrix for transforming from the geocentric coordinate system to the navigation coordinate system is specifically represented as follows:

[0058]

[0059] drone nodes and drone nodes The specific form of the relative velocity measurement model is as follows:

[0060]

[0061] in, This is the attitude rotation matrix for transforming the vehicle coordinate system into the navigation coordinate system. The antisymmetric matrix of the misaligned angle vectors, This refers to the components of the relative velocity measurement in the navigation coordinate system. The relative velocity measurement at the current drone node Components in the carrier coordinate system;

[0062] drone nodes and drone nodes The specific form of the relative position measurement model is as follows:

[0063]

[0064] in For Jacobian matrices, This is the error in relative distance measurement;

[0065] Therefore, the specific form of the velocity measurement model is as follows:

[0066]

[0067] The specific form of the location measurement model is as follows:

[0068]

[0069] The specific form of the measurement model for relative navigation is as follows:

[0070]

[0071] in, The velocity measurement matrix, The measurement matrix for the position, can be The transformation yields, ,in For neighboring nodes Speed ​​error, The positional error of the neighboring nodes. For the measurement error of relative velocity, This is the azimuth error. For elevation angle error, This represents the measurement error of the relative distance.

[0072] Furthermore, in step 3, a sliding window of length L is used to record the historical results of the chi-square test of the current node's measurement innovation, and the anomaly density within the window is calculated. The specific method is as follows:

[0073] For the current node drone Introduce a sliding window of length L to record the results of the most recent L chi-square tests. The results of the chi-square test The specific manifestations are as follows:

[0074]

[0075] The specific manifestations of abnormal density are as follows:

[0076]

[0077] in, For the reason Composition dimensional array, It reflects the overall trend of recent anomalies in the system, and its value ranges from 0 to 1.

[0078] Further, in step 4, for each neighboring node of the current node, calculate the mean and standard deviation of the historical chi-square statistic sequence of the state estimation residuals of each neighboring node; if the mean of a neighboring node exceeds the second preset threshold or the standard deviation exceeds the third preset threshold, then the neighboring node is determined to be an abnormal node and removed from the neighbor set of the current node; otherwise, it is determined to be a normal neighboring node, and the specific method is as follows:

[0079] The chi-square statistic is introduced to remove outliers, specifically using the residual statistic, which is expressed as follows:

[0080]

[0081] Among them, new information The specific manifestations are as follows:

[0082]

[0083] The specific form of the new covariance matrix is ​​as follows:

[0084]

[0085] A threshold is used to filter neighboring nodes based on their historical residual values ​​and standard deviations. The expression of the node removal criteria is shown below:

[0086]

[0087]

[0088] in, The threshold for the chi-square test. This represents the tolerable consistency fluctuation threshold. Setting the consistency fluctuation threshold too high may lead to missed detection of highly volatile nodes; setting it too low may easily misclassify nodes with slight fluctuations but normal volatility as anomalies. `mean` refers to the mean value, used to detect offset-type anomalies; `std` is the standard deviation, used to detect volatility anomalies.

[0089] Furthermore, in step 5, based on the set of normal neighbor nodes obtained in step 4, an improved Distributed Consensus Kalman Filter (IDCKF) is used for state estimation fusion. The IDCKF determines the confidence weight of each neighbor node based on the reciprocal of its prior error covariance matrix, and weights and fuses the state estimates of the neighbor nodes with the predicted value of the current node to obtain the navigation state estimate of the current node. Specifically, this is expressed as follows:

[0090] The specific form of the discrete state-space equations for each UAV is as follows:

[0091]

[0092] in, Here is the state transition matrix. Here is the noise perturbation matrix. for The state vector at time t, for The state vector at time t, for The system noise vector at time t;

[0093] The specific form of the measurement equation for each UAV node is as follows:

[0094]

[0095] in, For nodes exist The measured value at time, For the measurement matrix, For measuring noise vectors;

[0096] The specific form of the prediction step in a distributed consensus Kalman filter is as follows:

[0097]

[0098]

[0099] in, For the first The node at the th The posterior estimate of the step, For nodes In the The prior estimate of the step, For the first The node at the th The prior error covariance estimate of the step. Indicates the first The node at the th The number of neighboring nodes of the step. For the first The posterior error covariance estimate of each node;

[0100] The specific form of the update steps of a distributed consensus Kalman filter is as follows:

[0101]

[0102]

[0103]

[0104] in, It is a node In the Kalman gain of the step, For nodes The measurement noise covariance matrix, where The specific manifestations are as follows:

[0105]

[0106] For the consensus factor gain matrix The specific manifestations are as follows:

[0107]

[0108] Based on the aforementioned distributed consensus Kalman filter, the state residual term and the confidence weight are introduced, and their specific forms are as follows:

[0109]

[0110]

[0111]

[0112] in, For neighboring nodes In the The prior estimate of the step, For neighboring nodes In the The prior estimate of the step, For nodes In the The posterior estimation error covariance matrix of the step. This is a sensitivity factor, ranging from 0 to 1;

[0113] Nodes after combining credibility weights The specific form of the posterior state estimate is as follows:

[0114] .

[0115] Furthermore, in step 6, under relative navigation mode, a federated filter architecture is adopted. For each normal neighbor node obtained in step 4, a corresponding independent sub-filter is established using its relative navigation information, and the measurement update of each sub-filter is completed. The specific method is as follows:

[0116] The specific form of sub-filter initialization is as follows:

[0117]

[0118] in, The current node The initial values ​​of the error covariance matrix, Neighboring nodes The error covariance matrix, The current node The system noise covariance matrix, The current node The system noise covariance matrix, For nodes Shared factors;

[0119] The specific form of the prediction step of the sub-filter is as follows:

[0120]

[0121]

[0122] in, For sub-filters in The prior estimate at time 10:00. For sub-filters in The posterior estimate at time t. Let be the state transition matrix of the sub-filter. For sub-filters in The prior error covariance matrix at time t. For sub-filters in The posterior error covariance matrix at time t. Let be the system noise covariance matrix of the sub-filter.

[0123] The specific form of the sub-filter update step is as follows:

[0124]

[0125]

[0126]

[0127] in The Kalman gain of the sub-filter. This is the measurement matrix of the sub-filter. Let be the measurement noise covariance matrix of the sub-filter. For the observations of the sub-filter, Let be the posterior error covariance matrix of the sub-filter.

[0128] Furthermore, in step 7, the local state estimation results of each sub-filter in step 6 are fused with the prediction results of the SINS-based main filter. Based on the information allocation principle, a globally optimal state estimate is generated, and the navigation result of the current node is output in conjunction with the mechanical arrangement of inertial navigation. The specific method is as follows:

[0129] Combining the local state estimation results of the sub-filters, the specific manifestation of global information fusion is as follows:

[0130]

[0131]

[0132]

[0133] Among them, among them, The one-step prediction covariance matrix of the master filter. The state estimate predicted in the first step by the master filter. The system noise covariance matrix of the main filter. This is the global posterior estimate. To estimate the covariance matrix for the global posterior, This is the global system noise covariance matrix.

[0134] The allocation is based on the information allocation principles of the system covariance matrix and the system noise matrix, and the specific form is shown below:

[0135]

[0136]

[0137]

[0138] in, These are the posterior estimates for each filter. The posterior covariance matrix for each filter is given. Let be the noise covariance matrix of the system for each filter.

[0139] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the aforementioned unmanned swarm adaptive localization method under abnormal measurements.

[0140] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the aforementioned unmanned cluster adaptive localization method under abnormal measurements.

[0141] Compared with existing technologies, the significant advantages of this invention are as follows: The UAV formation navigation method based on an adaptive hybrid filtering algorithm significantly improves the robustness and positioning accuracy of the system when facing abnormal measurements and navigation information loss in complex environments. This is achieved by introducing a fusion strategy of an improved distributed consensus Kalman filter (IDCKF) and federated filtering. This method can identify and isolate failed nodes in real time, avoiding the negative impact of abnormal data on the overall navigation results. It also quickly restores positioning accuracy through a relative navigation strategy. Compared to traditional distributed Kalman filter (DCKF) and its sub-optimal algorithms, it has stronger fault tolerance and adaptability, ensuring stable operation even in environments with missing or severely interfered satellite signals, thus meeting the autonomous navigation needs of UAV formations in future complex and dynamic environments. Attached Figure Description

[0142] Figure 1 This is a flowchart illustrating an adaptive localization method for unmanned clusters under abnormal measurement conditions according to the present invention.

[0143] Figure 2 This is a diagram of a relative navigation measurement model.

[0144] Figure 3 This is a diagram of an INS / GNSS integrated navigation model.

[0145] Figure 4 This is a diagram of the improved distributed consensus Kalman filter model.

[0146] Figure 5 This is a diagram of the federated filtering model.

[0147] Figure 6 This is a flowchart of the adaptive switching strategy for hybrid filters.

[0148] Figure 7 It is a diagram showing the movement trajectory and topology of a drone swarm.

[0149] Figure 8 This is a position error curve of UAV 5 under abnormal conditions.

[0150] Figure 9 This is a comparison curve of the position error of four algorithms for UAV 6.

[0151] Figure 10 This is a curve showing the navigation and positioning error of a drone swarm using a hybrid filtering algorithm.

[0152] Figure 11 This is a curve showing the navigation and positioning error of a drone swarm using the IDCKF algorithm.

[0153] Figure 12 The navigation position error of drones under different numbers of abnormal drone nodes Histogram.

[0154] Figure 13 The navigation position error of drones under different numbers of abnormal drone nodes Heat map. Detailed Implementation

[0155] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0156] This invention proposes an adaptive localization method for unmanned aerial vehicle (UAV) swarms under abnormal measurement conditions. The algorithm combines an improved distributed consensus Kalman filter (IDCKF) with federated filtering, dynamically switching filtering modes to address abnormal measurements and satellite signal loss. This method introduces an anomaly detection mechanism based on chi-square statistics to identify and isolate failed nodes in real time, preventing abnormal data from affecting the system's navigation accuracy. Simultaneously, incorporating collaborative consistency theory, a switching mechanism based on sliding window statistics is designed to ensure the stability and accuracy of the navigation system in complex environments. Compared to existing distributed Kalman filter methods, this invention maintains high navigation accuracy and robustness in complex dynamic environments, especially effectively ensuring the stable operation of UAV swarms in the event of satellite signal loss.

[0157] Combination Figure 1 An adaptive localization method for unmanned swarms under abnormal measurements includes the following steps:

[0158] Step 1: Establish the SINS state-space equations for the UAV by combining the platform error angle equation, velocity error equation, position error equation, and inertial device error equation. Combine this with the position and velocity information output by GNSS to construct the UAV's SINS / GNSS integrated navigation model. The specific method is as follows:

[0159] The specific forms of the platform error angle equation, velocity error equation, position error equation, and inertial instrument error equation are as follows:

[0160]

[0161] Where represents the attitude rotation matrix for converting the vehicle coordinate system to the navigation coordinate system, is the measured value of the accelerometer in the vehicle coordinate system, is the velocity vector in the navigation coordinate system, including , the output of the gyroscope represents the angular velocity of the b coordinate system relative to the inertial i coordinate system, represents the rotation of the n coordinate system relative to the i coordinate system, including the rotation of the navigation system caused by the earth's rotation and the rotation of the n coordinate system due to the curvature of the earth's surface on the earth's surface, that is , is the radius of curvature of the prime vertical, is the radius of curvature of the卯酉圈 (not clear in Chinese, might need more context), is the altitude value, is the latitude value, is the longitude value, represents the gravitational acceleration.

[0162] Combining the platform error angle equation, velocity error equation, position error equation, and inertial device error equation to establish the error equation of the strapdown inertial navigation system. Combining the error equation of the strapdown inertial navigation system, the state equation of the integrated navigation system can be derived, and the expression form is:

[0163]

[0164] Where, , , represents the position error, represents the velocity error, represents the attitude error, represents the random zero bias of the accelerometer, represents the random drift of the gyroscope, represents the white noise of the gyro angular velocity measurement information, represents the white noise of the accelerometer specific force measurement information, is the state transition matrix, and the specific expression form is:

[0165]

[0166] Where, and are zero matrices, can be determined from the error state equation of the SINS. is the attitude rotation matrix for the vehicle coordinate system to the navigation coordinate system, is the perturbation matrix of the system, and the specific form is as follows:

[0167]

[0168] The specific form of the state-space equations of a discrete system is shown below:

[0169]

[0170] in, In order to be in The state vector at time t, It is the system's state transition matrix. It is the system's perturbation matrix. Is the system in The noise vector at time step 1.

[0171] The measurement model based on satellite navigation systems is represented as follows:

[0172]

[0173] in, , For position measurement noise, For speed measurement noise, The measurement matrix is ​​represented as follows:

[0174]

[0175] The specific form of the discrete observation equation is shown below:

[0176]

[0177] in, Is The observed value at time, Is The observation matrix at time, Is Observational noise at any given moment.

[0178] INS / GNSS models such as Figure 2 As shown, the above equations can be used to obtain the state prediction value of the integrated navigation under normal measurement conditions. Combined with the mechanical arrangement of the inertial navigation system, relatively accurate navigation and positioning information can be obtained.

[0179] Step 2: Construct the relative navigation measurement equation for the UAV by combining the relative distance and relative velocity information obtained from sensor measurements and the relative distance and relative velocity information calculated by inertial navigation;

[0180] The specific representation of the relative navigation vector calculation model is shown below:

[0181] Relative velocity calculation model:

[0182] Adjacent nodes The specific representation of the drone computational model is shown below:

[0183]

[0184] in, For nodes The calculated value of the eastward velocity. For nodes The calculated value of the northbound velocity, For nodes The calculated value of the celestial velocity. For nodes The true value of the eastward velocity, For nodes The true value of the northbound velocity, For nodes The true value of the upward velocity, For nodes The eastward velocity error, For nodes The northbound velocity error, For nodes The upward velocity error.

[0185] Current node The specific representation of the drone computational model is shown below:

[0186]

[0187] Therefore, the specific form of the relative velocity calculation model is as follows:

[0188]

[0189] in The relative calculation speed is in the eastward direction. The relative calculation speed is for the northward direction. The relative calculation speed of the sky direction, This is the true value of the eastward relative velocity. This is the true value of the northward relative velocity. This represents the true value of the relative velocity towards the sky.

[0190] Relative distance calculation model:

[0191] Adjacent nodes The specific representation of calculated position coordinates in the Earth-centered and Earth-fixed coordinate system is as follows:

[0192]

[0193] in, , , ; For the truth value of height, For the true value of latitude, For the truth value of height, , , For the corresponding related error, The radius of curvature of the Earth's geoid. denoted as the Earth's ellipsoid oblateness.

[0194] Adjacent nodes The specific representation of the true position coordinates in the Earth-centered Earth-fixed coordinate system is as follows:

[0195]

[0196] By calculating the Jacobian matrix of the actual location coordinates, the neighboring nodes can be derived. The specific manifestation of the coordinate error term of the UAV in the Earth-centered, Earth-fixed coordinate system:

[0197]

[0198] Among them, the Jacobian matrix The specific manifestations are as follows:

[0199]

[0200]

[0201] Current drone node The error term in the geocentric coordinate system can be derived from the Jacobian matrix, and its specific form is as follows:

[0202]

[0203] Therefore, the current node and neighboring nodes The specific form of the relative distance calculation value is as follows:

[0204]

[0205] in, The attitude rotation matrix for transforming from the geocentric coordinate system to the navigation coordinate system is specifically represented as follows:

[0206]

[0207] Combination Figure 3 The relative navigation measurement model uses the relative velocity vector. and relative position vector The decomposition is performed in the current node's carrier coordinate system, specifically in the following form:

[0208]

[0209]

[0210] in, , , , For yaw angle error, For pitch angle error, This represents the relative velocity error.

[0211] The specific expression for the error factor is:

[0212]

[0213]

[0214] drone nodes and drone nodes The specific form of the relative velocity measurement model is as follows:

[0215]

[0216] in, This is the attitude rotation matrix for transforming the vehicle coordinate system into the navigation coordinate system. The antisymmetric matrix of the misaligned angle vectors, This refers to the components of the relative velocity measurement in the navigation coordinate system. The relative velocity measurement at the current drone node The components in the carrier coordinate system.

[0217] drone nodes and drone nodes The specific form of the relative position measurement model is as follows:

[0218]

[0219] in For Jacobian matrices, This represents the error in relative distance measurement.

[0220] Therefore, the specific form of the velocity measurement model is as follows:

[0221]

[0222] The specific form of the location measurement model is as follows:

[0223]

[0224] The specific form of the measurement model for relative navigation is as follows:

[0225]

[0226] in, The velocity measurement matrix, The measurement matrix for the position, can be The transformation yields, ,in For neighboring nodes Speed ​​error, The positional error of the neighboring nodes. For the measurement error of relative velocity, This is the azimuth error. For elevation angle error, This represents the measurement error of the relative distance.

[0227] Step 3: Use a sliding window of length L to record the historical results of the chi-square test for the current node's measurement innovation, and calculate the anomaly density within the window:

[0228] For the current node drone Introduce a sliding window of length L to record the most recent The results of the second chi-square test, the first The results of the chi-square test The specific manifestations are as follows:

[0229]

[0230] The specific manifestations of abnormal density are as follows:

[0231]

[0232] in, For the reason Composition dimensional array, It reflects the overall trend of recent anomalies in the system, and its value ranges from 0 to 1.

[0233] Step 4: For each neighboring node of the current node, calculate the mean and standard deviation of the historical chi-square statistic sequence of the state estimation residuals of each neighboring node; if the mean of a neighboring node exceeds the second preset threshold or the standard deviation exceeds the third preset threshold, then the neighboring node is determined to be an abnormal node and removed from the neighbor set of the current node; otherwise, it is determined to be a normal neighboring node.

[0234] The chi-square statistic is introduced to remove outliers, specifically using the residual statistic, which is expressed as follows:

[0235]

[0236] Among them, new information The specific manifestations are as follows:

[0237]

[0238] The specific form of the new covariance matrix is ​​as follows:

[0239]

[0240] A threshold is used to filter neighboring nodes based on their historical residual values ​​and standard deviations. The expression of the node removal criteria is shown below:

[0241]

[0242]

[0243] in, The threshold for the chi-square test. This represents the tolerable consistency fluctuation threshold. Setting the consistency fluctuation threshold too high may lead to missed detection of highly volatile nodes; setting it too low may easily misclassify nodes with slight fluctuations but normal volatility as anomalies. `mean` refers to the average value, used to detect offset-type anomalies; `std` is the standard deviation, used to detect volatility anomalies.

[0244] Step 5: Based on the set of normal neighbor nodes obtained in Step 4, state estimation fusion is performed using an improved Distributed Consensus Kalman Filter (IDCKF). The IDCKF determines the confidence weight of each neighbor node based on the reciprocal of its prior error covariance matrix, and then weights and fuses the state estimates of the neighbor nodes with the predicted values ​​of the current node to obtain the navigation state estimate of the current node.

[0245] The specific form of the discrete state-space equations for each UAV is as follows:

[0246]

[0247] in, Here is the state transition matrix. Here is the noise perturbation matrix. for The state vector at time t, for The state vector at time t, for The system noise vector at time t.

[0248] The specific form of the measurement equation for each UAV node is as follows:

[0249]

[0250] in, For nodes exist The measured value at time, For the measurement matrix, This is the measurement noise vector.

[0251] The specific form of the prediction step in a distributed consensus Kalman filter is as follows:

[0252]

[0253]

[0254] in, For the first The node at the th The posterior estimate of the step, For nodes In the The prior estimate of the step, For the first The node at the th The prior error covariance estimate of the step. Indicates the first The node at the th The number of neighboring nodes of the step. For the first The posterior error covariance estimate of each node.

[0255] The specific form of the update steps of a distributed consensus Kalman filter is as follows:

[0256]

[0257]

[0258]

[0259] in, It is a node In the Kalman gain of the step, For nodes The measurement noise covariance matrix, where The specific manifestations are as follows:

[0260]

[0261] For the consensus factor gain matrix The specific manifestations are as follows:

[0262]

[0263] Based on the aforementioned distributed consensus Kalman filter, the state residual term and the confidence weight are introduced, and their specific forms are as follows:

[0264]

[0265]

[0266]

[0267] in, For neighboring nodes In the The prior estimate of the step, For neighboring nodes In the The prior estimate of the step, For nodes In the The posterior estimation error covariance matrix of the step. The sensitivity factor is 0 to 1.

[0268] Nodes after combining credibility weights The specific form of the posterior state estimate is as follows:

[0269]

[0270] Step 6: In relative navigation mode, using a federated filter architecture, for each normal neighbor node obtained in Step 4, establish a corresponding independent sub-filter using its relative navigation information and complete the measurement update for each sub-filter:

[0271] The specific form of sub-filter initialization is as follows:

[0272]

[0273] in, The current node The initial values ​​of the error covariance matrix, Neighboring nodes The error covariance matrix, The current node The system noise covariance matrix, The current node The system noise covariance matrix, For nodes Shared factors.

[0274] The specific form of the prediction step of the sub-filter is as follows:

[0275]

[0276]

[0277] in, For sub-filters in The prior estimate at time 10:00. For sub-filters in The posterior estimate at time t. Let be the state transition matrix of the sub-filter. For sub-filters in The prior error covariance matrix at time t. For sub-filters in The posterior error covariance matrix at time t. Let be the system noise covariance matrix of the sub-filter.

[0278] The specific form of the sub-filter update step is as follows:

[0279]

[0280]

[0281]

[0282] in The Kalman gain of the sub-filter. This is the measurement matrix of the sub-filter. Let be the measurement noise covariance matrix of the sub-filter. For the observations of the sub-filter, Let be the posterior error covariance matrix of the sub-filter.

[0283] Step 7: Combine the local state estimation results of each sub-filter in Step 6 with the prediction results of the SINS-based main filter, generate a globally optimal state estimate based on the information allocation principle, and output the navigation result of the current node by combining the mechanical arrangement of inertial navigation.

[0284] Combining the local state estimation results of the sub-filters, the specific manifestation of global information fusion is as follows:

[0285]

[0286]

[0287]

[0288] Among them, among them, The one-step prediction covariance matrix of the master filter. The state estimate predicted in the first step by the master filter. The system noise covariance matrix of the main filter. This is the global posterior estimate. To estimate the covariance matrix for the global posterior, This is the global system noise covariance matrix.

[0289] The allocation is based on the information allocation principles of the system covariance matrix and the system noise matrix, and the specific form is shown below:

[0290]

[0291]

[0292]

[0293] in, These are the posterior estimates for each filter. The posterior covariance matrix for each filter is given. Let be the noise covariance matrix of the system for each filter.

[0294] The present invention also proposes a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the aforementioned unmanned cluster adaptive localization method under abnormal measurement.

[0295] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the aforementioned unmanned cluster adaptive localization method under abnormal measurements.

[0296] Example

[0297] To verify the effectiveness of the present invention, the following simulation experiment was conducted.

[0298] 1. Simulation conditions

[0299] To verify the superiority of the IDCKF algorithm proposed in this invention, a three-dimensional scene is considered. A swarm of six UAVs is set up to perform a climb flight mission in a designated airspace. The topology of the UAV swarm is as follows: Figure 7 As shown. Each UAV in the cluster establishes a stable communication link with two adjacent nodes to achieve local information exchange. The entire navigation task is set to take 300 seconds. To verify the hybrid filter proposed in this invention, consider the following scenario: six UAVs, during takeoff, the UAVs... The drone accelerates and climbs for 20 seconds; then it enters the first phase of uniform reconnaissance flight, maintaining a constant speed for 40 seconds; after completing the reconnaissance, the drone makes a right turn and accelerates, lasting 30 seconds; then the drone performs a left turn return maneuver and accelerates during ascent, lasting 70 seconds. After completing the above tasks, the drone enters the second phase of high-altitude uniform reconnaissance, lasting 60 seconds; then it performs a descent and turn maneuver, lasting 18 seconds; finally, it enters the terminal flight phase, lasting 40 seconds, for a total flight time of 278 seconds.

[0300] To verify the superiority and effectiveness of the proposed IDCKF algorithm, it is compared and analyzed with the following three methods: 1) Traditional Distributed Consensus Kalman Filter (DCKF); 2) Information Consensus Filter (ICF); 3) Distributed Suboptimal Consensus Kalman Filter (DSOCKF). To verify the detection of anomalous neighbor nodes, verification was conducted for different thresholds, specifically: , , To verify the node selection capability of the IDCKF algorithm, a scenario was designed where 5 nodes of the drone exhibited anomalies within 50 seconds, such as... Figure 8 As shown. To improve the robustness of the proposed adaptive localization method for unmanned swarms under abnormal measurements, navigation scenarios with different numbers of unmanned aerial vehicles (UAVs) under abnormal conditions were designed.

[0301] The performance metric uses the root mean square error (RMSE), expressed as follows:

[0302]

[0303] Root mean square (RMS) The expression is as follows:

[0304]

[0305] In the formula, This is an estimated value. For the corresponding truth value, The root mean square error is in the eastward direction. This represents the root mean square error in the north direction. The root mean square error of the celestial direction.

[0306] 2. Simulation Content and Result Analysis

[0307] For the verification of the IDCKF algorithm, the position error curve of UAV 5 under abnormal operating conditions is as follows: Figure 8As shown, the position error suddenly increases at 50 seconds, but the overall trend shows convergence. UAV 6 and UAV 5 are neighboring nodes; therefore, abnormal operating conditions of UAV 5 will affect the navigation and positioning accuracy of UAV 6. The position error comparison curves of the four algorithms for UAV 6 are shown below. Figure 9 As shown in the simulation error comparison curves, the improved Distributed Consensus Kalman Filter (IDCKF) algorithm effectively suppressed the divergence trend of state errors after removing abnormal neighbor nodes, verifying the robustness and effectiveness of the method in dynamic environments. Further quantitative results show that the root mean square values ​​of the position errors in the east, north, and sky directions are 0.5467 m, 0.4657 m, and 0.4730 m, respectively, significantly outperforming the traditional DCKF algorithm (0.9255 m, 1.2125 m, 1.6983 m), ICKF algorithm (1.4719 m, 0.9851 m, 2.0789 m), and DSOCKF algorithm (0.6358 m, 0.9318 m, 2.4965 m).

[0308] To better verify the effectiveness of the hybrid filtering algorithm, Figure 10 make and Figure 11 This invention demonstrates the robustness of the algorithm proposed in this invention. Figure 10 The position error curves of different UAVs using the hybrid filtering algorithm involved in this invention are shown. Figure 11 The figure shows the position error curves of different UAVs using only the IDCKF algorithm. It can be seen that UAV 6, the node with measurement anomalies, showed improved navigation accuracy, making the overall navigation of the UAV swarm more robust. Figure 12 as well as Figure 13 The figure shows the navigation and positioning accuracy of a drone swarm under different numbers of drone failures. As can be seen from the figure, when fewer than 4 drones fail, the drone swarm can maintain a high positioning accuracy.

[0309] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0310] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of this application. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these modifications and improvements all fall within the protection scope of this application. Therefore, the protection scope of this application should be determined by the appended claims.

Claims

1. An adaptive localization method for unmanned swarms under abnormal measurements, characterized in that, Includes the following steps: Step 1: Construct a strapdown inertial navigation system / global navigation satellite system (SINS / GNSS) integrated navigation model, including establishing the SINS error state space equation of the UAV, and constructing the corresponding measurement equation by combining the position and velocity information output by GNSS; Step 2: Construct a relative navigation measurement equation based on airborne sensor measurements. The equation includes measurement information on the relative distance and relative velocity between UAV nodes. Step 3: Use a sliding window of length L to record the historical results of the chi-square test of the measurement information of the current node, and calculate the anomaly density within the window. The information is the difference between the observed value and the predicted observed value. When the anomaly density is lower than the first preset threshold, the measurement is determined to be normal, and proceed to step 4; when the anomaly density is higher than or equal to the first preset threshold, the measurement is determined to be abnormal, and proceed to step 6. Step 4: For each neighboring node of the current node, calculate the mean and standard deviation of the historical chi-square statistic sequence of the state estimation residuals of each neighboring node; If the mean of a neighboring node exceeds the second preset threshold or the standard deviation exceeds the third preset threshold, then the neighboring node is determined to be an abnormal node and removed from the current node's neighbor set; otherwise, it is determined to be a normal neighboring node. Step 5: Based on the set of normal neighbor nodes obtained in Step 4, state estimation fusion is performed using an improved distributed consensus Kalman filter (IDCKF). The IDCKF determines the confidence weight of each neighbor node based on the reciprocal of the prior error covariance matrix of each neighbor node, and performs weighted fusion of the state estimates of the neighbor nodes with the predicted values ​​of the current node to obtain the navigation state estimate of the current node. Step 6: When a measurement anomaly is detected, switch to relative navigation mode and adopt a federated filter architecture. For each normal neighbor node obtained in Step 4, use its relative navigation information to establish a corresponding independent sub-filter and complete the measurement update of each sub-filter. Step 7: Combine the local state estimation results of each sub-filter in Step 6 with the prediction results of the SINS-based main filter, generate the global optimal state estimate based on the information allocation principle, and output the navigation result of the current UAV node by combining the mechanical arrangement of inertial navigation.

2. The unmanned swarm adaptive localization method under abnormal measurement according to claim 1, characterized in that, Step 1: Construct a strapdown inertial navigation system / global navigation satellite system (SINS / GNSS) integrated navigation model, including establishing the SINS error state space equation for the UAV and constructing the corresponding measurement equations by combining the position and velocity information output by GNSS. The specific method is as follows: The specific forms of the platform error angle equation, velocity error equation, position error equation, and inertial instrument error equation are as follows: ； Among them represents the attitude rotation matrix for converting the vehicle coordinate system to the navigation coordinate system, is the measured value of the accelerometer in the vehicle coordinate system, is the velocity vector in the navigation coordinate system, including the output of the gyroscope, represents the angular velocity of the b coordinate system relative to the inertial i coordinate system, represents the rotation of the n coordinate system relative to the i coordinate system, including the rotation of the navigation system caused by the earth's rotation and the rotation of the n coordinate system due to the curvature of the earth's surface on the earth's surface, that is , is the radius of curvature of the prime vertical, is the radius of curvature of the卯酉圈(meridian circle), is the altitude value, is the latitude value, is the longitude value, represents the gravitational acceleration; By combining the platform error angle equation, velocity error equation, position error equation, and inertial device error equation, the error equation of the strapdown inertial navigation system is established. Based on this error equation, the state equation of the integrated navigation system is derived, expressed in the following form: ； in, , , Indicates positional error. Indicates speed error, Indicates attitude error. This indicates the random zero bias of the accelerometer. This represents the random drift of the gyroscope. White noise representing gyroscope angular velocity measurement information. White noise representing the specific force measurement information of the accelerometer. The state transition matrix is ​​expressed in the following form: ； in, and It is a zero matrix. It can be determined from the error state equation of SINS. Let be the attitude rotation matrix from the vehicle coordinate system to the navigation coordinate system. The specific form of the system's perturbation matrix is ​​as follows: ； The specific form of the state-space equations of a discrete system is shown below: ； in, In order to be in The state vector at time t, It is the system's state transition matrix. It is the system's perturbation matrix. Is the system in The noise vector at time step; The measurement model based on satellite navigation systems is represented as follows: ； in, , For position measurement noise, For speed measurement noise, The measurement matrix is ​​represented as follows: ； The specific form of the discrete observation equation is shown below: ； in, Is The observed value at time, Is The observation matrix at time, Is Observational noise at any given moment.

3. The unmanned swarm adaptive localization method under abnormal measurement according to claim 1, characterized in that, Step 2: Construct a relative navigation measurement equation based on airborne sensor measurements. This equation includes measurement information on the relative distance and relative velocity between UAV nodes, where: Adjacent nodes The specific representation of the drone computational model is shown below: ； in, For nodes The calculated value of the eastward velocity. For nodes The calculated value of the northbound velocity, For nodes The calculated value of the celestial velocity. For nodes The true value of the eastward velocity, For nodes The true value of the northbound velocity, For nodes The true value of the upward velocity, For nodes The eastward velocity error, For nodes The northbound velocity error, For nodes The upward velocity error; Current node The specific representation of the drone computational model is shown below: ； Therefore, the specific form of the relative velocity calculation model is as follows: ； in The relative calculation speed is in the eastward direction. The relative calculation speed is for the northward direction. The relative calculation speed of the sky direction, This is the true value of the eastward relative velocity. This is the true value of the northward relative velocity. This represents the true value of the relative velocity towards the sky. Adjacent nodes The specific representation of calculated position coordinates in the Earth-centered and Earth-fixed coordinate system is as follows: ； in, , , ; For the truth value of height, For the true value of latitude, For the truth value of height, , , For the corresponding related error, The radius of curvature of the Earth's geoid. The flattening of the Earth's ellipsoid; Adjacent nodes The specific representation of the true position coordinates in the Earth-centered Earth-fixed coordinate system is as follows: ； By calculating the Jacobian matrix of the actual location coordinates, the neighboring nodes are derived. The specific manifestation of the coordinate error term of the UAV in the Earth-centered, Earth-fixed coordinate system: ； Among them, the Jacobian matrix The specific manifestations are as follows: ； ； Current drone node The error term in the geocentric-fixed coordinate system is derived through the Jacobian matrix, and its specific form is as follows: ； Therefore, the current node and neighboring nodes The specific form of the relative distance calculation value is as follows: ； in, The attitude rotation matrix for transforming from the geocentric coordinate system to the navigation coordinate system is specifically represented as follows: ； drone nodes and drone nodes The specific form of the relative velocity measurement model is as follows: ； in, This is the attitude rotation matrix for transforming the vehicle coordinate system into the navigation coordinate system. The antisymmetric matrix of the misaligned angle vectors, This refers to the components of the relative velocity measurement in the navigation coordinate system. The relative velocity measurement at the current drone node Components in the carrier coordinate system; drone nodes and drone nodes The specific form of the relative position measurement model is as follows: ； in For Jacobian matrices, This is the error in relative distance measurement; Therefore, the specific form of the velocity measurement model is as follows: ； The specific form of the location measurement model is as follows: ； The specific form of the measurement model for relative navigation is as follows: ； in, For the velocity measurement matrix, The measurement matrix for the position, can be The transformation yields, ,in Neighboring nodes Speed ​​error, This represents the positional error of neighboring nodes. For the measurement error of relative velocity, This is the azimuth error. For elevation angle error, This represents the measurement error of the relative distance.

4. The unmanned swarm adaptive localization method under abnormal measurement according to claim 1, characterized in that, Step 3: Use a sliding window of length L to record the historical results of the chi-square test for the current node's measurement innovation, and calculate the anomaly density within the window. The specific method is as follows: For the current node drone Introduce a sliding window of length L to record the results of the most recent L chi-square tests. The results of the chi-square test The specific manifestations are as follows: ； The specific manifestations of abnormal density are as follows: ； in, For the reason Composition dimensional array, It reflects the overall trend of recent anomalies in the system, and its value ranges from 0 to 1.

5. The unmanned swarm adaptive localization method under abnormal measurement according to claim 1, characterized in that, Step 4: For each neighboring node of the current node, calculate the mean and standard deviation of the historical chi-square statistic sequence of the state estimation residuals of each neighboring node; If the mean of a neighboring node exceeds the second preset threshold or the standard deviation exceeds the third preset threshold, then the neighboring node is determined to be an abnormal node and removed from the current node's neighbor set; otherwise, it is determined to be a normal neighboring node, and the specific method is as follows: The chi-square statistic is introduced to remove outliers, specifically using the residual statistic, which is expressed as follows: ； Among them, new information The specific manifestations are as follows: ； The specific form of the new covariance matrix is ​​as follows: ； A threshold is used to filter neighboring nodes based on their historical residual values ​​and standard deviations. The expression of the node removal criteria is shown below: ； ； in, The threshold for the chi-square test. This represents the tolerable consistency fluctuation threshold. Setting the consistency fluctuation threshold too high may lead to missed detection of highly volatile nodes; setting it too low may easily misclassify nodes with slight fluctuations but normal volatility as anomalies. mean refers to the mean value, used to detect offset-type anomalies; std is the standard deviation, used to detect volatility anomalies.

6. The unmanned swarm adaptive localization method under abnormal measurement according to claim 1, characterized in that, Step 5: Based on the set of normal neighbor nodes obtained in Step 4, an improved Distributed Consensus Kalman Filter (IDCKF) is used for state estimation fusion. The IDCKF determines the confidence weight of each neighbor node based on the reciprocal of its prior error covariance matrix, and weights and fuses the state estimates of the neighbor nodes with the predicted value of the current node to obtain the navigation state estimate of the current node. Specifically, the expression is as follows: The specific form of the discrete state-space equations for each UAV is as follows: ； in, Here is the state transition matrix. Here is the noise perturbation matrix. for The state vector at time t, for The state vector at time t, for The system noise vector at time t; The specific form of the measurement equation for each UAV node is as follows: ； in, For nodes exist The measured value at time, For the measurement matrix, For measuring the noise vector; The specific form of the prediction step in a distributed consensus Kalman filter is as follows: ； ； in, For the first The node at the th The posterior estimate of the step, For nodes In the The prior estimate of the step, For the first The node at the th The prior error covariance estimate of the step. Indicates the first The node at the th The number of neighboring nodes of the step. For the first The posterior error covariance estimate of each node; The specific form of the update steps of a distributed consensus Kalman filter is as follows: ； ； ； in, It is a node In the Kalman gain of the step, For nodes The measurement noise covariance matrix, where The specific manifestations are as follows: ； For the consensus factor gain matrix The specific manifestations are as follows: ； Based on the aforementioned distributed consensus Kalman filter, the state residual term and the confidence weight are introduced, and their specific forms are as follows: ； ； ； in, Neighboring nodes In the The prior estimate of the step, Neighboring nodes In the The prior estimate of the step, For nodes In the The posterior estimation error covariance matrix of the step. This is a sensitivity factor, ranging from 0 to 1; Nodes after combining credibility weights The specific form of the posterior state estimate is as follows: 。 7. The unmanned swarm adaptive localization method under abnormal measurement according to claim 1, characterized in that, Step 6: In relative navigation mode, a federated filter architecture is adopted. For each normal neighbor node obtained in Step 4, a corresponding independent sub-filter is established using its relative navigation information, and the measurement update of each sub-filter is completed. The specific method is as follows: The specific form of sub-filter initialization is as follows: ； in, The current node The initial values ​​of the error covariance matrix, Neighboring nodes The error covariance matrix, The current node The system noise covariance matrix, The current node The system noise covariance matrix, For nodes Shared factors; The specific form of the prediction step of the sub-filter is as follows: ； ； in, For sub-filters in The prior estimate at time 10:

00. For sub-filters in The posterior estimate at time t. Let be the state transition matrix of the sub-filter. For sub-filters in The prior error covariance matrix at time t. For sub-filters in The posterior error covariance matrix at time t. The system noise covariance matrix of the sub-filter; The specific form of the sub-filter update step is as follows: ； ； ； in The Kalman gain of the sub-filter. This is the measurement matrix of the sub-filter. Let be the measurement noise covariance matrix of the sub-filter. For the observations of the sub-filter, Let be the posterior error covariance matrix of the sub-filter.

8. The unmanned swarm adaptive localization method under abnormal measurement according to claim 1, characterized in that, Step 7: Combine the local state estimation results of each sub-filter in Step 6 with the prediction results of the SINS-based main filter, generate a globally optimal state estimate based on the information allocation principle, and output the navigation result of the current node by combining the mechanical arrangement of inertial navigation. The specific method is as follows: Combining the local state estimation results of the sub-filters, the specific manifestation of global information fusion is as follows: ； ； ； Among them, among them, The one-step prediction covariance matrix of the master filter. The state estimate predicted in the first step by the master filter. The system noise covariance matrix of the main filter. This is the global posterior estimate. To estimate the covariance matrix for the global posterior, The global system noise covariance matrix; The allocation is based on the information allocation principles of the system covariance matrix and the system noise matrix, and the specific form is shown below: ； ； ； in, These are the posterior estimates for each filter. The posterior covariance matrix for each filter is given. Let be the noise covariance matrix of the system for each filter.

9. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, it implements the unmanned swarm adaptive localization method under abnormal measurement as described in any one of claims 1-8.

10. A computer-readable storage medium having a computer program stored thereon, wherein when executed by a processor, the computer program implements the unmanned swarm adaptive localization method under abnormal measurements as described in any one of claims 1-8.

Images