Research on UAV Robust Adaptive Positioning Algorithm Based on IMU/GNSS/VO in Complex Scenes

As an important component of autonomous intelligent systems, the research on autonomous positioning algorithms used by UAVs is of great significance. In order to resolve the problem whereby the GNSS signal is interrupted, and the visual sensor lacks sufficient feature points in complex scenes, which leads to difficulties in autonomous positioning, this paper proposes a new robust adaptive positioning algorithm that ensures the robustness and accuracy of autonomous navigation and positioning in UAVs. On the basis of the combined navigation model of vision/inertial navigation and satellite/inertial navigation, based on ESKF, a multi-source fusion model based on a federated Kalman filter is here established. Furthermore, a robust adaptive localization algorithm is proposed, which uses robust equivalent weights to estimate the sub-filters, and then uses the sub-filter state covariance to adaptively assign information sharing coefficients. After simulation experiments and dataset verification, the results show that the robust adaptive algorithm can effectively limit the impact of gross errors in observations and mathematical model deviations and can automatically update the information sharing coefficient online according to the sub-filter equivalent state covariance. Compared with the classical federated Kalman algorithm and the adaptive federated Kalman algorithm, our algorithm can meet the real-time requirements of navigation, and the accuracy of position, velocity, and attitude measurement is improved by 2–3 times. The robust adaptive localization algorithm proposed in this paper can effectively improve the reliability and accuracy of autonomous navigation systems in complex scenes. Moreover, the algorithm is general—it is not intended for a specific scene or a specific sensor combination– and is applicable to individual scenes with varied sensor combinations.


Introduction
With the rapid development of research on autonomous and intelligent unmanned systems, UAVs can now operate in high-risk and complex environments, thus expanding the scope for human activities by virtue of their flexibility, low cost, and strong adaptability. Therefore, research on their application is of great significance to the military and civilian fields [1][2][3].
At present, sensors that can be used for autonomous navigation and positioning include inertial sensors, visual sensors, satellite navigation sensors, and so on [4]. As the heart and eyes of autonomous navigation systems, these sensors are intrinsic to the realization of autonomous and intelligent drones. However, satellite signals are interrupted by urban canyons and complex environments; in fog, heavy snow, and disaster scenarios, visual sensors lack sufficient feature points; inertial sensors face problems such as long-term error accumulation. Therefore, a single type of sensor alone cannot meet the autonomous navigation requirements of UAVs used in complex scenarios; multi-source sensors need effectiveness of the proposed algorithm is proven through dataset validation. Finally, the conclusions are drawn in Section 6.

ESKF (Error State Kalman Filter)
Compared with the classical Kalman filter, the ESKF can constrain the error state to run at a position close to the origin, thereby avoiding the possible parameter singularity and gimbal lock problems and ensuring parameter linearization. In this paper, a loose combination of vision/inertial navigation and satellite/inertial navigation is modeled based on the ESKF. Similar to the classic Kalman approach, the ESKF performs prediction and measurement updates. The prediction model is kinematically updated based on the IMU (Inertial Measurement Unit) model, and the measurement is updated based on VO (Visual Odometry-the position and attitude data are obtained by solving camera image poses) and GNSS (Global Navigation Satellite Systems) measurement data.

Predictive Model
This paper adopts the local navigation coordinate system, and the system state quantity is [q, v, p, a b , ω b ] T , where q represents UAV attitude quaternion, v represents UAV speed, p represents UAV position, a b represents the accelerometer bias, and ω b represents the angular velocity bias. The UAV kinematics equation is as follows: Considering that the actual measurement contains errors, here, the state quantity is set to the error state x(t) = [δθ, δv, δp, δa b , δω b ] T . The UAV error state equation is as follows: where δθ is the attitude angle error state that satisfies δq = e δθ/2 , δv is the velocity error state, δp is the position error state, δa b is the accelerometer bias error state, and δω b is the angular velocity bias error state. ω m is the measurement value of the gyroscope, w ω is the measurement noise of the gyroscope, and w ω b is the noise of the gyroscope bias. a m is the measurement value of the accelerometer, w a is the measurement noise of the accelerometer, and w a b is the noise of the accelerometer bias. With reference to Equation (2), the equation for state is: .
x(t) = F(t)x(t) + G(t)w(t) Using Taylor expansion, the formula is discretized, and the following formula is thus obtained: where ∆T is the sampling time.

