CN113038364B

CN113038364B - 一种基于uwb技术的tdoa与ds_twr结合的井下二维定位方法

Info

Publication number: CN113038364B
Application number: CN202110212885.1A
Authority: CN
Inventors: 杨亦非; 杨立新
Original assignee: Individual
Current assignee: Wang Xuehui
Priority date: 2021-02-25
Filing date: 2021-02-25
Publication date: 2024-05-03
Anticipated expiration: 2041-02-25
Also published as: CN113038364A

Abstract

本发明公开了一种基于UWB技术的TDOA与DS_TWR结合的井下二维定位方法，该方法利用了识别卡到达多个UWB定位器之间的到达时间差值TDOA，和识别卡到达一个UWB定位器的TOA值，结合TDOA和TOA的测量计算值，采用Chan算法得到移动点二维坐标。其中，本发明采用了基于Kalman滤波的无线时钟同步方法实现TDOA的计算，实现了亚纳秒级的时钟精度，通过增加对一个基站的TOA测距值，有效提高了煤矿井下二维长距离定位的精度，解决了井下单一TDOA定位算法精度不稳定问题；同时，本发明方法还可以大大降低系统的耦合度，提高系统稳定性。

Description

一种基于UWB技术的TDOA与DS_TWR结合的井下二维定位方法

技术领域

本发明属于煤矿井下UWB无线定位技术领域，涉及一种基于UWB技术的TDOA 与DS_TWR结合的井下二维定位方法。

背景技术

现有技术问题如下：

(1)一种基于ADS_TWR的井下UWB多基站定位区域判定方法(专利CN 107948920A)，等间距布置基站，任意两个基站之间的距离小于定位覆盖半径，该专利实现二维定位问题在于：若获得识别卡的位置，需要识别卡与多个基站分别完成多次DS_TWR过程，占用空间时隙资源过多，系统容量和定位实时性差，可应用一维定位。

(2)一种基于UWB技术DS_TWR的井下精确定位方位判定方法及系统(专利CN111970760A)，采用了单基站(双天线)测距结构，该专利实现二维定位问题在于：使用该方法只能实现一维定位，无法实现井下二维或多为定位。

(3)一种基于UWB的联合无线时钟同步和TOA的测距系统(专利 CN106842175A)，采用了多基站测距结构，在实现TOA测距过程中，在通信的信息中插入无线时钟同步信息，其时钟同步的精度ms级别，其牺牲了时钟同步的精度，提升了系统的网络组建的易操作性。该专利实现二维定位问题在于：无线时钟同步的精度太差不能用于TDOA计算，其多维定位本质上还是使用DS_TWR 的改进形式，系统容量和定位实时性都较差。

发明内容

本发明解决的问题在于克服现有的煤矿井下精确人员定位技术中，二位或三维定位存在的问题，并提供一种适合井下环境的方法，可以提高系统定位精度，降低系统的耦合度，提高系统稳定性，提升系统容量和定位实时性。

本发明采用以下技术方案解决上述问题：

一种基于UWB技术的TDOA与DS_TWR结合的井下二维定位方法，其特征在于在井下巷道中间隔安装UWB定位基站单元，被定位目标佩戴识别卡，UWB定位基站单元由3个(或更多)UWB定位器组成，UWB定位器集中安装在集控器周围， UWB定位器之间通过无线时钟同步算法，实现UWB定位器之间的亚纳秒级别的时钟同步，各UWB定位器通过接收识别卡的Poll消息，系统可计算出定位器间的 TDOA差值；同时，使用DS_TWR方法完成UWB识别卡与UWB主定位器之间TOA测距，据此可以得到识别卡到达三个UWB定位器的TOA值，最终通过Chan算法得到识别卡的位置值。

进一步地，本发明实现流程如下：

(1)实现本发明方法的相关设备包括UWB定位基站单元(UWB定位器M、UWB 定位器S1、UWB定位器S2、集控器)、识别卡、UWB定位服务器等，如图1所示。

(2)实施本发明方法部署方案，如图2所示，沿着井下巷道两侧依次布设 UWB定位器M、集控器、UWB定位器S1、UWB定位器S2，其中三个UWB定位器不能同时共线，间距大于1米，小于20米。

(3)使用无线时钟同步法计算POLL消息到达UWB定位器之间的到达时间差值(TDOA)的主要步骤，如图4所示。

