CN105865453A - 一种位置传感器和姿态传感器的导航系统及其融合方法 - Google Patents

Abstract

本发明公开了一种位置传感器和姿态传感器的导航系统及其融合方法，将互补滤波与卡尔曼滤波相结合，简称互补卡尔曼滤波，将经过低通滤波后GPS/气压计测量的位置和速度信息，高通滤波后加速度估算的位置和速度信息作为卡尔曼滤波器的输入，通过卡尔曼滤波自动调节互补滤波的权值，有效的解决手动调节互补滤波的权值的难题，实现将GPS位置信息与姿态传感器的加速度信息融合输出横向位置与速度信息，将气压计高度信息与姿态传感器输出的z轴加速度进行融合并输出z轴位置和速度信息。融合后的输出的位置和速度信息和融合前普通GPS和气压计输出的位置信息相比更加精确，并且能提供较精确的速度信息。

Description

一种位置传感器和姿态传感器的导航系统及其融合方法

技术领域

本发明涉及一种位置传感器和姿态传感器的导航系统及其融合方法，属于导航定位与控制技术领域。

背景技术

利用GPS定位卫星，在全球范围内实时进行定位、导航的系统，称为全球卫星定位系统，简称GPS。姿态传感器是以陀螺和加速度计为敏感器件的导航参数解算系统，该系统根据陀螺的输出建立导航坐标系，输出运载体在导航坐标系中的加速度和欧拉角。气压计根据托里拆利的实验原理而制成，用以测量大气压强的仪器，再通过对大气压强与温度的计算得到高度，输出z轴高度信息。

目前GPS可提供导航信息，但是由于较高精度的GPS价格偏贵，成本太高。并且气压计高度输出不稳，读数在20cm内浮动，不利于定位。本发明使用成本较低的GPS、航姿传感器及气压计，将三者信息融合后输出运载体(比如飞行器)在地球坐标系的位置、速度、高度信息。

发明内容

本发明所要解决的技术问题是提供一种位置传感器和姿态传感器的导航系统及其融合方法，该导航系统将GPS位置信息与姿态传感器的加速度信息融合后输出运载体的横向位置与速度信息。同理，将气压计高度信息与姿态传感器输出的z轴加速度进行融合后输出运载体的z轴位置和速度信息。融合后的输出的位置、速度信息和融合前普通GPS和气压计输出的位置信息相比更加精确，并且能提供较精确的速度信息。

本发明为解决上述技术问题采用以下技术方案：

一方面，本发明提供一种位置传感器和姿态传感器信息融合方法，将互补滤波与卡尔曼滤波相结合，将经过低通滤波后位置传感器测量的位置和速度信息、高通滤波后加速度估算的位置和速度信息作为卡尔曼滤波器的输入，位置传感器测量的位置信息和姿态传感器测量的加速度信息经过互补滤波与卡尔曼滤波后进行融合，得到融合后的位置信息和速度信息：

1)融合后的速度信息通过以下方程计算得到：

v(k|k-1)＝v(k-1|k-1)+a_g(k)*dt

P₁(k|k-1)＝P₁(k-1|k-1)+Q

$K_1\left(k\right)=\frac{P_1\left(k\right|k-1)}{P_1\left(k\right|k-1)+R_1}$

$v\left(k\right|k)=(1-\frac{K_1\left(k\right)}{z+K_1\left(k\right)})*v\left(k\right|k-1)+\frac{K_1\left(k\right)}{z+K_1\left(k\right)}*v_{before}\left(k\right)$

式中，v(k|k-1)为根据加速度信息计算得到的k时刻速度信息，v(k-1|k-1)为k-1时刻融合后的速度信息，a_g(k)为k时刻姿态传感器测量的加速度信息，dt为k-1时刻和k时刻之间的时间间隔，P₁(k|k-1)为k时刻根据加速度信息计算得到的速度信息的协方差，P₁(k-1|k-1)为k-1时刻根据加速度信息计算得到的速度信息的协方差，Q为加速度信息的协方差，K₁(k)为第一卡尔曼滤波增益，v_before(k)为k时刻位置传感器测量的速度信息，R₁为v_before(k)的协方差；v(k|k)为k时刻融合后的速度信息；

