[发明专利]一种基于IMU的自适应EKF姿态测量改进方法

权利要求书

1.一种基于IMU的自适应EKF姿态测量改进方法，其特征在于：包括以下步骤：

S1、建立拓展卡尔曼滤波模型；

S2、求取第一级量测噪声方差阵；

S3、求取第二级量测噪声方差阵；

S4、通过步骤S2、S3得到总量测噪声方差阵；

S5、将总量测噪声方差阵代入步骤S1得到的拓展卡尔曼滤波模型中即可。

2.如权利要求1所述的基于IMU的自适应EKF姿态测量改进方法，其特征在于：所述步骤S1中建立拓展卡尔曼滤波模型包括以下步骤：

S11、构建非线性系统方程，其计算公式为：

式中：q_k是系统状态向量；y_k是量测向量；f(·)和h(·)均为非线性向量函数；B_k-1是已知的系统结构参数；W_k-1是系统噪声，v_k是量测噪声；

S12、根据捷联式惯性导航系统理论，得到四元数的微分方程，其计算公式为：

式中：表示b系下b系相对于n系的三轴陀螺仪实时角速度；符号表示四元数乘法；

将式(9)转换为矩阵形式，其计算公式为：

式中，为b系相对于n系的角速度矩阵；

S13、采用毕卡逐次逼近法取有限项，得四元数一阶近似求解方法，计算系统的先验估计，其计算公式为：

计算得到状态方程中的雅克比矩阵，其计算公式为：

式中，Δt为采样时间；式(12)给出了系统的先验估计，因此系统状态上标有“-”，“-”表示系统实际状态未知，卡尔曼滤波器提供了一个估计值；

S15、计算先验噪声均方误差，其计算公式为：

式中：P_k-1为前一次滤波迭代的后验噪声均方误差；Q_k-1为系统噪声方差；

S16、n系下，加速度计水平静止摆放时，三轴加速度用向量表示为[a_x a_y a_z]^T＝[0 0 1]^T，计算得到量测预测矩阵，其计算公式为：

进而对中每一个自变量求一阶偏导数，计算得到量测方程中的雅克比矩阵，其计算公式为：

S17、由式(14)、(16)得卡尔曼增益，其计算公式为：

式中，R_k为总量测噪声方差；

S18、计算后验估计，其计算公式为：

式中：为后验估计；z_k为实际量测；将实际量测与量测预测之间的残差称为第k次量测获得的信息；

S19、计算后验均方误差P_k，其计算公式为：

式(12)、(14)、(17)、(18)、(19)构成了拓展卡尔曼滤波模型的基本方程。

3.如权利要求2所述的基于IMU的自适应EKF姿态测量改进方法，其特征在于：所述步骤S2中第一级量测噪声方差阵的计算公式为：

R_1(k)＝k_a(|||a||-1|)I_3*3 (22)；

式中：k_a为权重因子；||a||为k+1时刻规范化加速度值。

4.如权利要求3所述的基于IMU的自适应EKF姿态测量改进方法，其特征在于：所述步骤S3中求取第二级量测噪声方差阵包括以下步骤：

S31、计算得到初始第二级量测噪声方差阵，其计算公式为：

S32、计算得到指数渐消记忆加权平均递推公式，其计算公式为：

式中：x_k、y_k分别为系统输入和系统输出；β_k为加权系数；b为渐消记忆因子，一般取b＝0.9～0.999；

S33、将式(23)中1/k替换为β_k便得到最终第二级量测噪声方差阵，其计算公式为：

5.如权利要求4所述的基于IMU的自适应EKF姿态测量改进方法，其特征在于：所述步骤S4中总量测噪声方差阵的计算公式为：