CN105919594A - 基于磁传感的自适应胶囊内窥镜定位方法 - Google Patents

Abstract

本发明涉及到磁传感技术、医疗器械，为提供一种用于胶囊内镜的磁传感定位方法提高系统抗干扰方法，通过粒子滤波抑制磁干扰与运动干扰，以达到定位目的。本发明采用的技术方案是，基于磁传感的自适应胶囊内窥镜定位方法，构建判定准则，判断磁干扰是否来临；并在此基础上，利用马尔科夫模型估计，构建环境磁干扰与运动干扰的数学表达式，从而设计系统的观测方程与状态方程；最终，通过粒子滤波抑制磁干扰与运动干扰，以达到定位目的。本发明主要应用于磁传感定位场合。

Description

基于磁传感的自适应胶囊内窥镜定位方法

技术领域

本发明涉及到磁传感技术、医疗器械，特别是基于磁传感的定位技术，具体讲，涉及基于磁传感的自适应胶囊内窥镜定位方法。

背景技术

无线胶囊内窥镜是消化内镜发展史上第四个里程碑。胶囊内镜是一个兼有拍摄和发送数据功能的小型检查装置。相比于传统的插入式内镜相比，胶囊内镜具有无创伤、无痛苦、无干扰等诸多优点，已经被越来越多的医生和患者所接受。GI、Medtronic、Olympus、JinShan公司均向市场推出了各自的胶囊内镜产品，并保持了较快的增长势头。

但是，胶囊内镜在临床应用上暴露出几大问题，如难以确定病患组织的确切位置、胶囊内镜主动反馈式驱动问题等。上述问题均与胶囊定位有关。目前，胶囊内镜的定位技术研究主要集中在射频定位、医学影像定位、超声定位、磁场定位上。相比于其他定位技术，磁场定位技术具有定位精度高、定位速度快、实时性好的优势。

对于基于磁场的胶囊内镜定位技术，国内外均有研究。但是时域采样和磁干扰给磁场定位方法带来的问题一直没被真正解决。时域的串行采样会降低动态定位精度，而当铁磁物质或含线圈的装置靠近定位系统时，会引入磁干扰，使磁场测量失真，造成定位误差骤增甚至失败。

发明内容

为克服现有技术的不足，本发明旨在提供一种用于胶囊内镜的磁传感定位方法提高系统抗干扰方法，通过粒子滤波抑制磁干扰与运动干扰，以达到定位目的。本发明采用的技术方案是，基于磁传感的自适应胶囊内窥镜定位方法，构建判定准则，判断磁干扰是否来临；并在此基础上，利用马尔科夫模型估计，构建环境磁干扰与运动干扰的数学表达式，从而设计系统的观测方程与状态方程；最终，通过粒子滤波抑制磁干扰与运动干扰，以达到定位目的。

具体步骤进一步细化为：

步骤1.建立圆柱形磁体的磁场分布模型；

步骤2.通过布置在定位空间中的磁强计对磁体的磁场进行测量；

步骤3.通过计算磁场测量值与磁场分布模型的差异，判定磁强计是否受到磁干扰；

步骤4.根据步骤3的判定结果，得到相应的磁干扰表达式；

步骤5.通过正态分布对磁强计自身的输出浮动，即传输噪声，进行拟合；

步骤6.根据步骤1所得的磁场分布模型，步骤2所得的磁场测量结果，步骤4所得的磁干扰表示，以及步骤5所得的磁强计传输噪声，构建系统观测方程；

步骤7.通过正态分布对内镜速度噪声与角速度的波动进行拟合；

步骤8.根据步骤7所得的速度噪声表示，构建系统观测方程。

步骤3通过欧式距离，判断当前定位周期下是否收到磁场干扰。判据如下：

${\mathrm{RSS}}_k=\left|\right|B_k^{meas}-B_{k-1}\left|\right|$

其中，B_k-1为根据k-1周期的定位结果以及磁场模型，得到的磁场矢量的理论值，为k周期中磁强计的测量值，根据上述判据，当欧式距离大于某一阈值T_RSS时，则认为测量受到磁干扰的影响。

步骤4根据判定结果得到逻辑因子g₁为：