步骤3.1：UWB定位器M(主定位器)间隔150ms发送无线时钟校准包TX_CPP (以下简称CCP包)，记录发送时刻本地时钟数据，并通过CAN总线把带有发送时间戳的CCP数据包传输到集控器。

步骤3.2：UWB定位器S1(从定位器1)和UWB定位器S2(从定位器2)接收RX_CCP包，并通过CAN总线把带有接收时间戳的RX_CCP1、RX_CCP2数据包传输到集控器。

步骤3.3：用基于Kalman滤波的时钟跟踪方法计算时钟偏移。

T为时钟周期，U_θ,K和U_γ,K分别是第K周期的时钟偏移和时钟偏移的修正值，θ_K和γ_K分别是k_T时钟偏移和时钟漂移。在(K+1)T时刻，时钟迭代关系：

(θ_k+1)＝θ_k-U_θ,k+(γ_k-U_γ,k)T+ω_θ,k

(γ_k+1)＝γ_k-U_γ,k+ω_γ

ω_θ,K是时钟偏差方差，ω_γ,K是时钟漂移方差。

假设ω_k＝[ω_θ,kω_γ,k]^T，协方差矩阵为Q，定义向量矩阵：

x_k＝[θ_kγ_k]^T,u_k＝[U_θ,kU_γ,k]^T.

使用Kalman滤波迭代：

建立预测方程：(X_k+1)＝Ax_k+Bu_k

其中：

Kalman增益矩阵：

K_k+1＝P_k+1|k[H_k+1]^T(R_k+1+H_k+1P_k+1|k[H_k+1]^T)^-1

R_K+1是观测噪声的协方差矩阵，H_K+1是测量矩阵，单位方阵；

修正：最小均方差误差矩阵：

P_k+1＝(1-K_k+1)P_k+1|k

经过Kalman滤波，第K+1时刻时钟周期的修正值另是补偿时钟偏移和时钟漂移，则有UWB定位器S1和UWB定位器S2 的时钟偏移分别为，S1U_K+1和S2U_K+1。

步骤3.4：接收识别卡发送的POLL消息

UWB定位器M、UWB定位器S1和UWB定位器S2分别接收识别卡发送的POLL 消息，并获取POLL消息的接收时间戳，分别记作t_{RX_POLL_M}、t_{RX_POLL_S1}、t_{RX_POLL_S2}；通过CAN总线发送这些时间戳到集控器。

步骤3.5：t时刻POLL消息时基校准

由于UWB定位器M、UWB定位器S1和UWB定位器S2设备之间的时钟系统是独立的，即非同的步时钟系统，它们所获得的t_{RX_POLL_M}、t_{RX_POLL_S1}、t_{RX_POLL_S2}时间戳为非同步时间，根据步骤3.3所计算得到的时钟偏移S1U_K+1和S2U_K+1，把上述时间校准到以UWB定位器M为时基时间值，则有如下时间：

T_{RX_POLL_M}＝t_{RX_POLL_M}

T_{RX_POLL_S1}＝t_{RX_POLL_S1}+S1U_K+1

T_{RX_POLL_S2}＝t_{RX_POLL_S1}+S2U_K+1

步骤3.6：计算t时刻识别卡到达UWB定位器M、UWB定位器S1和UWB定位器S2设备的距离差(TDOA)

d_{m_S1(t)}是t时刻识别卡到UWB定位器M和UWB定位器S1距离的差值；

d_{m_S2(t)}是t时刻识别卡到UWB定位器M和UWB定位器S2距离的差值；

d_{m_S1(t)}＝(T_{RX_POLL_S1}-T_{RX_POLL_M})*C_光速

d_{m_S2(t)}＝(T_{RX_POLL_S2}-T_{RX_POLL_M})*C_光速

(4)使用DS_TWR实现识别卡的TOA测距

应用DS_TWR方法UWB定位器M得到TOA结果。本实施例中，DS_TWR的时序方法，如图3所示。

计算识别卡与UWB定位器M的传播时间值，计算公式如下：

T_TA：识别卡Tag到UWB定位器A的传播时间；

T_round1是从识别卡开始发送并收到返回信息的首轮时间，T是UWB定位器A的间隔时间，T_round2是从UWB定位器A开始发送并收到返回信息的次轮时间，T_M是移动节点中的间隔时间。

计算t时刻识别卡到UWB定位器M的距离

