一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法技术

本发明专利技术涉及一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法。本发明专利技术包括在计算时间更新时，设置初始值及罗德里格斯参数；计算容积点值；计算四元数误差点；计算四元数容积点；计算迭代四元数及迭代四元数误差；计算迭代容积点；估计一步预测状态；估计平方根因子预测误差协方差阵；计算测量更新中的容积点；计算迭代容积点值；计算预测测量估计；计算平方根因子新息协方差矩阵估计；计算互协方差阵；卡尔曼增益；状态更新估计；计算估计误差协方差阵及相应的平方根因子；四元数更新；计算相应的估计误差欧拉角。本发明专利技术能够提高对飞行器姿态的精确性与稳定性。

【技术实现步骤摘要】
一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法
本专利技术属于非线性系统的飞行器姿态估计问题，特别涉及一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法。
技术介绍
非线性滤波是信号处理、目标跟踪和控制领域方面的热门话题之一，特别是，在卡尔曼滤波框架下的非线性滤波研究仍是一个很火的问题，在近年来受到越来越多的关注和研究。众所周知，姿态估计的本质是典型的非线性滤波问题。传统的非线性滤波器不能直接用来处理大多情况下的飞行器姿态确定。这是因为可用的姿态估计器需要非线性滤波器能够很好地结合实际的姿态估计背景。那就是，姿态估计原则必须要整合到非线性滤波器的设计过程中。传统的非线性滤波方法包括扩展卡尔曼滤波器(EKF)和无迹卡尔曼滤波器(UKF)。EKF是现在在非线性系统中应用最为广泛的方法。通过近似状态分布为高斯随机变量，然后对其进行一阶泰勒展开来近似非线性函数。但不幸的是，这种线性化方法只能得到低的估计精度，甚至会造成估计器的不稳定性。同时，EKF需要计算雅克比矩阵，且计算量会很大。为了克服EKF的线性化，无迹卡尔曼滤波器(UKF)是一个处理非线性滤波时更为实用的方法。因为UKF能够直接使用非线性系统的模型从而取代使用EKF时的线性化，同时也避免了对雅克比矩阵的计算。UKF是通过非线性无迹变换(UT)和最小采样即sigma点。这种策略使得UKF能够明显改善EKF的性能，尽管UKF已经有很多的应用，但它还是有很多缺点，尤其是在高维情况下。同时UKF计算过程中对于协方差矩阵并不能总是保证是正定的。因此，UKF的使用仍然受到限制。
技术实现思路
本专利技术针对现有技术的不足，提供了一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法。本专利技术的方法具体是：1).设置初始值参数,假设飞行器姿态带有平方根因子估计误差协方差S0|0和飞行器姿态状态估计2).设置姿态迭代其中k表示时间。3).计算第i次容积点参量其中表示i个数，i＝1,2,...,m，Sk-1|k-1表示k-1时刻飞行器姿态带有平方根因子估计误差协方差阵，ξi表示列向量序列，表示k-1时刻飞行器姿态状态估计。4).计算飞行器姿态参数误差点其中代表三个欧拉角的误差值,即飞行器姿态方位角的误差值，表示角度余弦值的误差值。5).将计算得到的飞行器姿态参数误差点带入第i次四元数容积点其中，为飞行器姿态参数估计。6).迭代四元数预测估计及迭代四元数预测误差Ω为飞行器角度函数。7).迭代容积点其中，f为标量因子，a∈[0,1]。8).计算一步状态预测估计带有平方根因子的预测误差协方差为状态估计的均方差，为的平方根因子。9).将带有平方根因子的预测误差协方差及飞行器姿态k-1时刻的状态预测估计值带入测量更新容积点方程10).测量更新中的迭代容积点Zi,k|k-1＝h[Xi,k|k-1,k]。11).计算测量预测估计平方根因子新息协方差阵Szz,k|k-1＝Tria([Zk|k-1SR,k])、互协方差阵卡尔曼增益其中，SR，k表示为测量噪声方差Rk的平方根因子。12).从上面计算中得到状态更新估计平方根因子Sk|k、估计误差协方差阵Pk|k＝[Xk|k-1-KkZk|k-1KkSR,k][Xk|k-1-KkZk|k-1KkSR,k]T。13).计算更新四元数估计四元数估计误差14).判断是k否小于迭代次数N，如果k小于N，则跳转到步骤2，k＝k+1，否则，执行步骤15。15).计算中得到三个欧拉角度(飞行器的滚动角，俯仰角和偏航角)的一致估计误差值16).输出结果飞行器姿态方位角实现对飞行器姿态的估计。本专利技术的有益效果：使用平方根容积卡尔曼滤波器的飞行器姿态估计方法，不仅提高了飞行器姿态估计的精确度和稳定性，而且使用(正交三角形)QR分解，避免在求解平方根操作中，出现误差协方差阵的对称性和正定性性质的破坏。附图说明图1为本专利技术方法流程图。具体实施方式下面首先给出姿态运动学模型的回顾，其次给出带有陀螺偏置的陀螺仪模型，再次给出广义罗格里格斯参数描述，再次对测量方程的建立，最后平方根容积卡尔曼滤波器的飞行器姿态估计的滤波公式，下面详细介绍本专利技术的实施过程。1.姿态运动学模型给出四元数表达式：其中，q4＝cos(θ/2)(3)为对应于旋转轴的单位矢量和θ是旋转角度。从公式(2)和(3)中，我们可以得出结论四元数满足标准化约束四元数相关的姿态矩阵为：A(q)＝ΞT(q)ψ(q)(4)其中，其中，[q13×]是差乘矩阵，它定义为：四元数的逆定义为：这里，因为四元数满足标准化约束，因此四元数的逆也可以表示为：四元数的直乘定义为：四元数姿态运动学方程表示为：这里，ω是飞行器的角速率，且有式(12)的离散时间描述为：qk+1＝Ω(ωk)qk(14)其中，Φk＝sin(0.5||ωk||Δt)ωk/||ωk||(17)这里，Δt是陀螺仪的采样周期。为了对比，介绍修正的罗德里格斯参数姿态运动学方程：其中，修正的罗德里格斯参数p＝[pxpypz]T可以定义为：这里，a∈[0,1]并且f是以标量因子。比较式(14)和式(18)，我们可以看出式(18)的表达式更为复杂。而式(14)比较简单且容易计算并更新。2.陀螺仪模型陀螺仪模型描述其中，是陀螺仪的测量角度速率，ω是陀螺仪真实的角度速率，ηv是零均值高斯白噪声...

