CN106500693A - 一种基于自适应扩展卡尔曼滤波的ahrs算法 - Google Patents

Abstract

本发明涉及导航定姿领域，尤其涉及姿态航向参考系统AHRS(attitude and heading reference system)中通过采用自适应卡尔曼滤波，解决不同动态下卡尔曼滤波观测噪声矩阵的确定问题，以及通过扩展卡尔曼滤波解决高动态场景下由于传统卡尔曼滤波线性化引入的误差，从而提高低成本AHRS的姿态测量精度。

Description

一种基于自适应扩展卡尔曼滤波的AHRS算法

技术领域

本发明涉及导航定姿领域，尤其涉及姿态航向参考系统AHRS(Attitude andHeading Reference System)中通过采用自适应卡尔曼滤波，解决不同动态下卡尔曼滤波观测噪声矩阵的确定问题，以及通过扩展卡尔曼滤波解决高动态场景下由于传统卡尔曼滤波线性化引入的误差，从而提高低成本AHRS的姿态测量精度。

背景技术

现代战争形势复杂多变，以位置、速度、姿态为核心信息的导航定位系统已经成为加快战争进程，决定战争胜负的关键因素。在现代导航系统中，GNSS(Global NavigationSatellite System)以其全球、全天候、高精度的优势成为目前应用最广泛的导航手段。INS(Inertial Navigation System)通过敏感载体的线性加速度信息以及角速度信息来提供载体的横滚、俯仰以及航向信息。GNSS和INS具有良好的互补性，两者的组合能够提供相较于单一系统更精确、可靠的导航定位结果。但是GNSS信号容易被干扰甚至被欺骗，除此之外如果信号长时间被遮挡，那么由惯导单独维持提供的导航信息也将随时间发散。例如在动中通领域，车辆装载的天线需要连续准确地指向卫星，当车辆通过较长的隧道时，由于山体对GNSS的遮挡，在GNSS/INS系统中由惯导单独维持的导航信息会随时间发散，当车辆驶离隧道时，天线定姿系统需要一定的收敛时间，无法满足天线快速定向的需求。

AHRS技术利用磁强计、加速度计以及陀螺仪通过测量地磁场、载体的线性加速度以及载体的角速度来提供载体的姿态信息。AHRS技术因为其不需要依赖于外部信息，具有较高的可靠性，因此已经广泛应用于航空航天领域、海洋探测领域、人体运动测量等应用领域。在航空航天领域中AHRS被广泛用于无人飞行器或者无人直升机的控制、稳定以及导航；在海洋探测领域中AHRS技术被广泛应用于远程操作车和水下自动设备的控制和稳定，稳定单元主要用于声波定位、回声等测量设备，也用于水下勘测和绘图；在人体测量领域，AHRS技术被广泛应用于真人全身运动姿态数据的采集，应用于体育、人体工程等。随着微机电加工技术的发展，MEMS(Micro-Electrical-Mechanical System)传感器得到了前所未有的发展，MEMS传感器以其价格低廉、重量轻、体积小以及功耗低等优势广泛地应用于AHRS系统中。

在AHRS技术中，通常通过卡尔曼滤波对磁强计、加速度计以及陀螺仪的数据进行融合。卡尔曼滤波的解算过程分为预测和更新两个过程，其中预测过程对状态量以及方差矩阵Q进行预测，在更新过程中，根据量测噪声R以及量测数据对观测量以及方差矩阵进行更新。在AHRS系统中，陀螺仪解算的姿态信息作为卡尔曼滤波的预测输入，而磁强计和加速度计的输出用来对陀螺仪计算的姿态信息进行更新。由陀螺仪测得的载体的角运动用来预测载体的横滚、俯仰以及航向信息，而加速度计和磁强计用来更新修正由陀螺预测得到的载体的姿态信息。

