Research on Kinematic and Static Filtering of the ESKF Based on INS/GNSS/UWB

With the widespread development of multiple sensors for UGVs, the multi-source fusion-navigation system, which overcomes the limitations of the use of a single sensor, is becoming increasingly important in the field of autonomous navigation for UGVs. Because federated filtering is not independent between the filter-output quantities, owing to the use of the same state equation in each of the local sensors, a new kinematic and static multi-source fusion-filtering algorithm based on the error-state Kalman filter (ESKF) is proposed in this paper for the positioning-state estimation of UGVs. The algorithm is based on INS/GNSS/UWB multi-source sensors, and the ESKF replaces the traditional Kalman filter in kinematic and static filtering. After constructing the kinematic EKSF based on GNSS/INS and the static ESKF based on UWB/INS, the error-state vector solved by the kinematic ESKF was injected and set to zero. On this basis, the kinematic ESKF filter solution was used as the state vector of the static ESKF for the rest of the static filtering in a sequential form. Finally, the last static ESKF filtering solution was used as the integral filtering solution. Through mathematical simulations and comparative experiments, it is demonstrated that the proposed method converges quickly, and the positioning accuracy of the method was improved by 21.98% and 13.03% compared to the loosely coupled GNSS/INS and the loosely coupled UWB/INS navigation methods, respectively. Furthermore, as shown by the error-variation curves, the main performance of the proposed fusion-filtering method was largely influenced by the accuracy and robustness of the sensors in the kinematic ESKF. Furthermore, the algorithm proposed in this paper demonstrated good generalizability, plug-and-play, and robustness through comparative analysis experiments.


Introduction
Navigation and positioning are the core technologies of the Internet of Things and location service applications, which occupy a pivotal position in national security and economic construction [1]. With the increasing demand for unmanned, intelligent, and autonomous vehicles in various fields, unmanned ground vehicles (UGVs) with flexibility, low cost, strong adaptability, and other characteristics, are widely used in storage and logistics, search and rescue, detection and excavation, reconnaissance and detonation, and other civilian and military fields [2][3][4]. In outdoor environments, the global navigation satellite system (GNSS) can provide high-accuracy positioning for UGVs [5], but when unmanned vehicles are in urban canyons, or in underground or indoor scenes, the GNSS produces a loss of lock, rejection, and other phenomena, which means that the availability, continuity, and reliability of GNSS signals are not guaranteed.
To address problems such as the inability of single-GNSS positioning to meet production and construction needs, Yang Yuanxi proposed the concept of integrated positioning, that the errors in the suboptimal estimation results of the FKF were excessively large to meet the high-precision-filtering needs of the actual system.
To avoid reusing the carrier equation-of-state information and to solve the problem of the correlation between each local filter and between the local filter and the main filter of the FKF, Yang Yuanxi [20] proposed a kind of sequential kinematic-and static-filteringfusion navigation, which divides the local filter into kinematic Kalman filtering and static Kalman filtering. After the kinematic Kalman filtering of the output of either observation ephemeris based on the dynamics model with the base sensor or the first sensor, the navigation information of the remaining sensors is added in a sequential form to create a static Kalman filter and, finally, a fusion solution of all the navigation information is obtained. Shi Jianxian et al. [21] proposed a combined GPS/BDS/INS navigation algorithm based on kinematic and static filtering. The algorithm combines the dynamics model of a satellite-navigation system and an inertial navigation system for a kinematic filtering solution, after which the static filter designed by the principle of sequential parity is used to further correct the kinematic filtering results and obtain the final state vector. Wang Yidi et al. [22] proposed a combined pulsar/CNS deep-space-survey navigation method based on improved kinematic and static filtering, which used a UKF in the kinematic filtering to process starlight-angular-distance measurements with a fast sampling rate, strong non-linearity in the measurement equation, and an EKF in the static filtering to process pulsar measurements with a slow sampling rate and obvious linearity in the measurement equation. This avoids the problem of the non-optimality of the fusion filter owing to the use of the same equation of state for each local filter.
In current navigation systems, because of their autonomous characteristics, INSs, which are less susceptible to external interference, are often used to provide continuous, high-update-frequency navigation data. Currently, in most INSs, the error-state Kalman filter (ESKF) is mostly used instead of the traditional Kalman filter. Lu Keke et al. [23] proposed a quadratic attitude-estimation method based on the ESKF, which can avoid covariance matrix singularities and maintain the ability to represent random variable uncertainties. Jun Dai et al. [24] proposed an IMU/GNSS/VO (visual odometry)-based UAV with a robust adaptive-positioning algorithm for use in complex environments, in which a combined ESKF-based navigation model of VO/INS and GNSS/INS was constructed as a local filter of the federated filter to improve the accuracy and robustness of the federated filter in complex environments. The authors of [25] proposed a rigorous attitude-andposition computation algorithm using tightly coupled sensor fusion for multi-antenna, multi-GNSS, and inertial sensor observations, which uses an extended Kalman filter (EKF) and current phase post-processed kinematic (PPK) methods to feed attitude information from multi-antenna GNSS measurements back into the INS, focusing on improving the position-and attitude-measurement accuracy of low-cost UAVs.
In this paper, a kinematic and static filtering method is proposed based on the ESKF and the related properties of kinematic and static filtering, with a UGV as the vehicle. Navigation data from the INS, GNSS, and UWB were used in the filtering. An INS was used as the primary sensor in the filtering, and the GNSS and UWB were used to correct the INS to improve the integral performance of the filtering. In the kinematic ESKF, the ESKF was used instead of the traditional Kalman filter, the error-state vector solved by the kinematic ESKF was injected into the INS, and the velocity and position components of the error-state vector were subsequently zeroed. In the static ESKF, the zeroed error-state vector and its covariance matrix were used as the state vector. Furthermore, the corrected position and velocity errors of both the INS and the UWB were used as the observation equations, which were statically filtered in a sequential form and fed back to the INS to obtain the final fused navigation results. On this basis, simulation and comparison experiments were carried out in this study. In the comparison experiments, the errors of the proposed scheme were compared with the two schemes of the loosely coupled GNSS/INS and the loosely coupled UWB/INS. Through these experiments, the usability of the proposed kinematic and static filtering of the ESKF based on INS/GNSS/UWB was explored. In addition, simulation experiments with different sensor parameters and complex environments were conducted to analyze the generalizability, robustness, and plug-and-play nature of the proposed algorithm. The experimental results indicate that the kinematic and static filtering of the ESKF based on INS/GNSS/UWB proposed in this paper has accuracy advantages over combined navigation algorithms, has good generalizability, and is applicable to different sensors with different accuracies. Its good robustness, the ease with which the filter structure can be changed, and the shielding of contaminated sensors can be applied to seamless indoor and outdoor positioning scenes, providing a filtering basis for seamless indoor-and outdoor-positioning-sensor switching.
This paper is organized as follows. The ESKF algorithm is introduced in Section 2. In Section 3, the kinematic and static filtering of the ESKF based on INS/GNSS/UWB is established. In Section 4, simulation experiments based on the proposed method are reported, and a comparison experiment between the loosely coupled GNSS/INS and the loosely coupled UWB/INS schemes with the same parameter settings is reported, demonstrating the usability of the method proposed in this paper. In Section 5, the generalizability of the proposed algorithm is analyzed by reporting the use of settings with different sensor accuracies. Based on this analysis, the robustness of the filtering was explored by adding the systematic errors of the GNSS and UWB and conducting the simulation experiments in a complex environment. Finally, the conclusions are drawn in Section 6.

