组合导航算法设计

INS/GPS组合导航算法设计1引言目前单一导航系统难以满足实际要求，把两种或多种导航系统组合起来，应用最优估计理论，形成最优组合导航系统，使组合后的导航系统在精度和可靠性都有所提高。本课题研究飞行器GPS/INS组合导航算法，通过对飞行器INS/GPS组合导航算法设计，以VC++6.0为平台组建INS/GPS组合导航仿真系统，对组合导航算法进行实现。并对飞行器的飞行状态进行仿真，仿真前预先设定飞行器的飞行参数（包括平飞、加速、减速、上升、下降、转弯等飞行动作以及每个动作开始结束的时间），通过设计的组合导航仿真系统得到飞行器的位置、速度、姿态角信息。并通过MATLAB对INS/GPS组合导航解算出来的数据与预先设定的实际飞行数据进行比较分析。惯性导航系统的优点是：（1）自主性强，它可以不依赖任何外界系统得支持，单独进行导航。（2）不受环境、载体运动和无线电干扰的影响，可连续输出包括基准在内的全部导航参数，实时导航数据更新率高。（3）具备很好的短期精度和稳定性。其主要缺点是导航定位误差随时间增长，难以长时间的独立工作。全球定位系统是一种高精度的全球三维实时导航的卫星导航系统，其导航定位的全球性、高精度、误差不随实践积累的优点，但是GPS系统也存在一些不足之处，主要是：GPS接收机的工作受飞行机动影响，当载体的机动超过GPS接收机的动态范围时，GPS接收机会失锁，从而不能工作，或者动态误差过大，超过允许值，不能使用。且GPS接收机的更新频率较低（1HZ），难以满足实时控制的要求。抗干扰能力差。此外GPS导航受制于人。因此GPS系统一般作为理想的辅助导航设备使用。GPS/惯性组合导航，克服了各自的缺点，取长补短，可以构成一个比较理想的导航系统，GPS/惯性组合导航可以大大降低导航系统的成本。随着MEMS技术的发展，惯导成本的降低都是组合导航系统发展的优势所在。我们用组合导航算法将惯性导航单元的信息和GPS的信息进行综合，来补偿惯性元件的误差，修正速度、姿态信号，从而构成一个精度适中、结构紧凑、成本低廉的导航系统。可用于飞行器的导航。2INS/GPS组合系统主要功能和算法选择INS/GPS组合导航系统的主要功能为：a）实时测量飞行器的角速率、加速度、速度、位置以及姿态，完成测量信息的采集和处理；b）完成惯性导航解算;c）完成惯性/卫星组合导航解算。滤波器算法选择组合滤波器是组合导航系统的核心，其本身性能的优劣将直接影响组合导航的质量。Kalman滤波算法是最为常用和成熟的滤波算法，它作为一种现代最优估计理论，采用了状态空间和递推计算形式，可对一般线性系统进行线性最小方差估计，并且具有数据存储量小，易于计算机实现等优点，在INS/GPS组合导航领域获得了广泛的应用。因此系统采用集中式Kalman滤波器进行惯性导航系统和GPS测量与导航信息的综合。3组合卡尔曼滤波算法设计组合卡尔曼滤波器设计的主要内容包括：确定真实系统的状态；列写系统状态方程；列写量测方程；编制滤波仿真程序；设计降阶滤波器。组合导航系统的实现和组合效果的充分发挥，很大程度上决定于状态的选择、模型方程的建立和简化、参数的选择和滤波算法设计等。组合卡尔曼滤波器设计的主要步骤包括：1、针对所选择的系统和测量方法，确定系统状态，以及尽可能真实地描述系统动态特性的状态方程和量测方程；2、根据系统噪声和量测噪声的统计特性，确定噪声的方差强度阵Q和R；3、连续系统离散化；4、确定滤波方程的计算方法；5、编制仿真计算程序，模拟滤波效果，初步确定滤波周期及系统其他参数，按状态协方差阵校核组合系统达到的精度；6、在组合系统满足任务要求精度的前提下，设计导航计算机简化滤波计算方法。3.1组合模式设计GPS接收机和惯性导航系统选用基于位置、速度的松组合方式。图3-1为位置、速度信息组合的原理框图。用GPS和惯导各自输出的位置和速度信息差值作为量测值，经组合卡尔曼滤波，估计惯性器件和惯性导航的误差，然后对器件误差和导航误差进行校正，以提高惯性导航的精度。图3-1位置、速度组合原理图松组合中，GPS和惯导仍独立工作，组合作用主要表现为用GPS辅助惯导;组合系统的输出是通过融合GPS的导航输出和SINS的导航输出而得到；能提供冗余度，有界的位置、速度和姿态估计，还提供高数据刷新率的与制导控制方程相配的平动和转动信息。位置、速度信息组合的主要优点是组合工作比较简单，便于工程实现；而且两个系统仍可独立工作，使导航信息有一定余度。3.2INS/GPS组合导航系统的估计方法组合导航系统利用间接法进行导航参数的估计。间接法以组合导航系统中某一导航系统输出的导航参数的误差为滤波器状态，滤波器估值的主要部分是导航参数误差估值，然后用估值校正状态。间接法估计时一般系统方程和量测方程是线性的。图3-2给出了SINS/GPS组合导航系统间接估计方法的原理框图。3.3组合滤波器的校正方法采用间接估计法的组合导航，系统的状态都是误差状态AX，利用状态估值AX去对原系统进行校正的方法有两种：输出校正和反馈校正。图3-2所示为SINS/GPS输出和反馈校正滤波示意图。输出校正是用导航参数误差的估值丞作为惯导系统输出的校正量。反馈校正是将惯导系统参数误差AX］的估计值反馈到惯导系统和其余子系统内，对误差状态进行校正。从形式上看，输出校正仅校正惯导系统的输出量，反馈校正则校正惯导系统的内部状态。从数学模型的推导结果可以证明，如果滤波器是最优滤波器，即它的状态基本上包括了真实系统的所有状态，状态方程真实地描述了真实系统的变化规律，则输出校正和反馈校正具有同样精度，组合系统具有相同的效果。然而，在考虑到一些实际情况时，两种校正方式仍有较大区别。输出校正的优点是工程实现比较方便，组合滤波器的故障不会影响惯导系统的工作。缺点是由于输出校正的滤波器所估计的状态是未经校正的导航参数误差，而惯导系统误差是随时间增长的，滤波器的数学模型建立在误差为小量且取一阶近似的基础上，因此在长时间工作时，由于惯导误差不再是小量，会使滤波方程出现模型误差，使滤波精度下降。反馈校正的滤波器所估计的状态是经过校正的导航参数误差，在反馈校正后，惯导系统的输出就是组合系统的输出，误差始终保持为小量，克服了输出校正的缺点，因此可以认为利用反馈校正的系统状态方程，更能接近真实地反映系统误差状态的动态过程，可认为没有模型误差。反馈校正的缺点是工程实现没有输出校正简单，且滤波器故障直接污染惯导系统输出，使系统可靠性降低。综上所述，校正方法选择的原则是：只要状态能够具体实施反馈校正，组合导航系统尽量采用反馈校正。在实际应用中，如果惯导系统精度较高，且连续工作时间不长，则采用输出校正。反之，如果惯导系统精度较差，且连续工作时间又长，则需采用反馈校正。由于低成本MEMS惯导精确较差，因此组合导航需要采用反馈校正的方式。4INS/GPS组合导航算法的设计4.1姿态矩阵的计算姿态矩阵是指从导航坐标系3系）到载体坐标系0系）的变换矩阵，当采用“东北天”地理坐标系为导航坐标系时，姿态矩阵为cosyco叩+sinysin0siw-cosysiw+sinysin0co叩-sinycoS9(4.1)Cb=cos9sin^cos9co叩sin0(4.1)sinyco叩一cosysin0sin^-sinysin^-cosysinOco叩cosycos9式中，W为方位角，。为俯仰角，/为横滚角，这三个角称为载体的姿态角.当MIMU固连的载体姿态发生变化时，MIMU中的陀螺仪就能敏感出相应的

