CN104392136A - 一种面向高动态非高斯模型鲁棒测量的高精度数据融合方法 - Google Patents

Abstract

一种面向高动态非高斯模型鲁棒测量的高精度数据融合方法，将高动态系统中的硬件传感器采样周期波动作为系统随机不确定度考虑，根据其波动范围和趋势建立包括UKF滤波器模型和模糊推理系统的滤波模型集，通过贝叶斯定理计算UKF滤波器模型与当前高动态系统状态匹配的概率，实时更新匹配概率，并将更新后的匹配概率作为模糊推理系统的输入，通过模糊推理系统得到自适应估计概率，最后基于该自适应估计概率融合多个状态估计得到高动态系统状态变量最终的均值及协方差估计，本发明不但能够实现高动态、强非线性、非高斯模型的组合系统的数据融合，而且能够降低预存储模型集的数量，同时提高模型概率更新的计算效率和高动态系统的测量鲁棒性。

Description

一种面向高动态非高斯模型鲁棒测量的高精度数据融合方法

技术领域

本发明涉及一种面向高动态非高斯模型鲁棒测量的高精度数据融合方法，其适应领域为组合导航以及其它多传感器信息融合领域。

背景技术

全球卫星导航系统(GNSS)是一种能提供全天候精确定位服务的导航系统，容易受人为和非人为干扰，导致其定位鲁棒性较差。惯性导航系统(INS)是一种完全自主的导航系统，具有良好的抗干扰能力，具有短时精度高，长时工作导航精度低的特点。将两种导航系统进行融合能取长补短，获得更好的导航效果，因而成为导航专业研究的热点。多传感器输出的数据融合算法是组合导航研究中的重点，近年来卡尔曼滤波器(KF)及其扩展算法EKF在工程领域得到了广泛的应用，EKF基于雅克比矩阵解决系统的非线性问题，其状态估计精度可达到泰勒级数展开的一阶水平，在载体静止或者低动态情况下获得了良好的效果，但是对强非线性系统估计精度较差，有时滤波器甚至会发散。为了提高基于KF的滤波算法的适用性，有学者提出UKF用于解决系统的强非线性，直接采用UT变换逼近系统的噪声驱动过程，避免了非线性问题线性化的过程，可以使任意非线性、非高斯噪声系统的后验状态变量的估计精度达到泰勒级数展开的二阶水平，而对非线性、高斯噪声模型的状态估计这一指标可以达到三阶。UKF算法较EKF没有引入更多的算法复杂度，且其提高了数据融合算法处理系统非线性的能力，但是由于其系统状态模型驱动过程基于UT变换更易处理噪声服从高斯分布的先决条件，其在解决非高斯噪声的最优估计问题上还存在一些不足。为了解决非高斯噪声环境下的最优估计，有学者提出了粒子滤波(PF)算法，采用样本形式而不是函数形式对状态概率密度进行描述，适用于任意非线性非高斯的动态系统，但是由于其存在粒子退化、重要性密度函数选取以及计算量大等问题，使其在实际的数据融合应用面临很多问题。

目前数据融合的最优估计研究多集中在对新息数据的估计上，围绕新息数据的非高斯性出现了两种解决方法。一种是针对单一系统模型的自适应参数估计，以自适应卡尔曼滤波(AKF)为代表,但是基于单一模型的参数自适应算法在系统参数变化复杂(如高机动系统)情况下无法及时准确的对系统的模型参数进行辨识，随着高动态以及复杂度较高的多传感器器融合系统的应用需求增大，基于单一模型的自适应算法适用性较弱。另一种就是基于多模型的交互算法，以多模交互(IMM)技术为例，IMM的基本思想是首先建立理想情况下的标称模型，然后根据系统可能的不确定性因素，构造多个模型组成模型集，在系统运行时刻根据系统状态自适应的调整每个模型的权值，使得最终的统计数据模型逼近系统真实状态。传统的IMM信息融合多用于动态目标的跟踪应用，最近才出现了将其应用于组合导航数据融合的论述，但是直接采用IMM仍存在许多问题，比如为了精确的匹配系统运行状态构造足够大的模型集使得计算量偏大，同时模型集的精确度受算法设计者的先验知识影响较大，而基于变结构的多模交互(VSIMM)算法可以减少系统预先存储的模型集数目，减少概率转移矩阵的运算量，并且算法在有限的模型集中自适应的产生新的模型以适应系统过程噪声统计特性的变化。因此提出基于VSIMM-UKF的组合导航系统来解决高动态、强非线性和非高斯的问题，对实现复杂系统状态变量的参数估计具有重要的指导意义。

发明内容

发明目的：为了克服现有技术中存在的不足，本发明提供一种面向高动态非高斯模型鲁棒测量的高精度数据融合方法，该方法不但能够实现高动态、强非线性、非高斯模型的组合系统的数据融合，而且能够降低预存储模型集的数量，同时提高模型概率更新的计算效率和高动态系统的测量鲁棒性。

为实现上述目的，本发明采用的技术方案为：一种面向高动态非高斯模型鲁棒测量的高精度数据融合方法，将高动态系统中的硬件传感器采样周期波动作为系统随机不确定度考虑，根据其波动范围和趋势建立滤波模型集，该滤波模型集包括一个以上UKF滤波器模型和模糊推理系统；所述UKF滤波器模型之间并行执行，通过贝叶斯定理计算每个UKF滤波器模型与当前高动态系统状态匹配的概率，实时更新每个UKF滤波模型与当前高动态系统的匹配概率，并将更新后的匹配概率作为模糊推理系统的输入，通过模糊推理系统得到UKF滤波器模型概率的自适应估计概率，最后基于该自适应估计概率融合多个UKF滤波器模型输出的状态估计得到高动态系统状态变量最终的均值及协方差估计。

所述UKF滤波器模型的建立方法如下：

步骤A，根据惯导系统的转移矩阵、系统状态变量、噪声驱动矩阵、系统噪声矢量以及白噪声建立系统状态方程；

步骤B，根据可见卫星数目、频率误差、相位误差以及接收机位置和速度建立UKF滤波器模型的系统量测方程。

所述步骤A中的系统状态方程为：

$\left[\begin{array}{c}{\stackrel{\·}{X}}_{\mathrm{INS}}\\ {\stackrel{\·}{X}}_{\mathrm{GPS}}\end{array}\right]=\left[\begin{array}{cc}{F}_{\mathrm{INS}}& 0\\ 0& F_{\mathrm{GPS}}\end{array}\right]X+\left[\begin{array}{cc}{G}_{\mathrm{INS}}& 0\\ 0& G_{\mathrm{GPS}}\end{array}\right]\left[\begin{array}{c}{W}_{\mathrm{INS}}\\ W_{\mathrm{GPS}}\end{array}\right]$

其中，X为系统的状态向量，F_INS为惯导系统的转移矩阵， $F_{\mathrm{INS}}=\left[\begin{array}{cc}{F}_N& F_S\\ 0_{6\×6}& F_M\end{array}\right],$ F_N为9维基本导航参数系统阵，F_S和F_M分别为 $F_S=\left[\begin{array}{cc}{c}_b^n& 0_{3\×3}\\ 0_{3\×3}& c_b^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right];$ F_M＝[0_6×15]，为姿态矩阵； $G_{\mathrm{INS}}=\left[\begin{array}{ccc}{c}_b^n& 0_{3\×3}& 0_{3\×9}\\ 0_{3\×3}& c_b^n& 0_{3\×9}\\ 0_{9\×3}& 0_{9\×3}& I_{9\×9}\end{array}\right]$ 为噪声驱动矩阵，W_INS＝[ω_ε ω_Δ 0_1×9]^T为惯导系统噪声矢量；ω_ε和ω_Δ分别为陀螺仪和加速度计的随机误差； $F_{\mathrm{GPS}}=\left[\begin{array}{cc}0& 0\\ 0& 1\end{array}\right];G_{\mathrm{GPS}}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ W_GPS＝[ω_b ω_f]^T，ω_b、ω_f为时钟偏置和时钟漂移过程对应的白噪声，C为光速。