Error-State Kalman Filter
Compared to the Kalman filter, the ESKF can constrain the error state to a position close to the origin, thus avoiding the singularity of the parameters used and ensuring the linearization of the parameters. Because the state and motion vectors of the ESKF are small, the second-order variables of the two vectors can be relatively negligible, thus reducing the error caused by the Taylor expansion in the linearization process. The ESKF defines the true state vector as the sum of the nominal state vector and the error-state vector. The ESKF for inertial navigation systems is divided into four main processes: the nominal-stateprediction process, the error-state-prediction process, the error-state-injection process, and the state-error-nulling process. At moment t k , the nominal-state-prediction process and the error-state-prediction process are carried out simultaneously. The prediction is updated for the current moment using the nominal-and error-state quantities from the previous iteration by means of a recursive equation. Next, the true-state estimate at that moment is obtained by injecting the error-state estimate into the nominal-state estimate. Finally, the velocity and position components of the state error are set to zero, and we proceed to the next iteration.
In this study, two ESKF models for fused navigation were designed in a loosely coupled formulation; these were the GNSS/INS-based ESKF and the UWB/INS-based ESKF. Both fusion models were constructed as ESKF models based on the INS equation of state. The difference in these new models is that after the nominal-state prediction and error-state prediction, the error-state estimates were injected into the calibration-fusion data for the GNSS and UWB. The calibrated-fusion data were then injected into the nominal state estimator, as shown in the structure in Figure 1. Similar to the Kalman filter, the ESKF includes state prediction and measurement prediction, in which the state is updated based on the kinematic model of the INS. Unlike the Kalman filter, the measurement predictions are updated based on the errors in the velocity and position of the GNSS compared to the INS and the errors in the velocity and position of the UWB compared to the INS.