2)融合后的位置信息通过以下方程计算得到：

$L\left(k\right|k-1)=L(k-1|k-1)+v_{after}(k-1)*dt+\frac{1}{2}*a_g\left(k\right)*dt*dt$

P₂(k|k-1)＝P₂(k-1|k-1)+Q

$K_2\left(k\right)=\frac{P_2\left(k\right|k-1)}{P_2\left(k\right|k-1)+R_2}$

$L\left(k\right|k)=(1-\frac{K_2\left(k\right)}{z+K_2\left(k\right)})*L\left(k\right|k-1)+\frac{K_2\left(k\right)}{z+K_2\left(k\right)}*L_{before}\left(k\right)$

式中，L(k|k-1)为根据加速度信息计算得到的k时刻位置信息，L(k-1|k-1)为k-1时刻融合后的位置信息，v_after(k-1)为k-1时刻融合后的速度信息，P₂(k|k-1)为k时刻根据加速度信息计算得到的位置信息的协方差，P₂(k-1|k-1)为k-1时刻根据加速度信息计算得到的位置信息的协方差，K₂(k)为第二卡尔曼滤波增益，L_before(k)为k时刻位置传感器测量的位置信息，R₂为L_before(k)的协方差；L(k|k)为k时刻融合后的位置信息。

另一方面，本发明提供一种位置传感器和姿态传感器的导航系统，包括CPU以及分别与其相连的姿态传感器、气压计、GPS，其中，姿态传感器用于测量运载体的加速度信息，气压计用于测量运载体的高度信息，GPS用于测量运载体的位置信息；CPU用于采用上述位置传感器和姿态传感器信息融合方法，分别对气压计测量的高度信息和姿态传感器测量的z轴加速度信息、GPS测量的x轴位置信息和姿态传感器测量的x轴加速度信息进行融合，GPS测量的y轴位置信息和姿态传感器测量的y轴加速度信息进行融合，输出运载体的x、y、z轴方向的位置信息与速度信息。

作为本发明的进一步优化方案，若GPS测量的运载体位置信息为经纬度信息，则根据以下公式转化为地面坐标系下水平位置信息：

$p_N=R\·({\mathrm{lat}}_1-{\mathrm{lat}}_0)\×\frac{\mathrm{\π}}{180}$

$p_E=R\·({\mathrm{long}}_1-{\mathrm{long}}_0)\×\frac{\mathrm{\π}}{180}\×cos({\mathrm{lat}}_1\×\frac{\mathrm{\π}}{180})$

式中，p_N、p_E分别为运载体当前x轴位移量、y轴位移量，(lat₁,long₁)为运载体当前经纬度，(lat₀,long₀)为运载体起飞点的经纬度，R为地球半径。

作为本发明的进一步优化方案，若GPS的测量频率小于姿态传感器的测量频率，则通过前一时刻融合得到的速度信息预估当前位置信息，对GPS测量的运载体位置信息进行插值处理，增加其频率。

作为本发明的进一步优化方案，CPU的型号为STM32F103。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1、将气压计高度与航姿传感器的z轴加速度信息融合，提高气压计高度信息精度，输出z轴高度信息和速度信息；

2、将GPS位置信息与航姿传感器的x、y轴加速度信息融合，得到大地坐标系中相对于home点(起飞点)的位置信息与速度信息；

3、采用STM32F103处理器，编程简单，易于入门；且传感器成本低，体积小，系统方便携带。

附图说明

图1为本发明硬件结构框图。

图2为本发明速度融合框图。

图3为本发明位置融合框图。

图4为本发明地面坐标系x轴位置融合前后对比图。

