本发明涉及组合导航自适应滤波技术领域，尤其涉及一种结合协方差和sage-husa滤波技术的自适应滤波方法。
背景技术
组合导航系统要求具有较高的导航精度，组合导航系统的常规卡尔曼滤波基于建模准确的线性模型和完备的噪声统计特性，在实际使用中受外部环境的影响较大，给模型建立的准确性和噪声统计特性的准确描述带来困难，这将导致常规卡尔曼滤波器不稳定甚至发散，因而有必要对常规卡尔曼滤波方法进行改进，用以解决组合导航系统的导航精度易受外部环境影响降低、可靠性差的问题。
技术实现要素：
鉴于上述的分析，本发明旨在提供一种结合协方差和sage-husa滤波技术的自适应滤波方法，用以解决组合导航系统的导航精度受外部环境影响降低、可靠性差的问题。
本发明提供了一种结合协方差和sage-husa滤波技术的自适应滤波方法，包括以下步骤：
将载波相位差分gps与捷联惯导组成sins/dgps组合导航系统，建立sins/dgps组合导航系统的滤波器数学模型；
利用sage-husa自适应卡尔曼滤波算法，对sins/dgps组合导航系统进行滤波；
基于协方差匹配技术的滤波发散判据对sage-husa自适应滤波算法进行改进，利用改进后的sage-husa自适应滤波算法对sins/dgps组合导航系统进行滤波。
上述技术方法的有益效果为：通过上述方案实现了sins/dgps组合导航系统的自适应滤波，提高了滤波精度与稳定性，满足了sins/dgps组合导航系统高精度和高可靠性的要求。
进一步地，建立sins/dgps组合导航系统的滤波器数学模型，具体包括，sins/dgps组合导航系统的滤波器数学模型以捷联惯导系统误差方程为基础建立状态方程，根据所选量测量建立量测方程，所选量测量包括sins的速度、位置分别与dgps的速度和位置之差。
进一步地，所述状态方程和量测方程分别为
其中，φe,φn,φu分别为东、北、天3个方向的平台姿态角误差；δve,δvn,δvu分别为东、北、天3个方向的速度误差；δl,δλ,δh分别为纬度、经度、高度位置误差；εbx,εby,εbz分别3个陀螺漂移误差；
上述进一步技术方案的有益效果为：通过上述建立了sins/dgps组合导航系统滤波器的状态方程和量测方程。
进一步地，利用sage-husa自适应卡尔曼滤波算法，对sins/dgps组合导航系统进行滤波，具体包括，对所述滤波器数学模型离散化处理得到滤波器的离散系统数学模型，根据所述离散系统数学模型构建sage-husa自适应滤波算法，利用所述算法通过极大后验估计器以各子系统导航参数的误差量为估计对象，在线估计系统噪声和量测噪声的次优无偏估计，其中，所述算法包括滤波方程和时变噪声估计器。
上述进一步技术方案的有益效果为：通过上述技术方案实现了利用sage-husa自适应卡尔曼滤波算法，对sins/dgps组合导航系统进行滤波。
进一步地，对所述滤波器数学模型离散化处理得到滤波器的离散系统数学模型为
其中，qk表示系统噪声序列的方差阵，假设为非负定阵；rk表示量测噪声序列的方差阵，假设为正定阵；δkj为kroneckerδ函数。
进一步地，上述滤波方程包括，
pk＝(i-kkhk)pk/k-1
其中，
进一步地，上述时变噪声估计器为
其中，dk＝(1-b)/(1-bk+1)，b为遗忘因子，
进一步地，所述基于协方差匹配技术的滤波发散判据为
式中，γ为储备系数，γ≥1，当γ＝1时为最严格的收敛判据条件；tr表示矩阵的迹，若上式成立，则表明第k次滤波出现异常，若上式不成立，则表明第k次滤波无异常。
上述进一步技术方案的有益效果为：采用基于协方差匹配技术的滤波发散判据对简化的sage-husa自适应滤波算法进行改进，提高了滤波的稳定性。
进一步地，对上述sage-husa自适应滤波算法进行简化，得到简化的sage-husa自适应滤波算法，所述简化的sage-husa自适应滤波算法公式中的
进一步地，在滤波过程中，若进行第k次滤波时，滤波出现异常，则对
本发明中，上述各技术方案之间还可以相互组合，以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书、权利要求书以及附图中所特别指出的内容中来实现和获得。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
图1为本发明实施例所述方法流程示意图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。
实施例1
本发明实施例提供一种结合协方差和sage-husa滤波技术的自适应滤波方法，所述方法流程示意图如图1所示。所述方法包括以下步骤：
步骤s01、将载波相位差分gps与捷联惯导组成sins/dgps组合导航系统，建立sins/dgps组合导航系统的滤波器数学模型；
由于载波相位差分gps(dgps)具有长时间绝对定位的稳定性，而捷联惯导(sins)具有短时间相对定位的稳定性，所以sins与dgps组合是最为理想的一种组合导航方案；sins/dgps组合导航系统可以动态快速地给出反映载体运动的线运动参数和角运动参数，据此可以提供很多有用的信息，例如，对运动轨迹进行精密定位，给出动态导航定位、测速、测定姿态信息；两种导航组合能够克服各自缺点，使组合后的导航精度高于两个系统单独工作的精度；
利用北斗地基增强系统和千寻位置网差分数据服务与网络通讯技术，可以得到载波相位差分gps(dgps)高精度定位信息，北斗地基增强系统作为基准站，主要是将卫星发送过来的信息误差进行修改，使得定位更加精确；基准站将观测值和测站坐标信息一起通过数据链传送给移动站(用户机)，移动站在系统内组成差分观测值进行实时处理，得到载波相位差分gps(dgps)高精度定位信息；载波相位差分gps(dgps)与捷联惯导(sins)组成组合导航系统，该组合导航系统也适用于北斗与sins进行组合；
支持卫星定位的用户机，通过接收卫星信号，可以获得所在地点的粗位置数据，通过网络将位置数据(gga语句)上传到千寻位置网差分数据服务平台，一同上传的还有ntrip协议参数；千寻位置网差分数据服务平台获得数据后，通过网络通讯下播位置差分数据，经过用户机的差分解算结合原始观测值和差分数据，解算和纠偏最终得到高精度的定位数据；
采用间接法卡尔曼滤波方式，即以各子系统导航参数的误差量作为估计对象来实现组合导航的滤波处理方法；sins/dgps组合导航系统的滤波器数学模型以捷联惯导系统误差方程为基础建立状态方程，根据所选量测量建立量测方程；sins与dgps组合导航系统滤波器的状态方程和量测方程为：
z1＝h1x1+v1(2)
其中，x1为系统状态矢量；f1为系统动态矩阵；g1为动态噪声驱动矩阵；z1为观测矢量；h1为观测系数矩阵；w1是系统噪声向量，v1是观测噪声向量，选取sins/dgps组合的滤波器系统状态为：
其中，φe,φn,φu表示东、北、天3个方向的平台姿态角误差；δve,δvn,δvu表示东、北、天3个方向的速度误差；δl,δλ,δh表示纬度、经度、高度位置误差；εbx,εby,εbz表示3个陀螺漂移误差；
组合导航系统采用位置速度组合方式，以捷联惯导系统的速度、位置与dgps速度和位置之差作为卡尔曼滤波器的量测量。
步骤s02、利用sage-husa自适应卡尔曼滤波算法，对sins/dgps组合导航系统进行滤波；
具体的，根据sage-husa自适应卡尔曼滤波算法，利用观测数据进行滤波递推计算的同时，通过时变噪声估计器实时估计和校正系统噪声和量测噪声的统计特性；
对上述滤波器数学模型离散化处理得到离散系统数学模型为：
其中，xk表示线性离散系统k时刻的n维状态向量；xk-1表示线性离散系统k-1时刻的n维状态向量；zk表示线性离散系统k时刻的m维观测序列；φk,k-1表示线性离散系统k-1时刻至k时刻的n×n维状态转移矩阵；hk表示线性离散系统k时刻的m×n维观测矩阵；wk表示线性离散系统k时刻的激励噪声；vk表示线性离散系统k时刻的量测噪声。wk和vk是相互独立的白噪声，其均值和协方差满足式(8)。
式(8)中，qk表示系统噪声序列的方差阵，假设为非负定阵；rk表示量测噪声序列的方差阵，假设为正定阵；δkj为kroneckerδ函数。
根据式(7)所描述的系统方程和量测方程，其自适应滤波算法为：
pk＝(i-kkhk)pk/k-1(14)
其中，
上述式(9)-(14)为滤波方程，在一个滤波周期内，从卡尔曼滤波在使用系统信息和观测信息的先后次序来看，卡尔曼滤波具有时间更新过程和观测更新过程，公式(9)和公式(13)将时间从k-1时刻推进至k时刻。式(15)、(16)为时变噪声估计器方程；其中，ek为滤波器的量测残差(新息)，dk＝(1-b)/(1-bk+1)，b为遗忘因子，且有0<b<1，一般取值范围为0.95～0.99，b的选择必须综合考虑时变渐消参数的跟踪性能与噪声的不敏感性；
sage-husa自适应滤波算法通过极大后验(map)估计器以sins/dgps组合导航系统各子系统导航参数的误差量为估计对象，在线估计系统噪声和量测噪声，得到噪声的次优无偏估计；
由式(15)和式(16)可知，陈旧信息与新息对噪声估计值的加权系数分别为(1-dk)和dk，它们的稳态值分别为遗忘因子b及1-b，由于有：
因而，随着滤波的进行，陈旧信息对噪声估计值的作用越来越小，而新息对噪声估计值的作用会越来越大，当滤波达到稳态时，噪声估计值主要受新息ek(或kkek)的影响，因此滤波器必然随着实际环境的变化而变化，从而实现噪声的“自适应”效果；
需要说明的是，理论上当系统噪声方差阵qk和量测噪声方差阵rk未知时，按照上述流程可以把qk和rk同时估计出来。然而事实上sage-husa自适应滤波不能在qk和rk均未知的情况下把它们同时估计出来，从式(15)和式(16)可知qk和rk都依赖新息向量ek，ek的异常变化会同时影响
步骤s03、基于协方差匹配技术的滤波发散判据对sage-husa自适应滤波算法进行改进，利用改进后的sage-husa自适应滤波算法对sins/dgps组合导航系统进行滤波；
首先，对sage-husa自适应滤波算法进行简化，假设qk为常数，则简化的sage-husa自适应滤波算法公式如下：
为了保证
即牺牲一定的滤波精度来换取滤波的稳定性；
为了进一步的提高滤波的稳定性，抑制滤波的发散，采用基于协方差匹配技术的滤波发散判据对简化的sage-husa自适应滤波算法进行改进；
协方差匹配技术在滤波的同时检验实际的余项，并判断余项是否是相容的，即滤波是否出现异常；当实际的余项在原假设qk-1、rk-1下不相容时，则对qk、rk进行估计来取代原来假设的qk-1、rk-1；
判断滤波异常的判据为：
式中，γ为储备系数，γ≥1，当γ＝1时为最严格的收敛判据条件；tr表示矩阵的迹。若式(20)成立，表明第k次滤波出现异常，原来的数学模型已不适合当前的滤波，这时式(19)不可用，则省去式(19)的计算，用
本发明提供了一种结合协方差和sage-husa滤波技术的自适应滤波方法，所述方法不需要在每次滤波时计算加权系数dk和量测噪声方差阵rk，计算的复杂度降低，尤其在系统阶数较高时能提高系统的实时性，同时对模型误差和量测噪声协方差具有自适应性，能够保证滤波收敛和稳定，从而满足组合导航的高精度和高可靠性要求，具有一定的工程应用价值。
本领域技术人员可以理解，实现上述实施例方法的全部或部分流程，可以通过计算机程序来控制相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。