Kinematic Models
In this paper, In the ESKF process, the real-state-dynamics model for the IMU (inertial measure ment unit) is modeled in Equation (2): where n a and n  represent acceleration and angular velocity measurements and whit noise, respectively. The a  and   represent acceleration bias and angular velocit bias, respectively. n b truth C represents the rotation matrix of the real state, which in the IN system is the coordinate-transformation matrix from the carrier coordinate system b t the navigation coordinate system n .  represents the quaternion multiplication opera tion and the extraction of the vector part of the result. The nominal-state IMU-dynamic model is presented as Equation (3):

Kinematic Models
In this paper, X = [q, v, p, a b , ω b ] T is used as the state vector of the overall system, where v is the attitude of the UGV, q is the velocity of the UGV, p is the position of the UGV, a b is the accelerometer bias, and ω b is the gyroscope bias. In the ESKF, the real-state vector X truth is formed by the nominal-state vector X and the error-state vector δX, and the equation is as follows: In the ESKF process, the real-state-dynamics model for the IMU (inertial measurement unit) is modeled in Equation (2): where a n and ω n represent acceleration and angular velocity measurements and white noise, respectively. The a ω and ω ω represent acceleration bias and angular velocity bias, respectively. C n b truth represents the rotation matrix of the real state, which in the INS system is the coordinate-transformation matrix from the carrier coordinate system b to the navigation coordinate system n. ⊗ represents the quaternion multiplication operation and the extraction of the vector part of the result. The nominal-state IMU-dynamics model is presented as Equation (3): where C n b represents the rotation matrix of the nominal state. The error-state dynamics model of the IMU is obtained from the above equation, as shown in Equation (4): where [a] × represents the antisymmetric matrix of vector a.
Since Equations (2)-(4) are all continuous-form equations, they need to be discretized. After discretization, the recursive equation for the nominal-state dynamics model of the IMU at moment t k is obtained, as shown in Equation (5): where ∆t represents the sampling interval of the IMU. The error-state dynamics model of the IMU at moment t k is derived as shown in Equation (6): where w θk represents the Gaussian random pulse noise at attitude. The w vk represents the Gaussian random pulse noise at velocity. The w ak represents the Gaussian random pulse noise at acceleration bias. The w ωk represents the Gaussian random pulse noise at angular velocity bias. Their covariance is defined by Equation (7): where σ 2 a n and σ 2 ω n represent the standard deviation of Gaussian white noise for acceleration and angular velocity, respectively. The σ 2 a ω and σ 2 ω ω represent the standard deviation of Gaussian white noise for acceleration bias and angular velocity bias. The I represents the unit matrix of 3 × 3.

ESKF State-Prediction Model
Under discrete conditions, the nominal-state vector can be defined as x k = [q k , v k , p k , a k , ω k ] T , the error-state vector as δx k = [δq k , δv k , δp k , δa bk , δω bk ] T , the IMU acceleration and angular velocity noise-measurement vector as u mk = [a mk , ω mk ] T , and the IMU attitude, velocity, and acceleration zero-bias and angular velocity zero-bias noise vectors as w k = [w θk , w vk , w ak , w ωk ] T . Bringing the vectors defined above into Equation (5), the recursive formula for the nominal-state vector at moment a is denoted by Equation (8): x k+1 = f (x k , u mk ) (8) where f (·) represents the recursive function of the nominal-state vector. Combining Equation (6) and linearizing it according to Taylor's formula, the recursive formula for the error-state vector at moment t k+1 is derived as follows: where f δ (·) represents the recursive function of the error-state vector. The F x k and G w k represent the Jacobi matrices corresponding to the error-state vector and the noise-state vector, respectively. The Φ k+1,k is the state-transfer matrix from moment t k to moment t k+1 . The Γ k+1,k is the noise-transfer matrix from moment t k to moment t k+1 . The specific expressions of F x k and G w k are as shown in Equation (10).
According to Equation (9), the covariance matrix Σ x k+1 of error-state vector δx k+1 at moment t k+1 is derived as shown in Equation (11): where Σ x k represents the covariance matrix of error-state vector δx k at moment t k . The Q w represents the state-noise matrix, which is represented as follows:

ESKF Measurement-Prediction Model
The accelerometer and gyroscope possess bias errors, causing the IMU to drift during the integration of attitude estimation, which gradually increases over time. In this paper, the velocity and position information obtained by the GNSS and UWB are used to correct the IMU observations, thus correcting the error-state estimates of the ESKF to reduce the Sensors 2023, 23, 4735 8 of 23 errors due to accelerometer and gyroscope bias during the fusion of the overall filter process. (13): Furthermore, the observation model for the UWB/INS-based ESKF is defined as shown in Equation (14):

The GNSS/INS-based ESKF observation model is defined as shown in Equation
where L UWB k represents the observation vector of the UWB/INS-based ESKF. The v UWB represents velocity measurements obtained from the UWB. The p UWB represents position measurements obtained from the UWB. The H UWB According to the basic principle of the Kalman filter, taking the ESKF based on the GNSS/INS as an example, the updated equation of the observation model at moment t k+1 is obtained as shown in Equation (15): represents the covariance matrix of the innovation vector. The K GNSS k+1 represents the Kalman Gain. The δx k+1 represents the estimated error-state vector at moment t k+1 . The Σ δx k+1 represents the covariance matrix of δx k+1 . Similarly, the updated equation of the observation model of UWB/INS-based ESKF at moment t k+1 is obtained.