状态方程的建立和方差矩阵的设定是卡尔曼滤波设计实施中两个关键的步骤。状态方程的建立主要是构造预测方程的状态转移矩阵以及更新过程中量测噪声的系数矩阵。传统的噪声系数矩阵的确立通常根据产品手册中的产品参数进行设定，然而由于加工工艺的缺陷，同一款传感器的不同样本之间的性能存在差异，因此产品手册中的产品参数并不能准确地反映每个样本的传感器性能，除此之外MEMS传感器容易受到外部环境例如温度、湿度、气压等因素的影响，从而导致不同的应用场景下传感器的噪声水平出现差异，例如MEMS陀螺零偏随温度的变化在全温范围内可达数百deg/h,MEMS加速度计的零偏随温度的变化在全温范围内可达数百mGal，而陀螺和加速度计的比例因子和交轴耦合误差可达数百ppm，为了得到精确可靠地导航结果，卡尔曼滤波的方差阵需要准确实时地反映传感器噪声的差异，传统的AHRS算法将噪声矩阵作为固定的参数，并不能正确反映传感器误差随着载体动态以及应用场景的变化，因此影响姿态测量精度。本发明专利中根据载体的实际运动情况来实时调整卡尔曼滤波中反映加速度测量精度的噪声矩阵。当载体处于静止或匀速直线运动状态时，给予加速度计测量值较高的权重，而当载体处于加速或者减速状态时，由于加速度计的输出并不能准确地反映出载体的姿态信息，这时给予加速度计测量值较低的测量权重，从而克服传统卡尔曼滤波设计中，加速度计量测噪声不能随载体的运动状态进行自适应调整的问题，以提供更加稳定可靠地导航定位结果。传统AHRS算法中卡尔曼滤波无法根据运动场景的动态自适应调整观测量的噪声矩阵，同时由于状态转移矩阵的线性化引入较大的误差。

发明内容

本发明所要解决的问题为根据加速度计的输出来判断载体动态的大小，通过载体动态的大小来自适应调整卡尔曼滤波中观测量的噪声矩阵，同时引入扩展卡尔曼滤波解决在高动态环境下由于线性化导致的误差，从而提高低成本姿态航向参考系统的精度。

本发明采用的技术方案为：

一种基于自适应扩展卡尔曼滤波的AHRS算法，其特征在于，包括以下步骤：

步骤一：将AHRS系统保持静止，分别记录AHRS系统中加速度计的输出和陀螺的输出；根据加速度计的输出计算加速度计的零偏噪声参数，根据陀螺的输出分别计算陀螺的零偏噪声参数和陀螺的零偏；

步骤二：根据加速度计的零偏噪声参数设置卡尔曼滤波量测信息的初始噪声矩阵，根据陀螺的零偏噪声参数设置卡尔曼滤波的初始噪声协方差矩阵；并根据陀螺的零偏设置卡尔曼滤波的初始状态向量；

步骤三：根据陀螺的输出构建卡尔曼滤波的状态转移矩阵，根据构建的状态转移矩阵、初始状态向量以及初始噪声协方差矩阵预测卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵；

步骤四：构建卡尔曼滤波的量测方程矩阵，根据加速度计的输出和初始噪声矩阵计算本次历元的噪声矩阵，根据量测方程矩阵和本次历元的噪声矩阵更新卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵；

步骤五：根据更新后的卡尔曼滤波本次历元的状态向量补偿反馈陀螺的输出；将更新后的本次历元的状态向量、更新后的本次历元的噪声协方差矩阵和本次历元的噪声矩阵分别一一对应作为初始状态向量、初始噪声协方差矩阵和初始噪声矩阵，转入步骤三。

其中，步骤一中根据加速度计的输出计算加速度计的零偏噪声参数，根据陀螺的输出分别计算陀螺的零偏噪声参数和陀螺的零偏，具体计算方法如下：

式中，N为在静止时间段内的历元数量，为加速度计x,y,z三轴的输出或者陀螺x,y,z三轴的输出，σ_bg ²为陀螺的零偏噪声参数或加速度计的零偏噪声参数；

陀螺的零偏计算方法如下：

式中，b_gx0、b_gy0、b_gz0分别为陀螺x,y,z三轴的零偏，ω_x、ω_y、ω_z分别为陀螺x,y,z三轴的输出。

其中，步骤二中根据陀螺的零偏噪声参数设置卡尔曼滤波的初始噪声协方差矩阵并根据陀螺的零偏设置卡尔曼滤波的初始状态向量具体为：

卡尔曼滤波的状态向量X表示为：

X＝[q₀q_x q_y q_z b_gx b_gy b_gz]^T；

根据陀螺的零偏设置卡尔曼滤波的初始状态向量X₀：

X₀＝[1 0 0 0 b_gx0 b_gy0 b_gz0]^T；

式中，b_gx0、b_gy0、b_gz0为陀螺x,y,z轴的零偏，q₀ q_x q_y q_z所设定的四元数；

设置卡尔曼滤波方程的初始噪声协方差矩阵Q₀：

其中为陀螺x,y,z轴的零偏噪声参数，为四元数的噪声。

