CN105300379B

CN105300379B - 一种基于加速度的卡尔曼滤波姿态估计方法及系统

Info

Publication number: CN105300379B
Application number: CN201510670207.4A
Authority: CN
Inventors: 吕印新; 张家奇; 陈养彬
Original assignee: SHANGHAI NEW ERA ROBOTER Co Ltd
Current assignee: SHANGHAI NEW ERA ROBOTER Co Ltd
Priority date: 2015-10-13
Filing date: 2015-10-13
Publication date: 2017-12-12
Anticipated expiration: 2035-10-13
Also published as: CN105300379A

Abstract

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

Description

一种基于加速度的卡尔曼滤波姿态估计方法及系统

技术领域

本发明涉及姿态测量技术领域，尤其涉及一种卡尔曼滤波姿态估计方法及卡尔曼滤波姿态估计系统。

背景技术

在基于MEMS(Micro-Electro-Mechanical System，微机电系统)惯性传感器的姿态测量系统中，对扰动加速度的感知和处理是决定系统性能的关键。目前常用的方法有两种：

1)通过卡尔曼滤波新息进行判据；

v_k＝Z_k-H_kX_k/k-1

所谓卡尔曼滤波新息是指量测值与状态预测值的偏差，当卡尔曼滤波器工作正常且量测新息正确反应重力场矢量时，新息的模值为一小量，因而可设定适当的门限值对载体的运动加速度情况进行判定。

2)基于三轴加速度计输出的判别机制；当载体无运动加速度时，加速度计的输出满足

f＝(2ω_ie+ω_en)×v-g

其中，(2ω_ie+ω_en)×v在低速应用中的基本可以忽略，因而基于加速度计的判据可表示为：

当时，则认为此时无运动加速度；

当时，则认为此时存在运动加速度；

其中，f_x,y,z为三轴加速度计的输出，δ为根据加速度计噪声特性确定的一个很小的值。这样，通过以上加速度判别机制，进而调整扰动加速度存在时滤波器的量测噪声矩阵，亦或断开卡尔曼滤波的量测修正回路。

第一种方法的判别机制是较为严格的，但它不能区分新息超阈是由于扰动加速度的影响还是陀螺漂移误差造成的。这种情况在低成本MEMS陀螺中尤为严重。实际应用中由于陀螺漂移误差，在载体机动时间较长、温度变化较大或者初始零偏扣除不准确的情况下，新息将会迅速超出阈值并失效。因而基于此方法的卡尔曼滤波算法鲁棒性较差，系统对温度、机动时间等因素都较为敏感。

第二种方法中需要根据加速度计噪声特性设定合适的门限值，虽然不像第一种方法那样受诸多因素影响，但其仅为载体无运动加速度的必要条件。在实际应用过程中，存在诸多漏判的情况；以二维水平情况举例，当扰动运动加速度矢量a和重力矢量g的合成矢量模值和重力加速度g相等时即为漏判的情况；而当发生漏判时，系统的姿态将会被错误的量测信息迅速修坏，造成较大的姿态误差。

以上两个方法当扰动加速度进入卡尔曼滤波新息时，只能增大量测矩阵亦或屏蔽量测修正回路，而增大量测矩阵并不能抵消长时间机动对姿态造成的错误修正。

发明内容

针对上述问题，本发明旨在提供基于加速度的卡尔曼滤波姿态估计方法及卡尔曼滤波姿态估计系统，其将载体运动加速度引入卡尔曼姿态估计和修正中，提高了卡尔曼滤波姿态估计的精确度。

本发明提供的技术方案如下：

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

S1建立MEMS(Micro-Electro-Mechanical System，微机电系统)陀螺输出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(t)～N(0，Q(t))，Q(t)为系统噪声矩阵，w_gx、w_gy、w_gz分别为陀螺中x、y、z三个轴上的噪声；

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

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

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

S6离散化所述状态方程和所述测量方程；

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循环迭代入卡尔曼滤波公式实现载体姿态的修正。

在本技术方案中，偏置项b_t即为MEMS陀螺零偏噪声。

优选地，在步骤S1中，所述偏置项b_t使用一个受白噪声驱动的一阶马尔科夫过程表示：

b_t＝b_t-1+w_t,t

其中，b_t表示t时刻的偏置项，b_t-1表示t-1时刻的偏置项，w_t,t表示t时刻的第三高斯白噪声。