ESKF Error-State-Vector Injection and Zeroing
According to Equations (1), (9), and (15), the formula for injecting the error-state vector into the nominal-state vector at moment a to obtain the true-state vector is derived as shown in Equation (16) Sensors 2023, 23, 4735 9 of 23 wherex k+1 represents the estimated truth-state vector at moment t k+1 . The x k+1 represents the estimated nominal-state vector at moment t k+1 . Based on Equation (16), the components are represented in Equation (17).
To reduce error accumulation, the velocity and position components of the error-state vector are zeroed, as shown in Equation (18): where G = 0 3×3 −I 6×6 0 6×6 represents the zeroing matrix.

Kinematic and Static Filtering of the ESKF Based on INS/GNSS/UWB
The entire structure of the ESKF-based kinematic and static filter is shown in Figure 2. The integral filter is mainly divided into the kinematic ESKF and static ESKF. First, the IMU navigation data in the INS are used as the model of the kinematic ESKF. The position and velocity errors of the INS and GNSS were used as the observation equations of the kinematic ESKF. Subsequently, the kinematic ESKF solution is calculated to obtain the filtered solution of the kinematic ESKF with its covariance. Since the kinematic ESKF filter results in an error-state vector, the kinematic ESKF filter solution needs to be injected into the nominal-state vector. In addition, the velocity and position components of the error-state vector of the kinematic ESKF need to be set to zero. On this basis, the zeroed error-state vector and the covariance of the kinematic ESKF's estimated state quantity are passed to the static ESKF in a sequential form as the initial value of the static ESKF. The position and velocity errors of the INS and GNSS are used as the observation equations of the static ESKF, and the static ESKF is solved. The error-state vector of the filtered solution of the static ESKF is then injected into the nominal-state vector of the INS to obtain the true-state vector of the INS after the static ESKF after zeroing the velocity and position components of the error-state vector. Finally, the truth-state quantities obtained from the last static ESKF are output as a result of the navigation of the entire system.

Kinematic ESKF Process
Since different sensors on the same carrier theoretically have the same state vector, the state equations among the different subsystems all coincide with the main system's state equations. According to Equations (9) and (15), the GNSS/INS-based kinematic ESKF filter solution at moment t k+1 can be obtained as shown in Equation (19): where K GNSS k+1 represents the Kalman gain of the kinematic ESKF, as represented by Equation (20).
where Σ δx k+1 represents the covariance matrix of δx k+1 , as shown in Equation (21): According to Equations (16) and (18), when the error-state vector δx k+1 is injected into the nominal state vector x k+1 of the INS, the INS truth-state vectorx k+1 and the error-state vector δx 0 k+1 after setting to zero are obtained.

Kinematic ESKF Process
Since different sensors on the same carrier theoretically have the same state vector, the state equations among the different subsystems all coincide with the main system's state equations. According to Equations (9) and (15), the GNSS/INS-based kinematic ESKF filter solution at moment 1 k t + can be obtained as shown in Equation (19): represents the Kalman gain of the kinematic ESKF, as represented by Equation (20).

Static ESKF Process
In the static ESKF, to avoid reusing the kinetic model forecasts of the state vector and its covariance matrix at moment t k , the kinematic ESKF filter solution is used to obtain the error-state vector δx 0 k+1 and the covariance matrix ΣX 1k of the error-state vector after zeroing as the static ESKF state vector x k+1 and its covariance matrix Σ x k+1 . At this point, the state equation of the static ESKF is shown in Equation (22).
The error between the truth-state vectorx 1(k+1) from the kinematic ESKF filter solution and the position and velocity data from the UWB observation is used as the observation equation. Equations (9), (14) and (15) are used to obtain the static ESKF filter solution based on the UWB/INS, as shown in Equation (23): where K UWB 2(k+1) represents the Kalman gain of the static ESKF, as shown in Equation (24). where Σ δx 2(k+1) represents the covariance matrix of δx 2(k+1) , as shown in Equation (25).

Simulation-Experiment Verification
The following simulation experiments were developed and implemented based on the PSINS toolbox by Prof. Yan Gongmin of the Northwestern Polytechnic University.

