CN110375731A - 一种混合交互式多模型滤波方法 - Google Patents

Abstract

本发明公开了一种混合交互式多模型滤波方法，包括：首先建立应用对象的系统模型与观测模型，根据外信息噪声统计特性变化情况，建立总模型集并采用临近的三个模型描述当前噪声特性；采用滑动窗口对历史序列中鲁棒滤波实时噪声方差阵进行存储，根据平滑后的噪声统计特性自适应选择合适的模型；设置各模型的初始概率及各滤波器的混合初始状态、协方差矩阵，根据系统模型、观测模型进行状态估计与协方差矩阵更新过程，并采用贝叶斯假设检验方法进行模型更新；根据权重进行输出交互过程，输出最终滤波结果。以惯性/多普勒组合导航系统为例，本发明能有效地缓解多普勒测速信息中存在的噪声突变型误差及野值的干扰。

Description

一种混合交互式多模型滤波方法

技术领域

本发明是一种混合交互式多模型滤波方法，属于多传感器信息融合技术，特别适用于水下INS/DVL(惯导/多普勒)组合导航领域。

背景技术

随着科学技术的发展，依靠单传感器提供的信息已无法满足控制需要，面向复杂应用背景的多传感器系统的信息融合技术应运而生，已渗透到现代信息社会的各个角落，它的重要性越来越突出。多传感器信息融合技术就是利用计算机技术将来自多传感器或多源的信息和数据，在一定的准则下加以自动分析和综合，以完成所需要的决策和估计而进行的信息处理过程。由于各个分散的传感器获得的数据会不可避免地受到许多因素制约，因此需要一种可靠的信息融合算法。

以水下INS/DVL组合导航为例，由于水下环境复杂多变，多普勒测速仪测量噪声的统计特性会随着环境的变化而变化。一个单一的观测方差矩阵不能适应长航程的积分系统。在未知水域航行时，很难得到测量噪声方差矩阵的经验值。缓解复杂水下环境下单一滤波参数对导航精度的影响成为水下导航技术的难点之一。针对以上情况亟需一种更有效的滤波方法。

发明内容

发明目的：为了更可靠更精确地信息融合，本发明提出了一种混合交互式多模型滤波方法。

技术方案：本发明所述的一种混合交互式多模型滤波方法，包括以下步骤：

S1：建立应用对象的系统模型与观测模型，根据外信息噪声统计特性变化情况，设置总模型集M＝{m₁ m₂ ... m_n}并采用临近的三个模型{m_s-1 m_s m_s+1}描述当前噪声特性；

S2：采用滑动窗口对前n时刻鲁棒滤波实时噪声方差阵R_k进行存储，根据平滑后的噪声统计特性选择合适的模型；

S3：计算各模型的初始概率，并给出各滤波器的混合初始状态及协方差矩阵；

S4：根据系统模型和各个模型采用的滤波模型，分别进行模型滤波，得到每个模型当前时刻的状态估计和协方差矩阵；

S5：采用贝叶斯假设检验方法进行模型更新；

S6：根据权重，将各模型的估计值进行加权融合，计算联合状态估计与协方差矩阵，输出交互式多模型最终滤波结果。

进一步地，所述步骤S1包括：建立系统模型与观测模型后，根据外信息噪声统计特性变化情况，设置总模型集M＝{m₁ m₂ ... m_n}，分别由小到大对应不同的噪声方差阵R_k，并采用临近的三个模型{m_s-1 m_s m_s+1}描述当前噪声特性，m_s-1与m_s+1对应滤波更新过程为普通卡尔曼滤波，主模型m_s对应鲁棒滤波。

所述步骤S2具体包括：

S2.1、根据鲁棒滤波的残差r_2(k)计算实时方差阵R_2(k)：

其中为鲁棒滤波的状态估计向量，φ_2(k,k-1)为状态转移矩阵，为k-1时刻状态向量，G_2(k)为k时刻系统噪声输入矩阵，P_2(k,k-1)为一步预测估计方差阵，H_2(k)为k时刻的量测矩阵，P_O2(k-1)为k-1时刻误差方差阵，Q_2(k)为k时刻状态方差阵，r_2(k)为残差，Z_2(k)为k时刻的量测向量；

