[发明专利]一种航向快速估计方法

权利要求书

1.一种航向快速估计方法，该方法通过控制无人机进行S弯机动，使卡尔曼组合导航滤波器状态变量中的航向误差可观性得到极大提高，加快卡尔曼组合导航滤波的航向收敛速度；同时增加滤波器中的协方差阵Q的每一个数值，从而增加了外部观测量的权重，导致增益矩阵K增大，进一步加快了航向收敛速度，实现一个机动条件下的航向粗对准，最终完成航向快速估计，其特征在于，该方法具体步骤为：

步骤1：无人机根据故障检测发现机载航向传感器故障或长时间无有效航向输入，开始航向快速估计方法；

步骤2：飞行控制器发送姿态控制指令，控制无人机做S弯机动，同时将卡尔曼组合导航滤波器中协方差阵Q增大为原始值的3-5倍；

步骤3：在无人机做S弯机动过程中，采用根据步骤2调整后协方差阵Q数值的卡尔曼组合导航滤波器对所述无人机的航向进行实时校准；

具体步骤为：

S3.1:k-1时刻，卡尔曼组合导航滤波器通过一步转移矩阵Ф_k,k-1估计系统k时刻的状态估计，

S3.2:根据协方差阵Q、观测量协方差阵R、k-1时刻系统协方差阵P_k,k-1，计算k时刻系统的增益矩阵K_k，结合当前时刻的系统观测值Z_k完成对进行修正，更新系统协方差阵P_k，继而迭代进行k+1时刻的系统状态估计；

步骤4：当无人机做S弯机动结束后，将卡尔曼组合导航滤波器中协方差阵Q中的增大后数值恢复到卡尔曼组合导航滤波器的初始设定值，完成对航向的校准，即完成航向快速估计。

2.根据权利要求1所述的方法，其特征在于，所述S弯机动为三轴摇摆运动与线运动组合。