Coordinate-and Trajectory-Simulation Settings
As the coordinate systems used by the INS, GNSS, and UWB are distinct, we converted the angular velocity and acceleration data measured by the IMU with respect to the geocentric inertial coordinate system and the xyz coordinate data measured by the UWB with respect to the local coordinate system into the coordinate data of the navigation-coordinate system (n system), which is denoted by o n x n y n z n . As the navigation -coordinate system was the reference coordinate system, the "East-North-Sky" geographic coordinate system (g system) was selected as the navigation reference coordinate system, represented by o g x g y g z g , where the origin was defined as the center of gravity of the UGV, the o g x g axis denoted the geographic east, the o g y g axis denoted the geographic north, and the o g z g axis denoted the sky perpendicular to the local rotating ellipsoid, and the three axes formed a right-handed coordinate system. In addition, the coordinates of the UGV under the g system were expressed in terms of longitude λ, latitude L, and ellipsoidal altitude h. The DW3000 was used as the base station for the UWB simulation and received the signals from the tags mounted on the carrier. The tag information received by the base station was used to locate the tag using the TDOA (time difference of arrival) algorithm. The specific location of the base station is shown in Figure 3, with the red circle representing the UWB base station.  h, the velocity random-wander error was 1 µg/ √ Hz, and the sampling frequency was 100 Hz. The velocity-system error of the GNSS was 0.5 m/s, the position-system error was [1 m; 1 m; 1 m], and the sampling frequency was 1 Hz. The velocity-system error of the UWB was 0.5 m/s, the position-system error was [0.8 m; 0.8 m; 0.8 m], and the sampling frequency was 1Hz. The sensor parameters of the UGV were set as shown in Table 1.

Simulation Results and Analysis
After designing the simulation trajectory related to the UGV, the measurement noise v GNSS In Figure 4, the blue line represents the curve of the velocity error with time in the east direction during the filter process, the red line represents the curve of the velocity error with time in the north direction during the filter process, and the yellow line represents the curve of the velocity error with time in the sky direction during the filter process. All the units are in m/s. In Figure 5, the blue line represents the curve of the eastward position error with time during the filter process, the red line represents the curve of the northward position error with time during the filter process, and the yellow line represents the curve of the skyward position error with time during the filter process. All of these measurements are in m.   In Figure 4, the blue line represents the curve of the velocity error with time in the east direction during the filter process, the red line represents the curve of the velocity error with time in the north direction during the filter process, and the yellow line represents the curve of the velocity error with time in the sky direction during the filter process. All the units are in m/s. In Figure 5 On this basis, this paper sets up two schemes for comparison between the loosely coupled GNSS/INS and the loosely coupled UWB/INS . The sensor parameters of the two schemes were consistent with Table 1; the observation error of the loosely coupled  The starting point in Figure 6 is the starting point of the UGV trajectory. In the figure, the black trajectory represents the real simulation trajectory, the blue dotted line represents the loosely coupled GNSS/INS simulation trajectory, the green dotted line represents the GNSS simulation trajectory, the purple dotted line represents the loosely coupled UWB/INS simulation trajectory, the blue dashed line represents the UWB simulation trajectory, and the red dashed line represents the trajectory of the kinematic and static filter simulation proposed in this paper.
According to Figure 6, in the straight-line operation phase, the simulation trajectory of the filter proposed in this paper was closer to the real trajectory than the simulation trajectory of the loosely coupled GNSS/INS scheme and the simulation trajectory of the loosely coupled UWB/INS scheme, as shown in the enlarged area on the left in the example diagram. The simulated trajectory of the method proposed in this paper also had a constraining effect on the error in the simulated trajectory of the two compared solutions during the turning-maneuver phase of the carrier. An example is given in the enlarged area on the right in the figure.
A comparison of the errors in the attitude, velocity, and position in the three simulation scenes i shown in  The starting point in Figure 6 is the starting point of the UGV trajectory. In the figure, the black trajectory represents the real simulation trajectory, the blue dotted line represents the loosely coupled GNSS/INS simulation trajectory, the green dotted line represents the GNSS simulation trajectory, the purple dotted line represents the loosely coupled UWB/INS simulation trajectory, the blue dashed line represents the UWB simulation trajectory, and the red dashed line represents the trajectory of the kinematic and static filter simulation proposed in this paper.
According to Figure 6, in the straight-line operation phase, the simulation trajectory of the filter proposed in this paper was closer to the real trajectory than the simulation trajectory of the loosely coupled GNSS/INS scheme and the simulation trajectory of the loosely coupled UWB/INS scheme, as shown in the enlarged area on the left in the example diagram. The simulated trajectory of the method proposed in this paper also had a constraining effect on the error in the simulated trajectory of the two compared solutions during the turning-maneuver phase of the carrier. An example is given in the enlarged area on the right in the figure.
A comparison of the errors in the attitude, velocity, and position in the three simulation scenes i shown in According to Figure 7, there were no significant differences between the errors in the yaw, roll, and pitch between the three schemes; the errors in the yaw and pitch angle both decreased with time, while the errors in the roll angle gradually increased with time, mainly because of the lack of feedback correction in the INS attitude data from both the kinematic ESKF and the static ESKF. Furthermore, the error curves for the yaw and roll angle both jittered around 20, 45, 60, and 90 s, which was mainly due to the carriers performing turning maneuvers at these times.     According to Figure 7, there were no significant differences between the errors in the yaw, roll, and pitch between the three schemes; the errors in the yaw and pitch angle both decreased with time, while the errors in the roll angle gradually increased with time, mainly because of the lack of feedback correction in the INS attitude data from both the According to Figure 8, compared with the other two schemes, the absolute values of the velocity errors in the east-east, north, and sky directions of the schemes proposed in this paper were significantly reduced during the acceleration and steady phases from 0 to 20 s. After 20 s, the absolute values of the velocity errors all decreased slightly. The main reason for this was that all three schemes converged at around 20 s in the filter. Before this convergence, the difference between the three velocity errors was large because of the unstable filter. After the convergence, as the filter gradually stabilized and there was no coarse difference interference, the difference in velocity error between the three schemes was smaller.
According to Figure 9, the absolute values of the position errors in the longitude (east), latitude (north), and ellipsoidal altitude (down) of the proposed scheme in this paper were smaller than those in the other two schemes. However, since the fusion filter proposed in this paper fused the two ESKFs in a sequential form, the trend of its position-errorvariation curve with time was similar to that of the kinematic ESKF (the ESKF based on the GNSS/INS). Therefore, in the construction of the overall kinematic and static filters based on the ESKF, the selection of the sensors in the kinematic ESKF largely affected the navigation accuracy of the overall filter. Therefore, the sensors in the kinematic ESKF should be selected to be more accurate and robust to correct the INS measurement data, thus improving the accuracy and robustness of the overall filter. Comparisons of the root mean square error (RMSE) and mean absolute error (MAE) of the attitude, position, and velocity of the three scenes are shown in Tables 2 and 3. According to Table 3

