基于信息异常检测的改进Sage-Husa自适应融合滤波方法及多源信息融合设备与流程

基于信息异常检测的改进sage-husa自适应融合滤波方法及多源信息融合设备
技术领域
1.本发明属于导航定位技术领域，涉及基于信息异常检测的改进sage-husa自适应融合滤波方法及多源信息融合设备。


背景技术：

2.多源信息融合，它是一种对采集到的各传感器输出的数据进行归纳、整合，采用某种规则组合出最优效果的技术。在智能交通系统的导航定位领域，多源信息融合最经典且高效易行的方法为卡尔曼滤波算法(kalman filtering，kf)。但该算法也存在一些局限性，kalman滤波器的计算过程中会不断累积系统误差，导致误差协方差矩阵的正定性受到影响，从而造成滤波估计结果不准确，所以需要改进。


技术实现要素：

3.本发明的目的是提出基于信息异常检测的改进sage-husa自适应融合滤波方法及多源信息融合设备。
4.本发明的技术方案：基于信息异常检测的改进sage-husa自适应融合滤波方法，步骤一：获取传感器量测信息；惯性测量单元中的陀螺仪和加速度计输出相应的角速度和比力的量测信息，gps输出相应的经纬度量测信息；
5.步骤二：建立gps/ins组合导航系统模型，确定出由位置、速度、姿态以及偏差量构成的多维状态量，建立状态方程和量测方程；
6.步骤三：信息异常检测过程，根据预测残差向量构造检验统计量，判断是否有异常观测的存在；当系统无异常量测时，检验统计量不超过置信极限，采用标准卡尔曼滤波进行预测和校正，从而输出融合滤波结果；当系统有异常量测检测时，采用改进的sage-husa自适应滤波，将卡尔曼滤波增益置零，引入指数衰减自适应因子调整观测量测噪声；
7.步骤四：改进的sage-husa自适应滤波方法对组合导航系统进行滤波处理，在标准kalman滤波的基础上对q阵和r阵实时预测和修正，反馈调节滤波增益k的自适应调节过程，同时信息异常时滤波增益置零，还原为标准kalman，从而实现抑制信息异常对滤波的影响；改进的sage-husa自适应滤波的算法流程如下：
8.初始化状态估计值和协方差，经
[0009][0010][0011][0012]
判断是否在置信空间内，若是，则进行标准卡尔曼滤波，若不是，
[0013]
则进行抗差自适应滤波，过程为：
[0014]
由得出
[0015][0016]
且k(：，i)＝0，得出，
[0017][0018][0019]
pk＝[i-k
khk
]p
k，k-1
[0020][0021]
最后得出
[0022]
为状态一步预测量，φ
k，k-1
为状态转移矩阵，为k-1时刻的状态估计量，为k时刻系统白噪声均值，p
k，k-1
为预测状态协方差矩阵，φ
k，k-1
为状态转移矩阵，p
k-1
为观测矩阵，为协方差矩阵，为预测残差估计量，zk为预测残差，hk为系统状态量，为rk的估计量，为预测残差估计量，为预测残差的均方差，dk为自适应因子，为r
k-1
的估计量，为噪声协方差矩阵，βk为与式(16)中定义相同，为观测噪声协方差阵，r
max
为观测噪声协方差阵最小值，为与式(16)中定义相同，为zk的转置矩阵，为hk的转置矩阵，r
max
为观测噪声协方差阵最大值，kk为增益矩阵，为状态一步预测量，为状态噪声的协方差阵，为状态估计量，为预测残差估计量，pk为观测矩阵，为观测矩阵，为k时刻的白噪声均值，dk为自适应因子，为k-1时刻系统白噪声均值，为k时刻的状态估计量，φ
k，k-1
为状态转移矩阵，为协方差矩阵，为预测残差，为预测残差的转置，为增益矩阵的转置，φ
k，k-1
为状态转移矩阵，φ
k，k-1t
为状态转移矩阵的转置。
[0023]
步骤二中，所建立状态方程为：
[0024][0025]
式(1)中，x(k)为状态变量，f(k)为系统状态转移矩阵，g(k)为系统噪声转移矩阵；w(k)为系统噪声矢量，x(k)为状态变量，
[0026]
选取状态变量x表示为：
[0027][0028]
式(2)中，[φ
e φ
n φu]为惯性平台东、北、天向的姿态失准角，单位为角分；[δv
e δv
n δvu]为东、北、天向的速度误差，单位为米/秒；[δl δλ δh]表示纬度、经度、高度的误差，单位为米；[ε
x ε
y εz]为陀螺仪常值漂移误差，单位度/小时；为加速度计的漂移误差，单位为ug，
[0029]
步骤二中所建立量测方程为：
[0030][0031]
式(9)中，zv(t)为速度量测矢量，z
p
(t)为位置量测矢量，v(t)为观测噪声，速度量测矢量为：
[0032][0033]
式(10)中，hv＝[03×
3 diag(1 1 1) 03×9]，vv＝[v
ge v
gn v
gu
]
t
，v
ge
、v
gn
、v
gu
分别为gnss沿东、北、天三个方向的速度误差，
[0034]
位置量测矢量为：
[0035][0036]
式(11)中，h
p
＝[03×
6 diag(1 1 1) 03×6]，v
p
＝[n
ge n
gn n
gu
]
t
，n
ge
、n
gn
、n
gu
分别为gnss沿东、北、天三个方向的位置误差。
[0037]
步骤三中的信息异常检测过程，利用预测残差向量构造检验统计量，从而判断是否存在观测异常误差，
[0038]
预测残差表示k时刻的真实量测值zk与量测一步预测之间存在的误差，定义式为：
[0039][0040]
式(12)中，x(k)为k时刻的状态矢量，h(k)为系统量测矩，vk为观测噪声，为状态一步预测，
[0041]
预测残差的均方误差阵为则
[0042][0043]
式(13)中，式(13)中，式(13)中，hk为系统量测噪声矩阵，vk为观测噪声矩阵，
[0044]
测量信息预测残差为白噪声序列，服从均值为零，方差的正态分布，即
[0045][0046]
将其归一化，得检验统计量为：
[0047][0048]
式(15)中，式(15)中，表示在时间k时刻的观测矩阵的第i行元素，表示观测噪声协方差矩阵的对角元素，
[0049]
假设置信度为检验统计量不超过置信极限，则认为没有观测异常；如果超过置信极限，引入指数衰减自适应因子来调整观测噪声协方差矩阵，达到识别和抑制观测量异常影响的目的。
[0050]
指数衰减自适应因子自动调整观测噪声过程：通过式(13)可以得出预测残差的方差的表达式为：
[0051][0052]
预测残差的方差代表随机序列的集总平均，可以用离散化方程中的时间平均代替，对方程(14)进行移项，观测噪声协方差矩阵可改写为：
[0053][0054]
将上式(15)中用b，0《b《1，代替，令式(14)转化为：
[0055][0056]
考虑到观测异常误差可能会很大，通过式(16)计算噪声协方差时会增加异常观测的影响，令所以等式(16)可以表示为：
[0057][0058]
当引入指数衰减自适应因子更新滤波器时，根据预测残差自动调整观测噪声，并设置了噪声方差的上下限，避免矩阵逆为负时降低滤波精度，同时，如果两个相邻的迭代差异未超过限制，则停止迭代。
[0059]
一种多源信息融合设备，含有存储器单元，用于存储计算机程序；
[0060]
处理器单元，用于执行计算机程序时实现如上述的改进sage-husa自适应融合滤波方法；处理器单元接收外部要处理的信息。
[0061]
进一步含有信息获取单元、数据传输接收单元，信息获取单元包括imu惯性导航传感器、gps卫星接收机，用于获取陀螺仪和加速度计输出相应的角速度和比力的量测信息，gps输出相应的经纬度量测信息；
[0062]
数据传输接收单元，用于将信息获取单元获得的信息传输给处理器单元。
[0063]
本发明的有益效果：1.本发明相比传统的sage-husa自适应融合滤波，添加了信息异常检测过程，提高了系统的导航精度和容错性。2.本发明解决了sage-husa自适应融合滤波中，连续信息异常出现时信息持续置零，系统导航解算误差增大的问题，自适应调节量测噪声方差，有效控制了量测异常对滤波结果的影响。
附图说明
[0064]
图1是基于信息异常检测的改进sage-husa自适应融合滤波方案过程图；
[0065]
图2是实验仿真轨迹图；
[0066]
图3是现有sage-husa方法和改进的sage-husa方法定位误差对比图。
具体实施方式
[0067]
基于信息异常检测的改进sage-husa自适应融合滤波方法，步骤一：获取传感器量测信息；惯性测量单元中的陀螺仪和加速度计输出相应的角速度和比力的量测信息，gps输出相应的经纬度量测信息；
[0068]
步骤二：建立gps/ins组合导航系统模型，确定出由位置、速度、姿态以及偏差量构成的多维状态量，建立状态方程和量测方程；
[0069]
步骤三：信息异常检测过程，根据预测残差向量构造检验统计量，判断是否有异常观测的存在；当系统无异常量测时，检验统计量不超过置信极限，采用标准卡尔曼滤波进行预测和校正，从而输出融合滤波结果；当系统有异常量测检测时，采用改进的sage-husa自适应滤波，将卡尔曼滤波增益置零，引入指数衰减自适应因子调整观测量测噪声；
[0070]
步骤四：改进的sage-husa自适应滤波方法对组合导航系统进行滤波处理，在标准kalman滤波的基础上对q阵和r阵实时预测和修正，反馈调节滤波增益k的自适应调节过程，同时信息异常时滤波增益置零，还原为标准kalman，从而实现抑制信息异常对滤波的影响；算法流程如图1所示。
[0071]
步骤二中，所建立状态方程为：
[0072][0073]
式(1)中，x(k)为状态变量，f(k)为系统状态转移矩阵，g(k)为系统噪声转移矩阵；w(k)为系统噪声矢量，
[0074]
选取状态变量x表示为：
[0075][0076]
式(2)中，[φ
e φ
n φu]为惯性平台东、北、天向的姿态失准角，单位为角分；[δv
e δv
n δvu]为东、北、天向的速度误差，单位为米/秒；[δl δλ δh]表示纬度、经度、高度的误差，单位为米；[ε
x ε
y εz]为陀螺仪常值漂移误差，单位度/小时；为加速度计的漂移误差，单位为ug，
[0077]
步骤二中所建立量测方程为：
[0078][0079]
式(9)中，zv(t)为速度量测矢量，z
p
(t)为位置量测矢量，v(t)为观测噪声，速度量测矢量为：
[0080][0081]
式(10)中，hv＝[03×
3 diag(1 1 1) 03×9]，vv＝[v
ge v
gn v
gu
]
t
，v
ge
、v
gn
、v
gu
分别为gnss沿东、北、天三个方向的速度误差，
[0082]
位置量测矢量为：
[0083][0084]
式(11)中，h
p
＝[03×
6 diag(1 1 1) 03×6]，v
p
＝[n
ge n
gn n
gu
]
t
，n
ge
、n
gn
、n
gu
分别为gnss沿东、北、天三个方向的位置误差。
[0085]
步骤三中的信息异常检测过程，利用预测残差向量构造检验统计量，从而判断是否存在观测异常误差，
[0086]
预测残差表示k时刻的真实量测值zk与量测一步预测之间存在的误差，定义式为：
[0087][0088]
预测残差的均方误差阵为则
[0089][0090]
测量信息预测残差为白噪声序列，服从均值为零，方差的正态分布，即
[0091][0092]
将其归一化，得检验统计量为：
[0093][0094]
式(15)中，式(15)中，表示在时间k时刻的观测矩阵的第i行元素，表示观测噪声协方差矩阵的对角元素，
[0095]
假设置信度为检验统计量不超过置信极限，则认为没有观测异常；如果超过置信极限，引入指数衰减自适应因子来调整观测噪声协方差矩阵，达到识别和抑制观测量异常影响的目的。
[0096]
上述指数衰减自适应因子自动调整观测噪声过程：通过式(13)可以得出预测残差的方差的表达式为：
[0097][0098]
预测残差的方差代表随机序列的集总平均，可以用离散化方程中的时间平均代替，对方程(14)进行移项，观测噪声协方差矩阵可改写为：
[0099][0100]
将上式(15)中用b，0《b《1，代替，令式(14)转化为：
[0101][0102]
考虑到观测异常误差可能会很大，通过式(16)计算噪声协方差时会增加异常观测的影响，令所以等式(16)可以表示为：
[0103][0104]
当引入指数衰减自适应因子更新滤波器时，根据预测残差自动调整观测噪声，并设置了噪声方差的上下限，避免矩阵逆为负时降低滤波精度，同时，如果两个相邻的迭代差异未超过限制，则停止迭代。
[0105]
步骤四改进的sage-husa自适应滤波的算法流程如图1所示。
[0106]
该方法实现用到一种多源信息融合设备，含有存储器单元，用于存储计算机程序；处理器单元，用于执行计算机程序时实现上述的改进sage-husa自适应融合滤波方法；处理器单元接收外部要处理的信息。进一步含有信息获取单元、数据传输接收单元，信息获取单元包括imu惯性导航传感器、gps卫星接收机，用于获取陀螺仪和加速度计输出相应的角速度和比力的量测信息，gps输出相应的经纬度量测信息；数据传输接收单元，用于将信息获取单元获得的信息传输给处理器单元。
[0107]
从图3看出rakf波动明显比akf小，说明本方法比原来的方法有明显提升效果，最大误差值从3.8m左右降低到了2.8m左右。