角速率，姿态矩阵。随之发生了变化，其微分方程为nCb=—ObCb(4.2)nnbn式中，Ob为角速度wb=「3bxwbywbzT构成的反对称阵；x,y,z为右前上方向。nbnbLnbnbnb」捷联惯导姿态矩阵的即时修正就是实时地给出捷联矩阵，它是捷联惯导的关键任务，而这要通过一定地算法来完成。进行姿态矩阵即时修正的算法很多，主要有欧拉角法、方向余弦算法、四元数法等。由于四元数算数法计算量小，存储容量小，仅需要进行简单的四元数规范化处理便可以保证姿态矩阵的正交性，因而成为目前实际应用中普遍采用的姿态计算方法。单位四元数可用如下形式的来描述:q。Q=q+qi+qj+qk=(4.3)%qq3」这个四元数的范数为:1^11=q。+q2+Q=q+qi+qj+qk=(4.3)这个四元数的范数为:1^11=q。+q2+q2+q=1(4.4)称作“规范化”四元数。我们可以通过求解一个四元数来计算导航参数。在捷联导航中，要求载体系到导航系的转换矩阵，要解下列四元数的运动方程:(4.5)Q=2OQ式中，\0—wbx—wby—(4.5)\0—wbx—wby—wbzqnbnbnb0wbx0wbz—wbyqnbnbnb，Q=1wby—wbz0wbxqnbnbnb2wbzwby—wbx0qnbnbnbL3-1O=(4.6)Q是从载体系到导航系的转动四元数；w表示载体坐标系相对导航坐标系的转动角速度在载体坐标系上的投影，它由陀螺仪的输出变换而得。它与其它速率的关系是:(4.7)wb=wb—wb=wb—CbWn=wb—Cb(wn+Wn)(4.7)nbibinibninibnieen式中，w.即为角速率陀螺的输出，wn为地球角速率，是已知的，式中，w.即为角速率陀螺的输出，ieen标系相对地球的角速度，它可以由瞬时速度"求得。enOby①为角速率陀螺直接测得的载体相对惯性空间的绝对角速率，则bzObx「o一-0-nbbxObyo-Cb(ocosLnbbyneObzoosinLOby①为角速率陀螺直接测得的载体相对惯性空间的绝对角速率，则bzObx「o一-0-nbbxObyo-Cb(ocosLnbbyneObzoosinLnbbze+-NR+hMVER+hNVetanLR+hN(4.8)式中，®bx®b®,bz」-CbnVNR+hMvosinL+re人tanLN匕为东北天的速度，rm、rn为地球子午面和卯酉面曲率半径，力、h为纬度、经度、高度。式(3.5.5)表示的四元数微分方程可通过毕卡逼近法求解:Q(k)=e2aOQ(k-1)(4.9)式中，A0=1t+1Qdt=0-AO-AO-AOAOX0AOy-AOXAO-AOz0AOyzXAOAO-AO0zyXzy，其中AO、AO、AO分别为y载体绕X、y、z轴转动的角度增量。若设AO='AO2+AO2+AO2Xyz则有AO一AO.AOsin——'1aO一-e2=cos——oI+——sin——o2AO02(4.10)AOAOAOQ(k)=(cos十I+—sin十)Q(k-1)2AO2(4.11)当A6为小量时，Q(k)"(I+A9/2)Q(k-1)0(4.12)式(3.5.5)表示的四元数微分方程也可采用四阶龙格-库塔法求解:1~、KTq(t)+-^-K=/t)q(t)12TOC\o"1-5"\h\z1TK=—O(t+-)\o"CurrentDocument"222叱1~T、，、KT<K=;O(t+~)q(t)+2(4.】3)KTq(t)+-^-\o"CurrentDocument"222*—-1K=10(t+—)［q(t)+KT］223—T一一q(t+T)=q(t)+-(%+2K2+2K3+K4)式中，T为计算步长。由于计算过程中存在舍入、截断等误差，使计算得到四元数的范数不再为1,因此应该对计算的四元数进行规范化处理(4.14)q+qi+qj+qkQ=0123(4.14)(q2+q2+q2+q2,0123利用规范化后的四元数，可以求得姿态矩阵q2+qq2+q2-q2-q20123Cb=2(qq-qq)\o"CurrentDocument"n12032(qq+qq)1302]C11C12C13]=C21C22C23C31C32C332(qq+qq)0312q2-q2+q2-q2