Comparative Experiments and Analysis
To verify the generalizability and robustness of the kinematic and static filtering of the ESKF based on the INS/GNSS/UWB algorithms proposed in this paper, two comparative experiments were set up. Experiment 1 analyzed the impact of the different sensors' accuracies on the algorithms by setting the relevant parameters of the GNSS, UWB, and IMU. Experiment 2 compared the traditional ESKF-based federated filtering, ESKF-based dynamic and static filtering, and ESKF-based dynamic and static filtering with the switching strategy by setting up a simulation scene with reduced positioning accuracy under conditions of GNSS rejection and UWB non-sight-range conditions and by increasing the positioning errors of the GNSS and UWB in this environment.

Experimental Analysis of Sensor-Accuracy Comparisons
To analyze the impact of different sensor accuracies on the performance of the proposed algorithm, four different simulation scenes were set up, and the sensor parameters were set as shown in Table 4. In Scene 1, the UGV was equipped with a high-accuracy IMU, a highaccuracy GNSS (RTK-corrected GNSS), and a low-accuracy UWB. The measurement noise of the kinematic filter was set to According to Figure 10, the kinematic and static filtering of the ESKF based on INS/GNSS/UWB proposed in this paper had good localization results in all four scenes, with different sensor accuracies. Because of the IMU, GNSS, and UWB on board the UGV in Scene 4 had the lowest accuracy, the UGV had a poorer localization effect in this scene than in the other three scenes. In the zoomed-in area of the straight-line running, the simulated trajectory of Scene 2 was closer to the real trajectory than those of Scene 1 and Scene 3, but the trajectory was not as smooth as the trajectory of the other two scenes. In the zoomed-in area for the turn run, the trajectory of Scene 3 was closer to the real value than the other three scenes, and the trajectories of Scene 1 and Scene 3 were relatively smooth compared to the other two scenes. The RMSE and MAE values for the four scenes are shown in Tables 5 and 6.  According to Tables 5 and 6, a comparison of the RMSE and MAE for Scene 1 and Scene 2 shows that the overall reduction in the attitude error for Scene 2 compared to Scene 1 was 9.19%, the overall reduction in the velocity RMSE for Scene 1 compared to Scene 2 was 40%, and the overall reduction in the position RMSE was 34.72%. The overall reduction in the pose error for Scene 2 was 8.94% compared to the overall reduction in the pose RMS error for Scene 1, the overall reduction in the velocity RMS error for Scene 1 compared to Scene 2 was 25%, and the overall reduction in the position RMS error was 32.76%. It can be deduced that compared to static filtering, the sensor accuracy of the dynamic filtering had a greater impact on the overall filtering accuracy. Because the algorithm proposed in this paper was used for sequential filtering, in the filtering design, the sensor with higher accuracy should enter into the overall filtering process first, as dynamic filtering.
Based on the analysis of the errors in Scene 3 in Tables 5 and 6, we learned that in the dynamic and static filtering based on the ESKF, the higher-accuracy GNSS (RKT corrected GNSS) and UWB had a better effect on the correction of the velocity and position of the  According to Tables 5 and 6, a comparison of the RMSE and MAE for Scene 1 and Scene 2 shows that the overall reduction in the attitude error for Scene 2 compared to Scene 1 was 9.19%, the overall reduction in the velocity RMSE for Scene 1 compared to Scene 2 was 40%, and the overall reduction in the position RMSE was 34.72%. The overall reduction in the pose error for Scene 2 was 8.94% compared to the overall reduction in the pose RMS error for Scene 1, the overall reduction in the velocity RMS error for Scene 1 compared to Scene 2 was 25%, and the overall reduction in the position RMS error was 32.76%. It can be deduced that compared to static filtering, the sensor accuracy of the dynamic filtering had a greater impact on the overall filtering accuracy. Because the algorithm proposed in this paper was used for sequential filtering, in the filtering design, the sensor with higher accuracy should enter into the overall filtering process first, as dynamic filtering.
Based on the analysis of the errors in Scene 3 in Tables 5 and 6, we learned that in the dynamic and static filtering based on the ESKF, the higher-accuracy GNSS (RKT corrected GNSS) and UWB had a better effect on the correction of the velocity and position of the low-accuracy IMU, and the overall positioning accuracy of the filter was higher than the positioning accuracy of the GNSS and UWB alone. However, as only the velocity and position information of the GNSS and UWB were used to correct the IMU during the observation of the dynamic and static filtering, which made its attitude errors accumulate gradually, the next step in the process was to equip the UGV with a dual-antenna GNSS, so that the GNSS could measure the heading (yaw) and course angle of the UGV, and then correct the attitude information of the IMU through the attitude angle obtained from the GNSS measurement. According to the analysis of the errors in Scene 4, we learned that the UGV can still show a relatively good positioning effect when equipped with a lowprecision IMU, GNSS, and UWB. Based on this observation, 10 Monte Carlo experiments were conducted in the four scenarios, and the experimental results are shown in Figure 11. sition information of the GNSS and UWB were used to correct the IMU during the observation of the dynamic and static filtering, which made its attitude errors accumulate gradually, the next step in the process was to equip the UGV with a dual-antenna GNSS, so that the GNSS could measure the heading (yaw) and course angle of the UGV, and then correct the attitude information of the IMU through the attitude angle obtained from the GNSS measurement. According to the analysis of the errors in Scene 4, we learned that the UGV can still show a relatively good positioning effect when equipped with a lowprecision IMU, GNSS, and UWB. Based on this observation, 10 Monte Carlo experiments were conducted in the four scenarios, and the experimental results are shown in Figure  11.

