[发明专利]一种基于加速度的卡尔曼滤波姿态估计方法及系统

权利要求书

1.一种基于加速度的卡尔曼滤波姿态估计方法，其特征在于，所述卡尔曼滤波姿态估计方法具体包括以下步骤：

S1建立MEMS陀螺输出y_G,t模型：

y_G,t＝ω_t+b_t+v_G,t

其中，ω_t表示t时刻载体的真实转动角速率；b_t表示t时刻的偏置项；v_G,t表示t时刻的第一高斯白噪声；G表示MEMS陀螺输出模型；

S2建立MEMS加速度计输出y_A,t模块：

y_A,t＝^ba_t-^bg+v_A,t

其中，^ba_t表示载体坐标系下t时刻的运动加速度；^bg表示载体坐标系下重力场向量；v_A,t表示t时刻的第二高斯白噪声；A表示MEMS加速度计输出模型；

S3使用AR过程建立地理坐标系下的运动加速度ⁿa_t模型：

ⁿa_t＝c_a·ⁿa_t-1+v_A,t

其中，c_α表示衰减系数，且取值范围为0～1；ⁿa_t-1表示t-1时刻的运动加速度；v_A,t表示t时刻的第二高斯白噪声；

S4搭建卡尔曼滤波姿态估计系统进行数据融合，得到卡尔曼滤波姿态估计系统的状态方程

其中，系统误差状态向量表示系统误差状态向量X(t)的导数，表示东向欧拉失准角，表示北向欧拉失准角，表示天向欧拉失准角，b_x表示x轴陀螺零偏值，b_y表示y轴陀螺零偏值，b_z表示z轴陀螺零偏值；系统状态转移矩阵表示载体坐标系到地理坐标系的变换坐标矩阵；系统零均值白噪声W(t)＝[w_gxw_gyw_gz000]^T，且W～N(0,Q)，Q为系统噪声矩阵，w_gx、w_gy、w_gz分别为陀螺中x、y、z三个轴上的噪声；

S5测量卡尔曼滤波姿态估计系统得到量测方程Z(t)；

Z(t)＝H(t)X(t)+V(t)

其中，测量向量δg_E表示地理坐标系中东向重力场矢量与测量东向重力场矢量的差值，δg_N表示地理坐标系中北向重力场矢量与测量北向重力场矢量的差值；测量矩阵测量零均值白噪声V(t)～N(0，R)，R为测量噪声矩阵；

S6离散化所述状态方程X(t)和所述量测方程Z(t)；

X_k＝F_k/k-1X_k-1+W_k-1

Z_k＝H_kX_k+V_k

其中，k-1、k分别表示t＝k-1时刻和t＝k时刻；F_k/k-1为t＝k时刻到t＝k-1时刻状态一步转移矩阵；W_k-1表示t＝k-1时刻零均值白噪声；H_k为t＝k时刻的测量矩阵；V_k表示t＝k时刻的零均值白噪声；

S7分别对状态一步转移矩阵F_k/k-1和系统噪声矩阵Q(t_k)进行二阶近似：

其中，F_k/k-1表示t＝k时刻到t＝k-1时刻状态一步转移矩阵；I表示单位矩阵；滤波周期T＝t_k-t_k-1；F(t_k-1)表示t＝k-1时刻的系统状态转移矩阵；

其中，Q(t_k-1)表示t＝k-1时刻的系统噪声矩阵，F(t_k-1)表示t＝k-1时刻的系统状态转移矩阵；

S8循环迭代入卡尔曼滤波公式实现载体姿态的修正。