一种基于迭代容积卡尔曼滤波姿态估计方法
【专利摘要】本发明公开了一种基于迭代容积卡尔曼滤波姿态估计方法，包括以下几个步骤：步骤一：采集陀螺与星敏感器的输出数据作为量测量；步骤二：确定状态向量和量测向量；步骤三：在k-1时刻利用迭代容积卡尔曼滤波估计出k时刻的误差四元数矢量部分以及陀螺漂移误差；步骤四：对于k时刻的估计求出四元数估计及陀螺漂移估计，对姿态及陀螺漂移进行校正，得到修正后k时刻姿态及陀螺漂移；步骤五：姿态估计非线性离散系统的运行时间为M，若k＝M，则输出姿态及陀螺漂移的结果；若k＜M，则重复步骤三至步骤四，直至姿态估计系统运行结束。本发明具有估计精度高、计算量少的优点。
【专利说明】一种基于迭代容积卡尔曼滤波姿态估计方法
【技术领域】
[0001]本发明属于利用非线性滤波技术及迭代理论进行姿态估计的【技术领域】，特别涉及一种基于迭代容积卡尔曼滤波的姿态估计方法。
【背景技术】
[0002]飞行器姿态估计的精度直接影响飞行器姿态控制的精度，因此，姿态估计是姿态控制的关键技术之一。姿态估计方法主要分为两类:确定性算法和状态估计法。确定性算法的优点在于可以根据某一时刻的一组量测信息直接求解出该时刻下飞行器的姿态矩阵，算法简单，物理意义明确，但是需要知道当前时刻的至少两个独立的观测矢量，即准确的量测信息，而在实际应用中由于其他因素的干扰很难保证观测矢量的绝对准确。与这类方法相反，状态估计法是以状态空间模型为基础的递推滤波算法，可以将一些非姿态参数作为状态进行估计，一定程度上克服了不确定因素对姿态估计精度的影响，能够提高姿态估计的精度。
[0003]陀螺与星敏感器组成的非线性姿态估计系统由于测姿精度高，被广泛的用于飞行器中。欧拉角、修正罗德里格斯参数、方向余弦、四元数等是飞行器的主要姿态描述参数。扩展卡尔曼滤波(Extended Kalman Filter, MEKF)由于其结构简单、易于实现等优点被广泛应用于姿态估计中。但是，EKF存在一定的局限性:1)由模型线性化引入的截断误差会导致滤波精度下降，且需要计算较为复杂的雅克比矩阵；2)当初始状态误差较大或系统模型非线性程度较高时，会严重影响滤波精度甚至导致滤波发散。
【发明内容】
[0004]本发明的目的是为了提高姿态估计的精度，设计了一种基于迭代容积卡尔曼滤波姿态估计方法。
[0005]本发明是通过以下技术方案实现的:
[0006]一种基于迭代容积卡尔曼滤波姿态估计方法，其特征在于，包括以下几个步骤:
[0007]步骤一:采集飞行器运动过程中陀螺与星敏感器的输出数据作为量测量；
[0008]步骤二:确定系统的状态向量和量测向量；
[0009]步骤2.1建立飞行器姿态估计系统的非线性误差四元数方程，确定系统的状态向量;
[0010]非线性误差四元数方程为:
[0011]
【权利要求】
1.一种基于迭代容积卡尔曼滤波姿态估计方法，其特征在于，包括以下几个步骤: 步骤一:采集飞行器运动过程中陀螺与星敏感器的输出数据作为量测量； 步骤二:确定系统的状态向量和量测向量； 步骤2.1建立飞行器姿态估计系统的非线性误差四元数方程，确定系统的状态向量； 非线性误差四元数方程为:
2.根据权利要求1所述的一种基于迭代容积卡尔曼滤波姿态估计方法，其特征在于:步骤二中，陀螺测量噪声标准差为σ v = 0.05° /h，陀螺漂移噪声标准差为σ； = 0-003 ，输出频率为IOOHz，星敏感器测量噪声标准差为σ s = 18"，输出频率为IHz，初始陀螺漂移β = [I I 1]T。/h，初始姿态角误差设定为[0.5 0.5 15]τ。。
3.根据权利要求1或2所述的一种基于迭代容积卡尔曼滤波姿态估计方法，其特征在于:步骤三中，滤波的初始姿态与陀螺漂移的估计值均设为零，初始方差阵分别设为(0.2。)2Ι3Χ3 和(1.2。/h)2I3X3，Nmax = 3。
【文档编号】G01C21/20GK103900574SQ201410136157
【公开日】2014年7月2日 申请日期:2014年4月4日 优先权日:2014年4月4日
【发明者】钱华明, 黄蔚, 沈忱, 孙龙, 富振铎, 彭宇 申请人:哈尔滨工程大学