[发明专利]一种基于无迹卡尔曼滤波的无人机组合导航方法和系统

权利要求书

1.一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，包括以下步骤：

步骤1，将无人机的状态变量生成为2n+1个原始sigma点，计算无人机状态变量的预测值和预测误差的协方差矩阵的加权平均值；

步骤2，通过无迹卡尔曼滤波器和k+1时刻各传感器的观测值对无人机状态变量的预测值进行估计，获得无人机状态变量的估计值和误差的协方差；

步骤2.1，通过协方差矩阵的加权平均值获得2n+1个新sigma点；

步骤2.2，通过测量方程对新sigma点进行传播；

步骤2.3，测量传播后的新的sigma点，进行加权平均，得到新sigma点的加权平均值；

步骤2.4，通过新sigma点的加权平均值计算量测的残差协方差矩阵；

步骤2.5，通过传播后的原始sigma点的加权平均值和新sigma点的加权平均值，计算无人机状态变量和测量变量的交叉协方差矩阵；

步骤2.6，通过残差协方差矩阵和交叉协方差矩阵计算卡尔曼增益系数；

步骤2.7，通过卡尔曼增益系数更新步骤1中的状态变量，获得无人机状态变量的估计值和误差的协方差；

步骤3，无人机运行期间不断重复步骤1和2，持续更新无人机的状态变量，对无人机进行导航。

2.根据权利要求1所述的一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，步骤1中，所述无人机的状态变量为：

其中：

[q₁ q₂ q₃ q₄]为无人机姿态的四元素；[v_x v_y v_z]为n系下的无人机三轴速度，其中v_x为北向的速度，v_y为东向的速度，v_z为地向的速度；

[p_x p_y p_z]为n系下的无人机三轴位置，其中，p_x为在北向的位置，p_y为在东向的位置，p_z为在地向的位置；

[gyro_bias_x gyro_bias_y gyro_bias_z]为陀螺仪在机体系下三轴零偏；

[acc_bias_x acc_bias_y acc_bias_z]为加速度计在机体系三轴零偏。

3.根据权利要求1所述的一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，步骤1的具体过程包括：

步骤1.1，将无人机的状态变量转变为2n+1个原始sigma点；

步骤1.2，计算2n+1个原始sigma点的权重；

步骤1.3，通过非线性状态方程获得传播后的原始sigma点；

步骤1.4，通过权重对传播后的原始sigma点进行加权平均处理，然后计算原始sigma点的协方差矩阵；

步骤1.5，在原始sigma点的协方差矩阵中添加过程噪声，然后进行加权平均，得到协方差矩阵的加权平均值。

4.根据权利要求3所述的一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，步骤1.2中，原始sigma点的权重计算公式为：

在实际使用中，取κ＝0，其中W⁰为初始sigma点的权重值，Wⁱ为过程sigma点的权重值。

5.根据权利要求3所述的一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，步骤1.4中，协方差矩阵为：

其中，为加权平均值，为传播后的sigma点；

步骤1.5中，协方差矩阵的加权平均值M_k为

Q_k-1为对角方阵，阶数为16维。