CN106989745A - 顶管姿态测量系统中的倾角仪与光纤陀螺仪的融合方法 - Google Patents

Abstract

本发明公开了顶管姿态测量系统中的倾角仪与光纤陀螺仪的融合方法。顶管姿态测量系统实现顶管机姿态信号的获取，所述融合方法包括以下步骤：1)对倾角仪输出的倾角数据进行一阶差分处理；2)收集光纤陀螺仪的动态数据和一阶差分处理后的倾角数据；3)设计卡尔曼滤波器；4)通过所述卡尔曼滤波器对光纤陀螺仪的动态数据和一阶差分处理后的倾角数据进行数据补偿和信息融合，获取顶管机姿态信号的姿态信号。本发明实质是利用稳态下倾角仪输出与光纤陀螺仪进行数据融合获取光纤陀螺仪实时的零漂，以便对突变环境下倾角仪失效后陀螺仪的零漂做出相对准确的估计。

Description

顶管姿态测量系统中的倾角仪与光纤陀螺仪的融合方法

技术领域

本发明涉及一种融合方法，尤其涉及一种顶管姿态测量系统中的倾角仪与光纤陀螺仪的融合方法。

背景技术

非开挖技术是利用岩石钻掘的技术手段，在地表以最少的开挖量或不需要开挖的条件下铺设、更换或修复各种地下管线的一种施工新技术。顶管施工是一种用于非开挖技术中铺设新管线的施工技术。随着我国城市化建设的不断推进与发展，顶管法施工以其不需要开挖地面、占地面积少、交通影响小、开挖速度快、经济、环保的优势在综合管廊、地下通道等项目建设中不断发挥重要作用。

在顶管机掘进过程中，由于地层土质变化、千斤顶推力不均、回填注浆不均、尾盾间隙不均以及已拼管节轴线不准等因素影响，不可能完全按设计方向推进，为了保证掘进轨道与设计轨道高度重合，一般会设计顶管姿态测量系统。顶管姿态测量系统对顶管机机头的位置和姿态进行实时测量，并根据测量结果对掘进方向进行调整。已有顶管姿态测量系统通过激光靶显示姿态偏差，并通过摄像头接入控制台，智能化、自动化程度低。

稳定环境下，顶管姿态测量系统的倾角仪输出具有较高的精度，在强振动环境下，倾角仪输出存在较大误差，而顶管姿态测量系统的陀螺仪在振动环境下有较高的稳定性，但因其测量机制，长时间工作会受到外界因素(温度、噪声等)干扰，导致陀螺仪轴测量的漂移，影响测量精度。

发明内容

为了降低外界干扰对光纤陀螺仪精度的影响，本发明提供一种顶管姿态测量系统中的倾角仪与光纤陀螺仪的融合方法。

本发明的解决方案是：一种顶管姿态测量系统中的倾角仪与光纤陀螺仪的融合方法，所述顶管姿态测量系统实现顶管机姿态信号的获取；所述融合方法包括以下步骤：

1)对倾角仪输出的倾角数据进行一阶差分处理；

2)收集光纤陀螺仪的动态数据和一阶差分处理后的倾角数据；

3)设计卡尔曼滤波器；

4)通过所述卡尔曼滤波器对光纤陀螺仪的动态数据和一阶差分处理后的倾角数据进行数据补偿和信息融合，获取顶管机姿态信号的姿态信号；

其中，在步骤3)和步骤4)中，根据光纤陀螺仪的零漂，建立相应的随机误差模型：其中x为光纤陀螺仪的实际输出，ω为光纤陀螺仪的实际角速率，由真实速率驱动白噪声n_ω驱动，n_ω的方差为Q_ω；b为光纤陀螺仪的速率随机游走噪声，白噪声n_b的方差为Q_b；建立光纤陀螺的仪角度随机游走噪声n_a，方差为Q_a；

建立光纤陀螺仪的状态模型：Y_k＝AY_k-1+AW_k，其中状态矢量： n_ω的方差为Q_ω，驱动序列：

建立光纤陀螺仪的观测矩阵：X_k＝CY_k+V_k，其中观测矢量：ω_k表示陀螺仪的输出值，θ表示在一个采样周期内倾角仪的角位移，n_ak为陀螺仪测量噪声，方差为Q_n；n_k＝δ_k-δ_k-1，其中δ_k为倾角仪输出的且经一阶差分处理后的倾角数据，方差为Q_δ，驱动序列：