优选地，在步骤S2中，还包括以下步骤：

对MEMS加速度计输出y_A,t采用滑动平均的方式进行低通滤波。

优选地，在步骤S5中，得到测量方程具体包括以下步骤：

在地理坐标系下重力场矢量ⁿg为：

ⁿg＝[0 0 -1]^T

由步骤S2得到，载体坐标系下重力场矢量^bg为：

^bg＝^ba_t-y_A,t+ν_A,t

得到地理坐标系下测量重力场矢量ⁿg^-为：

其中，为陀螺积分姿态环节解算出的方向余弦矩阵，具体为：

其中，三轴欧拉失准角的反对称矩阵

故，通过地理坐标系下重力场矢量ⁿg与地理坐标系下测量重力场矢量ⁿg^-之间的差值得到系统的测量方程：

其中，δg_E表示地理坐标系中东向重力场矢量与测量东向重力场矢量的差值，δg_N表示地理坐标系中北向重力场矢量与测量北向重力场矢量的差值，δg_U表示地理坐标系中天向重力场矢量与测量天向重力场矢量的差值；

又，由于测量方程对于天向欧拉失准角不具备可观性，因此将对应的测量值删除，得到测量方程：

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

一种基于加速度的卡尔曼滤波姿态估计系统，所述卡尔曼滤波姿态估计系统应用于上述卡尔曼滤波姿态估计方法，所述卡尔曼滤波姿态估计系统中包括：MEMS陀螺、MEMS加速度计以及处理器，其中，

所述MEMS陀螺，用于测量载体坐标系下载体的真实转动角速度、陀螺高斯白噪声以及陀螺零偏噪声；

所述MEMS加速度计，用于测量载体坐标系下载体的运动加速度、载体坐标系下重力场矢量以及加速度计高斯白噪声；

所述处理器，分别与所述MEMS陀螺和所述MEMS加速度计连接，所述处理器对所述MEMS陀螺和所述MEMS加速度计的测量数据进行处理，实现对载体的卡尔曼滤波姿态估计和姿态修正。

优选地，所述MEMS陀螺为三轴陀螺，所述MEMS加速度计为三轴加速度计。

优选地，所述三轴陀螺和所述三轴加速度计集成在一惯性测量模块中。

优选地，所述处理器中包括：

载体运动状态判定模块，用于判断载体是处于运动状态还是处于静止状态；

陀螺零偏计算模块，与所述载体运动状态判定模块连接，所述陀螺零偏计算模块用于计算陀螺零偏噪声，包括MEMS陀螺三轴上的零偏噪声；

初始对准模块，与所述陀螺零偏计算模块连接，所述初始对准模块用于完成MEMS陀螺和MEMS加速度计的初始对准；

卡尔曼滤波模块，与所述初始对准模块连接，所述卡尔曼滤波模块对所述MEMS陀螺和所述MEMS加速度计的测量数据进行处理，实现对载体的卡尔曼滤波姿态估计；

姿态修正模块，与所述卡尔曼滤波模块连接，所述姿态修正模块根据卡尔曼滤波模块对载体的卡尔曼滤波姿态估计实现对载体的姿态修正。

通过本发明提供的基于加速度的卡尔曼滤波姿态估计方法及系统，能够带来以下有益效果：

在本发明中，以三轴欧拉失准角(及)和MEMS陀螺零偏(b_t)构造卡尔曼滤波状态方程，由于三轴欧拉失准角模型在GPS/SINS组合导航模型中的应用较为成熟，测量出来的三轴欧拉失准角非常精确；因而大大提高了本发明中卡尔曼姿态估计方法的精确度；

再有，在本发明搭建的卡尔曼滤波姿态估计系统中，由于测量方程对于天向欧拉失准角的不具备可观性，因而删除了该天向欧拉失准角测量值，将测量方程降为二维，减小了量测环路矩阵运算的计算量，缩短了运算的时间，提高了对载体姿态修正的时间。

附图说明

下面将以明确易懂的方式，结合附图说明优选实施方式，对上述特性、技术特征、优点及其实现方式予以进一步说明。

图1为本发明中基于加速度的卡尔曼滤波姿态估计方法的流程示意图；

图2为本发明中搭建的MEMS陀螺输出y_G,t模型和MEMS加速度计输出y_A,t模块的结构图；