S2.2、对前n次实时噪声方差阵R_2(k)取平均，计算平滑后的噪声方差阵

S2.3、根据平滑后的噪声方差阵与当前模型集方差阵的关系选择模型，当超过当前模型集覆盖的范围时，令s＝s+1，即将R_k最小的模型m_s-1去除，引入模型m_s+2；当小于当前模型集覆盖的范围时，令s＝s-1，即将R_k最大的模型m_s+1去除，引入模型m_s-2。

所述步骤S3具体包括：

S3.1、模型转换概率遵循马尔可夫过程，从k-1时刻到k时刻，模型m_i(k-1)到模型m_j(k)的转移概率P{m_j(k)|m_i(k-1)}为马尔可夫转移概率，记为P_i→j，模型m_i(k-1)在时刻k的模型匹配概率为μ_i(k-1)，根据P_i→j和μ_i(k-1)，求得模型预测概率为：

S3.2、给出各滤波器的混合初始状态及协方差矩阵：

其中为上个时刻第i个滤波器的状态估计，P_i(k-1)为其对应的协方差矩阵，为此时刻滤波器j的状态输入，P_Oj(k-1)为其对应的状态误差协方差矩阵。

所述步骤S4具体包括：

S4.1、根据状态方程与观测方程列写系统方程：

其中j＝1～3，X_j(k)为k时刻第j个滤波器状态向量，X_j(k-1)为k-1时刻状态向量，G_j(k)为k时刻系统噪声输入矩阵，W_j(k)为k时刻系统噪声，Z_j(k)为k时刻量测向量，H_j(k)为k时刻量测矩阵，V_j(k)为k时刻量测噪声；

S4.2、计算卡尔曼滤波一步预测状态向量及误差方差阵：

其中，为滤波器j的状态估计，φ_j(k,k-1)为状态转移矩阵，P_j(k,k-1)为一步预测估计方差阵，P_Oj(k-1)为误差方差阵，Q_j(k)为状态方差阵；

S4.3、计算卡尔曼滤波残差及其协方差：

其中，r_j(k)为残差，A_j(k)为残差方差阵；

S4.4、计算卡尔曼滤波增益矩阵：

S4.5、计算卡尔曼滤波状态估计及其协方差阵：

P_j(k)＝(I-K_j(k)H_j(k))P_j(k,k-1)

S4.6、在主模型中，根据卡尔曼滤波方程递推出结合k时刻的观测量Z_k，得到残差：

S4.7、根据异常函数判别是否存在野值：

其中，λ_k为异常函数，T_D为预先设定的门限；无野值时进行正常滤波，有野值时剔除此时刻的观测量，采用残差修正补偿的方法，用一个长度为l的滑动数据窗，将前l时刻的残差信息保存：

r_2(k)＝(r_2(k-1)+r_2(k-2)+...+r_2(k-l))/l

若k时刻的量测量被判断为野值，则利用滑动数据窗中存储的前l时刻的残差序列平滑值替代此时刻的残差值，之后根据卡尔曼滤波方程计算最优状态向量。

所述步骤S5具体包括：

采用贝叶斯假设检验方法进行模型更新，在获取各滤波器在k时刻的残差与残差协方差矩阵后，计算该模型的似然函数：

其中m为量测向量的维数；

并计算模型在模型集中的权重，进行模型概率更新：

所述步骤S6具体包括：

根据权重，将各模型的估计值进行加权融合，计算联合状态估计与协方差矩阵，输出交互式多模型最终结果：

有益效果：

1、本发明采用多模型参数覆盖噪声协方差阵的方法，解决了单一观测噪声协方差阵在复杂环境下滤波精度下降的问题；

2、本发明在交互式多模型滤波器主模型中采用基于残差χ2检测的鲁棒滤波，有效抑制了野值干扰，同时采用残差修正补偿的方法保证了系统在野值点导航不间断；

3、本发明采用滑动窗对残差序列等信息进行存储，实时估计外信息噪声统计特性，自适应地选择模型集，提高了融合系统的可适性。本发明适用于所有信息融合系统，包括但不限于惯性/多普勒组合导航系统，惯性/卫星组合导航系统等。