其中，所述卡尔曼滤波器的驱动序列W_k、V_k满足以下条件：

其中：δ_kj是单位为1的量纲函数，

协方差矩阵：

其中，Q_k、R_k代表卡尔曼滤波器对相应传感器数据的信任程度，值越小，明信任程度越高，在顶管姿态测量系统中光纤陀螺仪的值更为接近准确值；

所述卡尔曼滤波器的滤波过程：

当前状态，Y_k|k-1＝AY_k-1|k-1+AW_k，其中，Y_k|k-1是k时刻的预测结果，Y_k-1|k-1是k-1时刻的最优结果；对应的Y_k|k-1的协方差为：P_k|k-1＝AP_k-1|k-1A^T+Q_k，则k时刻状态最优值：Y_K|k＝Y_k|k-1+K_k(X_k-HY_k|k-1)，其中H＝[1 0]，K为卡尔曼增益系数：为了使卡尔曼滤波器不断运行下去直至获得最优值，细化k状态下Y_K|k的协方差：P_k|k＝P_k|k-1-K_kHP_k|k-1；计算完时间更新方程和测量更新方程后，再次重复上一次计算得到的后验估计，作为下一次计算的先验估计，这样，周而复始、循环反复地运算下去直至找到最优的结果作为顶管机姿态信号的姿态信号。

本发明还提供一种顶管姿态测量系统，其实现顶管机姿态信号的获取，采用上述顶管姿态测量系统中的倾角仪与光纤陀螺仪的融合方法对所述顶管姿态测量系统中的倾角仪与光纤陀螺仪实现数据融合。

本发明还提供一种存储设备，其中存储多条指令，所述指令适于处理器加载并执行，所述多条指令为：

1)对倾角仪输出的倾角数据进行一阶差分处理；

2)收集光纤陀螺仪的动态数据和一阶差分处理后的倾角数据；

3)设计卡尔曼滤波器；

4)通过所述卡尔曼滤波器对光纤陀螺仪的动态数据和一阶差分处理后的倾角数据进行数据补偿和信息融合，获取顶管机姿态信号的姿态信号；

其中，在步骤3)和步骤4)中，根据光纤陀螺仪的零漂，建立相应的随机误差模型：其中x为光纤陀螺仪的实际输出，ω为光纤陀螺仪的实际角速率，由真实速率驱动白噪声n_ω驱动，n_ω的方差为Q_ω；b为光纤陀螺仪的速率随机游走噪声，白噪声n_b的方差为Q_b；建立光纤陀螺的仪角度随机游走噪声n_a，方差为Q_a；

建立光纤陀螺仪的状态模型：Y_k＝AY_k-1+AW_k，其中状态矢量： n_ω的方差为Q_ω，驱动序列：

建立光纤陀螺仪的观测矩阵：X_k＝CY_k+V_k，其中观测矢量：ω_k表示陀螺仪的输出值，θ表示在一个采样周期内倾角仪的角位移，n_ak为陀螺仪测量噪声，方差为Q_n；n_k＝δ_k-δ_k-1，其中δ_k为倾角仪输出的且经一阶差分处理后的倾角数据，方差为Q_δ，驱动序列：

其中，所述卡尔曼滤波器的驱动序列W_k、V_k满足以下条件：

其中：δ_kj是单位为1的量纲函数，

协方差矩阵：

其中，Q_k、R_k代表卡尔曼滤波器对相应传感器数据的信任程度，值越小，明信任程度越高，在顶管姿态测量系统中光纤陀螺仪的值更为接近准确值；

所述卡尔曼滤波器的滤波过程：

当前状态，Y_k|k-1＝AY_k-1|k-1+AW_k，其中，Y_k|k-1是k时刻的预测结果，Y_k-1|k-1是k-1时刻的最优结果；对应的Y_K|k-1的协方差为：P_k|k-1＝AP_k-1|k-1A^T+Q_k，则k时刻状态最优值：Y_K|k＝Y_k|k-1+K_k(X_k-HY_k|k-1)，其中H＝[1 0]，K为卡尔曼增益系数：为了使卡尔曼滤波器不断运行下去直至获得最优值，细化k状态下Y_K|k的协方差：P_k|k＝P_k|k-1-K_kHP_k|k-1；计算完时间更新方程和测量更新方程后，再次重复上一次计算得到的后验估计，作为下一次计算的先验估计，这样，周而复始、循环反复地运算下去直至找到最优的结果作为顶管机姿态信号的姿态信号。

