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

摘要

本发明提供了一种基于加速度的卡尔曼滤波姿态估计方法及系统，其中，该卡尔曼滤波姿态估计方法中包括以下步骤：S1建立MEMS陀螺输出y_G,t模型；S2建立MEMS加速度计输出y_A,t模块；S3使用AR过程建立地理坐标系下的运动加速度ⁿa_t模型；S4搭建卡尔曼滤波姿态估计系统进行数据融合，得到卡尔曼滤波姿态估计系统的状态方程S5测量卡尔曼滤波姿态估计系统得到量测方程Z(t)；S6离散化所述状态方程X(t)和所述量测方程Z(t)；S7分别对状态一步转移矩阵F_k/k-1和系统噪声矩阵Q(t_k)进行二阶近似；统状态转移矩阵；S8循环迭代入卡尔曼滤波公式实现载体姿态的修正。其将载体运动加速度引入卡尔曼姿态估计和修正中，大大提高了本发明中卡尔曼姿态估计方法的精确度。

主权项

一种基于加速度的卡尔曼滤波姿态估计方法，其特征在于，所述卡尔曼滤波姿态估计方法具体包括以下步骤：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_gx w_gy w_gz 0 0 0]^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‑1Z_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 循环迭代入卡尔曼滤波公式实现载体姿态的修正。