Multilayer Low-Cost Sensor Local-Global Filtering Fusion Integrated Navigation of Small UAV

Aimed at improving the nonlinear integrated navigation solution performance of multiple low-cost sensors fusion, this paper presents a multilayer loosely-coupled, local-global, and step-optimized MF5DCKF (Multisensor Federated fifth-degree Cubature Kalman filter) state estimation algorithm for the small unmanned aerial vehicle (UAV). This method establishes a multilayer nonlinear integrated navigation model composed of the nonlinear attitude and heading reference system (AHRS) error model, strapdown inertial navigation system/global positioning system (SINS/GPS) error model, and strapdown inertial navigation system/barometer (SINS/BARO) error model to enhance the robustness and richness of the navigation module. Further, based on the above navigation models, a loosely-coupled error state fusion frame is designed to obtain the local convergent state vector. Simultaneously, a three-layer fifth-degree Cubature Kalman filter is proposed to improve the local state estimation accuracy. Subsequently, to optimize the estimated local state, this paper presents a novel distributed MF5DCKF scheme fusing the local state vector to calculate the global optimal state parameters in a step-optimized process. The experimental flight test results show that the proposed algorithm achieves a higher state solution accuracy and a better convergent performance compared with some conventional multisensor fusion algorithms. The new algorithm framework can provide applicability and reliability for the small UAV during the flight.