D_TM＝T_AT*C_光速

(5)使用TDOA、DS_TWR结果计算t时刻识别卡坐标

使用Chan算法计算二维、三定位基站情况下的t时刻识别卡坐标，如图7所示。

UWB定位器M坐标(x₁，y₁)，UWB定位器S1坐标(x₂，y₂)，UWB定位器S2 坐标(x₃，y₃)；r₁、r₂、r₃分别是识别卡到UWB定位器M、S1、S2的距离；

设识别卡的位置坐标(x，y)；

根据几何关系公式可知下列表达式：

根据公式(5-1)有如下关系：

r_i ²＝(x_i-x)²+(y_i-y)²＝K_i-2x_ix-2y_iy+x²+y²

令：K_i＝x_i ²+y_i ²

其中r_i ²＝(r_i,1+r₁)²

则有如下矩阵：即Ax＝C。可利用矩阵的逆即 x＝A^-1C。

最后，求得：

式(5-6)中三个基站的坐标都是已知，r_2，1和r_3，1是d_{m_S1(t)}、d_{m_S12t)}的TDOA 差值，r₁是TOA距离D_TM，因此，可直接求得识别卡t时刻二维坐标值x，y。

在识别卡远离三基站的时候，双曲线的渐近线会趋于平行，这产生强烈的误差放大，直接导致了TDOA求解结果数据的精度极具下降，因此，在加入一个 TOA测距结果可以有效降低误差。

本发明的有益效果

(1)本发明方法解决煤矿井下巷道环境的二维定位精度问题，采用TDOA与 DS_TWR结合方法，限制了TDOA的误差抖动问题，解决了识别卡远离三定位基站的定位误差问题，提高了系统定位精度，完全满足煤矿定位技术要求；

(2)本发明方法中TDOA使用了无线时钟同步，采用Kalman滤波跟踪主时钟，时钟校准精度达到亚纳秒级，有效抑制了测量过程中的噪声和干扰数据，；

(3)本发明方法通过集中布放三定位基站阵，可以得到更大的覆盖区域，从而减少井下了定位基站的数量50％，大大降低了系统建设成本以及施工的维护的工作量；

(4)本发明方法中由于采用了单独区域的时钟同步方法，无需实现跨区域全网级联时钟同步，大大降低了系统的耦合程度，杜绝了因本级时钟问题影响后面基站群的定位，提高定位系统稳定性。

附图说明

图1：UWB最小系统结构图；

图2：井下系统布置结构图；

图3：DS_TWR时序图；

图4：TDOA时序图；

图5：TDOA和DS_TWR通信流程图；

图6：TDOA与DS_TWR结合的井下二维定位方法流程图；

图7：TDOA和DS_TWR结合的Chan算法几何示意图。

具体实施方式

为使本发明实施例的目的和技术方案更加清楚，下面将附图，并结合本发明实施例对本发明的技术方案进行清楚、完整的描述。显然，所描述实施例是本发明的一部分实施例，而不是全部实施例。基于所描述的本发明的实施例，本领域普通技术人员在无需创造性劳动的前提下，所获得的所有其他实施例，都属于本发明的保护范围。

本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义解释。

进一步地，本发明方法步骤：

步骤1：实现本发明方法的相关系统设备组成，如图1所示。

UWB定位基站单元：由UWB定位器M、UWB定位器S1、UWB定位器S2、集控器共同组成，UWB定位器与集控器采用RS-485或CAN总线连接，并实时上报接收到的定位数据和时钟同步数据，并在集控器内实现初步的位置信息解算。

UWB定位器M是主定位器，定时发送UWB无线时钟校准包TX_CCP，并完成与识别卡的DS_TWR测距，并上报这些消息。

UWB定位器S1是从定位器1，完成接收UWB无线时钟校准包RX_CCP，并上报。

UWB定位器S2是从定位器2，完成接收UWB无线时钟校准包RX_CCP，并上报。

UWB集控器是收集TX_CCP消息、RX_CCP消息、POLL消息，并计算TDOA，接收TOA消息。

UWB识别卡能够实现DS_TWR的UWB无线收发设备。

UWB定位服务器实现位置坐标解算。

步骤2：实施本发明方法的设备部署方案，如图2所示。

沿着井下巷道两侧依次布设UWB定位器M、集控器、UWB定位器S1、UWB定位器S2，其中三个UWB定位器不能同时共线，间距大于1米，小于20米(可依据CAN总线和RS-485传输速率、距离确定)该实际距离可通过系统自动测量(或人为实际测量)得到。

