本发明提供一种基于GPS/INS组合导航系统不同测量特性的自适应滤波方法，该方法利用IMU测量得到的数据与INS的初始数据进行捷联惯导实时解算，建立GPS/INS速度位置组合滤波系统方程，并根据GPS/INS双系统测量互差分序列，统计计算GPS测量噪声协方差估计值，进行自适应的卡尔曼滤波解算。本发明实现了GPS测量噪声的实时跟踪，以及滤波增益矩阵的自适应调节，提高了组合导航系统定位精度。
1.一种基于GPS/INS组合导航系统不同测量特性的自适应滤波方法，包括：步骤一：利用IMU惯性测量单元测量得到沿载体轴相对于惯性空间的角速率和加速度分量信息，与INS惯性导航系统的初始位置、速度、姿态及初始姿态矩阵信息，进行捷联惯导实时解算，得到INS位置、速度、姿态的输出值；
步骤二：根据INS的平台误差角方程、速度误差方程、位置误差方程以及陀螺、加速度计误差模型，并根据INS与GPS的位置差、速度差，分别建立GPS/INS组合导航系统卡尔曼滤波器的状态方程与观测方程；
步骤三：通过连续系统离散化，建立离散型卡尔曼滤波器的递推方程；
步骤四：根据GPS数据采集频率，计算GPS/INS双系统测量互差分序列；
其中，k为组合导航系统滤波解算时刻，即GPS的数据采集时刻；t0为惯性可信时间阈值，根据惯导系统测量精度而确定；ZINS(k)为k时刻捷联惯导实时解算的位置、速度；ZGPS/INS(k-1)为k-1时刻的组合导航系统输出的位置、速度； 通过如下方式得到：以ZGPS/INS(k-1)为k-1时刻的初值，采用k-1~k时间段内IMU测量数据，通过捷联惯导解算步骤，进行k-1~k时间段的惯性递推运算，得到k时刻的位置、速度，即为（2）GPS测量差分序列为：
ΔGPS(k)＝ZGPS(k)-ZGPS(k-1) k＝1,2,3,...
其中，ZGPS(k)为k时刻GPS输出的位置、速度；
（3）GPS/INS双系统测量互差分序列为C(k)：
C(k)＝ΔINS(k)-ΔGPS(k)k＝1，2,3,...；
步骤五：设定时间阈值T，若组合导航系统滤波解算时刻k大于时间阈值T则直接进行步骤六；若组合导航系统滤波解算时刻k不大于时间阈值T，则对步骤四中得到的GPS/INS双系统测量互差分序列进行小样本统计的可信度判别：
1）对GPS/INS双系统测量互差分序列，进行无重叠时间段的开窗统计，得到GPS/INS双系统测量的方差值序列D(k)：其中，k为组合导航系统滤波解算时刻，即GPS的数据采集时刻；N为无重叠统计窗的窗口宽度，C(i)为GPS/INS的双系统测量互差分序列在i时刻的值；
2）对1）中得到的GPS/INS双系统测量的方差值序列D(k)，进行小样本统计的可信度判别，该判别条件为：k≥3N,j＝{k-2N,k-N,k}
若组合导航系统滤波解算时刻k小于或等于时间阈值T时已满足上述可信度判别条件，则执行步骤六，且进入步骤六时的GPS测量噪声协方差估计值为：k≤T
若组合导航系统滤波解算时刻k等于时间阈值T时仍不满足所述的可信度判别条件，则执行步骤六，且进入步骤六时GPS测量噪声协方差估计值为：k＝T；
若组合导航系统滤波解算时刻k小于时间阈值T时不满足上述可信度判别条件，则等待下一时刻的统计结果；
步骤六：根据GPS/INS双系统测量互差分序列，采用连续滑动窗口法计算GPS测量噪声协方差估计值，将其代入步骤三所述的卡尔曼滤波器的递推方程，进行自适应的卡尔曼滤波解算；
①将步骤四中的GPS/INS的双系统测量互差分序列C作为统计样本，计算该统计样本在连续滑动窗内的均值和方差，计算公式依次为：其中，k为组合导航系统滤波解算时刻，即GPS的数据采集时刻；M为连续滑动窗的窗口宽度，C(i)为GPS/INS的双系统测量互差分序列在i时刻的值，E(k)为k时刻的均值，2
σ(k)为k时刻的标准差，σ(k)为k时刻的方差；
②针对连续滑动窗内的GPS/INS的双系统测量互差分序列，进行野值点判别，该判别条件为：|C(i)-E(k)|＞α·σ(k) k-M+1≤i≤k其中，α为可调剔除因子，若C(i)满足上述野值点判别条件，则将C(i)置为均值E(k)，得到新的统计样本，再执行步骤①，重新计算该统计样本在连续滑动窗内的均值和方差，最后执行步骤③；否则直接执行步骤③；
③基于统计样本在连续滑动窗内的均值和方差，计算GPS测量噪声协方差的初步估值④根据该GPS测量噪声协方差的初步估值 计算GPS测量噪声协方差估计值，计算公式为：其中，b为遗忘因子，0＜b<1；dk为k时刻的权系数， 为k时刻的GPS测量噪声协方差估计值； 为k-1时刻的GPS测量噪声协方差估计值；
⑤将GPS测量噪声协方差估计值 代入步骤三中卡尔曼滤波器的递推方程，实时调节滤波增益矩阵Kk，进行自适应卡尔曼滤波解算：其中，Xk为k时刻的状态向量，即被估计向量， 为状态Xk-1的卡尔曼滤波估计值，为利用 得到的对Xk的一步预测，Φk，k-1为k-1到k时刻的状态转移矩阵，Γk-1为系统噪声矩阵，Kk为k时刻的滤波增益矩阵，Pk|k-1为一步预测均方误差矩阵，Rk为观测噪声协方差矩阵，Qk-1为k-1时刻系统噪声协方差矩阵，Pk为估计均方误差矩阵，Hk为k时刻的量测矩阵，Zk为k时刻的观测向量，I为单位矩阵。
一种基于GPS/INS组合导航系统不同测量特性的自适应滤
波方法
技术领域
[0001] 本发明涉及一种GPS/INS组合导航信息融合的自适应滤波方法，可有效抑制GPS时变噪声引起的标准卡尔曼滤波精度下降情况，用于提高GPS/INS组合导航定位精度。
背景技术
[0002] GPS/INS(全球定位系统/惯性导航系统)组合导航系统因其互补性和定位高精度性而被广泛应用，其信息融合技术通常为卡尔曼滤波算法。标准的卡尔曼滤波算法是已知噪声统计特性时的最优估计，但在实际情况中，由于可见卫星数目、多路径效应以及仪器内部测量噪声等多种因素会造成GPS测量噪声的变化，标准的卡尔曼滤波算法无法对此进行检测与调节，从而导致精度下降甚至发散，如何有效解决GPS时变噪声对GPS/INS组合导航滤波算法的影响，对于提高组合导航定位精度具有重要意义。
[0003] 近年来，学者们提出了多种自适应算法，研究方向主要集中在：基于信息的自适应估计(IAE)和基于多模型的自适应估计(MMAE)。IAE是通过利用信息序列对噪声统计特性进行自适应估计(典型代表为sage-husa算法)，但此过程中滤波参数间的迭代运算容易导致误差的耦合，影响滤波精度。MMAE是通过并行运算各滤波模型的状态估计，自适应加权得系统状态的总体估计，但由于其计算量大，并没有广泛应用于工程实际中。此外，与模糊逻辑、神经网络等思想相结合，又发展了一些自适应方法，但目前仍处于理论研究阶段。本发明基于GPS、INS的不同测量特性，充分利用导航子系统的测量信息，实现了对GPS测量噪声的自适应估计，具有实际的工程应用意义。
发明内容
[0004] 在不知被测真值的情况下，若只存在一种测量手段，则测量噪声难以确定；若对同一量存在两种不同性质的测量，则可以实现部分噪声的估计。本发明的目的在于克服现有滤波技术的不足，鉴于INS、GPS误差传播的互补性以及惯导系统的短期高精度性，提供一种基于GPS/INS组合导航系统不同测量特性的自适应滤波方法，通过本发明方法能够充分利用测量信息实时计算GPS测量噪声，有效提高GPS噪声未知情况下组合导航系统定位精度。
[0005] 本发明一种基于GPS/INS组合导航系统不同测量特性的自适应滤波方法，通过下述步骤来完成：
[0006] 步骤一：利用IMU(惯性测量单元)的测量数据(沿载体轴相对于惯性空间的角速率和加速度分量信息)与INS(惯性导航系统)的初值(INS的初始位置、速度、姿态及初始姿态矩阵信息)，通过捷联惯导实时解算，得到INS的输出值(INS位置、速度、姿态的输出值)；
[0007] 步骤二：根据INS动态误差(包括：INS的平台误差角方程、速度误差方程、位置误差方程以及陀螺、加速度计误差模型)以及INS与GPS的位置差、速度差，分别建立GPS/INS组合导航系统卡尔曼滤波器的状态方程与观测方程；
[0008] 步骤三：通过连续系统离散化，建立离散型卡尔曼滤波器的递推方程；
[0009] 步骤四：根据GPS数据采集频率，计算GPS/INS双系统测量互差分序列；
[0010] 步骤五：设定时间阈值T，若组合导航系统滤波解算时刻k不大于时间阈值T，则对步骤四中得到的GPS/INS双系统测量互差分序列进行小样本统计的可信度判别；若组合导航系统滤波解算时刻k大于时间阈值T则直接进行步骤六；
[0011] 步骤六：根据GPS/INS双系统测量互差分序列，采用连续滑动窗口法计算GPS测量噪声协方差估计值，将其代入步骤三所述的卡尔曼滤波器的递推方程，进行自适应的卡尔曼滤波解算。
[0012] 通过上述方法，构造GPS/INS双系统测量互差分序列，消去真实值的相对变化量，获得含GPS测量噪声信息的有效序列，实现了GPS测量噪声协方差的实时测量统计，自适应调整滤波增益，提高组合系统输出精度和滤波稳定性。
[0013] 本发明的优点在于：
[0014] 1、本发明通过对GPS/INS双系统测量互差分序列进行统计分析，实现了GPS测量噪声的实时跟踪，以及滤波增益矩阵的自适应调节，提高了组合导航系统定位精度；
[0015] 2、本发明充分利用GPS、陀螺、加速度计等测量信息完成对GPS测量噪声的自适应估计，有效避免了基于信息的自适应算法利用信息序列进行噪声统计特性估计时滤波参数间的误差耦合现象，具有良好的算法稳定性和系统鲁棒性；
[0016] 3、本发明方法简单，易于操作。
附图说明
[0017] 图1为本发明方法流程图；
[0018] 图2a为陀螺随机常值漂移0.01°/h，随机漂移0.005°/h时，本发明方法、标准卡尔曼滤波算法、基于信息的自适应滤波算法间经度方向R值与GPS测量噪声经度方向预设参考值的仿真结果对比图；
[0019] 图2b为陀螺随机常值漂移0.01°/h，随机漂移0.005°/h时，本发明方法与标准卡尔曼滤波算法、基于信息的自适应滤波算法间组合系统输出值的经度误差的仿真结果对比图；
[0020] 图2c为陀螺随机常值漂移0.01°/h，随机漂移0.005°/h时，本发明方法、标准卡尔曼滤波算法、基于信息的自适应滤波算法间纬度方向R值与GPS测量噪声纬度方向预设参考值的仿真结果对比图；
[0021] 图2d为陀螺随机常值漂移0.01°/h，随机漂移0.005°/h时，本发明方法与标准卡尔曼滤波算法、基于信息的自适应滤波算法间组合系统输出值的纬度误差的仿真结果对比图；
[0022] 图3a为陀螺随机常值漂移增为0.1°/h，随机漂移增为0.05°/h时，本发明方法、标准卡尔曼滤波算法、基于信息的自适应滤波算法间经度方向R值与GPS测量噪声经度方向预设参考值的仿真结果对比图；
[0023] 图3b为陀螺随机常值漂移增为0.1°/h，随机漂移增为0.05°/h时，本发明方法与标准卡尔曼滤波算法、基于信息的自适应滤波算法间组合系统输出值的经度误差的仿真结果对比图；
[0024] 图3c为陀螺随机常值漂移增为0.1°/h，随机漂移增为0.05°/h时，本发明方法、标准卡尔曼滤波算法、基于信息的自适应滤波算法间纬度方向R值与GPS测量噪声纬度方向预设参考值的仿真结果对比图；
[0025] 图3d为陀螺随机常值漂移增为0.1°/h，随机漂移增为0.05°/h时，本发明方法与标准卡尔曼滤波算法、基于信息的自适应滤波算法间组合系统输出值的纬度误差的仿真结果对比图。
具体实施方式
[0026] 下面结合附图与实施例对本发明做进一步的详细说明。
[0027] 本实施例中载体的初始位置为东经116°，北纬39.2°，高度1000m，速度300m/s，航向角35°，飞行7200s。陀螺随机常值漂移0.01°/h，随机漂移0.005°/h，加速度计随机常值漂移100μg，随机漂移50μg。GPS速度误差0.1m/s，高度误差120m，经纬度误差为15m。为了考察GPS/INS组合导航系统对GPS测量抗干扰的效果，GPS经纬度误差在1000s-3000s时段增为40m，如图2a、2c所示。应用本发明提供的一种基于GPS/INS组合导航系统不同测量特性的自适应滤波方法，如图1所示流程图，通过下列步骤实现自适应滤波：
[0028] 步骤一：利用IMU(惯性测量单元)测量得到沿载体轴相对于惯性空间的角速率和加速度分量信息)，与INS(惯性导航系统)的初始位置、速度、姿态及初始姿态矩阵信息，进行捷联惯导实时解算，得到INS的位置、速度、姿态的输出值；
[0029] 步骤二：根据INS动态误差(包括：INS的平台误差角方程、速度误差方程、位置误差方程以及陀螺、加速度计误差模型)，建立GPS/INS组合导航系统卡尔曼滤波器的状态方程；根据INS与GPS的位置差、速度差，建立GPS/INS组合导航系统卡尔曼滤波器的观测方程；
[0030] a、状态方程为：
[0031]
[0032] 其中，
[0033] XI(t)＝[φe，φn，φu，δve，δvn，δvu，δL，δλ，δh，εbx，εby，εbz，▽x，▽y，T▽z]
[0034] WI(t)＝[wgx，wgy，wgz，wax，way，waz]T
[0035]
[0036] 矩阵F9×9中非零元素为：
[0037]
[0038]
[0039]
[0040]
[0041]
[0042]
[0043] F(2，7)＝-wie sinL
[0044]
[0045]
[0046]
[0047]
[0048] F(4，2)＝-fu F(4，3)＝fn
[0049]
[0050]
[0051]
[0052]
[0053] F(5，1)＝fu F(5，3)＝-fe
[0054]
[0055]
[0056]
[0057]
[0058] F(6，1)＝-fn F(6，2)＝fe
[0059]
[0060]
[0061] F(6，7)＝-2wievesinL
[0062]
[0063]
[0064]
[0065] F(9，6)＝1
[0066] 上述公式中，下标e、n、u为导航系的东、北、天轴向，下标x、y、z为载体系的右、前、上轴向，φe、φn、φu为平台误差角，δve、δvn、δvu，为速度误差，δL、δλ、δh分别为纬度误差、经度误差、高度误差，εbx、εby、εbz为陀螺的随机常值漂移，wgx、wgy、wgz为陀螺的随机漂移，▽x、▽y、▽z为加速度计的随机常值漂移，wax、way、waz为加速度计的随机漂移， 为载体系到导航系的姿态转移矩阵，wie为地球自转角速度，Rm、Rn分别为子午圈、卯酉圈上各点的曲率半径，ve、vu、vn分别为沿东北天方向的速度，fe、fu、fn分别为沿东北天方向加速度值，λ、L、h分别为经度、纬度、高度。
[0067] b、观测方程为：
[0068]
[0069] 其中，T
[0070] Hp(t)＝[03×6 diag[Rm Rn cosL 1] 03×6]，Vp(t)＝[Nn，Ne，Nu]，T
[0071] Hv(t)＝[03×3 diag[1 1 1] 03×12]，Vv(t)＝[Me，Mn，Mu]。
[0072] 上述各式中，Zp(t)、Zv(t)分别为t时刻的位置测量矢量、速度测量矢量，Ne、Nn、Nu分别为GPS接收机沿东、北、天方向的位置误差，Me、Mn、Mu分别为GPS接收机的沿东、北、天方向的测速误差。
[0073] 步骤三：通过连续系统离散化，建立离散型卡尔曼滤波器的递推方程；
[0074] A、将步骤二得到的状态方程和观测方程进行离散化，得到系统方程：
[0075]
[0076] 其中，k为组合导航系统滤波解算时刻，即GPS的数据采集时刻；Xk为k时刻的状态向量，即被估计向量；Zk为k时刻的观测向量；Φk，k-1为k-1到k时刻的状态转移矩阵；Wk-1为k-1时刻的系统噪声；Γk-1为系统噪声矩阵；Hk为k时刻的量测矩阵；Vk为k时刻的量测噪声；
[0077] B、建立离散型卡尔曼滤波器的递推方程：
[0078]
[0079] 其中， 为状态Xk-1的卡尔曼滤波估计值， 为利用 得到的对Xk的一步预测，Kk为k时刻的滤波增益矩阵，Pk|k-1为一步预测均方误差矩阵，Rk为观测噪声协方差矩阵，Qk-1为k-1时刻系统噪声协方差矩阵，Pk为估计均方误差矩阵，I为单位矩阵。
[0080] 步骤四：根据GPS的数据采集频率，得到INS测量差分序列以及GPS测量差分序列，从而得到GPS/INS双系统测量互差分序列；
[0081] (1)INS测量差分序列为：
[0082]
[0083] 其中，t0为惯性可信时间阈值，根据惯导系统测量精度而确定，一般为20s～40s；ZINS(k)为k时刻捷联惯导实时解算的位置、速度；ZGPS/INS(k-1)为k-1时刻的组合导航系统输出的位置、速度； 通过如下方式得到：以ZGPS/INS(k-1)为k-1时刻的初值，采用k-1～k时间段内IMU测量数据，通过捷联惯导解算步骤，进行k-1～k时间段的惯性递推运算，得到k时刻的位置、速度，即为
[0084] (2)GPS测量差分序列为：
[0085] ΔGPS(k)＝ZGPS(k)-ZGPS(k-1) k＝1，2，3，…
[0086] 其中，ZGPS(k)为k时刻GPS输出的位置速度；
[0087] (3)GPS/INS双系统测量互差分序列为C(k)：
[0088] C(k)＝ΔINS(k)-ΔGPS(k) k＝1，2，3，…
[0089] 步骤五：设定时间阈值T为300s，若组合导航系统滤波解算时刻k不大于时间阈值T，则对步骤四中得到的GPS/INS双系统测量互差分序列进行小样本统计的可信度判别；若组合导航系统滤波解算时刻k大于时间阈值T则直接进行步骤六；
[0090] 其中，可信度判别过程为：
[0091] 1)对GPS/INS双系统测量互差分序列，进行无重叠时间段的开窗统计，得到统计结果，即GPS/INS双系统测量的方差值序列D(k)：
[0092]
[0093] 其中，C(i)为GPS/INS的双系统测量互差分序列在i时刻的值，N为无重叠统计窗的窗口宽度，取为50s；
[0094] 2)对1)中得到的GPS/INS双系统测量的方差值序列D(k)，进行小样本统计的可信度判别，该判别条件为：
[0095]
[0096] 其中，max为求最大值的函数；
[0097] 若组合导航系统滤波解算时刻k小于或等于时间阈值T时已满足上述可信度判别条件，则执行步骤六，且进入步骤六时的GPS测量噪声协方差估计值为：
[0098]
[0099] 若组合导航系统滤波解算时刻k等于时间阈值T时仍不满足所述的可信度判别条件，则执行步骤六，且进入步骤六时GPS测量噪声协方差估计值为：
[0100]
[0101] 若组合导航系统滤波解算时刻k小于或等于时间阈值T时不满足上述可信度判别条件，则等待下一时刻的统计结果。
[0102] 在上述可信度判别过程中，只进行小样本统计的可信度判别，不进行GPS测量噪声协方差的自适应调节，该值恒为初始时刻预设的经验值，GPS/INS组合导航滤波系统进行标准的卡尔曼滤波递推解算，并输出。
[0103] 步骤六：根据GPS/INS双系统测量互差分序列，采用连续滑动窗口法计算GPS测量噪声协方差估计值，将其代入步骤三中卡尔曼滤波器的递推方程，进行自适应的卡尔曼滤波解算。
[0104] ①将步骤四中的GPS/INS的双系统测量互差分序列C作为统计样本，计算该统计样本在连续滑动窗内的均值和方差，计算公式依次为：
[0105]
[0106] 其中，M为连续滑动窗的窗口宽度，取为40s，C(i)为GPS/INS的双系统测量互差2
分序列在i时刻的值，E(k)为k时刻的均值，σ(k)为k时刻的标准差，σ(k)为k时刻的方差；
[0107] ②针对连续滑动窗内的GPS/INS的双系统测量互差分序列C，进行野值点判别，该判别条件为：
[0108] |C(i)-E(k)|＞α·σ(k) k-M+1≤i≤k
[0109] 其中，α为可调剔除因子，2.5≤α≤3.5，若C(i)满足上述野值点判别条件，则将C(i)置为均值E(k)，得到新的统计样本，再执行步骤①，重新计算该统计样本在连续滑动窗内的均值和方差，最后执行步骤③；否则直接执行步骤③；
[0110] ③基于统计样本在连续滑动窗内的均值和方差，计算GPS测量噪声协方差的初步估值
[0111]
[0112] ④根据该GPS测量噪声协方差的初步估值 计算GPS测量噪声协方差估计值，计算公式为：
[0113]
[0114] 其中，b为遗忘因子，0＜b＜1，一般取为0.95～0.995；dk为k时刻的权系数，为k时刻的GPS测量噪声协方差估计值； 为k-1时刻的GPS测量噪声协方差估计值；
[0115] ⑤将GPS测量噪声协方差估计值 代入步骤三中卡尔曼滤波器的递推方程，实时调节滤波增益矩阵Kk，进行自适应卡尔曼滤波解算：
[0116]
[0117] 卡尔曼滤波解算的结果为惯导系统INS的误差估计值，根据该误差估计值对捷联惯导实时解算结果进行修正，得到GPS/INS组合导航系统输出。
[0118] 通过上述方法，构造GPS/INS双系统测量互差分序列，消去真实值的相对变化量，获得含GPS测量噪声信息的有效序列，实现了GPS测量噪声协方差的实时测量统计，自适应调整滤波增益，提高组合系统输出精度和滤波稳定性。
[0119] 依据上述步骤，本实施例仿真结果如图2a～2d、图3a～3d细实线所示。图中：点虚线为标准卡尔曼滤波算法仿真结果，虚线为基于信息的自适应滤波算法(sage-husa算法)仿真结果，粗实线为GPS测量噪声预设参考值。
[0120] i、由图2a、图2c可看出：标准卡尔曼滤波算法的经纬度方向R始终为预设经验值2
(100m)，不能进行自适应调节；本发明方法与基于信息的自适应滤波算法的经纬度方向R可以跟踪实际的GPS测量噪声；
[0121] ii、由图2b、图2d可看出：标准卡尔曼滤波算法的经纬度误差抖动较大，尤其在1000s-3000s时段；本发明方法与基于信息的自适应滤波算法的误差值抖动较小，定位精度高；在4000s-6000s时段，基于信息的自适应滤波算法存在误差耦合现象，导致经纬度误差值偏离零值，而本发明的自适应算法的经纬度误差值保持零值附近抖动。
[0122] 当将本实施例中陀螺随机常值漂移增为0.1°/h，随机漂移增为0.05°/h，其他条件保持不变时，滤波结果如图3a～3d所示。由图可看出，当惯导系统精度降低时，基于信息的自适应滤波算法中误差耦合现象更为严重，经度方向R的估计值已不可信，导致滤波误差很大，而本发明的自适应算法能保持较高精度和良好的滤波稳定性。
