[发明专利]一种自适应四元数粒子滤波姿态数据融合方法

摘要

本发明提出的一种自适应四元数粒子滤波姿态数据融合方法，属于数字滤波和多传感器数据融合技术领域，主要用于解决四旋翼飞行过程中存在干扰时，无法获得准确姿态角的问题。该方法以四元数粒子滤波为框架，融合陀螺仪、加速度计及磁力计数据。针对干扰造成的传感器异常观测值，采用广义似然比检测方法检测并定位干扰，调整相应传感器的量测噪声方差，进而滤除干扰，解算出精确的姿态角。本发明适用于四旋翼姿态测量系统，具有良好的抗干扰性，能够为四旋翼飞行器的稳定飞行提供保障。

主权项

1.一种自适应四元数粒子滤波姿态数据融合方法，其特征在于，具体包括以下步骤：步骤1：根据初始观测值及当地重力加速度矢量获取初始四元数粒子；步骤2：以四元数为系统状态量，采用陀螺仪数据建立四元数微分方程作为系统状态方程，以加速度计及磁力计的输出为观测量建立量测方程；步骤3：采用步骤2中建立的状态方程对粒子进行状态更新；步骤4：利用广义似然比检测(GLRT)判断加速度计，磁力计输出是否存在异常观测值，若传感器观测值正常，则进行量测更新，否则跳转至步骤5；步骤5：判断异常观测值由何种干扰造成，调整相应传感器的噪声方差矩阵，进行量测更新；步骤6：根据量测更新得到的权值，求取最优四元数估计值，并将其转换为姿态角。