# Improved Adaptive Federated Kalman Filtering for INS/GNSS/VNS Integrated Navigation Algorithm
## 4 Simulation Experiment
### Scenario 2  
Simulating the driving process of unmanned vehicles in an obstructed space may result in GNSS signal interference or blockage, as well as a shortage of feature points due to environmental issues in VNS. In this simulation, these situations occurred during different time periods. Period 1: a time period between 250 s-400 s, where the unmanned vehicle drove into a dark scene where there were fewer feature points, which caused a malfunction in the visual navigation. Therefore, a 10-fold measurement error was introduced in the visual positioning. The following formula (34) shows this, where _pos_ is the true position information, \(d_{VNS}\) is the visual navigation position error, and \(randn(3,1)\) is a \(3\times 1\) matrix of the standard normal distribution:  
\[pos_{VNS}=\left\{\begin{array}{ll}pos+10\text{-}d_{VNS}\text{-}randn(3,1)&(250 \text{ s }\sim 400\text{ s})\\ pos+d_{VNS}\text{-}randn(3,1)&\text{ \  
Through the analysis of the results shown in Figure 4a,b and Table 3, it was found that when the simulated unmanned vehicle experienced failures in satellite navigation and visual navigation during time periods 1 and 2, the improved AFKF proposed in this paper performed significantly better than the classic FKF. Without interference to the navigation signal, the improved AFKF showed a 35.4% reduction in the position error and a 54.8% reduction in the velocity error compared to the classic FKF. Across all the time periods, the improved AFKF exhibited an average reduction of 25.1% in the position error and 62.4% in the velocity error compared to the classic FKF. In order to prevent the randomness of non-Gaussian noise and verify the adaptive and fault detection capabilities of the algorithm proposed in this paper, the system was subjected to 50 independent simulations in scenario 2,  
From Figure 5a,b and Table 4, it can be seen that in these INS/GNSS/VNS integrated navigation simulation experiments, the position and velocity errors determined using the improved FKF proposed in this paper were significantly smaller than those using the classic FKF. The average root mean square errors of the position in the three directions of east, north, and up decreased by 56.4%, 54.8%, and 43.4%, respectively, and the average root mean square errors of the velocity in the three directions of east, north, and up decreased by 71.0%, 72.1%, and 28.4%, respectively. In order to validate the real-time performance of the proposed improved AFKF algorithm, using the same simulation software and platform as described above and with the same raw data, we compared the entire filtering process time between the improved AFKF and the classic FKF in 50 simulation experiments, as shown in Figure 6. The driving time of the unmanned vehicle in the simulation was 887.5 s, with an average runtime of 11.2493 s for the improved AFKF and 10.3526 s for the classic FKF. Although the average runtime of the improved AFKF proposed in this paper was increased by 8.66% compared to the classic FKF, it still met the real-time requirements of the integrated navigation of unmanned vehicles. The filtering cycle of the improved AFKF was 1 s, and the INS update period was 0.02 s. The average filtering time was less than 3.69 ms, which was smaller than the INS update period, satisfying the real-time requirements of the system. To further verify the effectiveness of the algorithm, we conducted simulation experiments on the Gazebo platform, an open-source robot simulation platform that relies on the popular robot operating system (ROS), as shown in Figure 7a. The unmanned vehicle model used in the simulation experiment was the TurtleBot3-Waffle-Pi, which is a widely used open-source robot for research and education. It is small in size and highly scalable, and the main model parameters and movements used during the simulation are shown in Table 5. As shown in Figure 7b, an environment for the unmanned vehicle to travel in was constructed on the Gazebo platform, and the unmanned vehicle traveled at a constant