Analysis of Complex-Environment-Simulation Experiments
To verify the robustness of the algorithm proposed in this paper in complex environments, we selected the relevant parameters and measurement noise of the sensors in Scene 3 in Section 5.1 as the experimental conditions and set up two environments with errors and three filtering-solution schemes. In Environment 1, from 10 s to 40 s, there was a nonline-of-sight situation for the UWB and occlusion between the tag and the base station, leading to a reduction in its positioning accuracy, which developed as follows. In the simulation scenario, the systematic error of the UWB was 20 times the initial error (taking 20 times as an example), the velocity's systematic error was

Analysis of Complex-Environment-Simulation Experiments
To verify the robustness of the algorithm proposed in this paper in complex environments, we selected the relevant parameters and measurement noise of the sensors in Scene 3 in Section 5.1 as the experimental conditions and set up two environments with errors and three filtering-solution schemes. In Environment 1, from 10 s to 40 s, there was a non-line-of-sight situation for the UWB and occlusion between the tag and the base station, leading to a reduction in its positioning accuracy, which developed as follows. In the simulation scenario, the systematic error of the UWB was 20 times the initial error (taking 20 times as an example), the velocity's systematic error was R V UWB = 20R V UWB = 2 m/s, and the position's systematic error was R P UWB = 20R P UWB = 4 m. In Environment 2, from 66 s to 108 s, the GNSS was rejected and the receiver was located in the indoor scenario, resulting in a systematic error of the GNSS that was 20 times the initial error; the velocity's systematic error was R V GNSS = 20R V GNSS = 2 m/s and the position's systematic error was R P GNSS = 20R P GNSS = 4 m. The GNSS and UWB positioning systems were normal at all the other times.
In the setup of the filtering solution, ESKF-based federated filtering was used in Scheme 1. In the federated filtering, the two local filters were composed of INS/GNSS and INS/UWB, and the ESKF was used for the local filtering. To avoid matrix singularities in the inversion process due to the small values of the state vector and its covariance matrix in the ESKF, the local sensors were fused in the main filter in a feedback-free form, and the assignment factors of each local filter were β i = 1 2 (i = 1, 2). The kinematic and static filtering of the ESKF based on the INS/GNSS/UWB proposed in this paper was used in Scheme 2. Scheme 3, based on Scheme 2, introduced an evaluation system and switching strategy to block a particular sensor during the overall filtering in case its accuracy degraded.
Taking this experimental scheme as an example, in the period of 10~40 s, because of the presence of large errors in the UWB data, the UWB data were shielded; only kinematic filtering was performed, and the kinematic filtering solution was output as the navigation solution for the overall filtering. In the period of 66~108 s, because of the large errors in the GNSS data, the GNSS data were shielded, and the overall filtering was only updated in time in the kinematic filtering. The UWB data were used to measure and update in the static filtering, the error-state vector was fed back to the INS, and the feedback INS data were obtained, which were then output as the navigation solution of the overall filtering. A comparison of the solved trajectories for the three schemes is shown in Figure 12, and the velocity-error and position-error comparisons are shown in Figures 13 and 14, respectively. Scheme 2. Scheme 3, based on Scheme 2, introduced an evaluation system and switching strategy to block a particular sensor during the overall filtering in case its accuracy degraded. Taking this experimental scheme as an example, in the period of 10~40 s, because of the presence of large errors in the UWB data, the UWB data were shielded; only kinematic filtering was performed, and the kinematic filtering solution was output as the navigation solution for the overall filtering. In the period of 66~108 s, because of the large errors in the GNSS data, the GNSS data were shielded, and the overall filtering was only updated in time in the kinematic filtering. The UWB data were used to measure and update in the static filtering, the error-state vector was fed back to the INS, and the feedback INS data were obtained, which were then output as the navigation solution of the overall filtering. A comparison of the solved trajectories for the three schemes is shown in Figure  12, and the velocity-error and position-error comparisons are shown in Figure 13 and Figure 14, respectively.
.     According to Figures 13 and 14, the kinematic and static filtering of the ESKF based on INS/GNSS/UWB with the evaluation system and switching strategy had smaller localization errors for velocity and position than Scheme 1 and Scheme 2. The error variation was smoother, mainly because of the shielding of the contaminated GNSS and UWB, which in turn improved the overall localization accuracy of the filtering. Compared to the According to Figures 13 and 14, the kinematic and static filtering of the ESKF based on INS/GNSS/UWB with the evaluation system and switching strategy had smaller localization errors for velocity and position than Scheme 1 and Scheme 2. The error variation was smoother, mainly because of the shielding of the contaminated GNSS and UWB, which in turn improved the overall localization accuracy of the filtering. Compared to the federated filtering using the ESKF, the proposed algorithm had a higher overall positioning accuracy and a smoother error-variation curve. The RMSE and MAE of the three schemes are shown in Tables 7 and 8, respectively. According to Tables 7 and 8, Scheme 3 had the highest accuracy, as the dynamic and static filtering had good plug-and-play capability, which shielded the contaminated sensors more quickly and conveniently, thus improving the overall system accuracy. Comparing Scheme 1 with Scheme 2, the RMSE and the MAE of the kinematic and static filtering of the ESKF based on the INS/GNSS/UWB in terms of attitude, velocity, and position were smaller than those of the ESKF-based federated filtering. Therefore, the proposed method had good robustness.

