[发明专利]基于MEMS惯性传感器的载体动态姿态估计方法

权利要求书

1.基于MEMS惯性传感器的载体动态姿态估计方法，其特征在于，包括以下步骤：

获取三轴加速度计数据并通过加速度计噪声模型进行处理；

获取三轴陀螺仪数据并通过陀螺仪噪声模型进行处理；

将以上三轴陀螺仪处理后的数据和三轴加速度计数据处理后的数据通过EKF滤波器进行处理，求取姿态角，更新姿态信息；

所述EKF滤波器进行多传感信息融合包括以下步骤：构建卡尔曼滤波器系统模型：

其中：x(k)∈Rⁿ表示k时刻系统的状态矢量；z(k)∈R^m表示k时刻系统的观测矢量；f(·)为n维矢量函数，h(·)为m维矢量函数，f(·)和h(·)对其自变量都是非线性的；w(k)∈Rⁿ和v(k)∈R^m是协方差分别为Q(k)和R(k)的过程噪声矢量；

建立EKF滤波器方程：包括时间更新、量测更新；

其中，时间更新包括系统状态量的先验估计和预测误差协方差矩阵的先验估计；

量测更新包括系统的卡尔曼增益、系统状态量的后验估计和预测误差协方差矩阵的后验估计；

设系统的状态向量为：

x(k)＝(a_bx(k)a_by(k)a_bz(k)ω_x(k)ω_y(k)ω_z(k)b_ωx(k)b_ωy(k)bωz(k))^T

其中，a_bx(k) a_by(k) a_bz(k)表示载体坐标系中三轴加速度测量值，ω_x(k) ω_y(k) ω_z(k)表示载体坐标系中三轴陀螺仪测量值，b_ωx(k) b_ωy(k) b_ωz(k)分别为绕滚转轴、俯仰轴和航向轴的陀螺仪随机漂移向量，则所述系统的状态方程为：

对f(x(k-1)，(k-1))求取Jacobi矩阵：

由状态转移函数可得状态的一步预测值：

计算状态向量一步预测误差方差矩阵：

P(k，k-1)＝Φ(k，k-1)P(k-1)Φ^T(k，k-1)+Q(k)；

设系统的观测向量为z(k)＝(ω_bx(k) ω_by(k) ω_bz(k))^T，其中，ω_bx(k) ω_by(k) ω_bz(k)表示载体坐标系中三轴陀螺仪测量值，则所述系统的观测方程为：

对h(x(k)，k)求取Jacobi矩阵，可得系统量测矩阵为：

由量测函数可得量测向量的一步预测值：

将所述EKF滤波器输出数据和修正后的加速度计输出数据进行融合。

2.根据权利要求1所述的基于MEMS惯性传感器的载体动态姿态估计方法.

计算状态增益矩阵：

K(k)＝P(k，k-1)H^T(k)[H(k)P(k，k-1)H^T(k)+R(k)]^-1。

3.根据权利要求2所述的基于MEMS惯性传感器的载体动态姿态估计方法，其特征在于，K时刻的状态向量估计值为：

4.根据权利要求3所述的基于MEMS惯性传感器的载体动态姿态估计方法，其特征在于，更新状态误差协方差矩阵为：

P(k)＝[I-K(k)H(k)]P(k，k-1)[I-K(k)H(k)]T+K(k)R(k)K^T(k)。

5.根据权利要求1所述的基于MEMS惯性传感器的载体动态姿态估计方法，其特征在于，过程噪声协方差为：

其中，为加速度计测量噪声协方差矩阵；为量测噪声协方差矩阵；为陀螺仪漂移噪声的协方差矩阵；

量测噪声协方差矩阵为定常数对角阵。