Measurement Update
The UAV measurement update equation is as follows: The GNSS measurement data are converted into the local navigation coordinate system of this paper, and the measurement matrix is obtained as follows: where n GNSS v is the velocity measurement white noise, and n GNSS p is the white noise produced by position measurement.
Similarly, the VO measurement data are the position and attitude values obtained from the original image through pose calculation, and the measurement matrix is as follows: where n VO θ is the attitude measurement white noise, and n VO p is the position measurement white noise.

Fusion Model
The UAV measurement data are derived from two types of sensors, GNSS and VO, so the multi-source fusion method is used for state estimation. Considering the need to ensure the fault tolerance and reliability of the navigation system, the distributed filtering method is adopted in this paper. Figure 1 shows the classic fusion feedback mode of federated

Time Update
The measurement update equation is as follows: is the one-step predicted state covariance of the i-th filter.

Measurement Update
The measurement update equation is as follows:

Information Fusion
The state quantity and state covariance of the main filter are obtained by fusing the sub-filters. The fusion equation is as follows: where 1 g k P + is the state covariance after the main filter fusion, and

Time Update
The measurement update equation is as follows: where X i k is the state quantity of the i-th (i = 1 · · · N) filter at time k, X m k is the state quantity of the main filter at time k, X i k+1/k is the one-step predicted state, Q i k is the system state covariance, Φ i k+1/k is the state transition matrix of the i-th filter, and P i k+1/k is the one-step predicted state covariance of the i-th filter.

Measurement Update
The measurement update equation is as follows: where K i k+1 is the gain matrix, H i k+1 is the measurement matrix, R i K+1 is the measurement state covariance, X i k+1 is the predicted state, and P i k+1 is the predicted state covariance.

Information Fusion
The state quantity and state covariance of the main filter are obtained by fusing the sub-filters. The fusion equation is as follows: where P g k+1 is the state covariance after the main filter fusion, and X g k+1 is the state quantity after the fusion of the main filter.

Information Sharing and Feedback
The information sharing and feedback factor model is as follows: where β i is the sub-filter sharing factor, and β m is the main filter sharing factor.

Robust Adaptive Filtering
In a complex environment, considering that errors, or even gross errors, arise in the measurement values of random dynamic systems, the statistical characteristics of noise will change, which will reduce the accuracy of Kalman filtering, and even cause divergence [15,17]. In this case, the availability of sub-filter data is reduced or even completely eliminated. One should consider performing residual testing and robustness processing on the sub-filters before the data fusion of the main filter in order to reduce the availability of observations. Unusable observations are isolated from the main filter so as not to contaminate the entire filtering process, thus improving the accuracy and fault tolerance of the entire system.