进一步，步骤3：使用基于Kalman滤波无线时钟同步法计算得到POLL消息的TDOA时间差值

步骤3.3：用基于Kalman滤波的时钟跟踪方法计算时钟偏移

Kalman滤波采用最小均方差误差实现最佳的线性滤波方法。根据当前时刻的观测值、上一时刻的预测值以及预测误差来计算得到当前的最优值去预测下一时刻的值。第K+1同步周期主从时钟的时钟偏差和时钟漂移根据第K周期的信息预测。则根据Kalman标准方程可得到：

T为时钟周期，U_θ,K和U_γ,K分别是第K周期的时钟偏移和时钟偏移的修正值，θ_K和γ_K分别是kT时钟偏移和时钟漂移。在(K+1)T时刻，时钟迭代关系：

(θ_k+1)＝θ_k-U_θ,k+(γ_k-U_γ,k)T+ω_θ,k

(γ_k+1)＝γ_k-U_γ,k+ω_γ

ω_θ,K是时钟偏差方差，ω_γ,K是时钟漂移方差。假设ω_k＝[ω_θ,kω_γ,k]^T，协方差矩阵为Q，

定义向量矩阵：x_k＝[θ_kγ_k]^T,u_k＝[U_θ,kU_γ,k]^T.

使用Kalman滤波迭代：

建立预测方程：(X_k+1)＝Ax_k+Bu_k

其中：Kalman增益矩阵：

K_k+1＝P_k+1|k[H_k+1]^T(R_k+1+H_k+1P_k+1|k[H_k+1]^T)^-1

R_K+1是观测噪声的协方差矩阵，H_K+1是测量矩阵，单位方阵；

修正：

最小均方差误差矩阵：

P_k+1＝(1-K_k+1)P_k+1|k

经过Kalman滤波，第K+1时刻时钟周期的修正值

另是补偿时钟偏移和时钟漂移，则有UWB定位器S1和UWB定位器S2的时钟偏移分别为，S1U_K+1和S2U_K+1。

步骤3.4：接收识别卡发送的POLL消息

步骤3.5：t时刻POLL消息时基校准

T_{RX_POLL_M}＝t_{RX_POLL_M}

T_{RX_POLL_S1}＝t_{RX_POLL_S1}+S1U_K+1

T_{RX_POLL_S2}＝t_{RX_POLL_S1}+S2U_K+1

d_{m_S2(t)}是t时刻识别卡到UWB定位器M和UWB定位器S2距离的差值。

d_{m_S1(t)}＝(T_{RX_POLL_S1}-T_{RX_POLL_M})*C_光速

d_{m_S2(t)}＝(T_{RX_POLL_S2}-T_{RX_POLL_M})*C_光速

进一步，步骤4：使用DS_TWR方法得到识别卡的TOA测距值

步骤4.1识别卡向UWB定位器M发送POLL消息，并记录发送时刻时间T_Mtx(发送Poll消息的时间戳)，UWB定位器M收到此消息并记录时间T_Arx(接收Poll消息的时间戳)；

步骤4.2UWB定位器M向识别卡Respond消息，并记录发送时刻时间T_Atx(发送Respon消息时间戳)，识别卡s收到此消息，并记录时间戳T_Mrx(接收UWB定位器A的Respon消息时间戳)；

步骤4.3识别卡向UWB定位器M发送Final消息，并记录发送时刻时间T_Mtx(发送Final消息的时间戳)，UWB定位器M收到此消息并记录时间戳T_Brx(接收Final消息的时间戳)。

步骤4.4UWB定位器M计算TOA结果

本实施例中，DS_TWR的时序方法，见图3所示。

(1)测量的识别卡时间戳

T_Mtx：发送Poll消息的时间戳；

T_Mrx：接收UWB定位器M的Respon消息时间戳；

T_Mtx：发送Final消息的时间戳。

(2)测量的UWB定位器M时间戳

T_Arx：接收Poll消息的时间戳；

T_Atx：发送Respon消息时间戳；

T_Arx：接收Final消息的时间戳。

计算识别卡与UWB定位器M的传播时间值，计算公式如下：

T_TA：识别卡Tag到UWB定位器A的传播时间；

计算t时刻识别卡到UWB定位器M的距离