图3为本发明中搭建的卡尔曼滤波姿态估计系统的结构图；

图4为本发明中基于加速度的卡尔曼滤波姿态估计系统结构示意图；

图5为本发明中处理器的结构示意图；

图6为本发明中处理器中流程示意图；

图7为本发明中具体实施例中本发明与基准MTI-300传感器中俯仰姿态角对比图；

图8为本发明中具体实施例中本发明与基准MTI-300传感器中横滚姿态角对比图。

10-MEMS陀螺，20-MEMS加速度计，30-处理器，31-载体运动状态判定模块，32-陀螺零偏计算模块，33-初始对准模块，34-卡尔曼滤波模块34，35-姿态修正模块。

具体实施方式

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对照附图说明本发明的具体实施方式。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，并获得其他的实施方式。

如图1所示为本发明提供的基于加速度的卡尔曼滤波姿态估计方法的流程示意图，首先我们对MEMS陀螺10和MEMS加速度计20进行建模和条件假设，其中，MEMS陀螺10的假定条件包括：a)MEMS陀螺10测量载体坐标系下载体的真实转动角速率、陀螺零偏以及高斯白噪声；b)在MEMS陀螺10测量的数据中，陀螺零偏(陀螺零偏噪声)的能量主要集中在低频，具体为一变化十分缓慢的直流分量。MEMS加速度计20的假定条件包括：c)受载体固有频率的约束，载体运动加速度是连续变化的，并且可被建模为一阶AR环节；d)MEMS加速度计20的输出中包含重力场矢量、运动加速度矢量以及高斯白噪声，其中，有用的重力场矢量其能量主要集中在低频段。以上的条件假设使得本发明提供的卡尔曼滤波姿态估计方法能够在惯性导航领域中广泛使用，从而扩展了本发明的应用。

从图中1可以看出，该卡尔曼滤波姿态估计方法具体包括以下步骤：

S1如图2所示，假定上述条件a)和条件b)成立，建立MEMS陀螺10输出y_G,t模型：

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

其中，ω_t表示t时刻载体的真实转动角速率；b_t表示t时刻的偏置项；v_G,t表示t时刻的第一高斯白噪声；G表示MEMS陀螺10输出模型。在该步骤中，偏置项b_t即为图示中的“陀螺零偏”，具体来说，当载体处于静止状态时，进行测量的计算得到该陀螺零偏。图示中的“载体转动”表示载体进行运转，通过“等效转动矢量算法”得到方向余弦矩阵(载体坐标系到地理坐标系的变换坐标矩阵)。

进一步来说，在步骤S1中，偏置项b_t可以使用一个受白噪声驱动的一阶马尔科夫过程表示：

b_t＝b_t-1+w_t,t

S2如图2所示，假定上述条件c)和条件d)成立，建立MEMS加速度计20输出y_A,t模块：

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

其中，^ba_t表示载体坐标系下t时刻的运动加速度(图示中的“载体运动加速度”)；^bg表示载体坐标系下重力场矢量；v_A,t表示t时刻的第二高斯白噪声；A表示MEMS加速度计20输出模型。再有，在步骤S2中，还包括以下步骤：对MEMS加速度计20输出y_A,t采用滑动平均的方式进行低通滤波。在具体实施例中，滑动平均的时间为50ms。在其他实施例中，也可以设定其他的滑动平均时间，如40ms等，根据实际情况进行设定。

S3如图2所示，使用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对MEMS陀螺10和MEMS加速度计20的数据进行了以上处理之后，如图3所示，随即搭建卡尔曼滤波姿态估计系统进行数据融合，得到卡尔曼滤波姿态估计系统的状态方程：

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

S5通过MENS陀螺和MEMS加速度计20解算出来的重力场矢量和实际重力场矢量的差值测量方程；

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

其中，测量向量δg_E表示地理坐标系中东向重力场矢量与测量东向重力场矢量的差值，δg_N表示地理坐标系中北向重力场矢量与测量北向重力场矢量的差值；测量矩阵测量零均值白噪声V(t)～N(0，R)，R为测量噪声矩阵(该测量噪声矩阵的强度由MEMS加速度计20中的第二高斯白噪声的强度决定)。

具体来说，在该步骤中，得到测量方程具体包括以下步骤：

在地理坐标系下重力场矢量ⁿg为：

ⁿg＝[0 0 -1]^T，