Robust Equivalent Weight Filtering
The system state residual is determined by both the model error and the observation error. When the model error is small, the residual can be used to represent the observation error, and the robustness equivalent weight factor can be used to alter the observation availability gain [18][19][20][21].
The state residual is s i k = (z i k − H i k x i k/k−1 ), and its covariance matrix is Here, s i k represents the residual of the i-th filter at time k in distributed filtering. The residual of the i-th subfilter is normalized as follows: Here, the IGG3 [22] weight function is introduced for robust processing, and the residual gain matrix is adaptively adjusted using the system's normalized residual.
In the absence of gross errors in observations, the normalized residuals v i obey the standard state distribution: v i ∼ N(0, 1). Robust processing is performed on observations that exceed the 95% confidence level, where k 0 is set to 1 and k 1 is set to 2. After the observation robustness is processed, the measurement update is performed as follows:
In practice, considering that the filtering accuracy of different sub-filters is inconsistent, it is necessary to adjust the proportion of information in each of the sub-filters according to the filtering accuracy. The information sharing coefficient determines the role of each sub-filter in the information fusion process. Specifically, the larger the information sharing coefficient, the larger the proportion of the state estimates dealt with by the local subfilters [24].
In filtering, the state covariance P i positively reflects the filtering quality of the filter. The smaller the value of P i , the more accurate the filter, and vice versa. Here, the accuracy of the sub-filter λ i (k) and the state covariance P i are defined by Equation (15), as follows.
As discussed in the previous section, the normalized residuals v i reflect the availability of filter observations, so we can combine v i and P i to comprehensively consider the accuracy of the sub-filters. Here, the IGG3 weight function is introduced to constrain the availability of observations. Considering Equations (13) and (15), the accuracy of the sub-filter can be determined as follows: (16) given that in the federated Kalman filter, the information sharing coefficient satisfies [25][26][27][28]: where β i (k) is the information sharing coefficient of the i-th filter at step k.
Here, the main filter does not distribute information, so the adaptive information sharing coefficient and sub-filter precision λ i (k) are expressed as follows:

Robust Adaptive Multi-Source Model
As shown in Figure 2, based on the federated Kalman filter, this study uses IMU/GNSS/VO to build a multi-source fusion navigation system. Robust filtering is performed on IMU/GNSS and IMU/VO, respectively, and the information sharing coefficients are adaptively adjusted by robust equivalent weights.

Simulation Experiment
In order to verify the effectiveness and robustness of the algorithm proposed in this paper, the parameters are set in alignment with the real characteristics of different sensors, and the scene is set with consideration for the complexity of the real environment. Simulation experiments are carried out to compare and analyze different schemes in different

Simulation Experiment
In order to verify the effectiveness and robustness of the algorithm proposed in this paper, the parameters are set in alignment with the real characteristics of different sensors, and the scene is set with consideration for the complexity of the real environment. Simulation experiments are carried out to compare and analyze different schemes in different scenarios. We here highlight that the following simulation experiments have been developed and realized on the basis of the PSINS toolbox, completed by Prof. Yan Gongmin of Northwestern Polytechnical University.

Scene Settings
In consideration of the real urban environment, the challenging scenarios faced by UAV flight are here simulated. The following two periods are prone to measurement errors and have been designed considering the limitations of the motion model, and the complexity of the terrain and the environment.
Period 1: 100 s~200 s, when the UAV is flying between buildings; because there are few feature points, 20 times the R VO gross error is added to the VO positioning measurement. R VO is the measurement error value, including position error and attitude error, as shown in Table 1. Period 2: 270 s~370 s, when the UAV flying height drops; here, the urban canyon environment is simulated, and 20 times R GNSS gross error is added to the GNSS positioning measurement. R GNSS is the measurement error value, including position error and speed error, as shown in Table 1. Table 1 shows the measurement error parameters and update frequency settings of each sensor (IMU, GNSS, VO).

Simulation Scheme
In this paper, three schemes are designed to simulate the trajectory of the UAV in the above simulation. Scheme 2: Adaptive federated Kalman filtering. The sub-filter remains unchanged, and the main filter adaptively adjusts the information sharing coefficient according to the accuracy of the sub-filter.
Scheme 3: Robust adaptive federated Kalman filtering. The sub-filter performs robust filtering, and the main filter adaptively adjusts the information sharing coefficient according to the accuracy of the sub-filter.

Information Sharing Coefficient Simulation
The capacity of the information sharing coefficient for online adaptation can improve the accuracy and fault tolerance of the whole system in the case of partial sensor failure. The following shows a comparative analysis of the information sharing coefficients of the three schemes. Figure 3 shows the distribution of information sharing coefficients for the three schemes proposed in this paper. In Scheme 1, the sub-filter information sharing coefficients are evenly distributed. In Scheme 2, since the sub-filter observation error holds a fixed value, the mean square error shows a stable change trend, and the final factor weight of sub-filter information allocation is not changed. In Scheme 3, the information sharing coefficient shows a changing trend in periods 1 and 2. This is due to the presence of gross measurement or model errors, and the sub-filters worsen. The adaptive algorithm proposed in this paper can automatically reduce its corresponding information sharing coefficient and increase the sharing factors of the other two sub-filters. This is in line with expectations. Therefore, the algorithm can guarantee the fault tolerance of the whole system in a complex environment.