D_TM＝T_AT*C_光速

进一步，步骤5：根据TDOA与DS_TWR联合定位结果，使用Chan方法计算识别卡坐标值

步骤5.1计算t时刻识别卡与UWB定位器S之间的距离

根据步骤3.6中计算的t时刻识别卡发送POLL消息到达UWB定位器S的到达时间差(TDOA)，以及步骤4.4的t时刻识别卡到UWB定位器M距离(TOA)，有一下过程：

D_TS1＝D_TM(t)+d_{m_S1(t)}

D_TS2＝D_TM(t)+d_{m_S2(t)}

其中：D_TS1、D_TS2分别是t时刻识别卡到达UWB定位器S1和S2的距离值。

步骤5.2计算t时刻识别卡位置坐标

使用Chan算法计算二维、3定位基站情况下的t时刻识别卡坐标

(1)UWB定位器M坐标(x₁，y₁)，UWB定位器S1坐标(x₂，y₂)，UWB定位器 S2坐标(x₃，y₃)；r₁、r₂、r₃分别是识别卡到UWB定位器M、S1、S2的距离；

设识别卡坐标(x，y)位置；

据几何关系公式可知下列表达式：

根据公式(5-1)有如下关系：

r_i ²＝(x_i-x)²+(y_i-y)²＝K_i-2x_ix-2y_iy+x²+y² (5-3)

令：K_i＝x_i ²+y_i ²

根据公式(5-2)有如下关系：

r_i ²＝(r_i,1+r₁)² (5-4)

将(5-3)带入(5-4)有如下关系:

则有如下矩阵：即Ax＝C。可利用矩阵的逆即x＝A^-1C。最后，求得：

式(5-6)中三个基站的坐标都是已知，r_2，1和r_3，1分别是d_{m_S1(t)}、d_{m_S12t)}的 TDOA差值，r₁是TOA距离D_TM，因此，可直接求得t时刻识别卡二维坐标值x，y。

在识别卡远离三基站定位的时候，双曲线的渐近线会趋于平行，这产生强烈的误差放大，直接导致了TDOA求解结果数据的精度极具下降，因此，在加入一个TOA测距结果可以有效降低误差。

Claims

1.一种基于UWB技术的TDOA与DS_TWR结合的井下二维定位方法，其特征在于，包括以下步骤：

步骤一：实现本发明方法的相关系统设备由UWB定位器M、UWB定位器S1、UWB定位器S2、集控器共同组成；

步骤二：实施本发明方法的设备部署方案为：沿着井下巷道两侧依次布设所述UWB定位器M、集控器、UWB定位器S1、UWB定位器S2，其中三个UWB定位器不能同时共线，间距大于1米，小于20米；

步骤三：使用基于Kalman滤波无线时钟同步法计算得到POLL消息TDOA时间差值；

其中，步骤三包括：

步骤3.1：UWB定位器M间隔150ms发送无线时钟校准包CPP，记录发送时刻本地时钟数据，并通过CAN总线把带有发送时间戳的CCP数据包传输到集控器；步骤3.2：UWB定位器S1和UWB定位器S2接收RX_CCP包，并通过CAN总线把带有接收时间戳的RX_CCP1、RX_CCP2数据包传输到集控器；

步骤3.3：用基于Kalman滤波的时钟跟踪方法计算时钟偏移，则有UWB定位器S1和UWB定位器S2的时钟偏移分别为，S1U_K+1和S2U_K+1；

步骤3.4：UWB定位器M、UWB定位器S1和UWB定位器S2分别接收识别卡发送的POLL消息，并获取POLL消息的接收时间戳，分别记作t_{RX_POLL_M}、t_{RX_POLL_S1}、t_{RX_POLL_S2}；通过CAN总线发送这些时间戳到集控器；

步骤3.5：根据所述时钟偏移S1U_K+1和S2U_K+1，把t_{RX_POLL_M}、t_{RX_POLL_S1}、t_{RX_POLL_S2}校准到以UWB定位器M为时基时间值，则有如下时间：

T_{RX_POLL_M}＝t_{RX_POLL_M}

T_{RX_POLL_S1}＝t_{RX_POLL_S1}+S1U_K+1

T_{RX_POLL_S2}＝t_{RX_POLL_S1}+S2U_K+1；