所述步骤B中的UKF滤波器模型的系统量测方程建立方法如下，记Z(k)＝{η_I+dI,η_Q+dQ}_j为UKF观测向量，j为接收机跟踪的信号通道数，观测矩阵H可以表示为：

$H={\left[\begin{array}{ccccccccc}h{\left(I\right)}_{x1}& h{\left(I\right)}_{y1}& h{\left(I\right)}_{z1}& h{\left(I\right)}_{\stackrel{\·}{x}1}& h{\left(I\right)}_{\stackrel{\·}{y}1}& h{\left(I\right)}_{\stackrel{\·}{z}1}& 0_{1\×9}& 1& 0\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ h{\left(I\right)}_{\mathrm{xs}}& h{\left(I\right)}_{\mathrm{ys}}& h{\left(I\right)}_{\mathrm{zs}}& h{\left(I\right)}_{\stackrel{\·}{x}s}& h{\left(I\right)}_{\stackrel{\·}{y}s}& h{\left(I\right)}_{\stackrel{\·}{z}s}& 0_{1\×9}& 1& 0\\ h{\left(Q\right)}_{x1}& h{\left(Q\right)}_{y1}& h{\left(Q\right)}_{z1}& h{\left(Q\right)}_{\stackrel{\·}{x}1}& h{\left(Q\right)}_{\stackrel{\·}{y}1}& h{\left(Q\right)}_{\stackrel{\·}{z}1}& 0_{1\×9}& 0& 1\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ h{\left(Q\right)}_{\mathrm{xs}}& h{\left(Q\right)}_{\mathrm{ys}}& h{\left(Q\right)}_{\mathrm{zs}}& h{\left(Q\right)}_{\stackrel{\·}{x}s}& h{\left(Q\right)}_{\stackrel{\·}{y}s}& h{\left(Q\right)}_{\stackrel{\·}{z}s}& 0_{1\×9}& 0& 1\end{array}\right]}_{2s\×17},$

其中，s为可见卫星数目,以计算x轴向的速度和位置与I、Q的关系为例，得到

$h{\left(I\right)}_{\mathrm{xi}}=\frac{1}{2}[\frac{\∂E\left(I\right)}{{\∂\mathrm{\θ}}_e}\frac{{\∂\mathrm{\θ}}_e}{\∂x}+\frac{\∂E\left(I\right)}{{\∂\mathrm{\ω}}_e}\frac{{\∂\mathrm{\ω}}_e}{\∂x}];h{\left(I\right)}_{\stackrel{\·}{x}i}=\frac{1}{2}[\frac{\∂E\left(I\right)}{{\∂\mathrm{\θ}}_e}\frac{{\∂\mathrm{\θ}}_e}{\∂\stackrel{\·}{x}}+\frac{\∂E\left(I\right)}{{\∂\mathrm{\ω}}_e}\frac{{\∂\mathrm{\ω}}_e}{\∂\stackrel{\·}{x}}];$

$h{\left(Q\right)}_{\mathrm{xi}}=\frac{1}{2}[\frac{\∂E\left(Q\right)}{{\∂\mathrm{\θ}}_e}\frac{{\∂\mathrm{\θ}}_e}{\∂x}+\frac{\∂E\left(Q\right)}{{\∂\mathrm{\ω}}_e}\frac{{\∂\mathrm{\ω}}_e}{\∂x}];h{\left(Q\right)}_{\stackrel{\·}{x}i}=\frac{1}{2}[\frac{\∂E\left(Q\right)}{{\∂\mathrm{\θ}}_e}\frac{{\∂\mathrm{\θ}}_e}{\∂\stackrel{\·}{x}}+\frac{\∂E\left(Q\right)}{{\∂\mathrm{\ω}}_e}\frac{{\∂\mathrm{\ω}}_e}{\∂\stackrel{\·}{x}}];$

计算同相支路信号期望E(I)对相位误差θ_e和频率误差ω_e的偏微分方程得到：

$\frac{\∂E\left(I\right)}{{\∂\mathrm{\θ}}_e}=\frac{A}{{2\mathrm{\ω}}_e}[\cos ({\mathrm{\ω}}_e(k+1)T+{\mathrm{\θ}}_e)-\cos ({\mathrm{\ω}}_e\mathrm{kT}+{\mathrm{\θ}}_e)];$

$\begin{array}{c}\frac{\∂E\left(I\right)}{{\∂\mathrm{\ω}}_e}=\frac{-A}{{2\mathrm{\ω}}_e^2}[\sin ({\mathrm{\ω}}_e(k+1)T+{\mathrm{\θ}}_e)-\sin ({\mathrm{\ω}}_e\mathrm{kT}+{\mathrm{\θ}}_e)]\\ +\frac{A}{{2\mathrm{\ω}}_e}[(k+1)T\·\cos ({\mathrm{\ω}}_e(k+1)T+{\mathrm{\θ}}_e)-\mathrm{kT}\·\cos ({\mathrm{\ω}}_e\mathrm{kT}+{\mathrm{\θ}}_e)]\end{array};$

同理，计算正交相支路信号期望E(Q)对相位误差θ_e和频率误差ω_e的偏微分方程得到:

$\frac{\∂E\left(Q\right)}{{\∂\mathrm{\θ}}_e}=\frac{-A}{{2\mathrm{\ω}}_e}[-\sin ({\mathrm{\ω}}_e(k+1)T+{\mathrm{\θ}}_e)+\sin ({\mathrm{\ω}}_e\mathrm{kT}+{\mathrm{\θ}}_e)];$

$\begin{array}{c}\frac{\∂E\left(Q\right)}{{\∂\mathrm{\ω}}_e}=\frac{A}{{2\mathrm{\ω}}_e^2}[\cos ({\mathrm{\ω}}_e(k+1)T+{\mathrm{\θ}}_e)-\cos ({\mathrm{\ω}}_e\mathrm{kT}+{\mathrm{\θ}}_e)]\\ +\frac{A}{{2\mathrm{\ω}}_e}[(k+1)T\·\sin ({\mathrm{\ω}}_e(k+1)T+{\mathrm{\θ}}_e)-\mathrm{kT}\·\sin ({\mathrm{\ω}}_e\mathrm{kT}+{\mathrm{\θ}}_e)]\end{array};$

其中，相位误差θ_e和频率误差ω_e对接收机位置误差(x_e,y_e,z_e)和速度误差的偏微分计算如下：

$\frac{{\∂\mathrm{\θ}}_e}{\∂x}=\frac{\mathrm{\ω}}{C}\frac{x_e}{R_e};\frac{{\∂\mathrm{\θ}}_e}{\∂y}=\frac{\mathrm{\ω}}{C}\frac{y_e}{R_e};\frac{{\∂\mathrm{\θ}}_e}{\∂z}=\frac{\mathrm{\ω}}{C}\frac{z_e}{R_e};$

$\frac{{\∂\mathrm{\ω}}_e}{\∂x}=\frac{{\∂\mathrm{\ω}}_e}{\∂y}=\frac{{\∂\mathrm{\ω}}_e}{\∂z}=0;$

$\frac{{\∂\mathrm{\θ}}_e}{\∂\stackrel{\·}{x}}=\frac{-\mathrm{\ω}}{C}\frac{{\stackrel{\·}{x}}_{e}T}{V_e};\frac{{\∂\mathrm{\θ}}_e}{\∂\stackrel{\·}{y}}=\frac{-\mathrm{\ω}}{C}\frac{{\stackrel{\·}{y}}_{e}T}{V_e};\frac{{\∂\mathrm{\θ}}_e}{\∂\stackrel{\·}{z}}=\frac{-\mathrm{\ω}}{C}\frac{{\stackrel{\·}{z}}_{e}T}{V_e};$

$\frac{{\∂\mathrm{\ω}}_e}{\∂\stackrel{\·}{x}}=\frac{-\mathrm{\ω}}{C}\frac{{\stackrel{\·}{x}}_e}{V_e};\frac{{\∂\mathrm{\ω}}_e}{\∂\stackrel{\·}{y}}=\frac{-\mathrm{\ω}}{C}\frac{{\stackrel{\·}{y}}_e}{V_e};\frac{{\∂\mathrm{\ω}}_e}{\∂\stackrel{\·}{z}}=\frac{-\mathrm{\ω}}{C}\frac{{\stackrel{\·}{z}}_e}{V_e}.$