Comparison of State Estimation of Different Combined Systems
In Figure 4, the black line represents the real trajectory, the red line represents the VO/IMU estimated trajectory, the green line represents the GNSS/IMU estimated trajectory, and the blue line represents the VO/GNSS/IMU estimated trajectory. If the three subfilters are used for independent navigation, the state estimation accuracy will decrease due to the presence of gross errors in the observation values in different time periods (1, 2), which will cause a deviation from the true trajectory and mean the accuracy requirements of the entire navigation system are unmet. As expected, the performance of the VO/GNSS/IMU's global optimal fusion is not seriously affected by abnormal signals given by local sensors and can achieve high accuracy.

Comparison of State Estimation of Different Combined Systems
In Figure 4, the black line represents the real trajectory, the red line represents the VO/IMU estimated trajectory, the green line represents the GNSS/IMU estimated trajectory, and the blue line represents the VO/GNSS/IMU estimated trajectory. If the three sub-filters are used for independent navigation, the state estimation accuracy will decrease due to the presence of gross errors in the observation values in different time periods (1, 2), which will cause a deviation from the true trajectory and mean the accuracy requirements of the entire navigation system are unmet. As expected, the performance of the VO/GNSS/IMU's global optimal fusion is not seriously affected by abnormal signals given by local sensors and can achieve high accuracy.
due to the presence of gross errors in the observation values in different time periods (1,2), which will cause a deviation from the true trajectory and mean the accuracy requirements of the entire navigation system are unmet. As expected, the performance of the VO/GNSS/IMU's global optimal fusion is not seriously affected by abnormal signals given by local sensors and can achieve high accuracy.

Comparison of the Results of Different Schemes
In Figure 5, the black line represents the true trajectory. The red line represents scheme 1, which is the estimated trajectory of the federated Kalman filter. The green line represents scheme 2, which is the estimated trajectory of the adaptive federal Kalman filter. The blue line represents scheme 3, which is the estimated trajectory of the robust adaptive federal filtering proposed in this paper. It can be seen from Figure 5 that all three filtering methods can be used for VO/GNSS/GNSS system navigation. However, on the