图5为本发明地面坐标系y轴位置融合前后对比图。

图6为本发明高度融合前后对比以及融合后的速度对比图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

本发明提供一种位置传感器和姿态传感器的导航系统，如图1所示，包括CPU以及分别与其相连的姿态传感器、气压计、GPS，其中，姿态传感器用于测量运载体的加速度信息，气压计用于测量运载体的高度信息，GPS用于测量运载体的位置信息；CPU用于采用一种位置传感器和姿态传感器信息融合方法，将姿态传感器测量的加速度信息分别与气压计测量的高度信息、GPS测量的位置信息进行融合，得到融合后的地面坐标系下x、y、z轴方向的速度信息和位置信息。

本发明中，CPU采用STM32F103模块，STM32F103属于中低端的32位ARM微控制器，是基于1.25DMIPS的Cortex-M3架构的32位单片机，时钟频率高达72MHZ，其丰富的硬件接口资源(3个USART，2个I2C，2个SPI，1个CAN等等)及功能强大的DMA控制方式。气压计通过I2C与CPU相连，姿态传感器和GPS通过串口与CPU相连。，

姿态传感器由三轴加速度计三轴陀螺、三轴加速度计、三轴磁场传感器组成，用于测量载体三个方向的的绝对角速率、加速度以及磁场强度，并采用特定姿态解算方法和卡尔曼滤波信息融合得到载体的四元数、姿态数据等。解算方法在本文中不做赘述。本发明中，选用美国Invensense公司生产的六轴姿态传感器MPU-6000，其整合了3轴陀螺仪、3轴加速器，为全球首例整合性6轴运动处理组件。相较于多组件方案，MPU-6000免除了组合陀螺仪与加速器时之轴间差的问题，减少了大量的包装空间，具有低功耗、低成本、高性能的特点。传感器的测量数据最终可通过最高400kHz的I2C总线或最高20MHz的SPI总线输出。三轴磁场传感器采用的是Honeywell公司的HMC5883L。该传感器能在±8高斯的磁场中实现5毫高斯分辨率，内置自检功能，能让罗盘航向精度精确到1°-2°，采用霍尼韦尔各向异性磁阻(AMR)技术，具有在轴向高灵敏度和线性高精度的特点。

气压计采用由MEAS推出的新一代高分辨率气压传感器MS5611，该传感器用来测量多旋翼飞行器绝对飞行高度。该模块包含了一个高线性度的压力传感器和一个超低功耗的24位模数转换器，提供了一个精确的24位数字压力值和温度值以及不同的操作模式，可以提高转换速度并优化电流消耗。高分辨率的温度输出无须额外传感器可实现高度计/温度计功能。工作温度范围：-40-85℃，精确度：在飞行高度750m时，偏差-1.5m～+1.5m。

GPS模块采用ATK-NEO-6M，模块采用U-BLOX NEO-6M模组，体积小巧，性能优异，模块自带高性能无源陶瓷天线(无需再购买昂贵的有源天线了)，并自带可充电后备电池(以支持温启动或热启动，后备电池在主电源断电后，可以维持半小时左右的GPS接收数据保存)。模块通过串口与外部系统连接，串口波特率支持4800、9600、38400(默认)、57600等不同速率。

本发明中还提供一种位置传感器和姿态传感器信息融合方法，下面具体论述其方法及原理：

位置传感器测量的位置信息，其测量误差不随时间累加，但在短期内精度较差；而加速度计积分产生的位置，由于漂移等问题的存在，一段时间后精度下降甚至发散，但在短时间内可提供精度较高的位置数据。利用两者互补的特性，采用互补滤波器对二者进行数据融合，其互补滤波原理如图2和3所示。

其中，互补滤波融合后的速度信息L_after(k)具体为：

$L_{after}\left(k\right)=\frac{C_2}{z+C_2}{L}_{before}\left(k\right)+\frac{z}{z+C_2}{L}_g\left(k\right)$