附图说明

图1为本发明的混合交互式多模型滤波方法流程图；

图2为应用于INS/DVL系统时本发明的多模型滤波方法与传统方法姿态误差对比图；

图3为应用于INS/DVL系统时本发明的多模型滤波方法与传统方法速度误差对比图；

图4为应用于INS/DVL系统时本发明的多模型滤波方法与传统方法位置误差对比图。

具体实施方式

下面结合附图以水下INS/DVL组合导航为例对本发明的技术方案作进一步说明。应当了解，以下提供的实施例仅是为了详尽地且完全地公开本发明，并且向所属技术领域的技术人员充分传达本发明的技术构思，本发明还可以用许多不同的形式来实施，并且不局限于此处描述的实施例。对于表示在附图中的示例性实施方式中的术语并不是对本发明的限定。

本发明提供的一种混合交互式多模型滤波方法，实现原理如图1所示。以水下INS/DVL组合导航为例，其流程主要包括以下步骤：

步骤S1，根据INS和DVL的误差特性，建立INS/DVL组合系统模型。具体包括以下过程：

S1.1、建立INS姿态、速度、位置微分方程：

其中：

为从载体坐标系到导航坐标系的转换矩阵，为从导航坐标系到载体坐标系的转换矩阵，(×)为叉乘，Vⁿ为导航坐标系下的载体速度，Gⁿ为导航坐标系下重力矢量的投影，为在载体坐标系下从载体坐标系到导航坐标系的角速度，为导航坐标系下地球旋转矢量的投影，为导航坐标系下载体从地球坐标系到导航坐标系的角速度，L为纬度，λ为经度，R_M和R_N分别为子午圈与卯酉圈半径，和f^b为IMU输出，代表相对于惯性系的角速度与比力信息，Ω为地球旋转角速度，V_N和V_E分别为北向与东向速度。

S1.2、建立INS误差模型：

其中φⁿ为失准角， 为从载体坐标系到导航坐标系的转换矩阵，ε^b为陀螺仪误差，Vⁿ为速度矢量，δ为相关量误差，为导航坐标系下地球旋转矢量的投影，为加速度计误差，f^b为加速度计输出，L为纬度，λ为经度，h为高度，V_N V_E V_U分别为北向、东向与天向速度，R_M和R_N分别为子午圈与卯酉圈半径。

S1.3、建立DVL对地测速模型：

其中为DVL对地速度测量值，为DVL对地速度真值，δK为标度因子误差，v_d-G为量测噪声。

S1.4、根据前三步建立的INS和DVL模型，选取状态变量：

其中δV_E δV_N δV_U为东北天方向的速度误差，φ_x，φ_y，φ_z为各轴向的失准角，为各轴向的加速度误差，ε_x，ε_y，ε_z为各轴向的陀螺仪误差，δK为DVL的标度因子误差。

S1.5根据INS和DVL各自的误差模型，建立INS/DVL组合系统的状态方程：

其中，X为状态向量，F为系统矩阵，W为系统噪声。F_INS矩阵中C_ij(i,j＝1,2,3)为的元素，f_E，f_N，f_U分别为东北天方向的比力测量值。其余如下：

F₃₇＝2V_EΩsinL

F₅₇＝-ΩsinL

S1.6、选取INS载体系速度和DVL测得的外信息差值作为观测量，建立观测方程：

其中为载体坐标系下INS计算速度，为DVL对地速度测量值，为失准角对应的方向余弦矩阵。

略去小量项，得到组合系统的量测方程：

Z＝HX+V_d-G

其中，Z为量测向量，X为状态向量，H为量测方程，V_d-G为量测噪声。

步骤S2，根据组合系统外信息噪声统计特性变化情况，设置总模型集M＝{m₁ m₂... m_n}并采用临近的三个模型{m_s-1 m_s m_s+1}描述当前噪声特性。

具体包括以下过程：