其中，步骤三具体包括以下步骤：

(301)根据陀螺的输出构建卡尔曼滤波的状态转移矩阵T_k，计算方法如下：

式中Δt代表了采样间隔，为状态向量的偏微分，具体表达形式如下：

式中，ω_x、ω_y、ω_z分别代表陀螺x,y,z三轴的输出，q₀ q_x q_y q_z所设定的四元数；

(302)根据构建的状态转移矩阵、初始状态向量以及初始噪声协方差矩阵预测卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵,计算公式为：

X_k＝T_kX_k-1

式中，X_k-1为上一历元的初始状态向量，Q_k-1为上一历元的初始噪声协方差矩阵，X_K为本次历元的预测状态向量，Q_k为本次历元的预测噪声协方差矩阵。

其中，步骤四具体包括以下步骤：

(401)构建卡尔曼滤波的量测方程矩阵H_k

在上式中g代表当地的重力大小，q₀ q_x q_y q_z所设定的四元数；

(402)计算量测信息z_k：

(403)计算本次历元的卡尔曼滤波量测信息的噪声矩阵R_k：

R_k＝R₀·SF

其中，

式中f_x、f_y、f_z分别为加速度计x,y,z三轴的输出，R₀为初始噪声矩阵；

(404)根据量测方程矩阵和本次历元的卡尔曼滤波量测信息的噪声矩阵更新卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵：计算公式为：

X_k+1＝X_k-K_k·(n_ki-z_k)

Q_k+1＝Q_k-K_kH_kQ_k

式中，X_k+1为更新后的本次历元的状态向量、Q_k+1为更新后的本次历元的噪声协方差矩阵，X_K为本次历元的预测状态向量，Q_k为本次历元的预测噪声协方差矩阵；为陀螺x,y,z三轴的输出。

其中，步骤五中根据更新后的卡尔曼滤波本次历元的状态向量补偿反馈陀螺的输出具体为：

补偿方法如下；

式中，依次代表陀螺x,y,z三轴的输出，依次代表补偿后的陀螺x,y,z三轴的输出，b_gx、b_gy、b_gz为本次历元的状态向量中陀螺x,y,z轴的零偏。

本发明与背景技术相比的优点为：

本发明专利利用导航开始之前静止的数据来计算卡尔曼滤波的参数，从而能更加准确地反映应用场景下传感器实际的参数，使模型更加准确，从而增加导航解算的可靠性和准确性；滤波在更新阶段采用重力矢量作为量测信息从而避免了量测方程的线性化引入的误差，有效增加了算法的运算效率和实时性能。

附图说明

图1为卡尔曼滤波结构框图

具体实施方式

下面结合具体实施例和附图对本发明做进一步的描述：

如附图1所示，本发明专利主要步骤具体如下：

步骤一：由于MEMS传感器误差容易受到外部环境尤其是温度的影响，因此卡尔曼滤波中的参数必须能准确地反映当前环境下的误差水平。在静止状态下MEMS陀螺和加速度计输出的波动水平反映了传感器的噪声水平，该噪声体现了传感器的性能。为了得到准确的误差系数，AHRS系统在导航开始前静止一段时间例如30秒钟，静止过程中分别记录加速度计x,y,z三轴输出和陀螺x,y,z三轴输出，同时记录参加计算的历元数量N。按照以下公式计算AHRS系统陀螺以及加速度计x,y,z轴的零偏噪声参数。

在以上两式中N代表在静止时间段内的采样数量，代表加速度计或者陀螺x,y,z三轴的输出。为传感器输出在静止状态下的零偏噪声参数，反映了静止状态下传感器的噪声水平，通过静止时间段内的数据计算得到AHRS系统中陀螺和加速度计x,y,z轴分别对应的零偏噪声参数其中用于设定传感器噪声协方差矩阵Q₀的初值。利用静止状态下的传感器的输出可以同时计算出陀螺x,y,z轴对应的零偏误差b_gx0、b_gy0、b_gz0，其零偏误差计算方法如下：

式中，b_gx0、b_gy0、b_gz0分别为陀螺x,y,z三轴的零偏，ω_x、ω_y、ω_z分别为陀螺x,y,z三轴的输出。

步骤二：根据加速度计的零偏噪声参数设置卡尔曼滤波量测信息的初始噪声矩阵，根据陀螺的零偏噪声参数设置卡尔曼滤波的初始噪声协方差矩阵；并根据陀螺的零偏设置卡尔曼滤波的初始状态向量；