本发明还提供一种电子终端，其包括：

处理器，适于实现各种指令；以及

存储设备，适于存储多条指令，所述指令适于处理器加载并执行；

所述多条指令为：

1)对倾角仪输出的倾角数据进行一阶差分处理；

2)收集光纤陀螺仪的动态数据和一阶差分处理后的倾角数据；

3)设计卡尔曼滤波器；

4)通过所述卡尔曼滤波器对光纤陀螺仪的动态数据和一阶差分处理后的倾角数据进行数据补偿和信息融合，获取顶管机姿态信号的姿态信号；

其中，在步骤3)和步骤4)中，根据光纤陀螺仪的零漂，建立相应的随机误差模型：其中x为光纤陀螺仪的实际输出，ω为光纤陀螺仪的实际角速率，由真实速率驱动白噪声n_ω驱动，n_ω的方差为Q_ω；b为光纤陀螺仪的速率随机游走噪声，白噪声n_b的方差为Q_b；建立光纤陀螺的仪角度随机游走噪声n_a，方差为Q_a；

建立光纤陀螺仪的状态模型：Y_k＝AY_k-1+AW_k，其中状态矢量： n_ω的方差为Q_ω，驱动序列：

建立光纤陀螺仪的观测矩阵:X_k＝CY_k+V_k，其中观测矢量：ω_k表示陀螺仪的输出值，θ表示在一个采样周期内倾角仪的角位移，n_ak为陀螺仪测量噪声，方差为Q_n；n_k＝δ_k-δ_k-1，其中δ_k为倾角仪输出的且经一阶差分处理后的倾角数据，方差为Q_δ，驱动序列：

其中，所述卡尔曼滤波器的驱动序列W_k、V_k满足以下条件：

其中：δ_kj是单位为1的量纲函数，

协方差矩阵：

其中，Q_k、R_k代表卡尔曼滤波器对相应传感器数据的信任程度，值越小，明信任程度越高，在顶管姿态测量系统中光纤陀螺仪的值更为接近准确值；

所述卡尔曼滤波器的滤波过程：

当前状态，Y_k|k-1＝AY_k-1|k-1+AW_k，其中，Y_k|k-1是k时刻的预测结果，Y_k-1|k-1是k-1时刻的最优结果；对应的Y_k|k-1的协方差为：P_k|k-1＝AP_k-1|k-1A^T+Q_k，则k时刻状态最优值：Y_K|k＝Y_k|k-1+K_k(X_k-HY_k|k-1)，其中H＝[1 0]，K为卡尔曼增益系数：为了使卡尔曼滤波器不断运行下去直至获得最优值，细化k状态下Y_K|k的协方差：P_k|k＝P_k|k-1-K_kHP_k|k-1；计算完时间更新方程和测量更新方程后，再次重复上一次计算得到的后验估计，作为下一次计算的先验估计，这样，周而复始、循环反复地运算下去直至找到最优的结果作为顶管机姿态信号的姿态信号。

本发明实质是利用稳态下倾角仪输出与光纤陀螺仪进行数据融合获取光纤陀螺仪实时的零漂，以便对突变环境下倾角仪失效后陀螺仪的零漂做出相对准确的估计。

附图说明

图1本发明顶管姿态测量系统中的倾角仪与光纤陀螺仪的融合方法的流程图。

图2是卡尔曼滤波器的滤波流程图，所述卡尔曼滤波器依据顶管姿态测量系统中的倾角仪与光纤陀螺仪输出的数据信号而针对性设计的。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

针对光纤陀螺仪和倾角仪建立的姿态测量系统，本发明的顶管姿态测量系统中的倾角仪与光纤陀螺仪的融合方法，建立特征模型，采用卡尔曼滤波法，对光纤陀螺仪和倾角仪的输出信号进行融合，降低外界干扰对光纤陀螺仪精度的影响。

本发明实质是利用稳态下倾角仪输出与光纤陀螺仪进行数据融合获取光纤陀螺仪实时的零漂，以便对突变环境下倾角仪失效后陀螺仪的零漂做出相对准确的估计。