根据外信息噪声统计特性变化情况，如图1中模型池模块所示，设置总模型集M＝{m₁ m₂ ... m_n}，分别由小到大对应不同的噪声方差阵R_k。采用临近的三个模型{m_s-1 m_sm_s+1}描述当前噪声特性，m_s-1模型与m_s+1模型对应的滤波更新过程是普通的卡尔曼滤波，m_s为主模型对应鲁棒滤波。

步骤S3，采用滑动窗口对前n时刻鲁棒滤波实时噪声方差阵R_2(k)进行存储，根据平滑后的噪声统计特性选择合适的模型。

具体包括以下过程：

S3.1、如图1中自适应模型选择模块，根据上一时刻鲁棒滤波的残差r_2(k)计算实时方差阵R_2(k)：

其中为鲁棒滤波的状态估计向量，φ_2(k,k-1)为状态转移矩阵，为k-1时刻状态向量，G_2(k)为k时刻系统噪声输入矩阵，P_2(k,k-1)为一步预测估计方差阵，H_2(k)为k时刻的量测矩阵，P_O2(k-1)为k-1时刻误差方差阵，Q_2(k)为k时刻状态方差阵，r_2(k)为残差，Z_2(k)为k时刻的量测向量。

S3.2、对前n次实时噪声方差阵R_2(k)取平均，计算平滑后的噪声方差阵

S3.3、根据平滑后的噪声方差阵与当前模型集方差阵的关系滑动选择模型，当超过当前模型集覆盖的范围时，令s＝s+1，即将R_k最小的模型m_s-1去除，引入模型m_s+2。当小于当前模型集覆盖的范围时，令s＝s-1，即将R_k最大的模型m_s+1去除，引入模型m_s-2。

步骤S4，计算各模型的初始概率，并给出各滤波器的混合初始状态及协方差矩阵。

具体包括以下过程：

S4.1、模型转换概率遵循马尔可夫过程，从k-1时刻到k时刻，模型m_i(k-1)到模型m_j(k)的转移概率P{m_j(k)|m_i(k-1)}为马尔可夫转移概率，记为P_i→j，一般采用预先设置的常数。模型m_i(k-1)在时刻k的模型匹配概率为μ_i(k-1)。如图1中初始模型预测概率模块，根据P_i→j和μ_i(k-1)，可以推导出模型预测概率为：

S4.2、如图1中输入交互模块，给出各滤波器的混合初始状态及协方差矩阵：

其中为上个时刻第i个滤波器的状态估计，P_i(k-1)为其对应的协方差矩阵。为此时刻滤波器j的状态输入，P_Oj(k-1)为其对应的状态误差协方差矩阵。

步骤S5，根据步骤一建立的系统模型和各个模型采用的滤波模型(卡尔曼滤波、鲁棒滤波等)，分别进行模型滤波，得到每个模型当前时刻的状态估计和协方差矩阵。

如上对当前模型集建立相应的滤波模型，从小到大依次为：m_s-1模型对应的普通卡尔曼滤波(副模型)、m_s模型对应的鲁棒滤波(主模型)、m_s+1模型对应的普通卡尔曼滤波(副模型)。应当了解，此处采用的滤波模型仅是举例说明，而不是限制本发明的模型集中必须使用这样的滤波模型，其他不同的卡尔曼、自适应、鲁棒滤波器组合排列也在本发明保护范围之内。分别进行模型滤波，得到每个模型当前时刻的状态估计和协方差矩阵。具体包括以下过程：

S5.1、根据步骤S1建立的状态方程与观测方程列写离散化系统方程：

其中j＝1～3，X_j(k)为k时刻第j个滤波器状态向量，X_j(k-1)为k-1时刻状态向量，G_j(k)为k时刻系统噪声输入矩阵，W_j(k)为k时刻系统噪声，Z_j(k)为k时刻量测向量，H_j(k)为k时刻量测矩阵，V_j(k)为k时刻量测噪声；

S5.2、计算卡尔曼滤波一步预测状态向量及误差方差阵：

S5.3、计算卡尔曼滤波残差及其协方差：

其中，r_j(k)为残差，A_j(k)为残差方差阵；

S5.4、计算卡尔曼滤波增益矩阵：