式中，L_g(k)可表示为：

$L_g\left(k\right)=L_{after}(k-1)+v_{after}(k-1)*dt+\frac{1}{2}*a_g\left(k\right)*dt*dt$

从而得到互补滤波融合后的位置信息为：

$L_{after}\left(k\right)=\frac{C_2}{z+C_2}{L}_{before}\left(k\right)+\frac{z}{z+C_2}\left(L_{after}\right(k-1)+v_{after}(k-1)*dt+\frac{1}{2}*a_g(k)*dt*dt)$

式中，L_before(k)为k时刻位置传感器测量的位置信息，L_after(k)为k时刻融合后CPU输出的位置信息，L_after(k-1)为k-1时刻CPU融合输出的位置信息，v_after(k-1)为k-1时刻CPU融合后输出的速度信息，a_g(k)为k时刻姿态传感器测量的加速度信息，dt为k-1时刻和k时刻之间的时间间隔(即数据融合算法两次运行之间的时间间隔)，比如本文中数据融合算法频率为10HZ，则)。L_after(0)＝L_before(0)，具有低通特性，具有高通特性，本发明中系统为离散系统，故采用z变换进行滤波。

互补滤波对姿态传感器的加速度信息的高通作用，加速度计的低频漂移可以得到很好地抑制；低通作用可以有效抑制位置传感器测量的位置信息的噪音。选取合适的C₂值可以使互补滤波器获得合适的截止频率，截止频率在大于f_r的高频段，姿态传感器的加速度信息对结算结果起主要作用；在小于f_r的低频段，位置传感器测量的位置信息对结算结果起主要作用。

采用同样的互补滤波方式对运载体的速度信息进行融合，具体为：

$v_{after}\left(k\right)=\frac{C_1}{z+C_1}{v}_{before}\left(k\right)+\frac{z}{z+C_1}{v}_g\left(k\right)$

其中，v_g(k)＝v_after(k-1)+a_g(k)*dt

从而得到互补滤波融合后的速度信息为：

$v_{after}\left(k\right)=\frac{C_1}{z+C_1}{v}_{before}\left(k\right)+\frac{z}{z+C_1}\left(v_{after}\right(k-1)+a_g(k)*dt)$

式中，v_before(k)为k时刻位置传感器测量的速度信息，v_after(k)为k时刻融合后CPU输出的速度信息，v_after(k-1)为k-1时刻融合后CPU输出的速度信息，a_g(k)为k时刻姿态传感器测量的加速度信息。v_after(0)＝v_before(0)。

而在z轴方向，由于气压计不能直接输出速度信息，先将气压计高度信息进行微分处理得到，本发明使用下面公式计算速度

$v_{before}\left(k\right)=\frac{L_{before}\left(k\right)-L_{before}(k-3)}{3*dt}$

其中，v_before(0)、v_before(1)、v_before(2)为0，由于程序运行频率高，且程序运行时有初始化时间，前三帧数据为0，不影响融合结果。

加速度计易受振动影响，选取合适的C₁、C₂值，获得较好的静态、动态位置信息，较为困难，给调试工作带来很大的难题，因此本发明将互补滤波与卡尔曼滤波相结合，简称互补卡尔曼滤波，将经过低通滤波后gps/气压计测量的位置和速度信息，高通滤波后加速度估算的位置和速度信息作为卡尔曼滤波器的输入，通过卡尔曼滤波自动调节C₁、C₂的值，有效的解决手动调节C₁、C₂值的难题。

本发明中，将卡尔曼滤波应用于离散线性时变系统，线性随机微分方程可描述为

x(k)＝Ax(k-1)+Bu(k)+w(k)

y(k)＝Cx(k)+v(k)

式中，x(k)是k时刻的系统状态，x(k-1)是上一状态最优的结果，u(k)是k时刻对系统的控制量，y(k)是k时刻的测量值，w(k)和v(k)分别表示系统过程中和测量的噪音，它们的coveriance(协方差)分别用Q与R表示(假设不随系统状态而改变)A、B、C分别表示系统状态参数、系统输入控制参数和测量系统的参数。