由步骤S2得到，载体坐标系下重力场矢量^bg为：

^bg＝^ba_t-y_A,t+ν_A,t，

得到地理坐标系下测量重力场矢量ⁿg^-为：

其中，三轴欧拉失准角的反对称矩阵

又，由于测量方程对于天向欧拉失准角不具备可观性，因此将对应的测量值删除，结合上述公式，得到上述测量方程：

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

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

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

Z_k＝H_kX_k+V_k

S8至此完成了离散卡尔曼滤波方程的推导，通过定时迭代卡尔曼滤波公式，并反馈估计出的欧拉失准角即可完成对姿态的修正。

如图4所示为本发明中提供的基于加速度的卡尔曼滤波姿态估计系统的结构示意图，应用于上述卡尔曼滤波姿态估计方法，具体来说，该卡尔曼滤波姿态估计系统中包括：MEMS陀螺10、MEMS加速度计20以及分别与MEMS陀螺10和MEMS加速度计20连接的处理器30，其中，MEMS陀螺10，用于测量载体坐标系下载体的真实转动角速度、陀螺高斯白噪声以及陀螺零偏噪声；MEMS加速度计20，用于测量载体坐标系下载体的运动加速度、载体坐标系下重力场矢量以及加速度计高斯白噪声；处理器30，分别与MEMS陀螺10和MEMS加速度计20连接，处理器30对MEMS陀螺10和MEMS加速度计20的测量数据进行处理，实现对载体的卡尔曼滤波姿态估计和姿态修正。

在具体实施例中，上述的MEMS陀螺10为三轴陀螺，上述MEMS加速度计20为三轴加速度计。且将三轴陀螺和三轴加速度计集成在一型号为ADIS16445的惯性测量模块中，该惯性测量模块内置三轴加速度计和三轴陀螺，其各个传感器的灵敏度、偏置、正交性和陀螺的g值灵敏性均进行了工厂校准，因而极大的方便了在实际应用中的系统集成。上述的处理器30采用型号为STM32F405的芯片实现，该STM32F405芯片是基于Cortex-M4内核的ARM架构的处理器，主频168MHz，具有高达1M字节的片上闪存和196K字节的内嵌SRAM，其单精度浮点运算单元(FPU，Float Point Unit)和增强的DSP(Digital Signal Processing，数字信号处理)处理指令完全满足了卡尔曼滤波过程中对实时性的需求。在其他实施例中，还可以使用其他型号的MEMS陀螺10、MEMS加速度计20以及处理器30，只要其能实现本发明的目的，都包括在本发明的内容中。

再有，在该具体实施例中，通过Keil开发环境编译嵌入式代码。通过DMA(DirectMemory Access，存储器直接访问)的方式对上述惯性测量模块的数据采集，减少高采样速率下对CPU资源的占用。在处理器30中，利用DSP_STD库函数进行卡尔曼滤波中相关的数据处理，且该DSP_STD库函数提供包括矩阵运算、快速三角函数计算等多种高效的数学函数封装，使用方便。

更进一步来说，如图5所示，上述处理器30中具体包括：载体运动状态判定模块31、陀螺零偏计算模块32、初始对准模块33、卡尔曼滤波模块34以及姿态修正模块35，其中，陀螺零偏计算模块32与载体运动状态判定模块31连接，初始对准模块33与陀螺零偏计算模块32连接，卡尔曼滤波模块34与初始对准模块33连接，姿态修正模块35与卡尔曼滤波模块34连接。在实际应用中，如图6所示，首先进行上电及对系统进行初始化，随后使用载体运动状态判定模块31判断载体是处于运动状态(图示中“动态”)还是处于静止状态(图示中“静态”)，若判断出载体处于运动状态，则继续对载体的运动状态进行判断，不做其他动作；若判断出载体处于静止状态，则使用陀螺零偏计算模块32根据MEMS陀螺10发送的测量数据计算陀螺零偏噪声，该陀螺零偏噪声具体包括MEMS陀螺10三轴上的零偏噪声。计算出了陀螺零偏噪声之后，即使用初始对准模块33完成MEMS陀螺10和MEMS加速度计20的初始对准，以为系统的工作做准备。紧接着，进行卡尔曼滤波初始化操作，并使用卡尔曼滤波模块34对MEMS陀螺10和MEMS加速度计20的测量数据进行处理，实现对载体的卡尔曼滤波。最后，使用姿态修正模块35根据卡尔曼滤波模块34对载体的卡尔曼滤波姿态估计进行反馈姿态修正操作，且如图所示，卡尔曼滤波模块34和姿态修正模块35在捷联姿态环路中进行循环实现对载体的姿态修正。

