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).计算一步状态预测估计 带有平方根因子的预测误差协方差为状态估计的均方差, 为 的平方根因子;
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).输出结果飞行器姿态方位角 实现对飞行器姿态的估计。