$g_1=\left\{\begin{array}{c}0,RS{S}_k<T_{RSS}\\ 1,{\mathrm{RSS}}_k>T_{RSS}\end{array}\right.$

并由此得到k周期内磁干扰的表达式d_k为：

d_k＝g₁g₂d_k-1+g₁ε_k

其中，g₂为预设参数，表征干扰的持续性；ε_k为k周期内磁干扰项，其表达式为：

ε_k＝[b_x b_y b_z]^T

其中，b_x、b_y、b_z分别是x、y、z方向上磁干扰的补偿量。假设ε_k各分量服从均匀分布，即b_x、b_y、b_z～U(-a,a)，其中，a＝g₃·RSS_k，g₃为经验因子。

步骤5中磁强计的传输噪声v_k服从正态分布，通过静态条件下的输出统计，得到正态分布的方差为σ，即：v_k～N(0,σ)。

步骤6中系统观测为：

h_k＝B_k+d_k+v_k

其中，h_k是k周期的观测值；B_k为根据k周期的定位结果以及磁场模型，得到的磁场矢量的理论值；d_k是k周期内磁干扰的表达式；v_k是磁强计的传输噪声。

设定步骤7中胶囊内镜的速度与角速度的波动服从正态分布。

步骤8中系统的状态方程如下:

X_k＝CX_k-1+w_k-1

其中，

$C=\left[\begin{array}{ccccccccccccc}1& S& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& S& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& S& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& S& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1& S& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

w_k-1＝([0 w₁ 0 w₂ 0 w₃ 0 w₄ 0 w₅ 0 0 0]^T)_k-1

w_k-1即为k-1周期内速度与角速度的噪声；元素矩阵C中的S为传感器的采样周期。

本发明的特点及有益效果是：

本发明提供了一种基于磁传感的自适应胶囊内窥镜定位方法，利用粒子算法，有效的降低磁干扰对定位的影响。本方法利用磁强计阵列对安装于胶囊内窥镜内部的永磁体进行磁场测量，并带入事先建立好的永磁体磁场分布模型，以进行定位结果的解算。解算过程中，通过构建带有磁场补偿量的状态方程以及带有磁干扰和传输噪声的观测方程，对干扰对系统的影响进行定量描述。其中，在建立观测方程时，通过合适的规则，判定在当前定位周期内，是否存在可观测的磁干扰；并根据判定结果的不同，选择不同的磁干扰表达公式。最终，通过粒子滤波算法，对上述状态方程与观测方程进行实时更新，以抑制环境磁场引发的磁干扰与内镜运动引发的动态干扰，并最终完成实时的定位任务。

附图说明：

图1.本发明基于磁传感的自适应胶囊内窥镜定位方法的一种实施流程图；

图2.粒子滤波结构示意图；

图3.本发明提供的基于磁传感的自适应胶囊内窥镜定位方法的定位效果图；

具体实施方式

在现有技术的基础上，针对其存在的前述两个问题，本发明提出一种基于粒子滤波的抗磁干扰定位算法，并对此进行了仿真和实验的验证了它的有效性。

本发明的目的是提供一种用于胶囊内镜的磁传感定位方法提高系统抗干扰方法。本发明构建判定准则，判断磁干扰是否来临。并在此基础上，利用马尔科夫模型估计，构建环境磁干扰与运动干扰的数学表达式，从而设计系统的观测方程与状态方程。最终，通过粒子滤波抑制磁干扰与运动干扰，以达到定位目的。

一种基于磁传感的自适应胶囊内窥镜定位方法，该方法包括：

步骤1.建立圆柱形磁体的磁场分布模型；

步骤2.通过布置在定位空间中的磁强计对磁体的磁场进行测量；

步骤3.通过计算磁场测量值与磁场分布模型的差异，判定磁强计是否受到磁干扰；

步骤4.根据步骤3的判定结果，得到相应的磁干扰表达式；

步骤5.通过正态分布对磁强计自身的输出浮动，即传输噪声，进行拟合；

步骤6.根据步骤1所得的磁场分布模型，步骤2所得的磁场测量结果，步骤4所得的磁干扰表示，以及步骤5所得的磁强计传输噪声，构建系统观测方程；

步骤7.通过正态分布对内镜速度噪声与角速度的波动进行拟合；

步骤8.根据步骤7所得的速度噪声表示，构建系统观测方程；

较佳地，步骤3通过欧式距离，判断当前定位周期下是否收到磁场干扰。判据如下：

${\mathrm{RSS}}_k=\left|\right|B_k^{meas}-B_{k-1}\left|\right|$

其中，B_k-1为根据k-1周期的定位结果以及磁场模型，得到的磁场矢量的理论值，为k周期中磁强计的测量值。根据上述判据，当欧式距离大于某一阈值T_RSS时，则认为测量受到磁干扰的影响。

较佳地，步骤4根据判定结果得到逻辑因子g₁为：

$g_1=\left\{\begin{array}{c}0,{\mathrm{RSS}}_k<T_{RSS}\\ 1,{\mathrm{RSS}}_k>T_{RSS}\end{array}\right.$

并由此得到k周期内磁干扰的表达式d_k为：

d_k＝g₁g₂d_k-1+g₁ε_k

其中，g₂为预设参数，表征干扰的持续性；ε_k为k周期内磁干扰项，其表达式为：

ε_k＝[b_x b_y b_z]^T

其中，b_x、b_y、b_z分别是x、y、z方向上磁干扰的补偿量。假设ε_k各分量服从均匀分布，即b_x、b_y、b_z～U(-a,a)，其中，a＝g₃·RSS_k，g₃为经验因子。

较佳地，步骤5中磁强计的传输噪声v_k服从正态分布。通过静态条件下的输出统计，得到正态分布的方差为σ，即：v_k～N(0,σ)。

较佳地，步骤6中系统观测为：

h_k＝B_k+d_k+v_k

其中，h_k是k周期的观测值；B_k为根据k周期的定位结果以及磁场模型，得到的磁场矢量的理论值；d_k是k周期内磁干扰的表达式；v_k是磁强计的传输噪声。

较佳地，假设步骤7中胶囊内镜的速度与角速度的波动服从正态分布。

较佳地，步骤8中系统的状态方程如下:

X_k＝CX_k-1+w_k-1

其中，

$C=\left[\begin{array}{ccccccccccccc}1& S& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& S& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& S& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& S& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1& S& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

w_k-1＝([0 w₁ 0 w₂ 0 w₃ 0 w₄ 0 w₅ 0 0 0]^T)_k-1

w_k-1即为k-1周期内速度与角速度的噪声。矩阵C中的S为传感器的采样周期。

本发明的具体实施方式如图1所示：

101.建立圆柱形磁体的磁场分布模型。一个高度为2L、半径为R且磁化强度为M的圆柱永磁体被放置在参考坐标系O-XYZ中。磁体几何中心C的坐标为(x₀ y₀ z₀)；同时，磁体与Z轴夹角为θ，且其在OXY平面上的投影与X轴夹角为由此可知，该磁体的轴线所对应方向矢量为：

通常将上述永磁体近似成一个位于其几何中心处的点磁偶极子，该磁偶极子在观测点P(x_l y_l z_l)的磁感应强度为B_s＝(B_x B_y B_z)，其中：

$\begin{array}{c}{B}_x=\frac{{\mathrm{\&mu;}}_0{R}^{2}LM}{2r^5}\{e_x\&lsqb;2{(x_l-x_0)}^2-{(y_l-y_0)}^2-{(z_l-z_0)}^2\&rsqb;+3e_y(x_l-x_0)(y_l-y\\ +3e_y(x_l-x_0)(z_l-z_0)\}\end{array}$

$r=\sqrt{{(x_l-x_0)}^2+{(y_l-y_0)}^2+{(z_l-z_0)}^2}$

102.通过布置在定位空间中的磁强计对磁体的磁场进行测量；

通常，在定位空间内设置基本平面，在该基本平面上布置4×4微机电磁强计。每个磁强计都包含三个相互正交的感应轴，以测量观测点上每个感应轴的磁场分量。

103.通过计算磁场测量值与磁场分布模型的差异，判定磁强计是否受到磁干扰；

由于胶囊内镜在人体消化道内运动速度缓慢，在无干扰的条件下，只要定位频率足够高，本周期中磁强计测量结果与上一周期磁场矢量的理论值值应非常小。通过上述两量的欧式距离，可判断当前定位周期下是否收到磁场干扰。判据如下：

${\mathrm{RSS}}_k=\left|\right|B_k^{meas}-B_{k-1}\left|\right|$

其中，B_k-1为根据k-1周期的定位结果以及磁场模型，得到的磁场矢量的理论值，为k周期中磁强计的实际测量值。根据上述判据，当欧式距离大于某一阈值T_RSS时，则认为测量受到磁干扰的影响。

104.根据磁干扰判定结果，得到磁干扰表达式；

根据判定结果得到磁干扰判定的逻辑因子g₁为：

$g_1=\left\{\begin{array}{c}0,{\mathrm{RSS}}_k<T_{RSS}\\ 1,{\mathrm{RSS}}_k>T_{RSS}\end{array}\right.$

并假设磁干扰d_k服从一阶马尔科夫模型，由此得到k周期内磁干扰的表达式d_k为：

d_k＝g₁g₂d_k-1+g₁ε_k

其中，g₂为预设系数，g₂∈(0,1)，大小反映了磁干扰的持续性。逻辑因子g₁反映了若上一定位中，发现明显的磁干扰已经来临，那么此次定位将引入磁干扰的补偿量d_k。

另外，ε_k为k周期内磁干扰项，其表达式为：

ε_k＝[b_x b_y b_z]^T

其中，b_x、b_y、b_z分别是x、y、z方向上磁干扰的补偿量。假设ε_k各分量服从均匀分布，即b_x、b_y、b_z～U(-a,a)，其中，a＝g₃·RSS_k，g₃为经验因子。

105.通过正态分布对磁强计自身的传输噪声进行拟合；

在构建观测方程过程中，除去环境磁场以及内镜运动引发的磁干扰外，磁强计自身的传输噪声也会直接影响测量精度。传输噪声是指，在静态无干扰条件下，磁强计输出的大小浮动。通常，上述浮动符合正态分布，故通过正态分布模型对其进行拟合。通过拟合，传感器输出的标准差为1.3343。

106.构建磁传感系统的系统观测方程；

观测方程描述系统观测量的组成。去除地磁场引发的固定偏移量后，在磁定位系统中，观测主要由磁体激发的磁场，由环境磁场及磁体运动引发的磁干扰，以及传感器自身的传输噪声组成。故系统观测方程为：

h_k＝B_k+d_k+v_k

其中，h_k是k周期的观测值；B_k为根据k周期的定位结果以及磁场模型，得到的磁场矢量的理论值；d_k是k周期内磁干扰的表达式；v_k是磁强计的传输噪声。

107.通过正态分布拟合胶囊内镜的速度与角速度的波动；

通过均值为0的正态分布，对内镜的速度与角速度波动进行拟合。k周期速度与角速度的方差σ_k-1是由k周期的磁场测量值与k-1周期的磁场理论值的差异RSS_k所决定的，满足如下关系：

${\mathrm{\&sigma;}}_{k-1}=\mathrm{\&Phi;}\left({\mathrm{RSS}}_k\right)=\frac{1}{2}\&lsqb;1+erf\left(\frac{{\mathrm{RSS}}_k-80}{30\&times;\sqrt{2}}\right)\&rsqb;$

其中，erf函数为标准高斯误差函数。

108.构建系统的状态方程；

系统方程指的是目标状态的时间更新过程。由于待定位目标有一定的运动趋势，故系统更新模型应以二阶马尔科夫过程为核心，即k时刻的状态与k-1、k-2两时刻的状态均有关，若仅保留k-1时刻的状态只需加入k-2到k-1时刻的状态改变量，即速度量。此外，考虑到系统自身产生的磁噪声及外界产生的磁干扰，每个状态均需带有一定的磁场补偿量。建立如下的系统状态向量：

其中(x₀ y₀ z₀)为目标在参考坐标系下的位置信息，为目标的姿态信息，

(v_x v_y v_z)为目标的三轴分速度，为目标的分角速度，(b_x b_y b_z)为目标磁干扰补偿量。

系统模型建立为：

X_k＝CX_k-1+w_k-1

$C=\left[\begin{array}{ccccccccccccc}1& S& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& S& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& S& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& S& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1& S& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right];$

w_k-1＝([0 w₁ 0 w₂ 0 w₃ 0 w₄ 0 w₅ 0 0 0]^T)_k-1；

S为获取测量数据的时间间隔；X_k X_k-1分别表示k、k-1时刻的目标状态。

w_k-1为k-1时刻的观测噪声量，其中元素w_k-1依赖于对所有随机变量的高斯假设，即将系统状态的后验概率密度近似为一个最优的高斯分布。

状态转移中，w_k-1只引入速度量的噪声量，不对当前位置、姿态、磁补偿量进行干预。由此，系统的观测方程与状态方程均构建完毕。在此基础上，通过图2所示的粒子滤波，即可完成定位信息的获取。

Claims (8)

1.一种基于磁传感的自适应胶囊内窥镜定位方法，其特征是，构建判定准则，判断磁干扰是否来临；并在此基础上，利用马尔科夫模型估计，构建环境磁干扰与运动干扰的数学表达式，从而设计系统的观测方程与状态方程；最终，通过粒子滤波抑制磁干扰与运动干扰，以达到定位目的。

2.如权利要求1所述的基于磁传感的自适应胶囊内窥镜定位方法，其特征是，具体步骤进一步细化为：

步骤1.建立圆柱形磁体的磁场分布模型；

步骤2.通过布置在定位空间中的磁强计对磁体的磁场进行测量；

步骤3.通过计算磁场测量值与磁场分布模型的差异，判定磁强计是否受到磁干扰；

步骤4.根据步骤3的判定结果，得到相应的磁干扰表达式；

步骤5.通过正态分布对磁强计自身的输出浮动，即传输噪声，进行拟合；

步骤6.根据步骤1所得的磁场分布模型，步骤2所得的磁场测量结果，步骤4所得的磁干扰表示，以及步骤5所得的磁强计传输噪声，构建系统观测方程；

步骤7.通过正态分布对内镜速度噪声与角速度的波动进行拟合；

步骤8.根据步骤7所得的速度噪声表示，构建系统观测方程。

3.如权利要求2所述的基于磁传感的自适应胶囊内窥镜定位方法，其特征是，步骤3通过欧式距离，判断当前定位周期下是否收到磁场干扰，判据如下：

${\mathrm{RSS}}_k=\left|\right|B_k^{meas}-B_{k-1}\left|\right|$

其中，B_k-1为根据k-1周期的定位结果以及磁场模型，得到的磁场矢量的理论值，为k周期中磁强计的测量值，根据上述判据，当欧式距离大于某一阈值T_RSS时，则认为测量受到磁干扰的影响。

4.如权利要求2所述的基于磁传感的自适应胶囊内窥镜定位方法，其特征是，步骤4根据判定结果得到逻辑因子g₁为：

$g_1=\left\{\begin{array}{c}0,{\mathrm{RSS}}_k<T_{RSS}\\ 1,{\mathrm{RSS}}_k>T_{RSS}\end{array}\right.$

并由此得到k周期内磁干扰的表达式d_k为：

d_k＝g₁g₂d_k-1+g₁ε_k

其中，g₂为预设参数，表征干扰的持续性；ε_k为k周期内磁干扰项，其表达式为：

${\mathrm{\&epsiv;}}_k={\left[\begin{array}{ccc}{b}_x& b_y& b_z\end{array}\right]}^T$

其中，b_x、b_y、b_z分别是x、y、z方向上磁干扰的补偿量。假设ε_k各分量服从均匀分布，即b_x、b_y、b_z～U(-a,a)，其中，a＝g₃·RSS_k，g₃为经验因子。

5.如权利要求2所述的基于磁传感的自适应胶囊内窥镜定位方法，其特征是，步骤5中磁强计的传输噪声v_k服从正态分布，通过静态条件下的输出统计，得到正态分布的方差为σ，即：v_k～N(0,σ)。

6.如权利要求2所述的基于磁传感的自适应胶囊内窥镜定位方法，其特征是，步骤6中系统观测为：

h_k＝B_k+d_k+v_k

其中，h_k是k周期的观测值；B_k为根据k周期的定位结果以及磁场模型，得到的磁场矢量的理论值；d_k是k周期内磁干扰的表达式；v_k是磁强计的传输噪声。

7.如权利要求2所述的基于磁传感的自适应胶囊内窥镜定位方法，其特征是，设定步骤7中胶囊内镜的速度与角速度的波动服从正态分布。

8.如权利要求2所述的基于磁传感的自适应胶囊内窥镜定位方法，其特征是，步骤8中系统的状态方程如下：

X_k＝CX_k-1+w_k-1

其中，

其中，

$C=\left[\begin{array}{ccccccccccccc}1& S& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& S& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& S& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& S& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1& S& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

w_k-1＝([0 w₁ 0 w₂ 0 w₃ 0 w₄ 0 w₅ 0 0 0]^T)_k-1

w_k-1即为k-1周期内速度与角速度的噪声；元素矩阵C中的S为传感器的采样周期。