光纤陀螺仪和倾角仪融合流程图如图1所示，其过程为：

1.对光纤陀螺仪和倾角仪的输出进行采样；

2.对倾角仪输出数据进行差分处理；

3.由微处理器对步骤1、2中的数据进行采集；

4.通过卡尔曼滤波器对步骤3中的数据进行补偿和信息融合，获取比较准确的姿态信号。

在具体实施时，所述融合方法包括以下步骤：

1)对倾角仪输出的倾角数据进行一阶差分处理；

2)收集光纤陀螺仪的动态数据和一阶差分处理后的倾角数据；

3)设计卡尔曼滤波器；

4)通过所述卡尔曼滤波器对光纤陀螺仪的动态数据和一阶差分处理后的倾角数据进行数据补偿和信息融合，获取顶管机姿态信号的姿态信号。

本发明的重点在步骤3)和步骤4)，下面进行详细介绍。

一、根据光纤陀螺仪的零漂，建立相应的随机误差模型：其中x为光纤陀螺仪的实际输出，ω为光纤陀螺仪的实际角速率，由真实速率驱动白噪声n_ω驱动，n_ω的方差为Q_ω；b为光纤陀螺仪的速率随机游走噪声，白噪声n_b的方差为Q_b；建立光纤陀螺的仪角度随机游走噪声n_a，方差为Q_a。

二、建立光纤陀螺仪的状态模型:Y_k＝AY_k-1+AW_k，其中状态矢量: n_ω的方差为Q_ω，驱动序列：

三、建立光纤陀螺仪的观测矩阵：X_k＝CY_k+V_k，其中观测矢量：ω_k表示陀螺仪的输出值，θ表示在一个采样周期内倾角仪的角位移，n_ak为陀螺仪测量噪声，方差为Q_n；δ_k＝δ_k-δ_k-1，其中δ_k为倾角仪输出的且经一阶差分处理后的倾角数据，方差为Q_δ，驱动序列：

四、所述卡尔曼滤波器的驱动序列W_k、V_k满足以下条件：

其中：δ_kj是单位为1的量纲函数。

协方差矩阵：其中，Q_k、R_k代表卡尔曼滤波器对相应传感器数据的信任程度，值越小，明信任程度越高，在顶管姿态测量系统中光纤陀螺仪的值更为接近准确值。

五、所述卡尔曼滤波器的滤波过程。请参阅图2，当前状态，Y_k|k-1＝AY_k-1|k-1+AW_k，其中，Y_k|k-1是k时刻的预测结果，Y_k-1|k-1是k-1时刻的最优结果；对应的Y_k|k-1的协方差为：P_k|k-1＝AP_k-1|k-1A^T+Q_k，则k时刻状态最优值：Y_K|k＝Y_k|k-1+K_k(X_k-HY_k|k-1)，其中H＝[1 0]，K为卡尔曼增益系数：

为了使卡尔曼滤波器不断运行下去直至获得最优值，细化k状态下Y_K|k的协方差：P_k|k＝P_k|k-1-K_kHP_k|k-1；计算完时间更新方程和测量更新方程后，再次重复上一次计算得到的后验估计，作为下一次计算的先验估计，这样，周而复始、循环反复地运算下去直至找到最优的结果作为顶管机姿态信号的姿态信号。

本发明的方法，适于在计算机设备中执行，在实际应用中可以做成软件安装包的形式，也可以做成APP的形式，如手机APP等。本发明的方法在具体实现中，可以采用各种计算机语言设计成相应的多条指令，所述指令适于处理器加载并执行，可以采用存储设备存储这些指令。因此也可以自由地应用在电子终端上，只要电子终端存在适于实现各种指令的处理器，处理器加载并执行这些指令。电子终端也可以设置适于存储这些指令的存储设备。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种顶管姿态测量系统中的倾角仪与光纤陀螺仪的融合方法，所述顶管姿态测量系统实现顶管机姿态信号的获取；其特征在于：所述融合方法包括以下步骤：

1)对倾角仪输出的倾角数据进行一阶差分处理；

2)收集光纤陀螺仪的动态数据和一阶差分处理后的倾角数据；

3)设计卡尔曼滤波器；

4)通过所述卡尔曼滤波器对光纤陀螺仪的动态数据和一阶差分处理后的倾角数据进行数据补偿和信息融合，获取顶管机姿态信号的姿态信号；