S5.5、计算卡尔曼滤波状态估计及其协方差阵：

P_j(k)＝(I-K_j(k)H_j(k))P_j(k,k-1)

S5.6、在主模型中j＝2，根据卡尔曼滤波方程递推出结合k时刻的观测量Z_k，得到残差：

S5.7、根据异常函数判别是否存在野值：

其中，λ_k为异常函数，T_D为预先设定的门限。T_D决定了检测的性能，选取时要兼顾误警率和漏检率两个因素。无野值时进行正常滤波，有野值时剔除此时刻的观测量，采用残差修正补偿的方法。用一个长度为l的滑动数据窗，将前l时刻的残差信息保存：

r_2(k)＝(r_2(k-1)+r_2(k-2)+...+r_2(k-l))/l

当k时刻的量测量被判断为野值，则利用滑动数据窗中存储的前l时刻的残差序列平滑值替代此时刻的残差值，之后根据卡尔曼滤波方程计算最优状态向量。

步骤S6，采用贝叶斯假设检验方法进行模型更新。

如图1中模型概率更新模块，采用贝叶斯假设检验方法进行模型更新。在获取各滤波器在k时刻的残差与残差协方差矩阵后，计算该模型的似然函数：

其中m为量测向量的维数。

计算模型在模型集中的权重，进行模型概率更新：

步骤S7，根据权重，将各模型的估计值进行加权融合，计算联合状态估计与协方差矩阵，输出交互式多模型最终结果。

如图1中输出交互模块，根据权重将各模型的估计值进行加权融合，计算联合状态估计与协方差矩阵，输出交互式多模型最终滤波结果。

图2至图4为应用于INS/DVL系统时(DVL信息中包含噪声突变型误差和野值)，本发明所述方法与传统交互式多模型滤波方法的姿态、速度、位置误差对比图。从图中可以看出，与传统交互式多模型滤波方法相比，本发明提出的混合交互式多模型滤波的姿态、速度、位置误差性能均更优，本发明能有效地缓解多普勒测速信息中存在的噪声突变型误差及野值的干扰。最后应当说明的是，以上仅是本发明的较佳实施例例示，本发明适用于所有信息融合系统，包括但不限于惯性/多普勒组合导航系统，惯性/卫星组合导航系统等。

Claims (7)

1.一种混合交互式多模型滤波方法，其特征在于，所述方法包括以下步骤：

S1：建立应用对象的系统模型与观测模型，根据外信息噪声统计特性变化情况，设置总模型集M＝{m₁ m₂ ... m_n}并采用临近的三个模型{m_s-1 m_s m_s+1}描述当前噪声特性；

S2：采用滑动窗口对前n时刻鲁棒滤波实时噪声方差阵R_k进行存储，根据平滑后的噪声统计特性选择合适的模型；

S3：计算各模型的初始概率，并给出各滤波器的混合初始状态及协方差矩阵；

S4：根据系统模型和各个模型采用的滤波模型，分别进行模型滤波，得到每个模型当前时刻的状态估计和协方差矩阵；

S5：采用贝叶斯假设检验方法进行模型更新；

S6：根据权重，将各模型的估计值进行加权融合，计算联合状态估计与协方差矩阵，输出交互式多模型最终滤波结果。

2.根据权利要求1所述的混合交互式多模型滤波方法，其特征在于，所述步骤S1包括：建立系统模型与观测模型后，根据外信息噪声统计特性变化情况，设置总模型集M＝{m₁ m₂... m_n}，分别由小到大对应不同的噪声方差阵R_k，并采用临近的三个模型{m_s-1 m_s m_s+1}描述当前噪声特性，m_s-1与m_s+1对应滤波更新过程为普通卡尔曼滤波，主模型m_s对应鲁棒滤波。

3.根据权利要求2所述的混合交互式多模型滤波方法，其特征在于，所述步骤S2具体包括：

S2.1、根据鲁棒滤波的残差r_2(k)计算实时方差阵R_2(k)：

