[发明专利]一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法

摘要

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

主权项

一种基于平方根容积卡尔曼滤波器的飞行器姿态估计方法，其特征在于该方法包括以下步骤：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).输出结果飞行器姿态方位角实现对飞行器姿态的估计。