一种基于SKF的四旋翼GNSS/SINS/动力学融合算法

本发明公开了一种基于SKF的四旋翼GNSS/SINS/动力学融合算法，适用于四旋翼无人机的动力学状态估计与定位。首先构建简化的四旋翼动力学模型，获取推力系数估计初值；其次推导了顾及推力系数误差的动力学误差状态传播方程；随后利用SKF算法更新动力学速度量测来估计动力学状态，提出了一种顾及外力估计不确定度的自适应动力学‑重力对齐约束方案，该约束通过将外力、电机推力与重力对齐来对系统进行水平姿态约束。结果表明：本发明保证了动力学不会损害GNSS/SINS系统定位性能，并且可以准确的估计室外场景下的四旋翼动力学状态，同时，本发明提出的动力学‑重力对齐约束显著提升了系统的水平姿态估计精度和定位性能，特别是在GNSS中断场景下。