其中，位置误差和速度误差分别为其测量值与估计值之差，θ_e和ω_e分别为鉴相器和鉴频器输出。

所述将高动态系统中的硬件传感器采样周期波动作为系统随机不确定度考虑，根据其波动范围和趋势建立滤波模型集的方法：根据载体的运动状态构造模型集M＝{m₁,m₂,m₃}，其中m₁是采样周期偏大时对应的模型，m₂是采样周期偏小时对应的模型,m₃代表系统标称采样周期模型，设Q₁、Q₂、Q₃为三个模型对应的过程噪声方差，初始时Q₁、Q₂选择相差较大的值，Q₃为Q₁、Q₂两者之间的值。

所述模糊推理系统的建立方法如下：

首先，记从m_j(k)到m_i(k)的转移概率为π_ji,其满足为系统模型数目,m_j(k)到m_i(k+1)的条件转移概率记为μ_ji(k)，则：

$\begin{array}{c}{\mathrm{\μ}}_{\mathrm{ji}}\left(k\right)=p\left\{m_j\left(k\right)\right|m_i(k+1),Z^k\}\\ =\frac{{\mathrm{\π}}_{\mathrm{ji}}{\mathrm{\μ}}_j\left(k\right)}{\underset{j=1}{\overset{r}{\mathrm{\Σ}}}{\mathrm{\π}}_{\mathrm{ji}}{\mathrm{\μ}}_j\left(k\right)}\end{array}$

其中Z^k表示量测信息集合，μ_j(k)为m_j(k)在k时刻为系统匹配模型的概率称作模型概率；

其次，由于已知量测信息z(k)，进行一阶泰勒展开，则基于模型m_i(k)的滤波残差矢量为：

${\mathrm{\ϵ}}_i\left(k\right)\≈z\left(k\right)-[h_k({\hat{x}}_i\left(k\right|k-1),m_i\left(k\right))+r_i\left(k\right)]$

输出残差的协方差为：

$S_i\left(k\right)=E\left\{{\mathrm{\ϵ}}_i\left(k\right){\mathrm{\ϵ}}_i^T\left(k\right)\right\}$

因此k时刻模型m_i(k)为匹配模型的似然函数：

${{\mathrm{\Λ}}^i}_k=N[{\mathrm{\ϵ}}_i\left(k\right),0,S_i\left(k\right)]=\sqrt{\left[2\mathrm{\π}\right|S_i\left(k\right)]}\·\exp \{-\frac{1}{2}{\mathrm{\ϵ}}_i^T\left(k\right)S_i\left(k\right){\mathrm{\ϵ}}_i\left(k\right)\}$

得到模型概率的更新方程为：

${\widehat{\mathrm{\μ}}}_k^i=\frac{{{\mathrm{\Λ}}^i}_k{\mathrm{\Σ}}_{j=1}^r{\mathrm{\π}}_{\mathrm{ji}}{\mathrm{\μ}}_j(k-1)}{{\mathrm{\Σ}}_{i=1}^r{{\mathrm{\Λ}}^i}_k{\mathrm{\Σ}}_{j=1}^r{\mathrm{\π}}_{\mathrm{ji}}{\mathrm{\μ}}_j(k-1)};$

再次，进行归一化处理，模糊规则和模糊输出由一个隶属函数直接给出，使用升半梯形分布作为输出隶属度函数，其形式如下：

$f\left(x\right)=\left\{\begin{array}{cc}1,& x>b\\ \frac{x-a}{b-a},& a<x<b\\ 0,& x\&le;a\end{array}\right.$

其取值范围为[0,1]，根据隶属度函数可以得到模型的新概率经归一化处理就得到最终的模型匹配概率，即

$M_k={\mathrm{\&Sigma;}}_{i=1}^3{\widetilde{\mathrm{\&mu;}}}_k^i;{\mathrm{\&mu;}}_k^i={\widetilde{\mathrm{\&mu;}}}_k^i/M_k;$

最后，得到的估计输出为：

${\hat{x}}_{k|k}=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}{\hat{x}}_{k|k}^i{\mathrm{\&mu;}}_k^i$

$P_k^k=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}[P_{k|k}^i+({\hat{x}}_{k|k}-{\hat{x}}_{k|k}^i){({\hat{x}}_{k|k}-{\hat{x}}_{k|k}^i)}^T]u_k^i;$

其中，是考虑k时刻量测值后的最终状态融合估计值，为其对应的协方差。

本发明提供的一种面向高动态非高斯模型鲁棒测量的高精度数据融合方法，相比现有技术，具有以下有益效果：

(1)采用变结构多模交互算法(VSIMM)处理导航滤波器量测更新过程中由于新息数据的非高斯性引入的计算误差；

(2)引入模糊推理系统解决IMM模型集合的自适应问题，降低了预存储模型集的数量，提高了模型概率更新的计算效率；

(3)将高动态系统中硬件传感器采样周期波动作为系统的随机不确定性干扰，引入组合导航系统的滤波模型，提高高动态系统的测量鲁棒性；

(4)建立基于变结构多模型交互的UKF滤波器模型集合，实现高动态、强非线性、非高斯模型的组合系统的数据融合。

附图说明

图1超紧组合导航系统的原理框图；

图2基于UKF的滤波器结构框图；

图3变结构多模交互算法框图。

具体实施方式

下面结合附图对本发明作更进一步的说明。

一种面向高动态非高斯模型鲁棒测量的高精度数据融合方法，将高动态系统中的硬件传感器采样周期波动作为系统随机不确定度考虑，根据其波动范围和趋势建立滤波模型集，该滤波模型集包括一个以上UKF滤波器模型和模糊推理系统；所述UKF滤波器模型之间并行执行，通过贝叶斯定理计算每个UKF滤波器模型与当前高动态系统状态匹配的概率，实时更新每个UKF滤波模型与当前高动态系统的匹配概率，并将更新后的匹配概率作为模糊推理系统的输入，通过模糊推理系统得到UKF滤波器模型概率的自适应估计概率，最后基于该自适应估计概率融合多个UKF滤波器模型输出的状态估计得到高动态系统状态变量最终的均值及协方差估计。

以GPS/INS超紧组合导航系统为例，本发明提出的数据融合算法的实施内容如下：

步骤1、构造系统模型集

设非线性系统的状态方程和量测方程描述如下：

x(k+1)＝f_k(x(k),u(k),m(k))+w_k(x(k),m(k))

z(k)＝h_k(x(k),m(k))+v_k(x(k),m(k))

f_k(*)和h_k(*)分别为状态转移函数和量测函数,x(k)、z(k)为采样时刻为k，系统模型为m(k)时的状态向量和量测向量，设系统无约束(即此处u(k)＝0)时模型转换过程符合一阶马尔可夫过程，w_k(*)和v_k(*)分别为系统的过程噪声和量测噪声，其方差分别记为Q_k、R_k，其中根据高动态系统采样周期波动设计三种不同过程噪声方差，即Q₁＝100Q₂,Q₃为系统的期望噪声模型，其值为介于Q₁和Q₂之间的适当值。

步骤2、确定UKF滤波过程

已知x(k)为n(此处为n＝17)维状态向量，设初始样本均值和协方差分别为和P₀,UKF滤波的第一步是通过UT变换得到2n+1个Sigma点X_i和相应的权值W_i,进而实现对系统状态变量的先验估计，其实现过程如下：

$X_0=\stackrel{\&OverBar;}{x}$

$X_i=\stackrel{\&OverBar;}{x}+{\left(\sqrt{(n+\mathrm{\&lambda;})P_0}\right)}_i^T,i=1,...,n$

$X_{i+n}=\stackrel{\&OverBar;}{x}-{\left(\sqrt{(n+\mathrm{\&lambda;})P_0}\right)}_i^T,i=1,...,n$

$W_0^{\left(m\right)}=\frac{\mathrm{\&lambda;}}{n+\mathrm{\&lambda;}}$

$W_0^{\left(c\right)}=W_0^{\left(m\right)}+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})$

$W_i^{\left(m\right)}=W_i^{\left(c\right)}=\frac{\mathrm{\&lambda;}}{2(n+\mathrm{\&lambda;})},i=1,...2n$

