[发明专利]一种基于迭代容积卡尔曼滤波姿态估计方法

说明书

技术领域

本发明属于利用非线性滤波技术及迭代理论进行姿态估计的技术领域，特别涉及一种基于迭代容积卡尔曼滤波的姿态估计方法。

背景技术

飞行器姿态估计的精度直接影响飞行器姿态控制的精度，因此，姿态估计是姿态控制的关键技术之一。姿态估计方法主要分为两类：确定性算法和状态估计法。确定性算法的优点在于可以根据某一时刻的一组量测信息直接求解出该时刻下飞行器的姿态矩阵，算法简单，物理意义明确，但是需要知道当前时刻的至少两个独立的观测矢量，即准确的量测信息，而在实际应用中由于其他因素的干扰很难保证观测矢量的绝对准确。与这类方法相反，状态估计法是以状态空间模型为基础的递推滤波算法，可以将一些非姿态参数作为状态进行估计，一定程度上克服了不确定因素对姿态估计精度的影响，能够提高姿态估计的精度。

陀螺与星敏感器组成的非线性姿态估计系统由于测姿精度高，被广泛的用于飞行器中。欧拉角、修正罗德里格斯参数、方向余弦、四元数等是飞行器的主要姿态描述参数。扩展卡尔曼滤波（Extended Kalman Filter,MEKF）由于其结构简单、易于实现等优点被广泛应用于姿态估计中。但是，EKF存在一定的局限性：1）由模型线性化引入的截断误差会导致滤波精度下降，且需要计算较为复杂的雅克比矩阵；2）当初始状态误差较大或系统模型非线性程度较高时，会严重影响滤波精度甚至导致滤波发散。

发明内容

本发明的目的是为了提高姿态估计的精度，设计了一种基于迭代容积卡尔曼滤波姿态估计方法。

本发明是通过以下技术方案实现的：

一种基于迭代容积卡尔曼滤波姿态估计方法，其特征在于，包括以下几个步骤：

步骤一：采集飞行器运动过程中陀螺与星敏感器的输出数据作为量测量；

步骤二：确定系统的状态向量和量测向量；

步骤2.1建立飞行器姿态估计系统的非线性误差四元数方程，确定系统的状态向量；

非线性误差四元数方程为：

x·=Δρ·Δβ·=-[ω^×]Δρ+12[(Δβ+ηv)×]Δρ-12Δe(Δβ+ηv)ηu,Δe=1-||Δρ||22]]>