卡尔曼滤波的状态向量X表示为：

X＝[q₀ q_x q_y q_z b_gx b_gy b_gz]^T；

根据陀螺的零偏设置卡尔曼滤波的初始状态向量X₀：

X₀＝[1 0 0 0 b_gx0 b_gy0 b_gz0]^T；

式中，b_gx0、b_gy0、b_gz0为陀螺x,y,z轴的零偏，q₀ q_x q_y q_z所设定的四元数；

设置卡尔曼滤波方程的初始噪声协方差矩阵Q₀：

其中为陀螺x,y,z轴的零偏噪声参数，为四元数的噪声。反映初始姿态角的不确定性，一般根据加速度计的噪声水平进行确定，该参数可以通过加速度计的速率随机游走系数(VRW)进行确定。

步骤三：根据陀螺的输出构建卡尔曼滤波的状态转移矩阵，根据构建的状态转移矩阵、初始状态向量以及初始噪声协方差矩阵预测卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵；

完成对卡尔曼滤波初始化后，进入对状态量的预测过程。预测过程包括预测状态向量和噪声协方差矩阵X_k、Q_k，具体方法如下：

(301)根据陀螺的输出构建卡尔曼滤波的状态转移矩阵T_k，计算方法如下：

式中Δt代表了采样间隔，为状态向量的偏微分，具体表达形式如下：

式中，ω_x、ω_y、ω_z分别代表陀螺x,y,z三轴的输出，q₀ q_x q_y q_z所设定的四元数；

(302)根据构建的状态转移矩阵、初始状态向量以及初始噪声协方差矩阵预测卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵,计算公式为：

X_k＝T_kX_k-1

式中，X_k-1为上一历元的初始状态向量，Q_k-1为上一历元的初始噪声协方差矩阵，X_K为本次历元的预测状态向量，Q_k为本次历元的预测噪声协方差矩阵。

步骤四：构建卡尔曼滤波的量测方程矩阵，根据加速度计的输出和初始噪声矩阵计算本次历元的噪声矩阵，根据量测方程矩阵和本次历元的噪声矩阵更新卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵；

完成了对卡尔曼滤波的预测后需要对卡尔曼滤波的状态矢量和噪声矩阵进行更新。更新过程即为利用量测信息对预测的信息进行修正的过程，在传统的卡尔曼滤波中，量测信息一般为由加速度计推算的载体的姿态角的四元数，即利用量测信息中的四元数进行更新。在该方法中，首先利用载体的姿态信息将测量得到的重力矢量转换到载体坐标系下，通过载体测量得到重力矢量与转换得到的重力矢量的差值作为卡尔曼滤波更新的新息，该方法减小了运算量，能够极大的提高算法的运行效率。步骤四具体包括以下步骤：

(401)构建卡尔曼滤波的量测方程矩阵H_k

在上式中g代表当地的重力大小，q₀ q_x q_y q_z所设定的四元数；

(402)计算量测信息z_k：

(403)计算本次历元的卡尔曼滤波量测信息的噪声矩阵R_k：

R_k＝R₀·SF

其中，

式中f_x、f_y、f_z分别为加速度计x,y,z三轴的输出，R₀为初始噪声矩阵；

(404)根据量测方程矩阵和本次历元的卡尔曼滤波量测信息的噪声矩阵更新卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵：计算公式为：

X_k+1＝X_k-K_k·(n_ki-z_k)

Q_k+1＝Q_k-K_kH_kQ_k

式中，X_k+1为更新后的本次历元的状态向量、Q_k+1为更新后的本次历元的噪声协方差矩阵，X_K为本次历元的预测状态向量，Q_k为本次历元的预测噪声协方差矩阵；为陀螺x,y,z三轴的输出。

步骤五：根据更新后的卡尔曼滤波本次历元的状态向量补偿反馈陀螺的输出；将更新后的本次历元的状态向量、更新后的本次历元的噪声协方差矩阵和本次历元的噪声矩阵分别一一对应作为初始状态向量、初始噪声协方差矩阵和初始噪声矩阵，转入步骤三，直至导航计算结束。

补偿方法如下；

式中，依次代表陀螺x,y,z三轴的输出，依次代表补偿后的陀螺x,y,z三轴的输出，b_gx、b_gy、b_gz为本次历元的状态向量中陀螺x,y,z轴的零偏。