其中是矩阵平方根的第i行，λ＝α²(n+κ)-n是尺度调节因子，α确定附近Sigma点的传播特性，通常取1e-4≤α≤1，调节β可以提高方差精度，β≥0反映了状态矢量的先验信息量，一般选择β＝2作为正态分布的最优值，κ是次级尺度调节因子，一般取κ＝0，分别作为用来计算均值和协方差的权值。

将各采样点通过任意非线性模型f(*)，计算状态向量预测统计参数

y_i＝f(X_i),i＝0,…,2n

${\stackrel{\&OverBar;}{y}}_u=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^{\left(m\right)}{y}_i$

$\stackrel{\&OverBar;}{P_u}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}(y_i-{\stackrel{\&OverBar;}{y}}_u){(y_i-{\stackrel{\&OverBar;}{y}}_u)}^T$

所以基于k时刻的统计参数计算估计k+1时刻的统计参数步骤如下：

(1)确定先验状态估计参数

ξ_i,k+1|k＝f(X_i,k+1|k),i＝1,…,2n

${\hat{x}}_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^{\left(m\right)}{\mathrm{\&xi;}}_{i,k+1|k}$

$P_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}({\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_{k+1|k}){({\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_{k+1|k})}^T+Q_k$

(2)估计观测量采样点，更新协方差及与状态量的互协方差

ζ_i,k+1|k＝h(ξ_i,k+1|k)

${\hat{z}}_{k+1|k}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^{\left(m\right)}{\mathrm{\&zeta;}}_{i,k+1|k}$

$P_{\mathrm{ZZ}}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}({\mathrm{\&zeta;}}_{i,k+1|k}-{\hat{z}}_{k+1|k}){({\mathrm{\&zeta;}}_{i,k+1|k}-{\hat{z}}_{k+1|k})}^T+R_k$

$P_{\mathrm{XZ}}=\underset{i=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_i^{\left(c\right)}({\mathrm{\&xi;}}_{i,k+1|k}-{\hat{x}}_{k+1|k}){({\mathrm{\&zeta;}}_{i,k+1|k}-{\hat{z}}_{k+1|k})}^T$

(3)计算滤波增益，更新状态向量和协方差

$K_{k+1}=P_{\mathrm{XZ}}{P}_{\mathrm{ZZ}}^{-1}$

${\hat{x}}_{k+1}={\hat{x}}_{k+1|k}+K_{k+1}(z_{k+1}-{\hat{z}}_{k+1|k})$

$P_{k+1}=P_{k+1|k}-K_{k+1}{P}_{\mathrm{ZZ}}{K}_{k+1}^T$

步骤3、基于变结构IMM实现多个UKF滤波信息的融合

设模型间的初始马尔可夫转移矩阵为：

${\left({\mathrm{\&pi;}}_{\mathrm{ji}}\right)}_{3\&times;3}=\left[\begin{array}{ccc}{\mathrm{\&pi;}}_{11}& {\mathrm{\&pi;}}_{12}& {\mathrm{\&pi;}}_{13}\\ {\mathrm{\&pi;}}_{21}& {\mathrm{\&pi;}}_{22}& {\mathrm{\&pi;}}_{23}\\ {\mathrm{\&pi;}}_{31}& {\mathrm{\&pi;}}_{32}& {\mathrm{\&pi;}}_{33}\end{array}\right]$

其中π₁₁、π₂₂、π₃₃为模型m₁、m₂、m₃无约束条件下的模型不变概率(一般值较大)，其余元素为无约束条件下模型间的转移概率(一般值较小)，初始转移概率的大小，并不会影响最终的数据融合结果。模型j的k-1时刻模型概率为M_k为系统k时刻的模型集，对任一模型m_j∈M_k其由k-1时刻到k时刻的转换到模型i的数据融合过程如下所示：

1、模型条件初始化

系统k-1时刻状态量为对应的协方差为每个模型的匹配概率均为模型j的交互模型概率为

${\mathrm{\&mu;}}_{k|k-1}^j=\underset{m_i\&Element;M_{k-1}}{\mathrm{\&Sigma;}}{\mathrm{\&pi;}}_{\mathrm{ji}}{\mathrm{\&mu;}}_{k-1|k-1}^j$

模型融合权重可以通过计算,则有交互估计状态值和协方差值分别为

$X_{k-1|k-1}^{0i}=\underset{m_j\&Element;M_{k-1}}{\mathrm{\&Sigma;}}{X}_{k-1|k-1}^j{\mathrm{\&mu;}}_{k-1|k-1}^{\mathrm{ji}}$

$P_{k-1|k-1}^{0i}=\underset{m_j\&Element;M_{k-1}}{\mathrm{\&Sigma;}}{\mathrm{\&mu;}}_{k-1|k-1}^{\mathrm{ji}}\{P_{k-1|k-1}^j+[X_{k-1|k-1}^j-X_{k-1|k-1}^{0i}\left]{[X_{k-1|k-1}^j-X_{k-1|k-1}^{0i}]}^T\right\}$

为已知量测值Z^k-1条件下对目标状态的估计，为对应的协方差。

2、模型条件滤波

将作为下一时刻与模型m_j匹配的输入变量，对每个UKF滤波器应用步骤2介绍的步骤进行状态滤波，得到各自的状态估计

3、模型概率更新

设k时刻模型m_j为匹配模型的似然函数

${{\mathrm{\&Lambda;}}^i}_k=N[{\mathrm{\&epsiv;}}_i\left(k\right),0,S_i\left(k\right)]=\sqrt{\left[2\mathrm{\&pi;}\right|S_i\left(k\right)]}\&CenterDot;\exp \{-\frac{1}{2}{\mathrm{\&epsiv;}}_i^T\left(k\right)S_i\left(k\right){\mathrm{\&epsiv;}}_i\left(k\right)\}$

模型概率的更新方程为

${\widehat{\mathrm{\&mu;}}}_k^i=\frac{{{\mathrm{\&Lambda;}}^i}_k{\mathrm{\&mu;}}_{k|k-1}^j}{{\mathrm{\&Sigma;}}_{i=1}^r{{\mathrm{\&Lambda;}}^i}_k{\mathrm{\&mu;}}_{k|k-1}^j}$

取r＝3，将作为模糊推理系统的输入，以升半梯形函数作为模糊系统输出隶属度函数，得到校正后的模型概率经归一化处理即

$M_k={\mathrm{\&Sigma;}}_{i=1}^3{\widetilde{\mathrm{\&mu;}}}_k^i;{\mathrm{\&mu;}}_k^i={\widetilde{\mathrm{\&mu;}}}_k^i/M_k$

4、估计输出

融合估计输出为

${\hat{X}}_{k|k}=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}{X}_{k|k}^i{\mathrm{\&mu;}}_k^i$

$P_k^k=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}[P_{k|k}^i+({\hat{X}}_{k|k}-X_{k|k}^i){({\hat{X}}_{k|k}-X_{k|k}^i)}^T]u_k^i$

其中是考虑k时刻量测值后的最终状态融合估计值，为对应的协方差。

一种新的GNSS/INS超紧组合导航作为数据融合算法的应用平台，如图1所示，其利用INS的短时高精度特性，通过建立GNSS状态误差的回归模型，实现卫星失锁时导航系统输出定位，提高了组合导航系统的鲁棒性。由于超紧组合结构借助INS和接收机星历数据计算出载体当前的多普勒频移，从而实现捕获过程的频率轴搜索效率增大，此外，超紧组合导航结构一方面使用数据融合滤波器的输出校正INS导航解，另一方面还对GNSS的捕获和跟踪过程进行控制，使得组合导航系统可以工作在微弱信号和高动态的环境下。同时，超紧组合可以近似准确的得到当前时刻的多普勒频移，使得接收机的跟踪环路可以不考虑动态应力的影响，从而以较小的环路带宽实现高动态载体的持续跟踪，并且解码出质量较好的导航数据，其抗干扰能力显著增强的同时提高了GNSS的导航解算精度，使得该组合系统对IMU器件的精度要求不再苛刻，显著降低了系统成本。考虑低成本IMU器件在系统高动态情况下，采样周期的小幅度波动会对系统的不确定性带来显著影响，针对该不确定性因素，本发明提出了基于变结构多模交互算法实现过程噪声模型的自适应匹配。