01232(qq-qq)23012(qq-qq)13022(qq+qq)2301q2-q2-q2+q20123」(4.15)载体姿态角可从更新计算后的姿态矩阵Cb中提取:n0=sin-1Cy=tan-1(-C/C)w—tan-1(C/C)(4.16)主23主1333主2122由于俯仰角0定义在±90区间，和反正弦函数的主值一致，不存在多值问题，即O0=0主。而横滚角y定义在［-180,180］区间，航向角w定义在［0,360］区故OOOOy、w都存在多值问题，在计算出主值之后，可由Cb中的元素判断是在哪个象限。在四元数计算过程中采用迭代算法，将初始姿态角y0、00、w「弋入下式，即可求得导航开始时刻需要的初始四元数Q(0):

Q(0)=222222.0sin-ocosYoWcos—o0Q(0)=222222.0sin-ocosYoWcos—o0+cos—osin.Wsin—o222222cos近cos£cosK+sin近sinLsinK0.YW"Y•Wcos—sinocos—o-sin-ocos—osin—o(4.17)cos—ocos—osin—o-sin-osin—ocos—o2222224.2导航参数的计算速度计算方程为:Vn=fn—(①n+2①n)XVn+速度计算方程为:Vn=fn—(①n+2①n)XVn+genie(4.18)其中，o也e=cosL，①sinLieOn=enV—N——R+hMVE——R+hNV—e——tanLR+hNv「f]Lo一(2On+On)2On+On「V]「o一EEiezenzieyenyEvf—2On+Ono一(2On+On)V+o.NNiezenziexenxNVf一(2On+On)2On+OnoVL—g」•U」1-u」Lieyenyiexenx—1u把上式表示为矩阵形式为:(4.19)式中，fE、fN、fu分别为沿地理坐标系在东、北、天方向上的比力分量，它可以通过加速度计测量得到的沿载体坐标系的比力，经过坐标转换得到，「f]efbxf=Cfnb'byf」u」fUbz」即位置计算方程为:入(k)=X(k—1)+*("~TLR(k—1)+h(k—1)」cosL(k—1)ML(k)=L(k—1)+VN(kT)TR(k—1)+h(k—1)Nh(k)=h(k—1)+v(K—1)-Tu(4.2。)在经度、纬度进行更新计算后，将计算结果L(k)代入计算kT时刻的Rm(k)、Rn(k)(4.21)(4.22)R^(k)=R[1-2f+3fsin2L(k)一R^(k)=R[1+fsin2L(k)一式中，R为地球半径，f为椭圆度。(4.21)(4.22)重力随纬度和高度的变化规律近似为:g=g°[l+0.00527094sin2(L)+0.0000232718sin4(L)]-0.000003086h(4.23)式中，g0=9.7803267714。4.3捷联惯性导航系统误差方程惯性导航系统的误差源有很多，主要有惯性仪表本身的误差，惯性仪表的安装误差和标度误差，系统的初始条件误差，系统的计算误差以及各种干扰引起的误差等。惯导误差可分为两类：确定性误差和随机误差。确定性误差包括平台角误差、速度误差以及位置误差，随机误差主要是陀螺漂移和加速度计的零位偏置等。平台角误差方程(4.25)平台误差角方程为:(4.25)^n=§On+§On-(①n+①n)X。n+8nieenieen其中，■08①n=一①sinL8Lieie①cosL8LOn=enNOn=enNR+hMV——E—，R+hN8On=en——e—tanLR+hNNR+hM8vE—R+hN——V^tanL+—"Ve—sec2L8LR+hR+hNN把(4.25)式展开得:6=-一^^+皿sinL+—tan以-皿cosL+—)6+£ER+hieR+hNieR+hUE,8v.c,.v.,vt<6=『一①sinLSL-皿sinL+—e—tanL)6-——n—6+£(4.26)NR+h论ieR+hER+hUNi8v,…一.v.、"/一_v±一6=——itanL+皿cosL+——sec2L)8L+皿cosL+——)6+—6+£uR+hieR+hieR+hER+hNU•速度误差方程由vn=fn-(2s+S)Xvn+gn，可得8v=8fn-(28s+8s)xvn-(2s+s)x8vn+8gn(4.27)考虑8gn=0,8fn=fp-fn，其中fp为加速度计的实际输出。设加速度计的测量误差为VP，则fp=Cpfn+VP=[I-6n]Xfn+VPn故8fn=fp—fn=fnx6n+VP于是得到8vn=fnX6n+Vp—(28sn+8sn)Xvn—(2Sn+Sn)X8vnieenieen把(4.27)式展开成分量形式，则有8v=f6—f6+(--n—tanL^u—)8v+(2①sinL+——ve—tanL)8vENUUNR+hR+hEieR+hN—(2①cosL+——V^—)8v+(2①cosLv+-e-nsec2L+2①sinLv)8L+V(4.28)ieRN+hUieNRN+hieUE(4.28)8v=f6-f6-2(ssinL+^e^tgL)8vN—(2①cosL+—Ve—sec2L)v8L+VieRN+hEN—2ssinLv8L+2；nhSv+VM8v=f—2ssinLv8L+2；nhSv+VMUENNEieR+hE位置误差方程vcvsecL由L=rnh，人——R，h=?u可得:MN5L=nR+h5人="匕secL+Ve‘©CLtanl5LR+hR+h5h=5vU(4.29)(4.26)、(4.28)和(4.29)式合在一起，即为惯导系统的导航误差方程。惯性仪表误差惯性仪表误差包括初始对准误差、安装误差、刻度误差、常值误差和随机误差，卡尔曼滤波器只对后两者进行估计。针对实际的惯导系统，惯性仪表的随机误差建模是一项非常重要的工作。根据大量统计规律，可以认为陀螺仪和加速度计误差分别主要由三部分构成，方程如：8=8+8+w▽=七+V+w(4.30)式中，8广％——陀螺、加速度计随机常值漂移;气——陀螺、加速度计高斯白噪声;V——陀螺、加速度计一阶马尔柯夫过程漂移，其数学模型为:a8=-—8+WrTrgrr-1一V广—TVa^^ara(4.31)其中，。是陀螺马尔柯夫过程的相关时间，也,是均方差为气,的马尔柯夫过程白噪声。Tq是加速度计马尔柯夫过程的相关时间，%,是均方差为bq,的马尔柯夫过程白噪声。需要说明的是：对惯性仪表误差建模，取的是载体坐标系的状态矢量，而组合系统的状态方程是建立在东北天地理坐标系，这就需要一个载体系到地理系的坐标变换。8n=Cn8bbVn=CnVbb4.4KALMAN滤波器的设计(4.32)(1)状态方程SINS/GPS组合导航系统误差的全阶模型(35阶)取35个状态变量，其中SINS系统误差的9个(3个姿态误差、3个速度误差、3个位置误差)，陀螺误差9个(3个常值零位误差、3个随机漂移误差、3个比例系数误差)，加速度计误差9个(3个常值零位误差、3个随机漂移误差、3个比例系数误差)，GPS误差6个(3个常值位置误差、3个随机位置误差)，大气系统误差2个(1个常值高度误差、1个随机高度误差)。由于SINS/GPS的组合导航系统全阶滤波器所需要的数学模型过于复杂，难于应用于实际中。因此作为一个参考模型，应用时需要简化。平台误差角方程如式(4.26)速度误差方程如式(4.28)位置误差方程如式(4.29)上述公式组成了惯导的基本误差方程，当惯导和其他导航设备组合时，仍然可以使用上述九个基本误差方程。④惯性仪表误差方程实际应用中可以忽略微机械陀螺和加速度计的刻度系数误差及慢变漂移，即：8=8+3bgV=Vb+3式中，8b为陀螺随机常值漂移，3g为陀螺量测Gauss白噪声，Vb为加速度计随机常值偏置，3为加速度计量测Gauss白噪声。在位置、速度组合模式中，GPS接收机输出的位置、速度误差一般不作为扩充状态，而是将其作为量测噪声，并可以采用加大滤波器更新周期的方法处理。故综合以上各式，可得组合导航系统在位置速度组合模式下的15维状态方程为：X(t)=F(t)X(t)+G(t)W(t)(4.33)X(t)=「e&e8V8V8V8L8XSh888VVV]TLENUENUbxbybzbxbybz」为系统的状态矢量，其中，下标E、N、U分别表示东北天地理坐标系的三个方向，e、&、&为SINS平台误差角，8V、8V、8V为速度误差，8l、8人、ENUENU