用系统的过程模型预测下一状态。当前k时刻，可以根据上一状态预测出现在的状态：

x(k|k-1)＝Ax(k-1|k-1)+Bu(k)+w(k)

其中，x(k|k-1)是利用上一状态预测的结果，x(k-1|k-1)是上一状态最优的结果，x(k|k-1)的coveriance用P表示：

P(k|k-1)＝AP(k-1|k-1)A^T+Q

式中P(k|k-1)是x(k-1|k-1)时刻的coveriance。结合预测值和测量，可以得到现在状态k时刻的最优化估计值x(k|k)：

x(k|k)＝x(k|k-1)+K_g(k)(y(k)-Cx(k|k-1))

$K_g\left(k\right)=\frac{P\left(k\right|k-1)C^T}{CP\left(k\right|k-1)C^T+R}$

K_g(k)是k-1时刻的卡尔曼增益，k时刻x(k|k)的coveriance：

P(k|k)＝(I-K_g(k)C)P(k|k-1)

式中I为单位矩阵。本发明中，采用一阶系统，u(k)为0，I＝1，故上式可简化为

P(k|k-1)＝P(k-1|k-1)+Q

$K_g\left(k\right)=\frac{P\left(k\right|k-1)}{P\left(k\right|k-1)+R}$

x(k|k)＝x(k|k-1)+K_g(k)(y(k)-x(k|k-1))

P(k|k)＝(1-K_g(k))P(k|k-1)

本发明将GPS/气压计测量的数据低通滤波后的位置信息作为系统的测量值，高通滤波后加速度估算的信息作为系统的预估值，在控制系统中采用融合后的最优结果。这种组合的主要优点是实现简单、可以根据实际情况估算出C₁、C₂，可获得较好的位置和速度信息。

本发明的系统是一阶离散线性时变系统，结合互补滤波与卡尔曼滤波原理，因此得到融合后的位置信息和速度信息，具体为：

1)融合后的速度信息通过以下方程计算得到：

v(k|k-1)＝v(k-1|k-1)+a_g(k)*dt

P₁(k|k-1)＝P₁(k-1|k-1)+Q

$K_1\left(k\right)=\frac{P_1\left(k\right|k-1)}{P_1\left(k\right|k-1)+R_1}$

$v\left(k\right|k)=(1-\frac{K_1\left(k\right)}{z+K_1\left(k\right)})*v\left(k\right|k-1)+\frac{K_1\left(k\right)}{z+K_1\left(k\right)}*v_{before}\left(k\right)$

式中，v(k|k-1)为根据加速度信息计算得到的k时刻速度信息，v(k-1|k-1)为k-1时刻融合后的速度信息，a_g(k)为k时刻姿态传感器测量的加速度信息，dt为k-1时刻和k时刻之间的时间间隔，P₁(k|k-1)为k时刻根据加速度信息计算得到的速度信息的协方差，P₁(k-1|k-1)为k-1时刻根据加速度信息计算得到的速度信息的协方差，Q为加速度信息的协方差，K₁(k)为第一卡尔曼滤波增益，v_before(k)为k时刻位置传感器测量的速度信息，R₁为v_before(k)的协方差；v(k|k)为k时刻融合后的速度信息；

2)融合后的位置信息通过以下方程计算得到：

$L\left(k\right|k-1)=L(k-1|k-1)+v_{after}(k-1)*dt+\frac{1}{2}*a_g\left(k\right)*dt*dt$

P₂(k|k-1)＝P₂(k-1|k-1)+Q

$K_2\left(k\right)=\frac{P_2\left(k\right|k-1)}{P_2\left(k\right|k-1)+R_2}$