在完成步骤五后便可进入下一历元的预测过程，以上步骤便完成了一次卡尔曼滤波的预测更新过程，在整个导航过程中，不断地进行卡尔曼滤波的循环计算，从而在整个导航过程中提供连续可靠地导航定姿信息。

Claims (6)

1.一种基于自适应扩展卡尔曼滤波的AHRS算法，其特征在于，包括以下步骤：

步骤一：将AHRS系统保持静止，分别记录AHRS系统中加速度计的输出和陀螺的输出；根据加速度计的输出计算加速度计的零偏噪声参数，根据陀螺的输出分别计算陀螺的零偏噪声参数和陀螺的零偏；

步骤二：根据加速度计的零偏噪声参数设置卡尔曼滤波量测信息的初始噪声矩阵，根据陀螺的零偏噪声参数设置卡尔曼滤波的初始噪声协方差矩阵；并根据陀螺的零偏设置卡尔曼滤波的初始状态向量；

步骤三：根据陀螺的输出构建卡尔曼滤波的状态转移矩阵，根据构建的状态转移矩阵、初始状态向量以及初始噪声协方差矩阵预测卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵；

步骤四：构建卡尔曼滤波的量测方程矩阵，根据加速度计的输出和初始噪声矩阵计算本次历元的噪声矩阵，根据量测方程矩阵和本次历元的噪声矩阵更新卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵；

步骤五：根据更新后的卡尔曼滤波本次历元的状态向量补偿陀螺的输出；将更新后的本次历元的状态向量、更新后的本次历元的噪声协方差矩阵和本次历元的噪声矩阵分别一一对应作为初始状态向量、初始噪声协方差矩阵和初始噪声矩阵，转入步骤三。

2.根据权利要求1所述的一种基于自适应扩展卡尔曼滤波的AHRS算法，其特征在于，步骤一中根据加速度计的输出计算加速度计的零偏噪声参数，根据陀螺的输出分别计算陀螺的零偏噪声参数和陀螺的零偏，具体计算方法如下：

$E\[n_k\]=\frac{1}{N}\underset{i=1}{\overset{N}{\Σ}}{n}_{k_i}$

${{\mathrm{\σ}}_{bg}}^2=\frac{1}{N}\underset{i=1}{\overset{N}{\Σ}}{(n_{k_i}-E\[n_k\])}^2$

式中，N为在静止时间段内的历元数量，为加速度计x,y,z三轴的输出或者陀螺x,y,z三轴的输出，σ_bg ²为陀螺的零偏噪声参数或加速度计的零偏噪声参数；

陀螺的零偏计算方法如下：

$b_{gx0}=\frac{1}{N}\underset{i=1}{\overset{N}{\Σ}}{\mathrm{\ω}}_x$

$b_{gy0}=\frac{1}{N}\underset{i=1}{\overset{N}{\Σ}}{\mathrm{\ω}}_y$

$b_{gz0}=\frac{1}{N}\underset{i=1}{\overset{N}{\Σ}}{\mathrm{\ω}}_z$

式中，b_gx0、b_gy0、b_gz0分别为陀螺x,y,z三轴的零偏，ω_x、ω_y、ω_z分别为陀螺x,y,z三轴的输出。

3.根据权利要求1所述的一种基于自适应扩展卡尔曼滤波的AHRS算法，其特征在于，步骤二中根据陀螺的零偏噪声参数设置卡尔曼滤波的初始噪声协方差矩阵并根据陀螺的零偏设置卡尔曼滤波的初始状态向量具体为：

卡尔曼滤波的状态向量X表示为：

X＝[q₀ q_x q_y q_z b_gx b_gy b_gz]^T；

根据陀螺的零偏设置卡尔曼滤波的初始状态向量X₀：

X₀＝[1 0 0 0 b_gx0 b_gy0 b_gz0]^T；

式中，b_gx0、b_gy0、b_gz0为陀螺x,y,z轴的零偏，q₀ q_x q_y q_z所设定的四元数；

设置卡尔曼滤波方程的初始噪声协方差矩阵Q₀：

$Q_0=\left(\begin{array}{ccccccc}{\mathrm{\σ}}_{q_s}^2& 0& 0& 0& 0& 0& 0\\ 0& {\mathrm{\σ}}_{q_x}^2& 0& 0& 0& 0& 0\\ 0& 0& {\mathrm{\σ}}_{q_y}^2& 0& 0& 0& 0\\ 0& 0& 0& {\mathrm{\σ}}_{q_z}^2& 0& 0& 0\\ 0& 0& 0& 0& {\mathrm{\σ}}_{b_{gx}}^2& 0& 0\\ 0& 0& 0& 0& 0& {\mathrm{\σ}}_{b_{gy}}^2& 0\\ 0& 0& 0& 0& 0& 0& {\mathrm{\σ}}_{b_{gz}}^2\end{array}\right)$

