有色噪声下基于改进自适应滤波的组合导航方法

1.本发明涉及多传感器组合导航技术领域，具体涉及一种有色噪声下基于改进自适应滤波的组合导航方法。


背景技术：

2.惯性导航系统是一种可在空中、地面和水下等多种复杂环境中工作的自主式导航系统。陀螺仪和加速度计是惯性导航系统的主要组成部分，其测量值分别表示载体相对惯性空间的角速度与比力信息。通过力学编排便可解算得到载体的姿态、速度和位置。同时，惯性导航系统不需要借助任何外来信息，也不向外辐射任何信息，仅依靠惯性系统本身就能在任何环境下进行定位，因此惯性导航系统具有自主性、隐蔽性等优点，在国防、工业、运输等领域都有着十分重要的作用。惯性导航系统在导航阶段的定位精度由惯性器件精度、导航算法精度、误差补偿技术等决定。随着惯性器件精度的提高和导航算法的不断优化，惯性导航系统的导航与定位精度不断提高。但由于惯导的工作原理，不可避免的会有累积误差的出现，在短时间内影响不大，一旦进行长时间的定位导航，累积误差会逐渐增大，导致导航定位精度随工作时间的增加而下降。随着时代的发展，仅靠惯导无法满足多种应用情况的需求。为解决这一问题，可采用组合导航技术，利用其他传感器作为运动信息参考源，对惯性导航系统进行修正，提高定位精度。
3.组合导航相对于单一的惯性导航具有更高的可靠性，因此被广泛应用于各个领域。为了进一步提高系统的鲁棒性，可以使用多个参考源进行组合导航。但辅助设备的使用也为导航系统带来新的问题，当载体处于复杂环境中时，传感器测量值会受到外界干扰，异常的测量值会使组合导航系统的输出结果误差变大甚至发散，因此组合导航的容错是该领域的重点研究问题。为了提高系统的容错性，组合导航过程中需要对故障进行检测并隔离。故障检测方法大多用的是卡方检验，但在实际应用中量测噪声大多为有色噪声，噪声的统计特性发生改变，原统计量不满足卡方分布，需要构造新的统计量。现有的其他某些方法采用联邦滤波的方式进行故障隔离，但联邦滤波中各局部滤波器采用了相同的参考源，导致了各滤波器输出量之间不独立，使得状态估计不是最优的，同时联邦卡尔曼滤波计算量大，容易发散。除此之外，也有方法采用序贯自适应滤波对故障进行隔离，然而卡尔曼滤波序贯处理只有在量测噪声方差阵为对角阵或常值时适用，当量测噪声为有色噪声时不再适用。


技术实现要素：