步骤3.6：计算t时刻识别卡到达UWB定位器M和UWB定位器S1距离的差值和识别卡到UWB定位器M和UWB定位器S2设备的距离差(TDOA)；

C_光速是光速；

d_{m_S1(t)＝}(T_{RX_POLL_S1}-T_{RX_POLL_M})*C_光速

d_{m_S2(t)}＝(T_{RX_POLL_S2}-T_{RX_POLL_M})*C_光速；

步骤四：使用DS-TWR方法得到识别卡的TOA测距值；计算t时刻识别卡到UWB定位器M的距离；

T_TA：识别卡Tag到UWB定位器A的传播时间；

T_round1是从识别卡开始发送并收到返回信息的首轮时间，T是UWB定位器A的间隔时间，T_round2是从UWB定位器A开始发送并收到返回信息的次轮时间，T_M是移动节点中的间隔时间；

计算t时刻识别卡到UWB定位器M的距离：

D_TM＝T_TA*C_光速

步骤五：根据TDOA与DS-TWR联合定位结果，使用Chan方法计算t时刻识别卡坐标值，具体的：

UWB定位器M坐标(x₁，y₁)，UWB定位器S1坐标(x₂，y₂)，UWB定位器S2坐标(x₃，y₃)；r₁、r₂、r₃分别是识别卡到UWB定位器M、S1、S2的距离；

设识别卡的位置坐标(x，y)；令：K＝x_i ²+y_i ²；

得：

式中：y_2，1是指y_2，1＝y₂-y₁；y_3，1是指y_3，1＝y₃-y₁；x_3，1是指x_3，1＝x₃-x₁；

x_2，1是指x_2，1＝x₂-x₁；

r_2，1和r_3，1是d_{m_S1(t)}、d_{m_S12(t)}的TDOA差值，r₁是TOA距离D_TM，因此，可直接求得识别卡t时刻二维坐标值x，y。

2.根据权利要求1所述的一种基于UWB技术的TDOA与DS_TWR结合的井下二维定位方法，其特征在于：UWB定位器与集控器采用RS-485或CAN总线连接，并实时上报接收到的定位数据和时钟同步数据，并在集控器内实现初步的位置信息解算。

3.根据权利要求1所述的一种基于UWB技术的TDOA与DS_TWR结合的井下二维定位方法，其特征在于：使用基于Kalman滤波方法实现无线时钟同步法，并计算POLL消息到达各UWB定位器的到达时间差值(TDOA)；

T为时钟周期，

U_θ，K是第K周期的时钟偏移修正值；

U_γ，K是第K周期的时钟漂移修正值；

θ_K是kT时刻时钟偏移；

γ_K是kT时刻时钟漂移；

在(K+1)T时刻，时钟迭代关系：

(θ_k+1)＝θ_k-U_θ，k+(γ_k-U_γ，k)T+ω_θ，k

(γ_k+1)＝γ_k-U_γ，k+ω_γ，k

ω_θ，K是时钟偏差方差，ω_γ，k是时钟漂移方差；

假设ω_k＝[ω_θ，k ω_γ，k]^T，

ω_k是时钟漂移方差和时钟偏移方差矩阵；

x_k＝[θ_kγ_k]^T，u_k＝[U_θ，kU_γ，k]^T.

x_k是kT时刻时钟偏移和时钟漂移矩阵；

u_k是时钟偏移修正值和时钟漂移修正值矩阵；

使用Kalman滤波迭代：

建立预测方程：(X_k+1)＝Ax_k+Bu_k

其中：

Kalman增益矩阵：

K_k+1＝P_k+1|k[H_k+1]^T(R_k+1+H_k+1P_k+1|_k[H_k+1]^T)^-1

K_k+1是在(K+1)T时刻Kalman增益矩阵；

P_k+1|k是最小均方误差矩阵估计；

R_k+1是观测噪声的协方差矩阵，H_k+1是测量矩阵，单位方阵；修正：

Z_k+1是第K+1时刻时钟周期误差；

是经过Kalman滤波，第K+1时刻时钟周期的修正值；

是第K时刻时钟周期对第K+1时刻的时钟预测值；

最小均方差误差矩阵：

P_k+1＝(1-K_k+1)P_k+1|k

经过Kalman滤波，第K+1时刻时钟周期的修正值另是补偿时钟偏移和时钟漂移，则有UWB定位器S1和UWB定位器S2的时钟偏移分别为，S1U_k+1和S2U_k+1。