作为一个完整的实施例，基于以上卡尔曼滤波姿态估计系统，我们选用型号为MTI-300的传感器作为姿态基准，通过上位机同步采集并记录卡尔曼滤波姿态估计系统与MTI-300传感器中获取的姿态数据。

为了验证本发明中提供的系统和方法在车载高机动状态下的效果，在测试过程中，选用有密集的弯道和上下坡、并方便进行频繁的加减速和变道机动的道路进行测试。选用最大扭矩为172NM、最大马力为139PS及百公里加速9～10S的跑车。且在测试过程中，将上述卡尔曼滤波姿态估计系统与Xsens传感器均安装在跑车内。

整个测试过程持续30分钟，如图7和图8所示，从上位机中记录的本发明提供的卡尔曼滤波姿态估计方法和基准MTI-300传感器的俯仰、横滚姿态角对比图中可以定性的看出，使用发明提供的方法解算出的姿态角与基准MTI-300传感器的姿态角吻合性较好，且根据计算姿态差值的标准差在1度以内，故可知，本发明提供的卡尔曼滤波姿态估计系统和方法具有很好的实践意义。

应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims

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

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

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

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

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

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

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

<mrow> <mover> <mi>X</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>F</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>X</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mi>W</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow>

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

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

S6离散化所述状态方程和所述测量方程；

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

Z_k＝H_kX_k+V_k

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

2.如权利要求1所述的卡尔曼滤波姿态估计方法，其特征在于，在步骤S1中，所述偏置项b_t使用一个受白噪声驱动的一阶马尔科夫过程表示：

b_t＝b_t-1+w_t,t

3.如权利要求1所述的卡尔曼滤波姿态估计方法，其特征在于，在步骤S2中，还包括以下步骤：

对MEMS加速度计输出y_A,t采用滑动平均的方式进行低通滤波。

4.如权利要求1或2或3所述的卡尔曼滤波姿态估计方法，其特征在于，在步骤S5中，得到测量方程具体包括以下步骤：

在地理坐标系下重力场矢量ⁿg为：

ⁿg＝[0 0 -1]^T

由步骤S2得到，载体坐标系下重力场矢量^bg为：

^bg＝^ba_t-y_A,t+ν_A,t

得到地理坐标系下测量重力场矢量ⁿg^-为：

<mrow> <msup> <mmultiscripts> <mi>g</mi> <mi>n</mi> </mmultiscripts> <mo>-</mo> </msup> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mrow> <mi>n</mi> <mo>-</mo> </mrow> </msubsup> <mo>&CenterDot;</mo> <mmultiscripts> <mi>g</mi> <mi>b</mi> </mmultiscripts> </mrow>

<mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mrow> <mi>n</mi> <mo>-</mo> </mrow> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mrow> <mi>n</mi> <mo>-</mo> </mrow> </msubsup> <mo>&CenterDot;</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> </mrow>

<mrow> <msubsup> <mi>C</mi> <mi>n</mi> <mrow> <mi>n</mi> <mo>-</mo> </mrow> </msubsup> <mo>=</mo> <mi>I</mi> <mo>-</mo> <msub> <mi>&psi;</mi> <mo>&times;</mo> </msub> </mrow>

其中，三轴欧拉失准角的反对称矩阵

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

5.一种基于加速度的卡尔曼滤波姿态估计系统，其特征在于，所述卡尔曼滤波姿态估计系统应用于如权利要求1-4任意一项所述的卡尔曼滤波姿态估计方法，所述卡尔曼滤波姿态估计系统中包括：MEMS陀螺、MEMS加速度计以及处理器，其中，

6.如权利要求5所述的卡尔曼滤波姿态估计系统，其特征在于：所述MEMS陀螺为三轴陀螺，所述MEMS加速度计为三轴加速度计。

7.如权利要求6所述的卡尔曼滤波姿态估计系统，其特征在于：所述三轴陀螺和所述三轴加速度计集成在一惯性测量模块中。

8.如权利要求6或7所述的卡尔曼滤波姿态估计系统，其特征在于，所述处理器中包括：