基于上述系统结构和特征，本发明提出了一种新型的数据融合算法：

(1)建立多组UKF多模型

将载体机动过程引入的传感器采样周期波动作为系统的随机不确定性因素考虑，根据载体的运动状态构造模型集M＝{m₁,m₂,m₃}，其中m₁是采样周期偏大时对应的模型，m₂是采样周期偏小时对应的模型,m₃代表系统标称采样周期模型，也即系统期望模型，设Q₁、Q₂、Q₃为三个模型对应的过程噪声方差，初始时Q₁、Q₂选择相差较大的值，Q₃在两者之间选择一个合适的值，确保初始时系统有较宽的噪声范围以覆盖系统各种运动突变，下面建立UKF模型的状态方程和观测方程选择INS姿态误差、速度误差、位置误差、陀螺零偏、加速度计零偏以及GNSS时钟偏移、时钟漂移为系统状态变量，表示成如下形式：

作为系统的状态向量，以GPS为例，系统的状态方程如下：

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{X}}_{\mathrm{INS}}\\ {\stackrel{\&CenterDot;}{X}}_{\mathrm{GPS}}\end{array}\right]=\left[\begin{array}{cc}{F}_{\mathrm{INS}}& 0\\ 0& F_{\mathrm{GPS}}\end{array}\right]X+\left[\begin{array}{cc}{G}_{\mathrm{INS}}& 0\\ 0& G_{\mathrm{GPS}}\end{array}\right]\left[\begin{array}{c}{W}_{\mathrm{INS}}\\ W_{\mathrm{GPS}}\end{array}\right]$

其中F_INS为惯导系统的转移矩阵

$F_{\mathrm{INS}}=\left[\begin{array}{cc}{F}_N& F_S\\ 0_{6\&times;6}& F_M\end{array}\right]$

其中F_N为9维基本导航参数系统阵，F_S和F_M分别为

$F_S=\left[\begin{array}{cc}{c}_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& c_b^n\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right];$ F_M＝[0_6×15]

为姿态矩阵，噪声驱动矩阵G_INS和系统噪声矢量W_INS分别为

$G_{\mathrm{INS}}=\left[\begin{array}{ccc}{c}_b^n& 0_{3\&times;3}& 0_{3\&times;9}\\ 0_{3\&times;3}& c_b^n& 0_{3\&times;9}\\ 0_{9\&times;3}& 0_{9\&times;3}& I_{9\&times;9}\end{array}\right];$ W_INS＝[ω_ε ω_Δ 0_1×9]^T

其中ω_ε和ω_Δ分别为陀螺仪和加速度计的随机误差。

状态方程的另一部分为GNSS的时钟偏移和时钟漂移，其与接收机位置和速度对应的微分方程表示如下

$C\stackrel{\&CenterDot;}{\mathrm{\&delta;}}b\left(t\right)=\mathrm{C\&delta;f}\left(t\right)+C{\mathrm{\&omega;}}_b$

$C\stackrel{\&CenterDot;}{\mathrm{\&delta;}}f\left(t\right)=C{\mathrm{\&omega;}}_f$

所以有

$F_{\mathrm{GPS}}=\left[\begin{array}{cc}0& 0\\ 0& 1\end{array}\right];G_{\mathrm{GPS}}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ W_GPS＝[ω_b ω_f]^T

其中ω_b、ω_f为时钟偏置和时钟漂移过程对应的白噪声，C为光速。

在图2所示UKF滤波器结构中，将GNSS相关器输出I、Q分量和基于INS输出好和星历数据估计得到的另一组I、Q相减作为UKF的观测量，设接收机接收到的数字中频信号为

y(t)＝A·CA(t)·D(t)cos(2πf₀(t-τ)+θ₀)+η(t)

其中A是信号幅值，CA(t)是C/A码序列，D(t)为导航数据，τ为传输延迟，f₀和θ₀分别为载波频率和初始相位，η(t)为高斯噪声，则相关器的两路正交相(Q)和同相(I)输出可表示为

$I={\&Integral;}_{\mathrm{kT}}^{(k+1)T}\left\{\cos (\widehat{\mathrm{\&omega;}}t+\widehat{\mathrm{\&theta;}})\right[A\cos (\mathrm{\&omega;t}+\mathrm{\&theta;})+{\mathrm{\&eta;}}_I\left]\right\}\mathrm{dt}$

$Q={\&Integral;}_{\mathrm{kT}}^{(k+1)T}\left\{\sin (\widehat{\mathrm{\&omega;}}t+\widehat{\mathrm{\&theta;}})\right[A\cos (\mathrm{\&omega;t}+\mathrm{\&theta;})+{\mathrm{\&eta;}}_Q\left]\right\}\mathrm{dt}$

将上述两式展开，并求期望得到

$E\left(I\right)=\frac{A}{{2\mathrm{\&omega;}}_e}[\sin ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)-\sin ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)]$

$E\left(Q\right)=\frac{-A}{{2\mathrm{\&omega;}}_e}[\cos ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)-\cos ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)]$

其中为锁频环输出，为锁相环输出，T是预检积分时间，为了使E(I)、E(Q)与接收机的位置和速度关联，分别将ω_e、θ_e表示成下述形式

${\mathrm{\&omega;}}_e=\frac{2\mathrm{\&pi;}{f}_0}{C}|{\hat{v}}_u-v_u|=\frac{\mathrm{\&omega;}}{C}{V}_e$

${\mathrm{\&theta;}}_e=\frac{{2\mathrm{\&pi;f}}_0}{C}\left(\right|{\hat{v}}_u-v_u|t-|{\hat{R}}_u-R_u\left|\right)=\frac{\mathrm{\&omega;}}{C}(V_e-R_e)$

其中V_e、R_e分别为接收机速度及位置的估计值与测量值的误差，其定义形式为

$V_e=\sqrt{{(\widehat{\stackrel{\&CenterDot;}{x}}-\stackrel{\&CenterDot;}{x})}^2+{(\widehat{\stackrel{\&CenterDot;}{y}}-\stackrel{\&CenterDot;}{y})}^2+{(\widehat{\stackrel{\&CenterDot;}{z}}-\stackrel{\&CenterDot;}{z})}^2}$

$R_e=\sqrt{{(\hat{x}-x)}^2+{(\hat{y}-y)}^2+{(\hat{z}-z)}^2}$

基于上述分析可以建立E(I)、E(Q)与接收机位置和速度的关系式如下

$\mathrm{dE}\left(I\right)=\frac{1}{2}[\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&theta;}}_e}\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;x}+\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&omega;}}_e}\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;x}]\mathrm{dx}$

$\mathrm{dE}\left(Q\right)=\frac{1}{2}[\frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&theta;}}_e}\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}+\frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&omega;}}_e}\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}]d\stackrel{\&CenterDot;}{x}$

记Z(k)＝{I+dI,Q+dQ}_i-{I-η_I,Q-η_Q}_i＝{η_I+dI,η_Q+dQ}_i为UKF观测向量，其中η_I、η_Q为GPS测量的I、Q成分中的噪声成分，j为接收机跟踪的信号通道数,dI和dQ为惯导系统中由于惯性器件误差引起的I、Q预测误差，则观测矩阵H可以表示为