4.有鉴于此，本发明提供了一种有色噪声下基于改进自适应滤波的组合导航方法，通过构造新的统计量实现故障检测，通过自适应的改变新量测噪声方差实现故障隔离，从而提高组合导航系统的可靠性。
5.本发明的有色噪声下基于改进自适应滤波的组合导航方法，采用的量测单元为惯导系统与辅助量测设备的组合；组合导航方法包括：
6.步骤1，建立组合导航系统的状态方程和量测方程；其中，组合导航系统的状态向
量为惯导系统、辅助量测设备的测量误差；
7.将组合导航系统的有色量测噪声白化，并基于观测增广法得到改进量测方程；
8.步骤2，量测单元实时量测，并基于状态方程和改进量测方程进行卡尔曼滤波；基于卡尔曼滤波结果进行导航；
9.步骤3，计算改进量测方程的量测新息的马氏距离；对马氏距离的平方值进行卡方检验，若马氏距离的平方值大于设定阈值，则判断组合导航系统出现故障，转入步骤4；若马氏距离的平方值小于或等于所述阈值，则判断组合导航系统正常，返回步骤2继续执行卡尔曼滤波；
10.步骤4，利用放大系数，对改进量测方程的量测噪声方差进行修正；其中，量测向量的每一维元素对应一个放大系数，分别对其所对应的量测噪声方差矩阵对角线元素进行修正；所述放大系数是一个大于1的数；返回步骤2并利用修正后的量测噪声方差进行卡尔曼滤波。
11.较优的，设有色量测噪声为1阶ar模型，则量测噪声满足以下方程：
[0012][0013]
式中，v为组合导航系统量测噪声；ψ
k,k-1
为有色噪声的相关系数矩阵；为方差为r的零均值白噪声序列且与wk不相关，w为组合导航系统过程噪声，满足高斯白噪声。
[0014]
较优的，采用观测增广法得到的改进量测方程为：
[0015][0016]
式中：
[0017][0018][0019][0020]
其中，x为组合导航系统的状态向量；z为量测值；ψ
k,k-1
为有色噪声的相关系数矩阵；h为组合导航系统量测矩阵；φ
k,k-1
表示组合导航系统k-1时刻到k时刻的状态一步转移矩阵；γ
k,k-1
表示离散化后k-1时刻到k时刻的噪声输入矩阵；wk为组合导航系统过程噪声，为零均值白噪声序列；为方差为r的零均值白噪声序列且与wk不相关；
[0021]vk*
是零均值的白噪声，其方差矩阵为：
[0022][0023]
其中，q为过程噪声方差矩阵；表示hk的转置矩阵；r为量测噪声方差矩阵。
[0024]
由于中含有wk项，因此二者相关，其互协方差矩阵为：
[0025][0026]
较优的，所述步骤3中，量测新息为：满足如下正态分布：
[0027]
较优的，量测新息的马氏距离为：
[0028]
[0029]
式中，式中，表示k时刻新息的第i个元素，σk(i,i)表示k时刻矩阵σ的第i个对角元素。
[0030]
较优的，所述步骤4中，放大系数κi为：
[0031][0032]
其中，表示k时刻新息的第i个元素；t
α
为设定的阈值，为所设阈值的倒数；表示取矩阵的第i行；表示矩阵的第i个对角元素。
[0033]
较优的，所述辅助量测设备为里程计、测速仪或gps的一种或多种组合。
[0034]
有益效果：
[0035]
传统的故障检测与故障隔离方法在量测噪声为有色噪声时不再适用。本发明提供了一种基于改进自适应滤波的组合导航方法，先将噪声白化使其满足卡尔曼滤波的基本条件，通过构造新的统计量实现故障检测，通过引入放大系数自适应地改变新量测噪声方差，减少故障单元对结构的影响，实现故障隔离。本发明能有效检测并隔离组合导航过程中量测单元的故障，并且当一组量测量中某一维有异常时不影响其余数据的融合，提高了数据的利用率，解决了量测异常影响导航精度的问题，提高组合导航系统的可靠性。
附图说明
[0036]
图1为改进自适应滤波计算流程图。
[0037]
图2为不同方法组合导航结果。
具体实施方式
[0038]
下面结合附图并举实施例，对本发明进行详细描述。
[0039]
本发明提供了一种有色噪声下基于改进自适应滤波的组合导航方法。有色噪声下普通卡尔曼滤波不再适用，因此先利用观测增广法将量测噪声白化，进而建立有色量测噪声条件下的改进卡尔曼滤波；然后，构造新的满足卡方分布的统计量，并设定合适的阈值进行故障检测；最后，根据检测结果自适应地调整新量测噪声方差，出现故障则计算放大系数将新量测噪声方差放大，更新改进卡尔曼滤波的状态量，完成量测故障容错隔离的自适应滤波。
[0040]
本发明流程图如图1所示，具体包括：
[0041]
根据组合导航系统中各测量单元的误差模型，建立组合导航系统的状态空间模型，其中，组合导航系统的状态方程为：
[0042][0043]
式中，f为组合导航系统状态矩阵；g为噪声输入矩阵；w为组合导航系统过程噪声，满足高斯白噪声；x为组合导航系统的状态向量。
[0044]
组合导航系统的量测方程为：
[0045]
z＝hx+v
[0046]
式中，h为组合导航系统量测矩阵，v为组合导航系统量测噪声。
[0047]
将状态方程和量测方程离散化可得：
[0048]
xk＝φ
k,k-1
x
k-1
+γ
k,k-1wk-1
[0049]
zk＝hkxk+vk[0050]
式中，φ
k,k-1
表示组合导航系统由k-1时刻到k时刻的状态一步转移矩阵，γ
k,k-1
表示离散化后k-1时刻到k时刻的噪声输入矩阵。
[0051]
若组合导航系统过程噪声wk为零均值白噪声序列，量测噪声vk为有色噪声，设有色量测噪声为1阶ar模型，则量测噪声满足以下方程：
[0052][0053]
式中，ψ
k,k-1
为有色噪声的相关系数矩阵，为方差为r的零均值白噪声序列且与wk不相关。
[0054]
采用观测增广法可得到新的量测方程：
[0055][0056]
式中：
[0057][0058][0059][0060]
其中，是零均值的白噪声，其方差矩阵为：
[0061][0062]
其中，q为过程噪声方差矩阵；表示hk的转置矩阵；r为量测噪声方差矩阵。
[0063]
由于中含有wk项，因此二者相关，其互协方差矩阵为：
[0064][0065]
接下来可以根据白噪声相关条件下的卡尔曼滤波方程进行处理，递推方程如下：
[0066][0067][0068][0069][0070][0071]
pk＝(i-k
khk
)p
k,k-1
[0072]
根据新的量测方程可构造量测新息满足如下正态分布：
[0073][0074]
马氏距离是一种有效的计算两个未知样本集的相似度的方法，定义量测新息的马氏距离为：
[0075][0076]
式中，式中，表示k时刻新息的第i个元素，σk(i,i)表示k时刻矩阵σ的第i个对角元素。由于新息是零均值高斯白噪声，可得到服从自由度为1的卡方分布的统计量：
[0077][0078]
若量测出现异常，量测新息的马氏距离的平方就不再服从卡方分布，因此可以根据数理统计里的假设检验原理进行故障检测。设置卡方检验显著性水平α和其对应的阈值t
α
，可得到显著性水平α下的马氏距离的置信概率为：
[0079]
p([mk(i)]2＞t
α
)＝α
[0080]
正常情况下[mk(i)]2＞t
α
是小概率事件，其概率小于α，所以当[mk(i)]2＞t
α
时可认为系统存在故障，判定准则如下：
[0081]
若[mk(i)]2＞t
α
，判定对应的新息异常，系统有故障；
[0082]
若[mk(i)]2≤t
α
，判定对应的新息正常，系统无故障。
[0083]
当新息异常时，需要进行故障隔离。量测噪声方差表示量测值的可信度，随着方差的增大量测值的可信度降低，此时卡尔曼增益将会减小，量测更新时新息所占的比重降低，状态估计更多的依赖状态预测提供的信息。当检测到新息异常时，引入放大系数κi将方差放大：
[0084][0085]
式中表示修正后的方差，此时统计量计算如下：
[0086][0087]
放大系数满足如下方程：
[0088][0089]
可解得
[0090][0091]
用修正后的方差进行量测更新完成状态估计。
[0092]
实施例
[0093]
下面以里程计和测速仪联合辅助惯导系统的组合导航系统为例，进行具体说明。
[0094]
一、首先建立组合导航系统模型：
[0095][0096]
式中，f
sins
为惯性导航系统的状态矩阵，g
sins
为惯性导航系统的噪声输入矩阵，x为
组合导航系统的状态向量，具体形式为：
[0097][0098]
式中φn表示姿态角误差，δv表示速度误差，δp表示位置误差，εb和分别表示陀螺仪和加速度计的常值漂移，δθ和表示载体系b系与车体系m系之间的安装误差角，δk
od
和δk
ldv
分别表示里程计和测速仪的标度因数误差，δv0表示测速仪的零位误差。
[0099]
以里程计、测速仪输出的速度和非完整性约束作为量测信息建立量测方程如下：
[0100]
z＝hx+v
[0101]
式中，为量测量，其中为量测量，其中和分别为里程计和测速仪输出的速度。h为量测矩阵，v为量测噪声，h的具体形式为：
[0102][0103][0104]
式中vn×
表示向量vn的反对称矩阵，表示取矩阵的第二行。
[0105]
二、设定初始的过程噪声方差矩阵q、量测噪声方差矩阵r和状态初值x0，初始化系数矩阵j和互协方差矩阵s。k为当前时刻，令k＝1。
[0106]
三、每次卡尔曼滤波时首先进行时间更新，用k-1时刻的后验估计值通过系统模型预测k时刻的系统状态和协方差矩阵，更新公式为：
[0107][0108][0109]
式中，φ为状态转移矩阵，γ为噪声输入矩阵。
[0110]
四、计算量测量和并分别计算量测量中每一维的马氏距离mk(i)(i＝1,2,3,4),计算公式如下：
[0111][0112][0113][0114][0115][0116]
式中，ψ为有色噪声的相关系数矩阵，r(i)表示向量r的第i个元素，h(i,:)表示矩阵h的第i行，r(i,i)表示矩阵r的第i个对角元素。
[0117]
五、设置卡方检验显著性水平α和对应的阈值t
α
，根据判定准则判断新息是否出现
异常，判定准则如下：
[0118]
若[mk(i)]2＞t
α
，判定对应的新息异常，系统有故障；
[0119]
若[mk(i)]2≤t
α
，判定对应的新息正常，系统无故障。
[0120]
六、根据判断结果自适应的调整如果新息正常，则对应的保持不变，如果新息异常，则通过放大系数κi将对应的放大，放大系数计算公式为：
[0121][0122]
七、得到新的后计算k时刻的卡尔曼增益系数，计算公式为：
[0123][0124]
八、更新系数矩阵jk和互协方差矩阵sk，计算公式为：
[0125][0126][0127]
九、进行量测更新，利用卡尔曼增益系数更新状态，得到k时刻系统状态和协方差矩阵的后验估计值，计算公式为：
[0128][0129][0130]
得到的系统状态量可用于导航解算的反馈补偿，如导航状态未结束，则令k＝k+1，由步骤三开始下一次的卡尔曼滤波。
[0131]
图2给出了利用上述步骤，在有色量测噪声条件下存在系统故障时进行组合导航的结果。从图中可以看到，当系统出现故障时，使用本发明组合导航方法可以有效检测并隔离故障，有效减小对结果的影响。
[0132]
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种有色噪声下基于改进自适应滤波的组合导航方法，采用的量测单元为惯导系统与辅助量测设备的组合；其特征在于，包括：步骤1，建立组合导航系统的状态方程和量测方程；其中，组合导航系统的状态向量为惯导系统、辅助量测设备的测量误差；将组合导航系统的有色量测噪声白化，并基于观测增广法得到改进量测方程；步骤2，量测单元实时量测，并基于状态方程和改进量测方程进行卡尔曼滤波；基于卡尔曼滤波结果进行导航；步骤3，计算改进量测方程的量测新息的马氏距离；对马氏距离的平方值进行卡方检验，若马氏距离的平方值大于设定阈值，则判断组合导航系统出现故障，转入步骤4；若马氏距离的平方值小于或等于所述阈值，则判断组合导航系统正常，返回步骤2继续执行卡尔曼滤波；步骤4，利用放大系数，对改进量测方程的量测噪声方差进行修正；其中，量测向量的每一维元素对应一个放大系数，分别对其所对应的量测噪声方差矩阵对角线元素进行修正；所述放大系数是一个大于1的数；返回步骤2并利用修正后的量测噪声方差进行卡尔曼滤波。2.如权利要求1所述的组合导航方法，其特征在于，设有色量测噪声为1阶ar模型，则量测噪声满足以下方程：式中，v为组合导航系统量测噪声；ψ
k,k-1
为有色噪声的相关系数矩阵；为方差为r的零均值白噪声序列且与w
k
不相关，w为组合导航系统过程噪声，满足高斯白噪声。3.如权利要求2所述的组合导航方法，其特征在于，采用观测增广法得到的改进量测方程为：式中：式中：式中：其中，x为组合导航系统的状态向量；z为量测值；ψ
k,k-1
为有色噪声的相关系数矩阵；h为组合导航系统量测矩阵；φ
k,k-1
表示组合导航系统k-1时刻到k时刻的状态一步转移矩阵；γ
k,k-1
表示离散化后k-1时刻到k时刻的噪声输入矩阵；w
k
为组合导航系统过程噪声，为零均值白噪声序列；为方差为r的零均值白噪声序列且与w
k
不相关；是零均值的白噪声，其方差矩阵为：其中，q为过程噪声方差矩阵；表示h
k
的转置矩阵；r为量测噪声方差矩阵。由于中含有w
k
项，因此二者相关，其互协方差矩阵为：
4.如权利要求3所述的组合导航方法，其特征在于，所述步骤3中，量测新息为：满足如下正态分布：5.如权利要求4所述的组合导航方法，其特征在于，量测新息的马氏距离为：式中，式中，表示k时刻新息的第i个元素，σ
k
(i,i)表示k时刻矩阵σ的第i个对角元素。6.如权利要求5所述的组合导航方法，其特征在于，所述步骤4中，放大系数κ
i
为：其中，表示k时刻新息的第i个元素；t
α
为设定的阈值，为所设阈值的倒数；表示取矩阵的第i行；表示矩阵的第i个对角元素。7.如权利要求1所述的组合导航方法，其特征在于，所述辅助量测设备为里程计、测速仪或gps的一种或多种组合。

技术总结
本发明公开了一种有色噪声下基于改进自适应滤波的组合导航方法。本发明先将噪声白化使其满足卡尔曼滤波的基本条件，通过构造新的统计量实现故障检测，通过引入放大系数自适应地改变新量测噪声方差，减少故障单元对结构的影响，实现故障隔离。本发明能有效检测并隔离组合导航过程中量测单元的故障，并且当一组量测量中某一维有异常时不影响其余数据的融合，提高了数据的利用率，解决了量测异常影响导航精度的问题，提高组合导航系统的可靠性。提高组合导航系统的可靠性。提高组合导航系统的可靠性。


技术研发人员：缪玲娟 郝一达 周志强 张卫笑
受保护的技术使用者：北京理工大学
技术研发日：2023.04.14
技术公布日：2023/7/12