$L\left(k\right|k)=(1-\frac{K_2\left(k\right)}{z+K_2\left(k\right)})*L\left(k\right|k-1)+\frac{K_2\left(k\right)}{z+K_2\left(k\right)}*L_{before}\left(k\right)$

式中，L(k|k-1)为根据加速度信息计算得到的k时刻位置信息，L(k-1|k-1)为k-1时刻融合后的位置信息，v_after(k-1)为k-1时刻融合后的速度信息，P₂(k|k-1)为k时刻根据加速度信息计算得到的位置信息的协方差，P₂(k-1|k-1)为k-1时刻根据加速度信息计算得到的位置信息的协方差，K₂(k)为第二卡尔曼滤波增益，L_before(k)为k时刻位置传感器测量的位置信息，R₂为L_before(k)的协方差；L(k|k)为k时刻融合后的位置信息。

由于GPS模块采集一般是运载体当前的经纬度信息，那么一般需要转化为地面坐标系下水平位置信息。假设地球是一个球体，不同的纬度对应不同的曲率半径，当运载体处在p点时，曲率半径(R')近似计算公式为：其中，角度a为当前的纬度，R为地球半径。若p点经纬度为(lat₁,long₁)，运载体的起飞点设为home点，home点的经纬度为(lat₀,long₀)，将运载体当前位置近似转化到以home点为原点的二维平面坐标系中，转换公式如下：

$p_N=R\·({\mathrm{lat}}_1-{\mathrm{lat}}_0)\×\frac{\mathrm{\π}}{180}$

$p_E=R\·({\mathrm{long}}_1-{\mathrm{long}}_0)\×\frac{\mathrm{\π}}{180}\×cos({\mathrm{lat}}_1\×\frac{\mathrm{\π}}{180})$

式中，p_N、p_E分别为运载体当前x轴位移量、y轴位移量。

由于GPS的频率一般较低，则通过前一时刻融合得到的速度信息预估当前位置信息，将预估位置信息插入到GPS测量的位置信息中，以增加GPS测量的位置信息的频率。本发明中，对GPS信息进行插值处理，插值处理每10hz运行一次，GPS读取程序4hz一次，输出相对于起点的相对位置，在插值处理时，会比较k时刻GPS测量得到的x、y轴位置信息L_x(k)、L_y(k)是否与k-1GPS测量得到的x、y轴位置信息L_x(k-1)、L_y(k-1)一样，如果不一样，则表示GPS数据有更新，则不进行插值，若不一样，则表示GPS未更新，通过前一时刻融合得到的速度信息预估当前位置信息。

图4为本发明地面坐标系x轴位置融合前后对比图，图5为本发明地面坐标系y轴位置融合前后对比图，图6为本发明高度融合前后对比以及融合后的速度对比图，其中，超声波高度是用来做比较的，因为超声波的高度在高度比较小的时候比较准确。从图中可以看出本发明可以有效抑制波动。

以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解想到的变换或替换，都应涵盖在本发明的包含范围之内，因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (5)

1.一种位置传感器和姿态传感器信息融合方法，其特征在于，将互补滤波与卡尔曼滤波相结合，将经过低通滤波后位置传感器测量的位置和速度信息、高通滤波后加速度估算的位置和速度信息作为卡尔曼滤波器的输入，位置传感器测量的位置信息和姿态传感器测量的加速度信息经过互补滤波与卡尔曼滤波后进行融合，得到融合后的位置信息和速度信息：

1)融合后的速度信息通过以下方程计算得到：

v(k｜k-1)＝v(k-1｜k-1)+a_g(k)*dt

P₁(k｜k-1)＝P₁(k-1｜k-1)+Q

$K_1\left(k\right)=\frac{P_1\left(k\right|k-1)}{P_1\left(k\right|k-1)+R_1}$

$v\left(k\right|k)=(1-\frac{K_1\left(k\right)}{z+K_1\left(k\right)})*v\left(k\right|k-1)+\frac{K_1\left(k\right)}{z+K_1\left(k\right)}*v_{before}\left(k\right)$