Comparison of the Results of Different Schemes
In Figure 5, the black line represents the true trajectory. The red line represents Scheme 1, which is the estimated trajectory of the federated Kalman filter. The green line represents Scheme 2, which is the estimated trajectory of the adaptive federal Kalman filter. The blue line represents Scheme 3, which is the estimated trajectory of the robust adaptive federal filtering proposed in this paper. It can be seen from Figure 5 that all three filtering methods can be used for VO/GNSS/GNSS system navigation. However, on the whole, and especially during the 100~200 s and 270~370 s periods, compared to the estimated trajectories of adaptive federated Kalman filtering and federated Kalman filtering, the estimated trajectories of the robust adaptive federal filter are the closest to the true values. This is because the robust adaptive federated Kalman can perform robust and adaptive adjustments on the sub-filter estimates, and the main filter performs adaptive information sharing coefficient allocation according to the estimated weights of the sub-filters. As expected, the performance of the main globally optimal fusion filter is not severely affected by local sensor anomalies and can achieve high accuracy. whole, and especially during the 100~200 s and 270~370 s periods, compared to the estimated trajectories of adaptive federated Kalman filtering and federated Kalman filtering, the estimated trajectories of the robust adaptive federal filter are the closest to the true values. This is because the robust adaptive federated Kalman can perform robust and adaptive adjustments on the sub-filter estimates, and the main filter performs adaptive information sharing coefficient allocation according to the estimated weights of the subfilters. As expected, the performance of the main globally optimal fusion filter is not severely affected by local sensor anomalies and can achieve high accuracy. The attitude error estimation curve is shown in Figure 6. The attitude angle estimates obtained by the three filtering methods can all track the change in the true attitude angle, but their estimation accuracies are different. The error in the pitch angle and roll angle is within 100 arc seconds, and the error in the yaw angle is within 30 arc minutes. Compared with the actual data, the error here is small, which has a strong impact on the accuracy of  The attitude error estimation curve is shown in Figure 6. The attitude angle estimates obtained by the three filtering methods can all track the change in the true attitude angle, but their estimation accuracies are different. The error in the pitch angle and roll angle is within 100 arc seconds, and the error in the yaw angle is within 30 arc minutes. Compared with the actual data, the error here is small, which has a strong impact on the accuracy of the initial value and initial state covariance set by the simulation. At the same time, it can be seen that the accuracy of Scheme 3 is greatly improved compared with the other two schemes. The attitude error estimation curve is shown in Figure 6. The attitude angle estimates obtained by the three filtering methods can all track the change in the true attitude angle, but their estimation accuracies are different. The error in the pitch angle and roll angle is within 100 arc seconds, and the error in the yaw angle is within 30 arc minutes. Compared with the actual data, the error here is small, which has a strong impact on the accuracy of the initial value and initial state covariance set by the simulation. At the same time, it can be seen that the accuracy of scheme 3 is greatly improved compared with the other two schemes. According to the error estimation curve, it can be seen that the overall error of Scheme 3 is relatively stable, with a slight oscillation around the zero value. In the two time periods set in this paper, even when the local sensors are interfered with or fail entirely, the whole system can still maintain sufficiently high precision for navigation. This is because Scheme 3 can switch between different systems in time to reassign weights when local sensors are affected by external disturbances. Therefore, the robust adaptive filter can use the current adaptive state of each local system and can effectively utilize sub-filters with higher state accuracy, thereby reducing the estimated value of the error. According to the error estimation curve, it can be seen that the overall error of scheme 3 is relatively stable, with a slight oscillation around the zero value. In the two time periods set in this paper, even when the local sensors are interfered with or fail entirely, the whole system can still maintain sufficiently high precision for navigation. This is because scheme 3 can switch between different systems in time to reassign weights when local sensors are affected by external disturbances. Therefore, the robust adaptive filter can use the current adaptive state of each local system and can effectively utilize sub-filters with higher state accuracy, thereby reducing the estimated value of the error.    Table 2, and the average error precision is shown in Figure 9. As shown in Figure 9 and Table 2, the accuracy of Scheme 3 (robust adaptive Federated Kalman filtering) is significantly better than those of the other two schemes. The average position error accuracies of the 20 experiment groups were calculated separately, and the errors of the three schemes were obtained as follows: 0.4009 m, 0.2117 m, and 0.0719 m. Compared with Scheme 2, the accuracy of Scheme 3 increased by 66%, and compared with Scheme 1, it increased by 82%. The discussion and analysis of the above results further prove that the robust adaptive federated Kalman filtering algorithm proposed in this paper achieves high accuracy and good robustness, and the algorithm can be applied to complex environments.  As shown in Figure 9 and Table 2, the accuracy of scheme 3 (robust adaptive Federated Kalman filtering) is significantly better than those of the other two schemes. The average position error accuracies of the 20 experiment groups were calculated separately, and the errors of the three schemes were obtained as follows: 0.4009 m, 0.2117 m, and 0.0719 m. Compared with scheme 2, the accuracy of scheme 3 increased by 66%, and compared with scheme 1, it increased by 82%. The discussion and analysis of the above results further prove that the robust adaptive federated Kalman filtering algorithm proposed in The calculation times of the three algorithms have been tested. The test environment was Windows C++, and the test platform was configured at 1.99 GHz, with Intel(R) Core (TM) i7-8550U CPU. The times required for the single-step execution of the three schemes are shown in Table 3. The time required for the single-step execution of the robust adaptive federated Kalman filter algorithm was 2.12 × 10 −2 , which meets the real-time requirements of practical applications. Table 3. The time required for the single-step execution of the three schemes.

