一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法
【专利摘要】本发明涉及一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法。本发明包括在计算时间更新时，设置初始值及罗德里格斯参数；计算容积点值；计算四元数误差点；计算四元数容积点；计算迭代四元数及迭代四元数误差；计算迭代容积点；估计一步预测状态；估计平方根因子预测误差协方差阵；计算测量更新中的容积点；计算迭代容积点值；计算预测测量估计；计算平方根因子新息协方差矩阵估计；计算互协方差阵；卡尔曼增益；状态更新估计；计算估计误差协方差阵及相应的平方根因子；四元数更新；计算相应的估计误差欧拉角。本发明能够提高对飞行器姿态的精确性与稳定性。
【专利说明】一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法
【技术领域】
[0001] 本发明属于非线性系统的飞行器姿态估计问题，特别涉及一种基于平方根容积卡 尔曼滤波器的飞行器姿态估计方法。
【背景技术】
[0002] 非线性滤波是信号处理、目标跟踪和控制领域方面的热门话题之一，特别是，在卡 尔曼滤波框架下的非线性滤波研究仍是一个很火的问题，在近年来受到越来越多的关注和 研究。
[0003] 众所周知，姿态估计的本质是典型的非线性滤波问题。传统的非线性滤波器不能 直接用来处理大多情况下的飞行器姿态确定。这是因为可用的姿态估计器需要非线性滤波 器能够很好地结合实际的姿态估计背景。那就是，姿态估计原则必须要整合到非线性滤波 器的设计过程中。传统的非线性滤波方法包括扩展卡尔曼滤波器（EKF)和无迹卡尔曼滤波 器（UKF)。EKF是现在在非线性系统中应用最为广泛的方法。通过近似状态分布为高斯随 机变量，然后对其进行一阶泰勒展开来近似非线性函数。但不幸的是，这种线性化方法只能 得到低的估计精度，甚至会造成估计器的不稳定性。同时，EKF需要计算雅克比矩阵，且计算 量会很大。为了克服EKF的线性化，无迹卡尔曼滤波器（UKF)是一个处理非线性滤波时更 为实用的方法。因为UKF能够直接使用非线性系统的模型从而取代使用EKF时的线性化， 同时也避免了对雅克比矩阵的计算。UKF是通过非线性无迹变换（UT)和最小采样即sigma 点。这种策略使得UKF能够明显改善EKF的性能，尽管UKF已经有很多的应用，但它还是有 很多缺点，尤其是在高维情况下。同时UKF计算过程中对于协方差矩阵并不能总是保证是 正定的。因此，UKF的使用仍然受到限制。
【发明内容】
[0004] 本发明针对现有技术的不足，提供了一种基于平方根容积卡尔曼滤波器的飞行器 姿态估计方法。
[0005] 本发明的方法具体是：
[0006] 1).设置初始值参数，假设飞行器姿态带有平方根因子估计误差协方差S+和飞 行器姿态状态估计夫。
[0007] 2).设置姿态迭代
【权利要求】
1. 一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法，其特征在于该方法包括 以下步骤： 1) .设置初始值参数，假设飞行器姿态带有平方根因子估计误差协方差S+和飞行器 姿态状态估计; 2) .设置姿态迭代
其中k表示时间； 3) .计算第i次容积点参量
，其中表示i个数，i = 1，2，...， m，表示k-Ι时刻飞行器姿态带有平方根因子估计误差协方差阵，ξ ,表示列向量序 列，表示k-Ι时刻飞行器姿态状态估计； 4) .计算飞行器姿态参数误差点
三个欧拉角，即飞行器姿态方位角，表示角度的余弦值； 5) .将计算得到的飞行器姿态参数误差点带入第i次四元数容积点
为飞行器姿态参数估计； 6) .迭代四元数预测估计
及迭代四元数预测误差
为飞行器角度函数； 7) .迭代容积点.
其中，f为标量因子，a e [〇，1]; 8) .计算一步状态预测估计、带有平方根因子的预测误差协方差
为状态估计的均方差，
的平方根因子； 9) .将带有平方根因子的预测误差协方差及飞行器姿态k-Ι时刻的状态预测估计值 毛η带入测量更新容积点方程
10) .测量更新中的迭代容积点
11) .计算测量预测估计^_、平方根因子新息协方差阵SzUHiTriaaZkh SK, k])、互协方差阵
、卡尔曼增益，其中，SK,k表 示为测量噪声方差Rk的平方根因子；
12) .从上面计算中得到状态更新估计
、平方根因子Sk|k、估 计误差协方差阵
13) .计算更新四元数估计4_、四元数估计误差的 14) .判断是k否小于迭代次数N，如果k小于N，则跳转到步骤2, k : = k+Ι，否则，执行 步骤15 ; 15) .计算中得到三个欧拉角度（飞行器的滚动角，俯仰角和偏航角）的一致估计误差 值
16).输出结果飞行器姿态方位角δ?Β#，实现对飞行器姿态的估计。
【文档编号】G01C21/20GK104121907SQ201410369663
【公开日】2014年10月29日 申请日期:2014年7月30日 优先权日:2014年7月30日
【发明者】葛泉波, 邵腾, 程天发, 文成林 申请人:杭州电子科技大学