式中，v(k｜k-1)为根据加速度信息计算得到的k时刻速度信息，v(k-1｜k-1)为k-1时刻融合后的速度信息，a_g(k)为k时刻姿态传感器测量的加速度信息，dt为k-1时刻和k时刻之间的时间间隔，P₁(k｜k-1)为k时刻根据加速度信息计算得到的速度信息的协方差，P₁(k-1｜k-1)为k-1时刻根据加速度信息计算得到的速度信息的协方差，Q为加速度信息的协方差，K₁(k)为第一卡尔曼滤波增益，v_before(k)为k时刻位置传感器测量的速度信息，R₁为v_before(k)的协方差；v(k｜k)为k时刻融合后的速度信息；

2)融合后的位置信息通过以下方程计算得到：

$L\left(k\right|k-1)=L(k-1|k-1)+v_{after}(k-1)*dt+\frac{1}{2}*a_g\left(k\right)*dt*dt$

P₂(k｜k-1)＝P₂(k-1｜k-1)+Q

$K_2\left(k\right)=\frac{P_2\left(k\right|k-1)}{P_2\left(k\right|k-1)+R_2}$

$L\left(k\right|k)=(1-\frac{K_2\left(k\right)}{z+K_2\left(k\right)})*L\left(k\right|k-1)+\frac{K_2\left(k\right)}{z+K_2\left(k\right)}*L_{before}\left(k\right)$

式中，L(k｜k-1)为根据加速度信息计算得到的k时刻位置信息，L(k-1｜k-1)为k-1时刻融合后的位置信息，v_after(k-1)为k-1时刻融合后的速度信息，P₂(k｜k-1)为k时刻根据加速度信息计算得到的位置信息的协方差，P₂(k-1｜k-1)为k-1时刻根据加速度信息计算得到的位置信息的协方差，K₂(k)为第二卡尔曼滤波增益，L_before(k)为k时刻位置传感器测量的位置信息，R₂为L_befo_re(k)的协方差；L(k｜k)为k时刻融合后的位置信息。

2.一种位置传感器和姿态传感器的导航系统，其特征在于，包括CPU以及分别与其相连的姿态传感器、气压计、GPS，其中，姿态传感器用于测量运载体的加速度信息，气压计用于测量运载体的高度信息，GPS用于测量运载体的位置信息；CPU用于采用如权利要求1所述的融合算法，分别对气压计测量的高度信息和姿态传感器测量的z轴加速度信息、GPS测量的x轴位置信息和姿态传感器测量的x轴加速度信息进行融合，GPS测量的y轴位置信息和姿态传感器测量的y轴加速度信息进行融合，输出运载体的x、y、z轴方向的位置信息与速度信息。

3.根据权利要求2所述的一种位置传感器和姿态传感器的导航系统，其特征在于，若GPS测量的运载体位置信息为经纬度信息，则根据以下公式转化为地面坐标系下水平位置信息：

$p_N=R\·({\mathrm{lat}}_1-{\mathrm{lat}}_0)\×\frac{\mathrm{\π}}{180}$

$p_E=R\·({\mathrm{long}}_1-{\mathrm{long}}_0)\×\frac{\mathrm{\π}}{180}\×cos({\mathrm{lat}}_1\×\frac{\mathrm{\π}}{180})$

式中，p_N、p_E分别为运载体当前x轴位移量、y轴位移量，(lat₁,long₁)为运载体当前经纬度，(lat₀,long₀)为运载体起飞点的经纬度，R为地球半径。

4.根据权利要求2所述的一种位置传感器和姿态传感器的导航系统，其特征在于，若GPS的测量频率小于姿态传感器的测量频率，则通过前一时刻融合得到的速度信息预估当前位置信息，对GPS测量的运载体位置信息进行插值处理，增加其频率。

5.根据权利要求2所述的一种位置传感器和姿态传感器的导航系统，其特征在于，CPU的型号为STM32F103。