Dataset Validation
On the basis of the simulation verification preformed in the previous section, the dataset collected by the Shanghai Beidou Navigation and Location Services Key Laboratory (UAV configuration sensors and related parameters are shown in Figure 10 and Table 4) are used for verification. This dataset includes four scenarios: 5 × 5 × 2.5 m testing room with Vicon, "Room"; 8 × 12 × 5 m hall of office with Vicon, "Hall"; 20 × 20 m outdoor square, "OutSquare"; 50 m 2 outdoor area near the building, "OutBuilding". Among the four scenarios, "OutBuilding" ("OutBuilding" is shown in Figure 10) is the most representative, and offers the conditions of short-term errors in or interruptions of GNSS and VO measurement due to signal occlusion or single features. In order to test the applicability of the algorithm in this paper in a complex environment, the SE_OutBuilding_06.bag data are here used to artificially add errors in different time periods. By comparing the final results of the three different schemes, the effectiveness of the algorithm in this paper is verified.
First, based on the ESKF model, the state estimation results of the VO/IMU, GNSS/IMU, and VO/GNSS/IMU integrated navigation systems are obtained, as shown in Figure 11. It can be seen that these integrated navigation systems meet the needs of navigation and positioning, without model or measurement errors. The positioning accuracy is determined by the accuracy and combination of the sensors themselves. The VO/GNSS/IMU combination shows the highest accuracy, followed by the VO/IMU combination, and finally GNSS/IMU. four scenarios, "OutBuilding"("OutBuilding" is shown in Figure 10) is the most representative, and offers the conditions of short-term errors in or interruptions of GNSS and VO measurement due to signal occlusion or single features. In order to test the applicability of the algorithm in this paper in a complex environment, the SE_OutBuilding_06.bag data are here used to artificially add errors in different time periods. By comparing the final results of the three different schemes, the effectiveness of the algorithm in this paper is verified.    First, based on the ESKF model, the state estimation results of the VO/IMU, GNSS/IMU, and VO/GNSS/IMU integrated navigation systems are obtained, as shown in Figure 11. It can be seen that these integrated navigation systems meet the needs of navigation and positioning, without model or measurement errors. The positioning accuracy is determined by the accuracy and combination of the sensors themselves. The VO/GNSS/IMU combination shows the highest accuracy, followed by the VO/IMU combination, and finally GNSS/IMU. In consideration of the real properties of the sensor and the complex external environment, the following two time periods are set. These two periods contain model and measurement errors, which can enable us to more effectively verify the algorithm proposed in this paper.
Period 1: 10~40 s; non-Gaussian noise is added to RGB-D Camera measurement, which obeys the following distribution: In consideration of the real properties of the sensor and the complex external environment, the following two time periods are set. These two periods contain model and measurement errors, which can enable us to more effectively verify the algorithm proposed in this paper. Period 1: 10~40 s; non-Gaussian noise is added to RGB-D Camera measurement, which obeys the following distribution: where σ 1 = 2500 µrad, σ 2 = 4σ 1 and ε = 0.5. Period 2: 60~90 s; 20R random error is added to RTK GNSS receiver positioning measurement.
As shown in Figure 12, the information sharing coefficients of different schemes show different trends, as consistent with the simulation results in Section 4.2. Since the measurement accuracy of the vision sensor is higher than that of GNSS, when the information sharing coefficient in Scheme 2 stabilizes, the ratio of VO/IMU will be higher. At the same time, the information sharing coefficient of Scheme 3 shows a change trend, which indicates that the information sharing coefficient of the robust adaptive equivalent Kalman filter algorithm can be adjusted online when the environment changes, thereby improving the accuracy of the entire system. Using the three schemes set in Section 4.1, a position estimate is obtained as shown in Figure 13. Here, the black line represents the true trajectory; the red line (scheme 1) represents the estimated trajectory of the federated Kalman filter; the green line (scheme 2) represents the estimated trajectory of the adaptive information sharing coefficient of the main filter; the blue line (scenario 3) represents the estimated trajectory of the robust adaptive federated filter proposed in this paper. As can be seen, the robust adaptive federated Kalman filter proposed in this paper can effectively track the ground truth.  Using the three schemes set in Section 4.1, a position estimate is obtained as shown in Figure 13. Here, the black line represents the true trajectory; the red line (Scheme 1) represents the estimated trajectory of the federated Kalman filter; the green line (Scheme 2) represents the estimated trajectory of the adaptive information sharing coefficient of the main filter; the blue line (scenario 3) represents the estimated trajectory of the robust adaptive federated filter proposed in this paper. As can be seen, the robust adaptive federated Kalman filter proposed in this paper can effectively track the ground truth. Figure 14 shows a comparison of the position, velocity, and attitude errors of the three schemes. Compared with Scheme 1 and Scheme 2, Scheme 3 has a higher overall accuracy, which is consistent with the simulation results shown in Section 4.2. For the next 20 analyses, the mean absolute errors (MAEs) and standard deviations (STDs) of the state estimation errors of the three schemes are obtained individually, as shown in Table 5.
As can be seen from Table 5, compared with Scheme 1 and Scheme 2, the average value of the pitch angle and roll angle in Scheme 3 is increased by 1 degree, and the average value of the yaw angle is increased by 2 degrees. The average speed is increased by 0.2 m/s, and the average position is increased by about 0.2 m. These experimental results further demonstrate that the robust adaptive Kalman filter algorithm proposed in this paper can effectively improve the accuracy and robustness of the multi-source fusion navigation system. Scheme 3 is significantly better than the other two schemes, with an overall accuracy improvement of 2-3-fold.
represents the estimated trajectory of the federated Kalman filter; the green line (scheme 2) represents the estimated trajectory of the adaptive information sharing coefficient of the main filter; the blue line (scenario 3) represents the estimated trajectory of the robust adaptive federated filter proposed in this paper. As can be seen, the robust adaptive federated Kalman filter proposed in this paper can effectively track the ground truth.  Figure 14 shows a comparison of the position, velocity, and attitude errors of the three schemes. Compared with scheme 1 and scheme 2, scheme 3 has a higher overall accuracy, which is consistent with the simulation results shown in Section 4.2. For the next 20 analyses, the mean absolute errors (MAEs) and standard deviations (STDs) of the state estimation errors of the three schemes are obtained individually, as shown in Table 5.   Figure 14. Comparison of position, speed, and attitude errors of different schemes.