$H={\left[\begin{array}{ccccccccc}h{\left(I\right)}_{x1}& h{\left(I\right)}_{y1}& h{\left(I\right)}_{z1}& h{\left(I\right)}_{\stackrel{\&CenterDot;}{x}1}& h{\left(I\right)}_{\stackrel{\&CenterDot;}{y}1}& h{\left(I\right)}_{\stackrel{\&CenterDot;}{z}1}& 0_{1\&times;9}& 1& 0\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ h{\left(I\right)}_{\mathrm{xs}}& h{\left(I\right)}_{\mathrm{ys}}& h{\left(I\right)}_{\mathrm{zs}}& h{\left(I\right)}_{\stackrel{\&CenterDot;}{x}s}& h{\left(I\right)}_{\stackrel{\&CenterDot;}{y}s}& h{\left(I\right)}_{\stackrel{\&CenterDot;}{z}s}& 0_{1\&times;9}& 1& 0\\ h{\left(Q\right)}_{x1}& h{\left(Q\right)}_{y1}& h{\left(Q\right)}_{z1}& h{\left(Q\right)}_{\stackrel{\&CenterDot;}{x}1}& h{\left(Q\right)}_{\stackrel{\&CenterDot;}{y}1}& h{\left(Q\right)}_{\stackrel{\&CenterDot;}{z}1}& 0_{1\&times;9}& 0& 1\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ h{\left(Q\right)}_{\mathrm{xs}}& h{\left(Q\right)}_{\mathrm{ys}}& h{\left(Q\right)}_{\mathrm{zs}}& h{\left(Q\right)}_{\stackrel{\&CenterDot;}{x}s}& h{\left(Q\right)}_{\stackrel{\&CenterDot;}{y}s}& h{\left(Q\right)}_{\stackrel{\&CenterDot;}{z}s}& 0_{1\&times;9}& 0& 1\end{array}\right]}_{2s\&times;17}$

其中s为可见卫星数目,以x轴向的速度和位置与I、Q关系为例(1≤i≤s)得到

$h{\left(I\right)}_{\mathrm{xi}}=\frac{1}{2}[\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&theta;}}_e}\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;x}+\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&omega;}}_e}\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;x}];h{\left(I\right)}_{\stackrel{\&CenterDot;}{x}i}=\frac{1}{2}[\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&theta;}}_e}\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}+\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&omega;}}_e}\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}]$

$h{\left(Q\right)}_{\mathrm{xi}}=\frac{1}{2}[\frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&theta;}}_e}\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;x}+\frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&omega;}}_e}\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;x}];h{\left(Q\right)}_{\stackrel{\&CenterDot;}{x}i}=\frac{1}{2}[\frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&theta;}}_e}\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}+\frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&omega;}}_e}\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}]$

计算同相支路信号期望对相位误差和频率误差的偏微分如下

$\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&theta;}}_e}=\frac{A}{{2\mathrm{\&omega;}}_e}[\cos ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)-\cos ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)]$

$\begin{array}{c}\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&omega;}}_e}=\frac{-A}{{2\mathrm{\&omega;}}_e^2}[\sin ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)-\sin ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)]\\ +\frac{A}{{2\mathrm{\&omega;}}_e}[(k+1)T\&CenterDot;\cos ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)-\mathrm{kT}\&CenterDot;\cos ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)]\end{array}$

计算正交相支路信号期望对相位误差和频率误差的偏微分如下

$\frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&theta;}}_e}=\frac{-A}{{2\mathrm{\&omega;}}_e}[-\sin ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)+\sin ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)]$

$\begin{array}{c}\frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&omega;}}_e}=\frac{A}{{2\mathrm{\&omega;}}_e^2}[\cos ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)-\cos ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)]\\ +\frac{A}{{2\mathrm{\&omega;}}_e}[(k+1)T\&CenterDot;\sin ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)-\mathrm{kT}\&CenterDot;\sin ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)]\end{array}$

此外，频率误差和相位误差对接收机位置和速度的偏微分如下

$\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;x}=\frac{\mathrm{\&omega;}}{C}\frac{x_e}{R_e};\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;y}=\frac{\mathrm{\&omega;}}{C}\frac{y_e}{R_e};\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;z}=\frac{\mathrm{\&omega;}}{C}\frac{z_e}{R_e}$

$\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;x}=\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;y}=\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;z}=0$

$\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}=\frac{-\mathrm{\&omega;}}{C}\frac{{\stackrel{\&CenterDot;}{x}}_{e}T}{V_e};\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{y}}=\frac{-\mathrm{\&omega;}}{C}\frac{{\stackrel{\&CenterDot;}{y}}_{e}T}{V_e};\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{z}}=\frac{-\mathrm{\&omega;}}{C}\frac{{\stackrel{\&CenterDot;}{z}}_{e}T}{V_e}$

$\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}=\frac{-\mathrm{\&omega;}}{C}\frac{{\stackrel{\&CenterDot;}{x}}_e}{V_e};\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{y}}=\frac{-\mathrm{\&omega;}}{C}\frac{{\stackrel{\&CenterDot;}{y}}_e}{V_e};\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{z}}=\frac{-\mathrm{\&omega;}}{C}\frac{{\stackrel{\&CenterDot;}{z}}_e}{V_e}$

至此，基于上述参数可以确定UKF滤波器的观测矩阵H。

考虑ω_ε和ω_Δ受传感器采样周期等不确定因素的影响，可以构造不同的过程噪声方差，本发明提出了在标称采样周期的基础上，考虑采样周期偏大和偏小的情况构造了三组不同的系统状态方程,其反应在系统状态方程为过程噪声的方差

Q₁＝100Q₂。

(2)建立模糊推理系统

记从m_j(k)到m_i(k)的转移概率为π_ji,其满足为系统模型数目,本发明取r＝3，在获得系统量测信息后，m_j(k)到m_i(k+1)的条件转移概率记为μ_ji(k)，计算公式为

$\begin{array}{c}{\mathrm{\&mu;}}_{\mathrm{ji}}\left(k\right)=p\left\{m_j\left(k\right)\right|m_i(k+1),Z^k\}\\ =\frac{{\mathrm{\&pi;}}_{\mathrm{ji}}{\mathrm{\&mu;}}_j\left(k\right)}{\underset{j=1}{\overset{r}{\mathrm{\&Sigma;}}}{\mathrm{\&pi;}}_{\mathrm{ji}}{\mathrm{\&mu;}}_j\left(k\right)}\end{array}$

其中Z^k表示量测信息集合，μ_j(k)为m_j(k)在k时刻为系统匹配模型的概率称作模型概率，模型概率的计算就是假设检验过程，已知量测信息z(k)，进行一阶泰勒展开，则基于模型m_i(k)的滤波残差矢量为

${\mathrm{\&epsiv;}}_i\left(k\right)\&ap;z\left(k\right)-[h_k({\hat{x}}_i\left(k\right|k-1),m_i\left(k\right))+r_i\left(k\right)]$

输出残差的协方差为

$S_i\left(k\right)=E\left\{{\mathrm{\&epsiv;}}_i\left(k\right){\mathrm{\&epsiv;}}_i^T\left(k\right)\right\}$

因此k时刻模型m_i(k)为匹配模型的似然函数

${{\mathrm{\&Lambda;}}^i}_k=N[{\mathrm{\&epsiv;}}_i\left(k\right),0,S_i\left(k\right)]=\sqrt{\left[2\mathrm{\&pi;}\right|S_i\left(k\right)]}\&CenterDot;\exp \{-\frac{1}{2}{\mathrm{\&epsiv;}}_i^T\left(k\right)S_i\left(k\right){\mathrm{\&epsiv;}}_i\left(k\right)\}$

模型概率的更新方程为

${\widehat{\mathrm{\&mu;}}}_k^i=\frac{{{\mathrm{\&Lambda;}}^i}_k{\mathrm{\&Sigma;}}_{j=1}^r{\mathrm{\&pi;}}_{\mathrm{ji}}{\mathrm{\&mu;}}_j(k-1)}{{\mathrm{\&Sigma;}}_{i=1}^r{{\mathrm{\&Lambda;}}^i}_k{\mathrm{\&Sigma;}}_{j=1}^r{\mathrm{\&pi;}}_{\mathrm{ji}}{\mathrm{\&mu;}}_j(k-1)}$

如图3所示，IMM算法完成k时刻交互的计算后，得到单个模型的更新概率为了抑制随机干扰导致的更新概率的波动，对连续多个进行平均处理，将其作为模糊推理系统的输入，由于是归一化处理后的数据，因此可以省去输入隶属度函数。模糊规则和模糊输出由一个隶属函数直接给出，使用升半梯形分布作为输出隶属度函数，其形式如下