其中，在步骤3)和步骤4)中，根据光纤陀螺仪的零漂，建立相应的随机误差模型：其中x为光纤陀螺仪的实际输出，ω为光纤陀螺仪的实际角速率，由真实速率驱动白噪声n_ω驱动，n_ω的方差为Q_ω；b为光纤陀螺仪的速率随机游走噪声，白噪声n_b的方差为Q_b；建立光纤陀螺的仪角度随机游走噪声n_a，方差为Q_a；

建立光纤陀螺仪的状态模型：Y_k＝AY_k-1+AW_k，其中状态矢量： n_ω的方差为Q_ω，驱动序列：

建立光纤陀螺仪的观测矩阵：X_k＝CY_k+V_k，其中观测矢量：ω_k表示陀螺仪的输出值，θ表示在一个采样周期内倾角仪的角位移，n_ak为陀螺仪测量噪声，方差为Q_n；n_k＝δ_k-δ_k-1，其中δ_k为倾角仪输出的且经一阶差分处理后的倾角数据，方差为Q_δ，驱动序列：

其中，所述卡尔曼滤波器的驱动序列W_k、V_k满足以下条件：

其中：δ_kj是单位为1的量纲函数，协方差矩阵：

其中，Q_k、P_k代表卡尔曼滤波器对相应传感器数据的信任程度，值越小，明信任程度越高，在顶管姿态测量系统中光纤陀螺仪的值更为接近准确值；

所述卡尔曼滤波器的滤波过程：

当前状态，Y_k|k-1＝AY_k-1|k-1+AW_k，其中，Y_k|k-1是k时刻的预测结果，Y_k-1|k-1是k-1时刻的最优结果；对应的Y_k|k-1的协方差为：P_k|k-1＝AP_k-1|k-1A^T+Q_k，则k时刻状态最优值：Y_K|k＝Y_k|k-1+K_k(X_k-HY_k|k-1)，其中H＝[1 0]，K为卡尔曼增益系数：为了使卡尔曼滤波器不断运行下去直至获得最优值，细化k状态下Y_K|k的协方差：P_k|k＝P_k|k-1-K_kHP_k|k-1；计算完时间更新方程和测量更新方程后，再次重复上一次计算得到的后验估计，作为下一次计算的先验估计，这样，周而复始、循环反复地运算下去直至找到最优的结果作为顶管机姿态信号的姿态信号。

2.一种顶管姿态测量系统，其实现顶管机姿态信号的获取，其特征在于：采用如权利要求1所述的顶管姿态测量系统中的倾角仪与光纤陀螺仪的融合方法对所述顶管姿态测量系统中的倾角仪与光纤陀螺仪实现数据融合。

3.一种存储设备，其中存储多条指令，所述指令适于处理器加载并执行，其特征在于：所述多条指令为：

1)对倾角仪输出的倾角数据进行一阶差分处理；

2)收集光纤陀螺仪的动态数据和一阶差分处理后的倾角数据；

3)设计卡尔曼滤波器；

4)通过所述卡尔曼滤波器对光纤陀螺仪的动态数据和一阶差分处理后的倾角数据进行数据补偿和信息融合，获取顶管机姿态信号的姿态信号；

其中，在步骤3)和步骤4)中，根据光纤陀螺仪的零漂，建立相应的随机误差模型：其中x为光纤陀螺仪的实际输出，ω为光纤陀螺仪的实际角速率，由真实速率驱动白噪声n_ω驱动，n_ω的方差为Q_ω；b为光纤陀螺仪的速率随机游走噪声，白噪声n_b的方差为Q_b；建立光纤陀螺的仪角度随机游走噪声n_a，方差为Q_a；

建立光纤陀螺仪的状态模型：Y_k＝AY_k-1+AW_k，其中状态矢量： n_ω的方差为Q_ω，驱动序列：

建立光纤陀螺仪的观测矩阵：X_k＝CY_k+V_k，其中观测失量：ω_k表示陀螺仪的输出值，θ表示在一个采样周期内倾角仪的角位移，n_ak为陀螺仪测量噪声，方差为Q_n；n_k＝δ_k-δ_k-1，其中δ_k为倾角仪输出的且经一阶差分处理后的倾角数据，方差为Q_δ，驱动序列：

