一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法技术

本发明专利技术提出的一种Sage‑Husa自适应无迹卡尔曼滤波姿态数据融合方法，属于数字滤波和多传感器数据融合技术领域，主要用于提高载体姿态估计的精度。该方法以误差四元数无迹卡尔曼滤波作为框架，融合陀螺仪、加速度计及磁力计数据，通过对传感器误差分析建立较为准确的传感器测量模型，并结合误差四元数的方法以及UKF滤波算法实现传感器的数据融合。本发明专利技术适用于非线性姿态测量系统，具有良好的抗干扰性和姿态解算精度，可以有效解决复杂环境下载体姿态解算问题。

A Sage-Husa Adaptive Untracked Kalman Filter Attitude Data Fusion Method

【技术实现步骤摘要】
一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法
本专利技术提供的一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法，属于数字滤波和多传感器数据融合
，本专利技术提供的方法适用于非线性姿态测量系统。
技术介绍
载体的姿态测量系统实际上为一个非线性系统，为了实现对姿态的精确解算，需要采用非线性滤波融合陀螺仪、加速度计及磁力计的数据，但是由于传感器本身容易受到外界干扰导致传感器输出数据包含了较多的噪声，过程噪声和量测噪声都是未知且具有时变特性，错误的参数估计可能导致滤波发散，所以需要使用合适的滤波算法。因此，一种Sage-Husa自适应无迹卡尔曼滤波算法应运而生，该方法能够实现数据融合自适应调整量测噪声大小，抑制传感器随机漂移误差，自适应的补偿了非重力加速度对姿态估计检测的精度，相比较于传统算法，该算法具有更高的姿态解算精度。传统的无迹卡尔曼滤波(UnscentedKalmanFilter，UKF)算法通过无迹变换(UnscentedTransformation，UT)，用采样点的分布状态来近似估计概率密度的分布，这些点的均值和方差等于原状态分布的均值和方差。然而本文档来自技高网...

【技术保护点】
1.一种Sage‑Husa自适应无迹卡尔曼滤波姿态数据融合方法，其特征在于，具体包括以下步骤：步骤1：根据初始姿态角确定初始四元数值，选取四元数作为系统状态量并进行初始化，确定滤波初始化参数；步骤2：根据陀螺仪输出和陀螺仪漂移模型建立误差四元数无迹卡尔曼滤波状态方程；步骤3：以加速度计和磁力计输出作为观测量，建立误差四元数无迹卡尔曼滤波量测方程；步骤4：根据Sage‑Husa自适应滤波算法对系统量测噪声矩阵进行自适应估计，利用自适应估计得到的量测噪声矩阵及步骤2、步骤3得到的状态方程、量测方程进行系统状态更新和量测更新，得到量测更新后的误差四元数值；步骤5：根据步骤4中得到的误差四元数值求得四...

【技术特征摘要】
1.一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法，其特征在于，具体包括以下步骤：步骤1：根据初始姿态角确定初始四元数值，选取四元数作为系统状态量并进行初始化，确定滤波初始化参数；步骤2：根据陀螺仪输出和陀螺仪漂移模型建立误差四元数无迹卡尔曼滤波状态方程；步骤3：以加速度计和磁力计输出作为观测量，建立误差四元数无迹卡尔曼滤波量测方程；步骤4：根据Sage-Husa自适应滤波算法对系统量测噪声矩阵进行自适应估计，利用自适应估计得到的量测噪声矩阵及步骤2、步骤3得到的状态方程、量测方程进行系统状态更新和量测更新，得到量测更新后的误差四元数值；步骤5：根据步骤4中得到的误差四元数值求得四元数滤波值并对其进行归一化处理，利用四元数滤波值姿态解算方法求解得到姿态角。2.根据权利要求1所述的一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法，其特征在于步骤4中根据Sage-Husa自适应滤波算法对系统量测噪声矩阵进行自适应估计，并利用自适应估计得到k+1时刻的量测噪声矩阵Rk+1，对k+1时刻的量测输出协方差矩阵进行Sage-Husa自适应量测更新，具体步骤为：Step1：根据Sage-Husa自适应滤波算法对系统量测噪声矩阵Rk+1进...

【专利技术属性】
技术研发人员：周翟和，刘颀绮，程遵堃，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：江苏,32