专利名称：一种sins/gps组合导航系统自适应滤波方法
技术领域：
本发明涉及的是一种SINS/GPS组合导航系统自适应滤波方法，可有效地抑 制由时变噪声引起的传统卡尔曼滤波精度下降的情况。
(二)
背景技术：
SINS/GPS组合导航系统因其结合了惯导系统与GPS全球定位系统的优势， 一直吸引大量科研人员进行深入的理论研究，并在车载、舰船、飞行器中有着广 泛的应用。卡尔曼滤波作为通用的SINS/GPS组合导航系统信息融合算法，要求 精确获知量测噪声的统计信息；当噪声模型不准确或无法预知的情况下，传统卡 尔曼滤波精度会大大降低。而GPS全球定位系统的定位精度不仅取决于器件误 差，同时还依赖于星可见度及载体运行环境，仅按经验值预置GPS观测噪声无 法适应GPS的工作特性，如何有效地解决时变GPS噪声对卡尔曼滤波算法的影 响，对提高SINS/GPS组合导航系统精度具有重要意义。
近年来，许多学者对自适应滤波进行了深入的研究，如协方差及系统噪声縮 放算法，多模自适应估计算法(顧AE)，引入模糊逻辑的自适应算法，基于新息 序列的自适应算法，等等。多模自适应估计算法采用多个并行卡尔曼滤波器实现 对模型及噪声的估计，但由于其计算量大，并没有被广泛的应用于实际工程中。 常用的Sage-Husa噪声统计估值器采用新息序列均值法对噪声进行估计， 4=li>,《=^^4_1+^,，滑动窗长度为k(k为当前时刻)，易知，随着时间的增加，
当前新息对估值的作用逐渐降低，fc^，"v〖—0。系统长时间运行后，当由于环
境或载体机动引起噪声变化时，Saga-Husa算法无法解决该情况的噪声估算问题。 本发明基于新息序列的自适应算法，提出了根据基于当前量测残差自动调节滑动 窗长度的方法，进行在线噪声估计，将新息值与噪声估计有效地结合起来，既保 证了系统的原有平稳特性，又充分利用新息序列对时变噪声进行有效的在线估 计，具有实际的工程应用意义。
(三) 发明内容本发明的目的在于提供一种能够有效地提高GPS噪声未知的情况下组合导 航系统的定位精度的一种SINS/GPS组合导航系统自适应滤波方法。
本发明的目的是这样实现的
步骤l:用捷联惯导系统实时采集陀螺加速度信息，对实时采集的陀螺加速 度信息进行位置速度姿态解算；
步骤2:通过串口实时采集GPS信号，按照数据格式从缓冲区中读取GPS提 供的位置速度信息；
步骤3:根据SINS动态误差模型建立组合导航系统的状态方程，利用SINS 与GPS的位置速度误差建立量测方程；
步骤4:依组合导航解算频率，根据解算出的SINS位置、速度信息与GPS 信号计算新息大小，及量测残差值，量测残差大小反映了GPS信号噪声大小；
步骤5:利用量测残差值，对滑动窗口长度进行自动调节；
步骤6:依滑动窗大小计算新息均值，对GPS量测信号噪声协方差进行实时 计算；
步骤7:将噪声协方差代入到滤波预测过程及量测更新过程，计算滤波增益，
进行组合导航解算。
本发明根据当前新息自动调节滑动窗长度，对时变噪声进行计算的自适应组
合导航方法，具体步骤为
1、 根据SINS与GPS观测量及观测方程求取当前新息及量测残差值(第k 时刻)。
《—，
Ct=M[vt《] +&
a=《c:'v, = (zt—《_,)r(/ +,,'//'r+w(z广《—■) 其中，^为新息值，G为新息均方差，^为量测残差值。
2、 根据当前量测残差值求取滑动窗长度。
w=i, " 鹏
W = 4 , c < mi
W = J"teger(;t x A"—"腿)，A < 1， W之1 , amln < d < amax 易知，滑动窗最小长度为1，即W", G只取决于当前新息，与历史新息序
5列无关。此时，当前新息比重对噪声估值起了最大作用，残差值大于设置的最大 阈值，即"> _。
残差值小于设置的最小阈值时，滑动窗取最大长度w"，此时与Sage-Husa 极大后验噪声统计估值器一致，即6 4i;w = Y(t, +乂 。
当判断出残差值介于两者之间时，即，滑动窗长度为
w=/"teg^t"" )，义为调节因子，它的大小决定了滑动窗口随量测残差大小的变
化趋势，hi，化i。
3、 依滑动窗大小(滤波窗口大小)计算新息均值，计算时变信号噪声协方 差的方法为；
根据滑动窗大小计算K， K-1, K-2，…,K-N+l时刻新息方差的均值，作为进一 步估算观测噪声协方差阵的依据。
为了避免矩阵奇异，当《4<0时，进行规一化处理，令&=0
4、 将噪声协方差代入到滤波预测过程及量测更新过程，根据噪声方差调节
滤波增益，进行组合导航解算
A =(/-
其中，,为状态变量预测值，对于非线性系统，可通过一阶泰勒展开获得； 、为新息；^为新息协方差；么—,为自适应滤波中的系统噪声协方差；w,为量测
噪声协方差；g,w为一步预测误差均方差；^为滤波增益阵。
本发明利用了通用的导航设备，通过对噪声的实时计算，调节滤波增益，有 效地解决了由时变噪声导致的传统卡尔曼滤波精度下降的情况。常用的Sage-Husa噪声统计估值器作为一种极大后验估值方法，适用于对未知常值噪声 进行估计，其缺点和局限性是不能处理时变噪声。本发明通过对滑动窗大小的自 动调节，有效地将新息引入到噪声估值中来，可在量测信号噪声增大时，减小滤 波增益，降低量测信号对组合导航系统定位的作用；在量测信号噪声减小时，加 大滤波增益，增大新息对组合导航的影响，自动延长的滑动窗口又保持了系统的 平稳特性，有效的解决了由时变噪声导致的传统卡尔曼滤波精度下降的问题，提 高了组合导航系统的定位精度。
(四)
图1为本发明中，SINS/GPS自适应组合导航系统结构图。
图2为本发明中，量测噪声未知情况下自适应扩展卡尔曼滤波流程图。
具体实施方式
下面结合附图举例对本发明做更详细地描述
步骤l、根据捷联惯导系统的误差模型建立系统方程。 捷联惯导误差方程为
<formula>formula see original document page 7</formula>
其中，^为被估计状态；^为噪声序列；cD^为"至^时刻一步转移阵； IV,为系统噪声驱动阵；^为量测阵；^为量测噪声序列； 一，"表示纬度、经度 误差；^;,W,表示东向、北向速度误差；K,&表示东向、北向速度；O,表示
北向、东向水平失准角；么表示方位失准角；^SA表示陀螺漂移: 连续系统的状态方程为一阶泰勒展开，
》=/wo)+r(，， =e(')《('-o
其中，単)=, rA = *f r(,)w ， ￡ r = a, A =叫)，L = r(", a =抓)
SAT J
0w + ^(;)r
取SINS与GPS系统的位置速度误差为观测量，因而可以直接建立离散时间 t测方程
4 =
、脱'—^> /\V
p力
步骤2、计算当前新息值及量测残差大小，根据残差值求取滑动窗长度
v* = Z广股4," a =《ca = (z'-, f(仏尸w,w〖+ t )扁'(z'. —
w = i,""脆 w = /t, < 隱
W = /"tege/^/t x V—。訓')，；i < 1, W》1 , a關< d < max
步骤3、量测噪声未知时，本发明中涉及到的自适应扩展卡尔曼滤波预测方
程与更新过程为
S碌-A&o〖十r必r:
8
权利要求
1、一种SINS/GPS组合导航系统自适应滤波方法，其特征是步骤1用捷联惯导系统实时采集陀螺加速度信息，对实时采集的陀螺加速度信息进行位置速度姿态解算；步骤2通过串口实时采集GPS信号，按照数据格式从缓冲区中读取GPS提供的位置速度信息；步骤3根据SINS动态误差模型建立组合导航系统的状态方程，利用SINS与GPS的位置速度误差建立量测方程；步骤4依组合导航解算频率，根据解算出的SINS位置、速度信息与GPS信号计算新息大小，及量测残差值，量测残差大小反映了GPS信号噪声大小；步骤5利用量测残差值，对滑动窗口长度进行自动调节；步骤6依滑动窗大小计算新息均值，对GPS量测信号噪声协方差进行实时计算；步骤7将噪声协方差代入到滤波预测过程及量测更新过程，计算滤波增益，进行组合导航解算。
2、 根据权利要求1所述的一种SINS/GPS组合导航系统自适应滤波方法，其特征是所述的计算新息及量测残差的具体步骤为v,Z广喊4—,其中，n为新息值，G为新息均方差，d为量测残差值，zt，之为系统的量测 向量与状态向量，//为系统量测矩阵，^为量测噪声协方差，/V,为一步预测误 差协方差。
3、 根据权利要求1或2所述的一种SINS/GPS组合导航系统自适应滤波方 法，其特征是所述的利用量测残差值，对系统k时刻滑动窗口长度进行自动 调节，具体步骤为w = i， "> 腿<formula>formula see original document page 3</formula>其中，N为滑动窗长度；"^与"皿分别为预先设置的最大及最小阈值，当量测残差大于"_时，滑动窗口取最小值1;当量测残差小于"自时，滑动窗口取最大值k， k为当前时刻；A为调节因子，它的大小决定了滑动窗口随量测残 差大小的变化趋势，取整计算滑动窗大小。
4、 根据权利要求1或2所述的一种SINS/GPS组合导航系统自适应滤波方 法，其特征是所述的依滑动窗大小计算新息均值，实时计算量测信号噪声协 方差，其具体步骤为依据滑动窗大小计算相关新息序列均值，计算量测噪声； <formula>formula see original document page 3</formula>为了避免矩阵奇异，当《4<0时，进行规一化处理，令《4=0。
5、 根据权利要求3所述的一种SINS/GPS组合导航系统自适应滤波方法，其特征是所述的依滑动窗大小计算新息均值，实时计算量测信号噪声协方差， 其具体步骤为依据滑动窗大小计算相关新息序列均值，计算量测噪声；为了避免矩阵奇异，当w,O时，进行规一化处理，令&=0。
全文摘要
本发明提供的是一种SINS/GPS自适应组合导航方法，其特征是将捷联惯导系统与GPS全球定位系统进行信息融合，并按组合频率，对由串口接收到的GPS信号进行噪声实时计算。本发明以量测残差为判断基准，通过对滑动窗大小的自动调节，有效地将新息引入到量测信号噪声的实时计算中来，可在GPS量测信号噪声增大时，减小滤波增益，降低量测信号对组合导航系统定位的作用；在量测信号噪声减小时，加大滤波增益，增大新息对组合导航的影响，自动延长的滑动窗口又保持了系统的平稳特性，有效地解决了由时变噪声导致的传统卡尔曼滤波精度下降的问题，提高了组合导航系统的定位精度。
文档编号G01C21/20GK101464152SQ20091007123
公开日2009年6月24日 申请日期2009年1月9日 优先权日2009年1月9日
发明者强 于, 奔粤阳, 枫 孙, 博 徐, 郝燕玲, 真 郭, 伟 高 申请人:哈尔滨工程大学