期为位置误差，£、£、£陀螺仪的随机漂移，V.V.V加速度计的零bxbybz.bxbybz.位误差；W)=r«0)cocococo¥为系统过程白噪声矢量，其中，1-gxgygzaxayaz」co、co、co为陀螺的白噪声，①、co、①为加速度计的白噪声；FQ)为gxgygzaxayazr(F)(F)系统状态矩阵，G(t)为系统噪声传播矩阵，F(0=INS9x9sg件，o(F)9x9IMU6x6%5xl5FSgCnbo3x3oFSgCnbo3x3o1-3x3o3x3(Jnbo3x3*6F=[o],GQ)=IMU6x6Cnbo3x3o9x3o3x3Cnbo9x3J15x6其中，F是对应9个基本导航参数的矩阵，由捷联惯导基本误差方程决定，其SINS非零的各元素如下:vF(l,2)=cosinL+e—R+hNtanL,ieV7^(1,3)=—(cocosL+—e—vF(l,2)=cosinL+e—R+hNtanL,ieV7^(1,3)=—(cocosL+—e—),ieR+hNE(l,5)=,R+hM/^(2,1)——(osinL+

ieV,、—e—tanL),R+hNv

g(2,3)=——,

R+hM1g(2,4)=,R+hNF(2,7)=-cosinL,

ieF(3,l)=wcosL+

ieVE—,R+hNv尸(3,2)=—i,

R+hM1g(3,4)=—-R+hNtanL,F(3,7)=coieTVcosL+——e—sec2R+hNL,"(4,3)=fvVVF(4,4)=(——n—tanL-——b—),

R+hR+hMMVF(4,5)=2cosinL+——e—tanL,

ieR+hNV

r(4,6)——(2cocosL+—e—),

ieR+hNVV尸(47)=2cocosLv+—sec^L+2cosinLv,F(5,l)=f,ieNR+hieUUNF(5,4)=—2(wsinL+—V^—tanL)，

ieR+hF(5,5)=-七，Rm+hvF(5,6)=——nF(5,5)=-七，Rm+hvF(5,6)=——n—，Rm+hvF(5,7)=-2(wcosL+——e—secieRn+hL)v，EF(6,2)=fE，F(6,4)=2(wiecosL+—V2vF(6,5)=广七，R+hF(6,7)=-2vwsinL〜1F(7,5)=-，R+hF(8,4)=浩，

R+hvF(8,7)=——e—secLtanL，Rn+hF(9,6)=1。(2)量测方程选取MEMS惯组输出的位置、速度参数与GPS接收机提供的位置、速度参数之差作为观测量，以上参数均在导航坐标系下进行计算。位置、速度联合组合的SINS/GPS组合系统的观测方程为rz(rz(t)]「H(t)]X(t)+「V(t)]p=ppZV(t)H(t)V(t)Z(t)=(4.34)卜面分别建立SINS/GPS组合的位置观测方程和速度观测方程。①位置观测方程在INS/GPS组合系统中，INS给出的实时位置是地理纬经度和高度，但含有误差。设L、人和h分别是INS输出的纬度、经度和高度，L、人和h分别是iii飞行器真实纬度、经度和高度，则有Li=L+aL(4.35)人=X+5X^(4.35)1ch=h+8h\GPS接收机也给出飞行器的实时位置，其主要误差源是卫星钟和轨道误差、接收机钟和测量噪声误差、大气层传输延迟、SA作用等，这些误差构成了GPS

随机测量伪距误差。设匕、、g和顷别是GPS接收机输出的纬度、经度和高度，则有LG力GhG『LG力GhG(R+h)M(4.36)以-—(4.36)(R+h)cosL—h-NU式中，Ne、Nn、Nv为GPS接收机北、天、东方向的距离测量误差。(L-L)(R+h)(X-X)(R(L-L)(R+h)(X-X)(R+h)cosLIGNh-hIG(R+h)5L+NMN(R+h)cosL5X+NNE5h+NU(4.37)把上式表示为Zp(t)—Hp(t)X(t)+Vp(t)(4.38)式中H(t)—【0diag[(R+h)(Rn+h)cosL1]0]Vp(t)—[NnNeNu/距离测量噪声V作为白噪声处理，其标准差分别为:p(4.39)b=b・HDOPbpN=b「・HDOpb:=bVDOP式中％为伪距测量误差，HDOP是水平精度衰减因子，VDOP是垂直精度衰减因子。(4.39)②速度观测方程设v、v和v分别是INS输出的北向速度、天向速度和东向速度，u、vINIUIENU和v分别是飞行器真实的北向速度、天向速度和东向速度，则有E

*N=V+5Vv=v+8v>v=v+8v设vGN^vGu和vGE分别是GPS接收机输出的北向速度、天向速度和东向速度，则V二二V-8vGNNGNV-二V-8vGUUGUV-二V-8vGEEGE式中，8v、8v、8v为GPS接收机沿北天东方向的速度测量误差。GNGUGE(4.40)取INS和GPS的速度差作为观测量时，观测方程可由下式表示：(4.40)v-v「8v+8vINGNNGNv-v=8v+8vIUGUUGUv-v8v+8vIEGEEGE气()==H(t)X(t)+V(t)VV式中H(t)=[diag[111]qi2]VV(t)=[8vGN8vGu8vGE]r速度测量噪声作为白噪声处理，其标准差分别为:"Lp•HDOPN、b="•VDOP>"e="•.hdope式中，bp为伪距率测量误差。4.5系统离散化及Kalman滤波方程1状态方程和观测方程的离散化(4.41)(4.41)(4.42)X广气,-X-+『MZk=HkXk+匕其中，妙=I+FAt+—(FAt)2+1(FAt)3+1(FAt)4+...k,S12!3!4!

状态方程和观测方程中的系统噪声具有如下性质：E{W(t)}=0，E{W(t)Wt(t)}=Q(t)6(t-t)EV(t)}=0，E{V(t)Vt(t)}=R(t)6(t-t)E{W}=0，kE{WWt)=Q6火.,E{V}=0，kE{VVt)=R6火。式中，At为滤波更新周期，Q=GQGt,Q=rkQFto2离散Kalman滤波方程状态一步预测2离散Kalman滤波方程状态一步预测△t2Q△t2Q=QAt+[FQ+(FQ)t]-—+k2nrZFQ+(FQ)T+F^F^q+(F<Q)T3!(4.43)乂人/~=七-X』(4.44)状态估计X广Xk妇+K(.-"k履(4的滤波增益K=。比Ht[HP^Ht+RJ1(4.46)均方差一步预测Pk/k一1=4,k-iPk北-i+Qk-i(4.47)均方差误差估计kkkk/k-ikkkkk-i4.6量测方程选取MEMS惯组输出的位置、速度参数与GPS接收机提供的位置、速度参数之差作为观测量，以上参数均在导航系下进行计算，与4.5中Kalman滤波方案一样。P=(I-KH)kkkk/k-ikkkkk-i4.6量测方程选取MEMS惯组输出的位置、速度参数与GPS接收机提供的位置、速度参数之差作为观测量，以上参数均在导航系下进行计算，与4.5中Kalman滤波方案一样。惯导解算模块主要包括完成惯性仪表的误差补偿、捷联矩阵的计算、导航参数计算等。组合导航计算机系统每1ms采集一次陀螺和加表的原始输出数据，每5ms进行一次角速度、比力等信息的计算，具体解算流程如图5-1所示。度计T-2（、、►地速计—V转换hVPyT4T3陀螺T1四元数的即时修正OieQ加速fb_］比力的坐标速率度计T-2（、、►地速计—V转换hVPyT4T3陀螺T1四元数的即时修正OieQ加速fb_］比力的坐标速率m提取3）姿态计卷占、V速度p的即I修正■lJ4位置计（地球速率计算匕W性'姿态速率计算,、厂时视』'位置速率计％...位置矩阵'算■的即时修正1I0T阵的计算广四元数的最佳归一，伊高度计算g的计算组合导航采用基于速度、位置组合模式的卡尔曼滤波器，系统采用间接法反馈校正方式，并对修正幅度进行限幅，限幅值根据未修正时间、上次修正的幅度和GPS数据的DOP值确定，对于由卫星换星造成的跳变有很好的抑制作用。5.2INS/GPS组合算法信号流程具体流程如图5-2所示。

图5-2组合算法信号流程图6系统仿真6.1仿真系统介绍该仿真系统，按照上述组合导航仿真算法，在visualc++6.0软件上进行了实现。通过设定飞行器的飞行条件，系统得到飞行器的位置，速度，姿态角信息，在仿真结束后自动生成仿真数据。通过MATLAB对仿真数据进行分析，得到预先设定的飞行参数和仿真参数之间的误差。6.2模拟飞机飞行条件，初始化仿真数据仿真给定的初始位置：南京（纬度32:03N经度118:46E高度0m）仿真给定的初始速度：东向速度（0.00m/s）北向速度（0.00m/s）天向速度（0.00m/s）仿真给定的初始姿态航向角：东向（0.00度）北向（0.00度）天向（135.00度）设置飞行任务：任务类