$f\left(x\right)=\left\{\begin{array}{cc}1,& x>b\\ \frac{x-a}{b-a},& a<x<b\\ 0,& x\&le;a\end{array}\right.$

隶属度函数是表示某一对象隶属于某一集合程度的函数，其取值范围为[0,1]，如f(x)＝0表示对象不属于集合，如为1表示完全属于集合。根据隶属度函数可以得到模型的新概率经归一化处理就得到最终的模型匹配概率，即

$M_k={\mathrm{\&Sigma;}}_{i=1}^3{\widetilde{\mathrm{\&mu;}}}_k^i;{\mathrm{\&mu;}}_k^i={\widetilde{\mathrm{\&mu;}}}_k^i/M_k$

最后的估计输出为

${\hat{x}}_{k|k}=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}{\hat{x}}_{k|k}^i{\mathrm{\&mu;}}_k^i$

$P_k^k=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}[P_{k|k}^i+({\hat{x}}_{k|k}-{\hat{x}}_{k|k}^i){({\hat{x}}_{k|k}-{\hat{x}}_{k|k}^i)}^T]u_k^i$

其中是考虑k时刻量测值后的最终状态融合估计值，为对应的协方差。

由上述可知，本发明基于GNSS系统和INS系统的超紧组合改善组合导航系统的鲁棒性和应对高动态压力的能力，采用UKF滤波解决强非线性系统数据融合的高精度问题，同时，考虑惯性组合测量系统中低成本IMU传感器的硬件条件限制，传感器的内部会出现采样周期小幅波动，将其对组合导航系统的精度影响以系统随机不确定度的形式考虑，构造多个UKF滤波器。基于模糊推理系统提出变结构交互式多模型(VSIMM)算法以解决非高斯噪声模型的模型集自适应问题，从而提高高动态高阶惯性组合测量系统的精确度和鲁棒性。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (6)

1.一种面向高动态非高斯模型鲁棒测量的高精度数据融合方法，其特征在于：将高动态系统中的硬件传感器采样周期波动作为系统随机不确定度考虑，根据其波动范围和趋势建立滤波模型集，该滤波模型集包括一个以上UKF滤波器模型和模糊推理系统；所述UKF滤波器模型之间并行执行，通过贝叶斯定理计算每个UKF滤波器模型与当前高动态系统状态匹配的概率，实时更新每个UKF滤波模型与当前高动态系统的匹配概率，并将更新后的匹配概率作为模糊推理系统的输入，通过模糊推理系统得到UKF滤波器模型概率的自适应估计概率，最后基于该自适应估计概率融合多个UKF滤波器模型输出的状态估计得到高动态系统状态变量最终的均值及协方差估计。

2.根据权利要求1所述的向高动态非高斯模型鲁棒测量的高精度数据融合方法，其特征在于：所述UKF滤波器模型的建立方法如下：

步骤A，根据惯导系统的转移矩阵、系统状态变量、噪声驱动矩阵、系统噪声矢量以及白噪声建立系统状态方程；

步骤B，根据可见卫星数目、频率误差、相位误差以及接收机位置和速度建立UKF滤波器模型的系统量测方程。

3.根据权利要求2所述的面向高动态非高斯模型鲁棒测量的高精度数据融合方法，其特征在于：所述步骤A中的系统状态方程为：

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{X}}_{\mathrm{INS}}\\ {\stackrel{\&CenterDot;}{X}}_{\mathrm{GPS}}\end{array}\right]=\left[\begin{array}{cc}{F}_{\mathrm{INS}}& 0\\ 0& F_{\mathrm{GPS}}\end{array}\right]X+\left[\begin{array}{cc}{G}_{\mathrm{INS}}& 0\\ 0& G_{\mathrm{GPS}}\end{array}\right]\left[\begin{array}{c}{W}_{\mathrm{INS}}\\ W_{\mathrm{GPS}}\end{array}\right]$

其中，X为系统的状态向量，F_INS为惯导系统的转移矩阵， $F_{\mathrm{INS}}=\left[\begin{array}{cc}{F}_N& F_S\\ 0_{6\&times;6}& F_M\end{array}\right],$ F_N为9维基本导航参数系统阵，F_S和F_M分别为 $F_S=\left[\begin{array}{cc}{c}_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& c_b^n\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right];$ F_M＝[0_6×15]，为姿态矩阵； $G_{\mathrm{INS}}=\left[\begin{array}{ccc}{c}_b^n& 0_{3\&times;3}& 0_{3\&times;9}\\ 0_{3\&times;3}& c_b^n& 0_{3\&times;9}\\ 0_{9\&times;3}& 0_{9\&times;3}& I_{9\&times;9}\end{array}\right]$ 为噪声驱动矩阵， $W_{\mathrm{INS}}={\left[\begin{array}{ccc}{\mathrm{\&omega;}}_{\mathrm{\&epsiv;}}& {\mathrm{\&omega;}}_{\mathrm{\&Delta;}}& 0_{1\&times;9}\end{array}\right]}^T$ 为惯导系统噪声矢量，ω_ε和ω_Δ分别为陀螺仪和加速度计的随机误差； $F_{\mathrm{GPS}}=\left[\begin{array}{cc}0& 0\\ 0& 1\end{array}\right];$ $G_{\mathrm{GPS}}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ W_GPS＝[ω_b ω_f]^T，ω_b、ω_f为时钟偏置和时钟漂移过程对应的白噪声，C为光速。

4.根据权利要求3所述的面向高动态非高斯模型鲁棒测量的高精度数据融合方法，其特征在于：所述步骤B中的UKF滤波器模型的系统量测方程建立方法如下，记Z(k)＝{η_I+dI,η_Q+dQ}_j为UKF观测向量，其中η_I、η_Q为GPS测量的I、Q成分中的噪声成分，dI和dQ为惯导系统中由于惯性器件误差引起的I、Q预测误差，j为接收机跟踪的信号通道数，观测矩阵H可以表示为：

$H={\left[\begin{array}{ccccccccc}{h\left(I\right)}_{x1}& {h\left(I\right)}_{y1}& {h\left(I\right)}_{z1}& {h\left(I\right)}_{\stackrel{\&CenterDot;}{x}1}& {h\left(I\right)}_{\stackrel{\&CenterDot;}{y}1}& {h\left(I\right)}_{\stackrel{\&CenterDot;}{z}1}& 0_{1\&times;9}& 1& 0\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ {h\left(I\right)}_{\mathrm{xs}}& {h\left(I\right)}_{\mathrm{ys}}& {h\left(I\right)}_{\mathrm{zs}}& {h\left(I\right)}_{\stackrel{\&CenterDot;}{x}s}& {h\left(I\right)}_{\stackrel{\&CenterDot;}{y}s}& {h\left(I\right)}_{\stackrel{\&CenterDot;}{z}s}& 0_{1\&times;9}& 1& 0\\ {h\left(Q\right)}_{x1}& {h\left(Q\right)}_{y1}& {h\left(Q\right)}_{z1}& {h\left(Q\right)}_{\stackrel{\&CenterDot;}{x}1}& {h\left(Q\right)}_{\stackrel{\&CenterDot;}{y}1}& {h\left(Q\right)}_{\stackrel{\&CenterDot;}{z}1}& 0_{1\&times;9}& 0& 1\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ {h\left(Q\right)}_{\mathrm{xs}}& {h\left(Q\right)}_{\mathrm{ys}}& {h\left(Q\right)}_{\mathrm{zs}}& {h\left(Q\right)}_{\stackrel{\&CenterDot;}{x}s}& {h\left(Q\right)}_{\stackrel{\&CenterDot;}{y}s}& {h\left(Q\right)}_{\stackrel{\&CenterDot;}{z}s}& 0_{1\&times;9}& 0& 1\end{array}\right]}_{2s\&times;17}$

其中，s为可见卫星数目,以计算x轴向的速度和位置与I、Q的关系为例，得到：

${h\left(I\right)}_{\mathrm{xi}}=\frac{1}{2}[\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&theta;}}_x}\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;x}+\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&omega;}}_e}\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;x}];{h\left(I\right)}_{\stackrel{\&CenterDot;}{x}i}=\frac{1}{2}[\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&theta;}}_e}\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}+\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&omega;}}_e}\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}];$