【技术保护点】
一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法，其特征在于该方法包括以下步骤：1).设置初始值参数，假设飞行器姿态带有平方根因子估计误差协方差S0|0和飞行器姿态状态估计2).设置姿态迭代其中k表示时间；3).计算第i次容积点参量其中表示i个数，i＝1，2，...，m，Sk‑1|k‑1表示k‑1时刻飞行器姿态带有平方根因子估计误差协方差阵，ξi表示列向量序列，表示k‑1时刻飞行器姿态状态估计；4).计算飞行器姿态参数误差点δq^i,k-1|k-1=δq^13,i,k-1|k-1Tδq^4,i,k-1|k-1T;]]>其中代表三个欧拉角，即飞行器姿态方位角，表示角度的余弦值；5).将计算得到的飞行器姿态参数误差点带入第i次四元数容积点q^i,k-1|k-1=δq^i,k-1|k-1⊗q^k-1|k-1,]]>其中，为飞行器姿态参数估计；6).迭代四元数预测估计及迭代四元数预测误差δq^i,k|k-1=q^i,k|k-1⊗[q^0,k|k-1]-1,]]>Ω为飞行器角度函数；7).迭代容积点其中，f为标量因子，a∈[0，1]；8).计算一步状态预测估计带有平方根因子的预测误差协方差为状态估计的均方差，为的平方根因子；9).将带有平方根因子的预测误差协方差及飞行器姿态k‑1时刻的状态预测估计值带入测量更新容积点方程10).测量更新中的迭代容积点Zi，k|k‑1＝h[Xi，k|k‑1，k]；11).计算测量预测估计平方根因子新息协方差阵Szz,k|k‑1＝Tria([Zk|k‑1 SR，k])、互协方差阵卡尔曼增益其中，SR，k表示为测量噪声方差Rk的平方根因子；12).从上面计算中得到状态更新估计平方根因子Sk|k、估计误差协方差阵Pk|k＝[Xk|k‑1‑KkZk|k‑1 KkSR，k][Xk|k‑1‑KkZk|k‑1 KkSR，k]T；13).计算更新四元数估计四元数估计误差14).判断是k否小于迭代次数N，如果k小于N，则跳转到步骤2，k：＝k+1，否则，执行步骤15；15).计算中得到三个欧拉角度(飞行器的滚动角，俯仰角和偏航角)的一致估计误差值16).输出结果飞行器姿态方位角实现对飞行器姿态的估计。...

【技术特征摘要】
1.一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法，其特征在于该方法包括以下步骤：1).设置初始值参数,假设飞行器姿态带有平方根因子估计误差协方差S0|0和飞行器姿态状态估计2).设置姿态迭代其中k表示时间；3).计算第i次容积点参量其中表示i个数，i＝1,2,...,m，Sk-1|k-1表示k-1时刻飞行器姿态带有平方根因子估计误差协方差阵，ξi表示列向量序列，表示k-1时刻飞行器姿态状态估计；4).计算飞行器姿态参数误差点其中代表三个欧拉角的误差值,即飞行器姿态方位角的误差值，表示角度余弦值的误差值；5).将计算得到的飞行器姿态参数误差点带入第i次四元数容积点其中，为飞行器姿态参数估计；6).迭代四元数预测估计及迭代四元数预测误差Ω为飞行器角度函数；7).迭代容积点其中，f为标量因子，a∈[0,1]；8).计算一步状态预测估计带有平方根因子的预测误差协方差为状态估计的均方差，为的平方根...

【专利技术属性】
技术研发人员：葛泉波，邵腾，程天发，文成林，
申请(专利权)人：杭州电子科技大学，
类型：发明
国别省市：浙江;33