Conclusions
With the intention of improving the reliability and robustness of UAV autonomous navigation and positioning in complex scenarios, we have here designed an autonomous positioning fusion algorithm. The main innovation is that the algorithm can not only independently evaluate the working performance of the sub-filters online, but it can also dynamically adjust the information sharing coefficient. In order to verify the effectiveness and robustness of the algorithm proposed in this paper, an urban canyon scene has been simulated. Through comparative analysis of the two scenarios and three schemes set up, Scheme 3 displayed the highest accuracy of robust adaptive federal kalman filtering, followed by Scheme 2 (adaptive federal Kalman filtering), and finally Scheme 1 (federal Kalman filtering). In addition, by testing the time taken for the single-step debugging of the robust adaptive federal Kalman filter, it has been proven that the algorithm can meet the requirements of actual real-time measurements. Further, this paper used the Beidou Navigation and Location Services Key Laboratory dataset for verification. Using the "Out-Building" data, the artificially simulated model errors and measurement gross errors have been added, and the final results show that the overall accuracy of the algorithm proposed in this paper is improved 2-3-fold. In summary, the algorithm can significantly improve the accuracy and tolerance of the navigation system in complex environments and can be applied to UAV autonomous navigation in urban canyons and GNSS loss-of-lock scenarios. Moreover, the algorithm is general, and can be applied in similar complex scenes and other sensor combinations. Therefore, using the robust adaptive fusion algorithm proposed in this paper, reliable, adaptive, robust and high-precision positioning information can be obtained. Next, we will focus on the actual application of UAVs in complex environments to verify the effectiveness of the algorithm proposed in this paper.