${h\left(Q\right)}_{\mathrm{xi}}=\frac{1}{2}[\frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&theta;}}_e}\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;x}+\frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&omega;}}_e}\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;x}];{h\left(Q\right)}_{\stackrel{\&CenterDot;}{x}i}=\frac{1}{2}[\frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&theta;}}_e}\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}+\frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&omega;}}_e}\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}];$

计算同相支路信号期望E(I)对相位误差θ_e和频率误差ω_e的偏微分方程得到：

$\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&theta;}}_e}=\frac{A}{{2\mathrm{\&omega;}}_e}[\cos ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)-\cos ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)];$

$\begin{array}{c}\frac{\&PartialD;E\left(I\right)}{{\&PartialD;\mathrm{\&omega;}}_e}=\frac{-A}{{2\mathrm{\&omega;}}_e^2}[\sin ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)-\sin ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)]\\ +\frac{A}{{2\mathrm{\&omega;}}_e}[(k+1)T\&CenterDot;\cos ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)-\mathrm{kT}\&CenterDot;\cos ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)]\end{array};$

同理，计算正交相支路信号期望E(Q)对相位误差θ_e和频率误差ω_e的偏微分方程得到:

$\begin{array}{c}\frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&theta;}}_e}=\frac{-A}{{2\mathrm{\&omega;}}_e}[-\sin ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)+\sin ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)];\\ \frac{\&PartialD;E\left(Q\right)}{{\&PartialD;\mathrm{\&omega;}}_e}=\frac{A}{{2\mathrm{\&omega;}}_e^2}[\cos ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)-\cos ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)]\\ +\frac{A}{{2\mathrm{\&omega;}}_e}[(k+1)T\&CenterDot;\sin ({\mathrm{\&omega;}}_e(k+1)T+{\mathrm{\&theta;}}_e)-\mathrm{kT}\&CenterDot;\sin ({\mathrm{\&omega;}}_e\mathrm{kT}+{\mathrm{\&theta;}}_e)]\end{array};$

其中，相位误差θ_e和频率误差ω_e对接收机位置误差(x_e,y_e,z_e)和速度误差的偏微分计算如下：

$\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;x}=\frac{\mathrm{\&omega;}}{C}\frac{x_e}{R_e};\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;y}=\frac{\mathrm{\&omega;}}{C}\frac{y_e}{R_e};\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;z}=\frac{\mathrm{\&omega;}}{C}\frac{z_e}{R_e};$

$\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;x}=\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;y}=\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;z}=0;$

$\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}=\frac{-\mathrm{\&omega;}}{C}\frac{{\stackrel{\&CenterDot;}{x}}_{e}T}{V_e};\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{y}}=\frac{-\mathrm{\&omega;}}{C}\frac{{\stackrel{\&CenterDot;}{y}}_{e}T}{V_e};\frac{{\&PartialD;\mathrm{\&theta;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{z}}=\frac{-\mathrm{\&omega;}}{C}\frac{{\stackrel{\&CenterDot;}{z}}_{e}T}{V_e};$

$\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{x}}=\frac{-\mathrm{\&omega;}}{C}\frac{{\stackrel{\&CenterDot;}{x}}_e}{V_e};\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{y}}=\frac{-\mathrm{\&omega;}}{C}\frac{{\stackrel{\&CenterDot;}{y}}_e}{V_e};\frac{{\&PartialD;\mathrm{\&omega;}}_e}{\&PartialD;\stackrel{\&CenterDot;}{z}}=\frac{-\mathrm{\&omega;}}{C}\frac{{\stackrel{\&CenterDot;}{z}}_e}{V_e};$

其中，位置误差和速度误差分别为其测量值与估计值之差，θ_e和ω_e分别为鉴相器和鉴频器输出。

5.根据权利要求4所述的面向高动态非高斯模型鲁棒测量的高精度数据融合方法，其特征在于：所述将高动态系统中的硬件传感器采样周期波动作为系统随机不确定度考虑，根据其波动范围和趋势建立滤波模型集的方法：根据载体的运动状态构造模型集M＝{m₁,m₂,m₃}，其中m₁是采样周期偏大时对应的模型，m₂是采样周期偏小时对应的模型,m₃代表系统标称采样周期模型，设Q₁、Q₂、Q₃为三个模型对应的过程噪声方差，初始时Q₁、Q₂选择相差较大的值，Q₃为Q₁、Q₂两者之间的值。

6.根据权利要求5所述的面向高动态非高斯模型鲁棒测量的高精度数据融合方法，其特征在于：所述模糊推理系统的建立方法如下：

首先，记从m_j(k)到m_i(k)的转移概率为π_ji,其满足j＝1,…,r为系统模型数目,m_j(k)到m_i(k+1)的条件转移概率记为μ_ji(k)，则：

$\begin{array}{c}{\mathrm{\&mu;}}_{\mathrm{ji}}\left(k\right)=p\left\{m_j\left(k\right)\right|m_i(k+1),Z^k\}\\ =\frac{{\mathrm{\&pi;}}_{\mathrm{ji}}{\mathrm{\&mu;}}_j\left(k\right)}{\underset{j=1}{\overset{r}{\mathrm{\&Sigma;}}}{\mathrm{\&pi;}}_{\mathrm{ji}}{\mathrm{\&mu;}}_j\left(k\right)}\end{array}$

其中Z^k表示量测信息集合，μ_j(k)为m_j(k)在k时刻为系统匹配模型的概率称作模型概率；

其次，由于已知量测信息z(k)，进行一阶泰勒展开，则基于模型m_i(k)的滤波残差矢量为：

${\mathrm{\&epsiv;}}_i\left(k\right)\&ap;z\left(k\right)-[h_k({\hat{x}}_i\left(k\right|k-1),m_i\left(k\right))+r_i\left(k\right)]$

输出残差的协方差为：

$S_i\left(k\right)=E\left\{{\mathrm{\&epsiv;}}_i\left(k\right){\mathrm{\&epsiv;}}_i^T\left(k\right)\right\}$

因此k时刻模型m_i(k)为匹配模型的似然函数：

${\mathrm{\&Lambda;}}_k^i=N[{\mathrm{\&epsiv;}}_i\left(k\right),0,S_i\left(k\right)]=\sqrt{\left[2\mathrm{\&pi;}\right|S_i\left(k\right)]}\&CenterDot;\exp \{-\frac{1}{2}{\mathrm{\&epsiv;}}_i^T\left(k\right)S_i\left(k\right){\mathrm{\&epsiv;}}_i\left(k\right)\}$

得到模型概率的更新方程为：

${\widehat{\mathrm{\&mu;}}}_i^i=\frac{{\mathrm{\&Lambda;}}_k^i{\mathrm{\&Sigma;}}_{j=1}^r{\mathrm{\&pi;}}_{\mathrm{ji}}{\mathrm{\&mu;}}_j(k-1)}{{\mathrm{\&Sigma;}}_{i=1}^r{\mathrm{\&Lambda;}}_k^i{\mathrm{\&Sigma;}}_{j=1}^r{\mathrm{\&pi;}}_{\mathrm{ji}}{\mathrm{\&mu;}}_j(k-1)};$

再次，进行归一化处理，模糊规则和模糊输出由一个隶属函数直接给出，使用升半梯形分布作为输出隶属度函数，其形式如下：

$f\left(x\right)=\left\{\begin{array}{cc}1,& x>b\\ \frac{x-a}{b-a},& a<x<b\\ 0,& x\&le;a\end{array}\right.$

其取值范围为[0,1]，根据隶属度函数可以得到模型的新概率经归一化处理就得到最终的模型匹配概率，即

$M_k={\mathrm{\&Sigma;}}_{i=1}^3{\widetilde{\mathrm{\&mu;}}}_k^i;{\mathrm{\&mu;}}_k^i={\widetilde{\mathrm{\&mu;}}}_k^i/M_k;$

最后，得到的估计输出为：

${\hat{x}}_{k|k}=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}{\hat{x}}_{k|k}^i{\mathrm{\&mu;}}_k^i$

$P_k^k=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}[P_{k|k}^i+({\hat{x}}_{k|k}-{\hat{x}}_{k|k}^i){({\hat{x}}_{k|k}-{\hat{x}}_{k|k}^i)}^T]u_k^i;$

其中，是考虑k时刻量测值后的最终状态融合估计值，为其对应的协方差。