Conclusions
In this paper, we proposed a fusion-navigation algorithm with ESKF kinematic and static filtering of based on INS/GNSS/UWB. The algorithm not only corrected the velocity and position errors of the INS in turn by using the GNSS and UWB, but also ensured that the state and kinematic vectors of the overall system were small, which improved the convergence speed of the filter and reduced the errors in the linearization process. The simulation and control experiments demonstrated that the method proposed in this paper demonstrated faster filter convergence and improved the positioning accuracy by 21.98% and 13.03% compared to the loosely coupled GNSS/INS and UWB/INS methods, respectively. According to the comparison of the error-accumulation curves over time for the three navigation methods, the main performance of the overall system was largely influenced by the accuracy and robustness of the sensors in the kinematic ESKF because the kinematic and static filtering based on the ESKF was a sequential form of fusion filter. Therefore, in the construction of the integral kinematic and static filtering process based on the ESKF, the INS should be corrected first with a more accurate and robust sensor in the kinematic ESKF, in order to improve the performance of the overall filter system. In addition, through the simulation of the sensor parameters and complex-environmentsimulation experiments, the proposed method was found to be applicable to sensors with different accuracies, with good generalization, plug-and-play, and robustness. In the next study, we will focus on improving the overall filtering-observation equation by equipping the UGV with multiple GNSS receivers so that the INS can be corrected using the GNSS heading (yaw) and course-angle information. We will also utilize additional sensors, such as visual odometers, binocular cameras, Bluetooth, etc., to optimize the data collected by the sensors through deep-learning algorithms, to improve the accuracy and robustness of the overall filtering.