其中，所述卡尔曼滤波器的驱动序列W_k、V_k满足以下条件：

其中：δ_kj是单位为1的量纲函数，

协方差矩阵：

其中，Q_k、R_k代表卡尔曼滤波器对相应传感器数据的信任程度，值越小，明信任程度越高，在顶管姿态测量系统中光纤陀螺仪的值更为接近准确值；

所述卡尔曼滤波器的滤波过程：

当前状态，Y_k|k-1＝AY_k-1|k-1+AW_k，其中，Y_k|k-1是k时刻的预测结果，Y_k-1|k-1是k-1时刻的最优结果；对应的Y_k|k-1的协方差为：P_k|k-1＝AP_k-1|k-1A^T+Q_k，则k时刻状态最优值：Y_K|k＝Y_k|k-1+K_k(X_k-HY_k|k-1)，其中H＝[1 0]，K为卡尔曼增益系数：为了使卡尔曼滤波器不断运行下去直至获得最优值，细化k状态下Y_K|k的协方差：P_k|k＝P_k|k-1-K_kHP_k|k-1；计算完时间更新方程和测量更新方程后，再次重复上一次计算得到的后验估计，作为下一次计算的先验估计，这样，周而复始、循环反复地运算下去直至找到最优的结果作为顶管机姿态信号的姿态信号。

4.一种电子终端，其包括：

处理器，适于实现各种指令；以及

存储设备，适于存储多条指令，所述指令适于处理器加载并执行；

其特征在于：所述多条指令为：

1)对倾角仪输出的倾角数据进行一阶差分处理；

2)收集光纤陀螺仪的动态数据和一阶差分处理后的倾角数据；

3)设计卡尔曼滤波器；

4)通过所述卡尔曼滤波器对光纤陀螺仪的动态数据和一阶差分处理后的倾角数据进行数据补偿和信息融合，获取顶管机姿态信号的姿态信号；

其中，在步骤3)和步骤4)中，根据光纤陀螺仪的零漂，建立相应的随机误差模型：其中x为光纤陀螺仪的实际输出，ω为光纤陀螺仪的实际角速率，由真实速率驱动白噪声n_ω驱动，n_ω的方差为Q_ω；b为光纤陀螺仪的速率随机游走噪声，白噪声n_b的方差为Q_b；建立光纤陀螺的仪角度随机游走噪声n_a，方差为Q_a；

建立光纤陀螺仪的状态模型：Y_k＝AY_k-1+AW_k，其中状态矢量： n_ω的方差为Q_ω，驱动序列∶

建立光纤陀螺仪的观测矩阵：X_k＝CY_k+V_k，其中观测矢量∶ω_k表示陀螺仪的输出值，θ表示在一个采样周期内倾角仪的角位移，n_ak为陀螺仪测量噪声，方差为Q_n；n_k＝δ_k-δ_k-1，其中δ_k为倾角仪输出的且经一阶差分处理后的倾角数据，方差为Q_δ，驱动序列：

其中，所述卡尔曼滤波器的驱动序列W_k、V_k满足以下条件：

其中：δ_kj是单位为1的量纲函数，

协方差矩阵：

其中，Q_k、R_k代表卡尔曼滤波器对相应传感器数据的信任程度，值越小，明信任程度越高，在顶管姿态测量系统中光纤陀螺仪的值更为接近准确值；

所述卡尔曼滤波器的滤波过程：

当前状态，Y_k|k-1＝AY_k-1|k-1+AW_k，其中，Y_k|k-1是k时刻的预测结果，Y_k-1|k-1是k-1时刻的最优结果；对应的Y_k|k-1的协方差为：P_k|k-1＝AP_k-1|k-1A^T+Q_k，则k时刻状态最优值：Y_k|k＝Y_k|k-1+K_k(X_k-HY_k|k-1)，其中H＝[1 0]，K为卡尔曼增益系数：为了使卡尔曼滤波器不断运行下去直至获得最优值，细化k状态下Y_K|k的协方差：P_k|k＝P_k|k-1-K_kHP_k|k-1；计算完时间更新方程和测量更新方程后，再次重复上一次计算得到的后验估计，作为下一次计算的先验估计，这样，周而复始、循环反复地运算下去直至找到最优的结果作为顶管机姿态信号的姿态信号。