其中为鲁棒滤波的状态估计向量，φ_2(k,k-1)为状态转移矩阵，为k-1时刻状态向量，G_2(k)为k时刻系统噪声输入矩阵，P_2(k,k-1)为一步预测估计方差阵，H_2(k)为k时刻的量测矩阵，P_O2(k-1)为k-1时刻误差方差阵，Q_2(k)为k时刻状态方差阵，r_2(k)为残差，Z_2(k)为k时刻的量测向量；

S2.2、对前n次实时噪声方差阵R_2(k)取平均，计算平滑后的噪声方差阵

S2.3、根据平滑后的噪声方差阵与当前模型集方差阵的关系选择模型，当超过当前模型集覆盖的范围时，令s＝s+1，即将R_k最小的模型m_s-1去除，引入模型m_s+2；当小于当前模型集覆盖的范围时，令s＝s-1，即将R_k最大的模型m_s+1去除，引入模型m_s-2。

4.根据权利要求2所述的混合交互式多模型滤波方法，其特征在于，所述步骤S3具体包括：

S3.1、模型转换概率遵循马尔可夫过程，从k-1时刻到k时刻，模型m_i(k-1)到模型m_j(k)的转移概率P{m_j(k)|m_i(k-1)}为马尔可夫转移概率，记为P_i→j，模型m_i(k-1)在时刻k的模型匹配概率为μ_i(k-1)，根据P_i→j和μ_i(k-1)，求得模型预测概率为：

S3.2、给出各滤波器的混合初始状态及协方差矩阵：

其中为上个时刻第i个滤波器的状态估计，P_i(k-1)为其对应的协方差矩阵，为此时刻滤波器j的状态输入，P_Oj(k-1)为其对应的状态误差协方差矩阵。

5.根据权利要求2所述的混合交互式多模型滤波方法，其特征在于，所述步骤S4具体包括：

S4.1、根据状态方程与观测方程列写系统方程：

其中j＝1～3，X_j(k)为k时刻第j个滤波器状态向量，X_j(k-1)为k-1时刻状态向量，G_j(k)为k时刻系统噪声输入矩阵，W_j(k)为k时刻系统噪声，Z_j(k)为k时刻量测向量，H_j(k)为k时刻量测矩阵，V_j(k)为k时刻量测噪声；

S4.2、计算卡尔曼滤波一步预测状态向量及误差方差阵：

其中，为滤波器j的状态估计，φ_j(k,k-1)为状态转移矩阵，P_j(k,k-1)为一步预测估计方差阵，P_Oj(k-1)为误差方差阵，Q_j(k)为状态方差阵；

S4.3、计算卡尔曼滤波残差及其协方差：

其中，r_j(k)为残差，A_j(k)为残差方差阵；

S4.4、计算卡尔曼滤波增益矩阵：

S4.5、计算卡尔曼滤波状态估计及其协方差阵：

P_j(k)＝(I-K_j(k)H_j(k))P_j(k,k-1)

S4.6、在主模型中，根据卡尔曼滤波方程递推出结合k时刻的观测量Z_k，得到残差：

S4.7、根据异常函数判别是否存在野值：

其中，λ_k为异常函数，T_D为预先设定的门限；无野值时进行正常滤波，有野值时剔除此时刻的观测量，采用残差修正补偿的方法，用一个长度为l的滑动数据窗，将前l时刻的残差信息保存：

r_2(k)＝(r_2(k-1)+r_2(k-2)+...+r_2(k-l))/l

若k时刻的量测量被判断为野值，则利用滑动数据窗中存储的前l时刻的残差序列平滑值替代此时刻的残差值，之后根据卡尔曼滤波方程计算最优状态向量。

6.根据权利要求5所述的混合交互式多模型滤波方法，其特征在于，所述步骤S5具体包括：

采用贝叶斯假设检验方法进行模型更新，在获取各滤波器在k时刻的残差与残差协方差矩阵后，计算该模型的似然函数：

其中m为量测向量的维数；

并计算模型在模型集中的权重，进行模型概率更新：

7.根据权利要求6所述的混合交互式多模型滤波方法，其特征在于，所述步骤S6具体包括：

根据权重，将各模型的估计值进行加权融合，计算联合状态估计与协方差矩阵，输出交互式多模型最终结果：