其中为陀螺x,y,z轴的零偏噪声参数，为四元数的噪声。

4.根据权利要求1所述的一种基于自适应扩展卡尔曼滤波的AHRS算法，其特征在于，步骤三具体包括以下步骤：

(301)根据陀螺的输出构建卡尔曼滤波的状态转移矩阵T_k，计算方法如下：

$T_k=I_{7\×7}+\frac{1}{2}{\[\frac{\∂f}{X}\]}_k\·\mathrm{\Δ}t$

式中Δt代表了采样间隔，为状态向量的偏微分，具体表达形式如下：

式中，ω_x、ω_y、ω_z分别代表陀螺x,y,z三轴的输出，q₀ q_x q_y q_z所设定的四元数；

(302)根据构建的状态转移矩阵、初始状态向量以及初始噪声协方差矩阵预测卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵,计算公式为：

X_k＝T_kX_k-1

$Q_k=T_k{Q}_{k-1}{T}_k^T$

式中，X_k-1为上一历元的初始状态向量，Q_k-1为上一历元的初始噪声协方差矩阵，X_K为本次历元的预测状态向量，Q_k为本次历元的预测噪声协方差矩阵。

5.根据权利要求1所述的一种基于自适应扩展卡尔曼滤波的AHRS算法，其特征在于，步骤四具体包括以下步骤：

(401)构建卡尔曼滤波的量测方程矩阵H_k

$H_k=\left[\begin{array}{ccccccc}2g\·q_y& -2g\·q_z& 2g\·q_0& -2g\·q_x& 0& 0& 0\\ -2g\·q_x& -2g\·q_0& 2g\·q_z& -2g\·q_y& 0& 0& 0\\ -2g\·q_0& -2g\·q_x& 2g\·q_y& -2g\·q_z& 0& 0& 0\end{array}\right]$

在上式中g代表当地的重力大小，q₀ q_x q_y q_z所设定的四元数；

(402)计算量测信息z_k：

$z_k=\left[\begin{array}{c}-2g\·(q_x\·q_z-q_0\·q_y)\\ -2g\·(q_0\·q_x-q_y\·q_z)\\ -g(q_0\·q_0-q_x\·q_x-q_y\·q_y+q_z\·q_z)\end{array}\right]$

(403)计算本次历元的卡尔曼滤波量测信息的噪声矩阵R_k：

R_k＝R₀·SF

其中，

$SF=\frac{norm\_f}{g}$

式中f_x、f_y、f_z分别为加速度计x,y,z三轴的输出，R₀为初始噪声矩阵；

(404)根据量测方程矩阵和本次历元的卡尔曼滤波量测信息的噪声矩阵更新卡尔曼滤波本次历元的状态向量和本次历元的噪声协方差矩阵：计算公式为：

$K_k=Q_k{H}_k^T{(H_k{Q}_k{H}_k^T+R_k)}^{-1}$

X_k+1＝X_k-K_k·(n_ki-z_k)

Q_k+1＝Q_k-K_kH_kQ_k

式中，X_k+1为更新后的本次历元的状态向量、Q_k+1为更新后的本次历元的噪声协方差矩阵，X_K为本次历元的预测状态向量，Q_k为本次历元的预测噪声协方差矩阵；为陀螺x,y,z三轴的输出。

6.根据权利要求1所述的一种基于自适应扩展卡尔曼滤波的AHRS算法其特征在于，步骤五中根据更新后的卡尔曼滤波本次历元的状态向量补偿反馈陀螺的输出具体为：

补偿方法如下；

${\widehat{\mathrm{\ω}}}_x={\widetilde{\mathrm{\ω}}}_x+b_{gx}$

${\widehat{\mathrm{\ω}}}_y={\widetilde{\mathrm{\ω}}}_y+b_{gy}$

${\widehat{\mathrm{\ω}}}_z={\widetilde{\mathrm{\ω}}}_z+b_{gz}$

式中，依次代表陀螺x,y,z三轴的输出，依次代表补偿后的陀螺x,y,z三轴的输出，b_gx、b_gy、b_gz为本次历元的状态向量中陀螺x,y,z轴的零偏。