本发明主要涉及惯性导航以及传感器数据融合技术领域，属于惯性导航，gps导航及多传感器信息融合等技术领域，具体涉及一种基于gps与气压计融合数据的高度滤波方法。
背景技术：
惯性导航输出频率高，能够实时输出载体运动状态，且不需要外部信息摄入，不受天气影响，但是由于惯性传感器的数据漂移，使得解算得到的姿态、速度与位置发散特别快。gps能够全天候工作，为载体提供较为准确的时间，位置，速度等信息，同样不受天气影响，但是其必须工作在空旷无遮挡物的室外，在室内无法实现定位，在有高大遮挡物的室外由于信号遮挡使得定位信息不准。气压计通过测量大气压强解算高度，但是其受天气影响较为严重，大风环境影响周围气压，使得根据压强解算得到的高度出现误差或是漂移。此三种导航系统，各有优缺点，通过信息融合，能够结合各导航系统的优点。
惯性导航解算得到的高度是发散的，采用gps与气压计对高度进行观测来抑制高度漂移，同时得到较为精确的速度与位置。
技术实现要素：
为解决高度计算时，单个传感器观测高度无法适应复杂工作环境的弊端，本发明提出一种融合gps与气压计高度作为观测量的高度滤波方法。使用互补滤波将融合后的高度对惯导系统高度进行滤波，本发明能够得到较为精确的高度、速度与加速度，解决了单一传感器信号较弱或数据较差时对滤波效果产生的不利影响。
本发明采用的技术方案为：基于gps与气压计融合数据的高度滤波方法，包括如下步骤：
(1)一阶龙格-库塔解算四元数：
其中q0(t),q1(t),q2(t),q3(t)为t时刻的四元数，
(2)解算出四元数后，计算旋转矩阵：
同时可得到由导航系到载体坐标系的旋转矩阵：
cnb＝(cbn)t(6)
其中cbn为载体坐标系到导航坐标系的旋转矩阵，cnb为导航坐标系到载体坐标系的旋转矩阵，q0,q1,q2,q3为式(1)-(4)中解算得到的四元数。
(3)将加速度数据转换至导航坐标系下：
其中cbn为式(6)的旋转矩阵，
(4)计算滤波观测量中gps高度与气压计高度的比例系数
其中n为gps搜到的卫星数，x为gps解算输出的pdop三维位置精度因子，kh为比例系数。
(5)计算gps与气压计高度融合后的观测高度：
式中h1为gps观测高度，h2为气压计观测高度，
(6)采用互补滤波对惯导高度进行校正，同时得到较为精确的速度与加速度：
计算高度偏差：
h*(k-n)为距当前k时刻n个滤波周期前的滤波高度历史值。δh(k)为观测高度与滤波高度的偏差值。
计算加速度，速度及高度修正值：
ac(k)＝ac(k-1)+δh(k)kaδt(k)(11)
vc(k)＝vc(k-1)+δh(k)kvδt(k)(12)
pc(k)＝pc(k-1)+δh(k)kpδt(k)(13)
其中ac(k)、vc(k)及pc(k)分别为第k个滤波周期的加速度、速度和高度修正值。ac(k-1)、vc(k-1)及pc(k-1)分别为第k-1个滤波周期的加速度、速度和高度修正值。δt(k)为第k个滤波周期与第k-1个滤波周期之间的间隔时间。δh为式(10)中计算得到的高度偏差。δt(k)为滤波周期。ka、kv及kp分别为加速度、速度及高度修正值的计算比例系数，其计算方式为：
ka＝1/k3(14)
kv＝1/k2(15)
kp＝1/k(16)
其中k可取2～10之间的整数值，可以根据滤波效果具体调整。
进行加速度校正：
a*(k)＝an(k)+ac(k)(17)
式中a*(k)为校正后的加速度值，an(k)为加速度计输出转换至导航系下的垂直方向加速度值，ac(k)为式(11)计算得到的加速度校正值。
高度计算及高度校正：
δv(k)＝(a*(k)+a*(k-1))δt(k)/2(18)
ho(k)＝ho(k-1)+[v*(k-1)+(v*(k-1)+δv(k)/2)]δt(k)(19)
h*(k)＝ho(k)+pc(k)(20)
其中δv(k)为第k-1滤波周期至第k个滤波周期的速度增量，a*(k)为式(17)中经过校正后的加速度，h*(k)为最终滤波后的高度，ho(k)为中间计算未经滤波后的高度，v*(k-1)为第k-1个滤波周期的滤波速度，pc(k)为式(13)计算得到高度校正值。
计算速度及进行速度校正：
vo(k)＝vo(k-1)+δv(k)(21)
v*(k)＝vo(k)+vc(k)(22)
其中v*(k)为滤波后的速度，vo(k)为第k时刻未经校正的速度计算值。δv(k)为式(18)计算得到的速度增量，vc(k)为式(13)计算得到的速度校正值。
本发明与现有技术相比的优点在于：
(1)本发明的创新点在于融合多传感器的优势，通过对多传感器数据进行融合，将惯性导航、gps导航及气压计的优点进行结合，可以使算法能够应对各种工作环境。
(2)同时，本发明采用互补滤波对高度数据进行滤波，能够在抑制惯导发散的基础上，同时得到较为精确的速度与加速度。
(3)算法简易，容易理解，计算量小，能够在嵌入式平台中实时运行。
(4)本发明在滤波过程中计算高度误差时，对传感器观测高度的延时性做了相应处理，使滤波高度输出与观测高度输出处于相同的有效时间，保证算法稳健性。
(5)高度滤波作为独立系统进行处理，可以将发散的惯导高度通道从系统中分离出来，避免污染整个系统。
附图说明
图1为本发明的基于gps与气压计融合数据的高度滤波方法实现流程图。
具体实施方式
下面结合附图说明进一步说明本发明。
如图1所示，本发明的基于gps与气压计融合数据的高度滤波方法，滤波过程整体分为三步：首先进行姿态解算，得出旋转矩阵后，将载体上的加速度值转换至导航坐标系；然后根据gps可用卫星数及三维位置精度因子pdop计算比例系数，将gps观测高度与气压计高度进行融合，得到滤波的观测高度；最后，通过互补滤波对惯导解算高度进行滤波，同时得到较为精确的速度与位置。
(1)一阶龙格-库塔解算四元数：
其中q0(t),q1(t),q2(t),q3(t)为t时刻的四元数，
(2)解算出四元数后，计算旋转矩阵：
同时可得到由导航系到载体坐标系的旋转矩阵：
cnb＝(cbn)t(6)
其中cbn为载体坐标系到导航坐标系的旋转矩阵，cnb为导航坐标系到载体坐标系的旋转矩阵，q0,q1,q2,q3为式(1)-(4)中解算得到的四元数。因为陀螺仪数据存在漂移，所以单纯由陀螺仪解算得到的姿态也会漂移，此时可以通过加速度计及磁力计分别对姿态进行水平校正与偏航角校正，校正算法有很多，可以采用互补滤波，卡尔曼滤波或者是梯度下降法等。。
(3)将加速度数据转换至导航坐标系下：
其中cbn为式(6)的旋转矩阵，
(4)计算滤波观测量中gps高度与气压计高度的比例系数
其中n为gps搜到的卫星数，x为gps解算的pdop三维位置精度因子。kh为比例系数。pdop三维位置精度因子表示gps可用卫星与用户接收机之间的几何关系，其值越小，表明gps的定为精度越高。当pdop系数小于3时，表明有效定位。
(5)计算gps与气压计高度融合后的观测高度：
式中h1为gps观测高度，h2为气压计观测高度，
(6)采用互补滤波对惯导高度进行校正，同时得到较为精确的速度与加速度：
计算高度偏差：
h*(k-n)为距当前k时刻n个滤波周期前的滤波高度历史值。因为gps与气压计传感器观测高度存在延迟，所以为了在时间上保持计算同步，将当前的观测高度与n个滤波周期前的滤波高度作差值求取高度偏差。δh(k)为观测高度与滤波高度的偏差值。
计算加速度，速度及高度修正值：
ac(k)＝ac(k-1)+δh(k)kaδt(k)(11)
vc(k)＝vc(k-1)+δh(k)kvδt(k)(12)
pc(k)＝pc(k-1)+δh(k)kpδt(k)(13)
其中ac(k)、vc(k)及pc(k)分别为第k个滤波周期的加速度、速度和高度修正值。δt(k)为第k个滤波周期与第k-1个滤波周期之间的间隔时间。δh为式(10)中计算得到的高度偏差。δt(k)为滤波周期。ka、kv及kp分别为加速度、速度及高度修正值的计算比例系数，其计算方式为：
ka＝1/k3(14)
kv＝1/k2(15)
kp＝1/k(16)
其中k可取2～10之间的整数值，可以根据滤波效果具体调整。
进行加速度校正：
a*(k)＝an(k)+ac(k)(17)
式中a*(k)为校正后的加速度值，an(k)为加速度计输出转换至导航系下的垂直方向加速度值。
高度计算及高度校正：
δv(k)＝(a*(k)+a*(k-1))δt(k)/2(18)
ho(k)＝ho(k-1)+[v*(k-1)+(v*(k-1)+δv(k)/2)]δt(k)(19)
h*(k)＝ho(k)+pc(k)(20)
其中δv(k)为第k-1时刻至k时刻的速度增量，a*(k)为式(17)中经过校正后的加速度，h*(k)为最终滤波后的高度，ho(k)为中间计算未经滤波后的高度，v*(k-1)为k-1时刻的滤波速度，pc(k)为式(13)中计算得到的位置校正量。
计算速度以及进行速度校正：
vo(k)＝vo(k-1)+δv(k)(21)
v*(k)＝vo(k)+vc(k)(22)
其中v*(k)为滤波后的速度，vo(k)为第k时刻未经校正的速度计算值。δv(k)为式(18)计算得到的速度增量，vc(k)为式(12)中计算得到的速度校正值。
将gps数据与气压计数据进行融合，得到融合后的观测值，然后进行互补滤波，对高度进行抑制，同时得到较为精确的速度与加速度。可以通过融合惯性导航、gps导航及气压计的数据，将三者的优点结合起来，使算法能够适应更加复杂的环境。
以上详细描述了本发明的实施方式，本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换，这些等同变换均属于本发明的保护范围。