基于四点定位和拓展卡尔曼滤波的UWB定位及轨迹估计算法

UWB Positioning and Trajectory Estimation Algorithm Based on Four-Point Positioning and Expanded Kalman Filtering

摘要: 基于室内环境的复杂性和多变性，本文研究了超宽带(Ultra-Wideband, UWB)通信信号在信号干扰影响下的精确定位问题。首先采用四点定位法，利用UWB中的靶点与锚点的距离信息实现靶点的定位，并通过解决非线性最小二乘问题对定位算法进行改进；然后在随机生成具有固定靶点与锚点位置的数据集中，采用改进算法计算靶点的位置，并通过误差评估指标对算法进行误差分析，验证算法的有效性与适用性；最后利用拓展卡尔曼滤波对轨迹估计进行优化，得到精确的轨迹模型。

Abstract: Based on the complexity and variability of the indoor environment, this paper investigates the precise positioning of Ultra-Wideband (UWB) communication signals under the influence of signal interference. Firstly, the four-point positioning algorithm is adopted to achieve the target location using the distance information between the target point and the anchor point in UWB, and the positioning algorithm is improved by solving the nonlinear least squares problem. Secondly, in the randomly generated dataset with fixed target and anchor positions, the improved algorithm is used to calculate the position of the target, then the error analysis is carried out on the algorithm through the error assessment index to verify the effectiveness and applicability of the algorithm. Finally, the trajectory estimation is optimized using extended Kalman filtering to obtain an accurate trajectory model.

1. 引言

随着信息时代物联网技术的快速发展，人们对于定位应用的需求也在不断增加。GPS作为室外定位常用的手段之一，在地图导航及户外人员定位等领域有着较高的定位精度 [1] 。然而其在室内环境中会受各类建筑物遮挡，导致无线卫星信号多径传播和信号衰减，从而无法满足室内位置服务的需求，因此室内定位方案及技术成为当前研究的热点。

现如今室内定位技术种类繁多：红外线的定位系统通过红外信号作为测量信号对目标进行定位 [2] ；定位系统结构简单、定位精度较高、目标跟踪性较好但易受非视距的室内环境影响的超声波定位技术 [3] ；通过测距定位法和基于指纹库定位法的WiFi技术，但其传播距离较短，易受室内环境的噪声干扰 [4] ；可自动识别的射频识别技术，但其仅适合于定位区域小且精度需求不高情况下使用 [5] ；适用低速短距离传输的ZigBee定位技术，低功耗、低成本，但同样只能满足低定位精度要求的场景使用 [6] 。而超宽带(Ultra-Wideband, UWB)通信具有较高的信息传送速率、卓越的定位精度，以及较好的时间分辨和抗多径干扰能力，逐渐在室内环境中占有相当的比重 [7] 。但室内场景多变、环境复杂，会使得UWB通信信号极易受到遮挡，虽然UWB技术具有穿透能力，但仍然会产生误差，在较强干扰时，数据会发生异常波动(通常是时间延时)，基本无法完成室内定位。因此对信号干扰下的超宽带精确定位问题进行研究具有非常重要的应用价值。

本文首先利用UWB中的靶点与锚点的距离信息实现靶点的定位，并通过解决非线性最小二乘问题对定位算法进行改进；其次通过随机生成固定的靶点与锚点位置，应用改进算法计算靶点的位置，运用误差评估指标对算法进行误差分析，验证算法的有效性与适用性；最后采集按时间顺序的离散数据对其进行轨迹估计，并运用拓展卡尔曼滤波进行优化，得到精确的轨迹模型。

2. 定位算法

根据靶点与四个锚点之间的距离，需要通过定位技术确定靶点的位置。四点定位法利用靶点与四个锚点之间的空间距离来确定靶点的位置。原始的四点定位法直接进行定位，但在实际环境中靶点与锚点之间的距离难以避免地存在误差，导致解定位坐标时产生误差，而这种误差是无法估计的。因此在存在距离误差的前提下，需要获得最优解。由此产生的问题可被形式化为非线性最小二乘问题，故采用LM算法进行求解，得到改进的四点定位算法。

2.1. 四点定位

在实际应用中，需要在锚点数量有限的情况下尽可能准确地估计靶点位置，而四点定位法 [8] 能够利用靶点与四个锚点之间的空间距离来确定靶点的位置。

假设空间中已知四点A、B、C、D，坐标分别为 $\left(x_1,y_1,z_1\right)$ 、 $\left(x_2,y_2,z_2\right)$ 、 $\left(x_3,y_3,z_3\right)$ 、 $\left(x_4,y_4,z_4\right)$ ，待定位点P坐标为 $\left(x,y,z\right)$ ，P到四点的距离分别为 ${\rho }_1,{\rho }_2,{\rho }_3,{\rho }_4$ ，根据距离公式可得：

$\{\begin{array}{l}{\left(x_1-x\right)}^2+{\left(y_1-y\right)}^2+{\left(z_1-z\right)}^2={\rho }_1^2\\ {\left(x_2-x\right)}^2+{\left(y_2-y\right)}^2+{\left(z_2-z\right)}^2={\rho }_2^2\\ {\left(x_3-x\right)}^2+{\left(y_3-y\right)}^2+{\left(z_3-z\right)}^2={\rho }_3^2\\ {\left(x_4-x\right)}^2+{\left(y_4-y\right)}^2+{\left(z_4-z\right)}^2={\rho }_4^2\end{array}$ (1)

整理可得：

$\{\begin{array}{l}{x}^2-2x_{1}x+y^2-2y_{1}y+z^2-2z_{1}z={\rho }_1^2-x_1^2-y_1^2-z_1^2\\ x^2-2x_{2}x+y^2-2y_{2}y+z^2-2z_{2}z={\rho }_2^2-x_2^2-y_2^2-z_2^2\\ x^2-2x_{3}x+y^2-2y_{3}y+z^2-2z_{3}z={\rho }_3^2-x_3^2-y_3^2-z_3^2\\ x^2-2x_{4}x+y^2-2y_{4}y+z^2-2z_{4}z={\rho }_4^2-x_4^2-y_4^2-z_4^2\end{array}$ (2)

对方程进行三次差分消除掉高次项，得到：

$\{\begin{array}{l}2x\left(x_2-x_1\right)+2y\left(y_2-y_1\right)+2z\left(z_2-z_1\right)={\rho }_1^2-{\rho }_2^2+x_2^2-x_1^2+y_2^2-y_1^2+z_2^2-z_1^2\\ 2x\left(x_3-x_1\right)+2y\left(y_3-y_1\right)+2z\left(z_3-z_1\right)={\rho }_1^2-{\rho }_3^2+x_3^2-x_1^2+y_3^2-y_1^2+z_3^2-z_1^2\\ 2x\left(x_4-x_1\right)+2y\left(y_4-y_1\right)+2z\left(z_4-z_1\right)={\rho }_1^2-{\rho }_4^2+x_4^2-x_1^2+y_4^2-y_1^2+z_4^2-z_1^2\end{array}$ (3)

用矩阵形式表示： $Ac=b$ ，其中： $c={\left(x,y,z\right)}^{\text{T}}$ ，

$A=\left(\begin{array}{ccc}2\left(x_2-x_1\right)& 2\left(y_2-y_1\right)& 2\left(z_2-z_1\right)\\ 2\left(x_3-x_1\right)& 2\left(y_3-y_1\right)& 2\left(z_3-z_1\right)\\ 2\left(x_4-x_1\right)& 2\left(y_4-y_1\right)& 2\left(z_4-z_1\right)\end{array}\right)$ , $b=\left(\begin{array}{l}{\rho }_1^2-{\rho }_2^2+x_2^2-x_1^2+y_2^2-y_1^2+z_2^2-z_1^2\\ {\rho }_1^2-{\rho }_3^2+x_3^2-x_1^2+y_3^2-y_1^2+z_3^2-z_1^2\\ {\rho }_1^2-{\rho }_4^2+x_4^2-x_1^2+y_4^2-y_1^2+z_4^2-z_1^2\end{array}\right)$ (4)

求解P点坐标，即求解向量 $c$ ，如果矩阵 $A$ 的逆矩阵存在，可求得： $c=A^{-1}b$ 。

2.2. 改进定位算法

上述的四点定位法求解过程中，将四个方程做三次差分得到三个方程，是基于“靶点到四个锚点的距离是准确的”进行的。但在实际中由于受到各种干扰，测得的距离数据是不精确的，需要在受到干扰的情况下获得最优解。因此针对误差的产生，需要将问题化作非线性最小二乘问题形式，并采用LM算法进行求解。

1) 目标函数：

$F\left(x\right)=\frac{1}{2}{\displaystyle \underset{i=1}{\overset{m}{\sum }}{\left(f_i\left(x\right)\right)}^2}$ (5)

其中， $f_i:R^n\to R,i=1,\cdots ,m,$ 即 $x\in R^n$ ， $f_i\left(x\right)\in R$ ， $f_i$ 也可以将x映射到 $R^m$ 空间中。

2) 高斯牛顿法：

将目标函数改写成矩阵形式：

$F\left(x\right)=\frac{1}{2}{\left[f\left(x\right)\right]}^2$ (6)

其中：

$f\left(x\right)=\left[\begin{array}{c}{f}_1\left(x\right)\\ ⋮\\ f_m\left(x\right)\end{array}\right]$ (7)

记：

$J_i\left(x\right)=\frac{\partial f_i\left(x\right)}{\partial x}=\left[\begin{array}{ccc}\frac{\partial f_i\left(x\right)}{\partial x_1}& \cdots & \frac{\partial f_i\left(x\right)}{\partial x_n}\end{array}\right]$ (8)

$J\left(x\right)=\left[\begin{array}{ccc}\frac{\partial f_1\left(x\right)}{\partial x}& \cdots & \frac{\partial f_m\left(x\right)}{\partial x}\end{array}\right]=\left[\begin{array}{ccc}\frac{\partial f_1\left(x\right)}{\partial x_1}& \cdots & \frac{\partial f_m\left(x\right)}{\partial x_n}\\ ⋮& \ddots & ⋮\\ \frac{\partial f_1\left(x\right)}{\partial x_1}& \cdots & \frac{\partial f_m\left(x\right)}{\partial x_n}\end{array}\right]$ (9)

则 $J_i\left(x\right)$ 是一个 $1\times n$ 向量，而 $J\left(x\right)$ 是一个 $n\times n$ 矩阵。

将 $f\left(x\right)$ 进行一阶泰勒展开：

$l\left(\Delta x\right)=f\left(x\right)+J\Delta x\approx f\left(x+\Delta x\right)$ (10)

则：

$\begin{array}{l}F\left(x\right)\approx L\left(\Delta x\right)=\frac{1}{2}l{\left(\Delta x\right)}^{\text{T}}l\left(\Delta x\right)=\frac{1}{2}\left(f\left(x\right)+J\Delta x\right)\\ =\frac{1}{2}f{\left(x\right)}^2+f{\left(x\right)}^{\text{T}}J\Delta x+\Delta x^{\text{T}}{J}^{\text{T}}J\Delta x\end{array}$ (11)

对 $L\left(\Delta x\right)$ 求导并令其为0，则：

$\begin{array}{l}\frac{\partial L\left(\Delta x\right)}{\partial \Delta x}=f{\left(x\right)}^{\text{T}}J+\Delta x^{\text{T}}{J}^{\text{T}}J\\ =J^{\text{T}}f\left(x\right)+J^{\text{T}}J\Delta x\\ =0\end{array}$ (12)

可得：

$\Delta x=-{\left(J^{\text{T}}J\right)}^{-1}{J}^{\text{T}}f\left(x\right)$ (13)

3) LM法：

当 $J^{\text{T}}J$ 奇异或病态时，则需引入单位矩阵 $I$ ，使得：

$\left(J^{\text{T}}J+\mu I\right)\Delta x=-J^{\text{T}}f\left(x\right)$ (14)

其中， $\mu \ge 0$ 。

则：

$m_k\left(\Delta x\right)=\frac{1}{2}{\Vert x_k\Vert }^2+{\left(\Delta x\right)}^{\text{T}}{J}^{\text{T}}{x}_k+\frac{1}{2}{\left(\Delta x\right)}^{\text{T}}{J}^{\text{T}}J\Delta x$ (15)

4) 算法步骤

给定 $x_0$ ，初始乘子 ${\mu }_0$ ，参数 $0\le \beta \le r_1<r_2<1,\epsilon \ge 0,{\sigma }_1<1<{\sigma }_2,k:=0$ 。

步骤1：如果 $\Vert \nabla f\left(x_k\right)\Vert \le \epsilon$ ，算法停止， $x_k$ 是问题的解。

步骤2：计算 $\left(J^{\text{T}}J+\mu I\right)\Delta x=-J^{\text{T}}f\left(x\right)$ 得到 $\Delta x_k$ 。

步骤3：计算 $r_k=\frac{f\left(x_k\right)-f\left(x_k+\Delta x_k\right)}{m_k\left(0\right)-m_k\left(\Delta x_k\right)}$ ，

若 $r_k<r_1$ ，则 ${\mu }_{k+1}={\sigma }_2{\mu }_k$ ；

若 $r_k>r_2$ ，则 ${\mu }_{k+1}={\sigma }_1{\mu }_k$ ；

否则， $\mu$ 不变。

步骤4：若 $r_k>\beta$ 则 $x_{k+1}=x_k+\Delta x_k$ ；

否则 $x_{k+1}=x_k$ 。

令 $k=k+1$ ，转步骤1。

运用上述算法求解式(1)，即可得到改进后的四点定位法。

3. 轨迹估计

由于数据呈现为时间顺序的离散形式，轨迹估计可视为对离散时间系统状态进行估计的过程。通过采集数据，能够获取靶点的运动信息以及对锚点的观测数据。卡尔曼滤波 [9] 被广泛应用于线性系统状态的最优估计。但由于定位模型不符合线性系统的特性，因此需要采用拓展卡尔曼滤波进行优化，使轨迹估计更加精准。

3.1. 拓展卡尔曼滤波

标准卡尔曼滤波的状态转移方程和观测方程为：

$\{\begin{array}{c}{\theta }_k=A{\theta }_{k-1}+B{u}_{k-1}+s_k\\ z_k=C{\theta }_k+v_k\end{array}$ (16)

其中， $k-1$ 和 $k$ 分别表示上一状态和当前状态； $\theta \in R^n$ 表示要估计的状态； $A\in R^{n\times n}$ 表示上一状态到当前状态的转换矩阵； $u\in R^l$ 表示可选的控制输入，一般在实际使用中忽略； $B\in R^{n\times l}$ 表示控制输入到当前状态的转换矩阵； $z\in R^m$ 表示测量值； $C\in R^{m\times n}$ 表示当前状态到测量的转换矩阵； $s_k$ 表示过程噪声； $v_k$ 表示测量噪声。

设扩展卡尔曼滤波 [10] 的状态转移方程和观测方程为：

${\theta }_k=f\left({\theta }_{k-1}\right)+s_k$ (17)

$z_k=h\left({\theta }_k\right)+v_k$ (18)

利用泰勒展开式对式(17)在上一次的估计值 $\left({\theta }_{k-1}\right)$ 处展开得：

${\theta }_k=f\left({\theta }_{k-1}\right)+s_k=f\left(\langle {\theta }_{k-1}\rangle \right)+F_{k-1}\left({\theta }_{k-1}-\langle {\theta }_{k-1}\rangle \right)+s_k$ (19)

再利用泰勒展开式对式(18)在本轮的状态预测值 ${{\theta }^{\prime }}_k$ 处展开得：

$z_k=h\left({\theta }_k\right)+v_k=h\left({{\theta }^{\prime }}_k\right)+H_k\left({\theta }_k-{{\theta }^{\prime }}_k\right)+v_k$ (20)

其中， $F_{k-1}$ 和 $H_k$ 分别表示函数 $f\left(\theta \right)$ 和 $h\left(\theta \right)$ 在 $\langle {\theta }_{k-1}\rangle$ 和 ${{\theta }^{\prime }}_k$ 处的雅克比矩阵。

下面给出拓展卡尔曼滤波预测和更新两个步骤：

1) 预测

${{\theta }^{\prime }}_k=f\left(\langle {\theta }_{k-1}\rangle \right)$ , ${\displaystyle \sum {}_k^{\text{'}}}=F_{k-1}{\displaystyle \sum {}_{{}_{k-1}}}{F}_{k-1}^{\text{T}}+Q$

2) 更新

${S^{\prime }}_k={\left(H_k{\displaystyle \sum {}_k^{\text{'}}}{H}_k^{\text{T}}+R\right)}^{-1}$ , ${K^{\prime }}_k={\displaystyle \sum {}_k^{\text{'}}}{H}_k^{\text{T}}{S^{\prime }}_k$

$\langle {\theta }_k\rangle ={{\theta }^{\prime }}_k+K^{\prime }\left(z_k-h\left({{\theta }^{\prime }}_k\right)\right)$ , ${\displaystyle \sum {}_k}=\left(I-{K^{\prime }}_k{H}_k\right){\displaystyle \sum {}_k^{\text{'}}}$

其中的雅克比矩阵 $F_{k-1}$ 和 $H_k$ 分别为：

$F_{k-1}={\frac{\partial f}{\partial \theta }|}_{\langle {\theta }_{k-1}\rangle }$ , ${H_k=\frac{\partial h}{\partial \theta }|}_{{{\theta }^{\prime }}_k}$

其中， ${\theta }_k$ 为真实值， ${\theta }^{\prime }$ 为预测值， $\langle {\theta }_k\rangle$ 为估计值， $z_k$ 为观测值， ${\hat{z}}_k$ 为观测值的预测， ${\displaystyle \sum {}_k}$ 为估计值与真实值之间的误差协方差矩阵， $\langle \rangle$ 为求期望的符号。

状态转移方程和观测方程为：

${\theta }_k=f\left({\theta }_{k-1}\right)+s_k,s_k\sim N\left(0,Q\right),z_k=h\left({\theta }_k\right)+v_k,v_k\sim N\left(0,R\right)$

引入反馈：

$\langle {\theta }_k\rangle ={{\theta }^{\prime }}_k+K_k\left(z_k-{\hat{z}}_k\right)={{\theta }^{\prime }}_k+K_k\left(z_k-h\left({{\theta }^{\prime }}_k\right)\right)$ (21)

由式(19)、(20)可得：

$f\left({\theta }_{k-1}\right)-f\left(\langle {\theta }_{k-1}\rangle \right)=F_{k-1}\left({\theta }_{k-1}-\langle {\theta }_{k-1}\rangle \right)$ (22)

$h\left({\theta }_k\right)-h\left({{\theta }^{\prime }}_k\right)=H_k\left({\theta }_k-{{\theta }^{\prime }}_k\right)$ (23)

计算估计值与真实值之间的误差协方差矩阵，并把式(20)、(21)、(23)代入，得到：

$\begin{array}{l}{\displaystyle \sum {}_k=\langle e_k{e}_k^{\text{T}}\rangle =}\langle \left({\theta }_k-\langle {\theta }_k\rangle \right){\left({\theta }_k-\langle {\theta }_k\rangle \right)}^{\text{T}}\rangle \\ =\langle \left[{\theta }_k-{{\theta }^{\prime }}_k-K_k\left(z_k-h\left({{\theta }^{\prime }}_k\right)\right)\right]{\left[{\theta }_k-{{\theta }^{\prime }}_k-K_k\left(z_k-h\left({{\theta }^{\prime }}_k\right)\right)\right]}^{\text{T}}\rangle \\ =\langle \left[{\theta }_k-{{\theta }^{\prime }}_k-K_k\left(h\left({\theta }_k\right)-h\left({{\theta }^{\prime }}_k\right)+v_k\right)\right]{\left[{\theta }_k-{{\theta }^{\prime }}_k-K_k\left(h\left({\theta }_k\right)-h\left({{\theta }^{\prime }}_k\right)+v_k\right)\right]}^{\text{T}}\rangle \\ =\langle \left[\left(I-K_k{H}_k\right)\left({\theta }_k-{{\theta }^{\prime }}_k\right)+K_k{v}_k\right]{\left[\left(I-K_k{H}_k\right)\left({\theta }_k-{{\theta }^{\prime }}_k\right)+K_k{v}_k\right]}^{\text{T}}\rangle \\ =\left(I-K_k{H}_k\right)\langle \left({\theta }_k-{{\theta }^{\prime }}_k\right){\left({\theta }_k-{{\theta }^{\prime }}_k\right)}^{\text{T}}\rangle {\left(I-K_k{H}_k\right)}^{\text{T}}+K_{k}R{K}_k^{\text{T}}\\ =\left(I-K_k{H}_k\right){\displaystyle \sum {}_k^{\text{'}}}{\left(I-K_k{H}_k\right)}^{\text{T}}+K_{k}R{K}_k^{\text{T}}\end{array}$ (24)

其中 ${\displaystyle \sum {}_k^{\text{'}}}$ 表示真实值与预测值之间的误差协方差矩阵。于是得到：

${\displaystyle \sum {}_k=}{\displaystyle \sum {}_k^{\text{'}}}-K_k{H}_k{\displaystyle \sum {}_k^{\text{'}}}-{\displaystyle \sum {}_k^{\text{'}}}{H}_k^{\text{T}}+K_k\left(H_k{\displaystyle \sum {}_k^{\text{'}}}{H}_k^{\text{T}}+R\right)K_k^{\text{T}}$ (25)

由于 ${\displaystyle \sum {}_k}$ 的对角元即为真实值与估计值的误差的平方，矩阵的迹即为总误差的平方和，即：

$Tr\left[{\displaystyle \sum {}_k}\right]=Tr\left[{\displaystyle \sum {}_{{}_k}^{\text{'}}}\right]+Tr\left[K_k\left(H_k{\displaystyle \sum {}_{{}_k}^{\text{'}}}{H}_k^{\text{T}}+R\right)K_k^{\text{T}}\right]-2Tr\left[K_k{H}_k{\displaystyle \sum {}_{{}_k}^{\text{'}}}\right]$ (26)

为了估计值更接近于真实值，使 $Tr\left[{\displaystyle \sum {}_k}\right]$ 尽可能的小，需要取得合适的卡尔曼增益 $K_k$ ，即 $Tr\left[{\displaystyle \sum {}_k}\right]$ 对 $K_k$ 的偏导为0：

$\frac{\text{d}Tr\left[{\displaystyle \sum {}_k}\right]}{\text{d}{K}_k}=2K_k\left(H_k{\displaystyle \sum {}_k^{\text{'}}{H}_k^{\text{T}}+R}\right)-2{\left(H_k{\displaystyle \sum {}_{{}_k}^{\text{'}}}\right)}^{\text{T}}=0$ (27)

则卡尔曼增益为：

$K_k={\displaystyle \sum {}_{{}_k}^{\text{'}}}{H}_k^T{\left(H_k{\displaystyle \sum {}_{{}_k}^{\text{'}}}{H}_k^T+R\right)}^{-1}$ (28)

再由式(25)得到：

${\displaystyle \sum {}_k}={\displaystyle \sum {}_{{}_k}^{\text{'}}}-{\displaystyle \sum {}_{{}_k}^{\text{'}}}{H}_k^T{\left(H_k{\displaystyle \sum {}_{{}_k}^{\text{'}}}{H}_k^T+R\right)}^{-1}{H}_k{\displaystyle \sum {}_{{}_k}^{\text{'}}}=\left(I-K_k{H}_k\right){\displaystyle \sum {}_{{}_k}^{\text{'}}}$ (29)

则真实值与预测值之间的协方差矩阵 ${\displaystyle \sum {}_{{}_k}^{\text{'}}}$ 为：

${\displaystyle \sum {}_{{}_k}^{\text{'}}=\langle {e^{\prime }}_k{e^{\prime }}_k^{\text{T}}\rangle }=\langle \left({\theta }_k-{{\theta }^{\prime }}_k\right){\left({\theta }_k-{{\theta }^{\prime }}_k\right)}^{\text{T}}\rangle$ (30)

将式(31)带入到式(30)中，

${\theta }_k=f\left({\theta }_{k-1}\right)+s_k,{{\theta }^{\prime }}_k=f\left(\langle {\theta }_{k-1}\rangle \right)$ (31)

可得：

${\displaystyle \sum {}_{{}_k}^{\text{'}}}=\langle \left[f\left({\theta }_{k-1}\right)-f\left(\langle {\theta }_{k-1}\rangle \right)+s_k\right]{\left[f\left({\theta }_{k-1}\right)-f\left(\langle {\theta }_{k-1}\rangle \right)+s_k\right]}^{\text{T}}\rangle$ (32)

其中， ${\theta }_k$ 、 $\langle {\theta }_k\rangle$ 与观测噪声是独立的，期望为零； $\langle s_k{s}_k^{\text{T}}\rangle$ 表示观测噪声的协方差矩阵，用 $Q$ 表示。

于是得到：

$\begin{array}{l}{\displaystyle \sum {}_k^{\text{'}}}=F_{k-1}\langle \left({\theta }_{k-1}-\langle {\theta }_{k-1}\rangle \right)\left({\theta }_{k-1}-{\langle {\theta }_{k-1}\rangle }^{\text{T}}\right)\rangle F_{k-1}^{\text{T}}+\langle s_k{s}_k^{\text{T}}\rangle \\ =F_{k-1}{\displaystyle \sum {}_{k-1}}{F}_{k-1}^{\text{T}}+Q\end{array}$ (33)

式(33)的协方差矩阵的转置矩阵即为其本身。

3.2. 运动模型

靶点从A运动到B的轨迹如图1所示。

Figure 1. Target motion trajectory schematic diagram

图1.靶点运动轨迹示意图

轨迹估计是一个离散时间系统的状态估计 [11] ，记录终端在各个时刻的位置状态为：

$r_k={\left[x,y\right]}_k$ (34)

$k$ 表示终端在轨迹中的离散时间下标。用两个方程描述状态，估计问题：

$\{\begin{array}{c}{r}_k=f\left(r_{k-1},u_k,w_k\right)\\ z_k=g\left(r_k,v_k\right)\end{array}$ (35)

其中， $f$ 为状态方程， $g$ 为观测方程， $u$ 为输入， $w$ 为系统噪声， $z$ 为观测数据， $v$ 为观测噪声， ${{\hat{r}}^{\prime }}_k$ 为状态预测， ${\hat{r}}_{k-1}$ 为状态估计， ${\hat{z}}_k$ 为测量预测。

对应的预测方程和观测方程为：

$\{\begin{array}{c}{{\hat{r}}^{\prime }}_k=f\left({\hat{r}}_{k-1},w_k\right)\\ {\hat{z}}_k=g\left({{\hat{r}}^{\prime }}_k,v_k\right)\end{array}$ (36)

构造残差方程： $e_k=z_k-g\left({{\hat{r}}^{\prime }}_k,v_k\right)$ ，即预测和测量的误差，当残差最小时误差最小。通过这种反馈的形式降低噪声的影响，最终得到的各个位置的坐标即为残差平方最小化的解。

4. 实验及结果

根据设定的实验场景和数据，对改进的定位算法进行误差评估，以验证算法的有效性和适应性；然后生成以时间为序的靶点运动数据，应用本文算法来推断靶点的运动轨迹，进一步对轨迹模型进行评估。

4.1. 实验场景及数据

实验设置在5000 mm × 5000 mm × 3000 mm的测试环境中。分别在4个角落A0、A1、A2、A3放置UWB锚点，锚点向所有方向发送信号，如图2。Tag为UWB标签，即需要定位的目标(靶点)。Tag接收到4个UWB锚点的信号，利用TOF技术，分别解算出对应的4个距离数据。实验在实验场景中采集了Tag在324个不同位置，在信号干扰下的UWB数据，锚点与Tag之间每0.2~0.3秒之间会发送、接收信号一次。靶点范围为：5000 mm × 5000 mm × 3000 mm。锚点位置(单位：mm)为：A0 (0, 0, 1300)、A1 (5000, 0, 1700)、A2 (0, 5000, 1700)、A3 (5000, 5000, 1300)。

Figure 2. Experimental environment schematic diagram

图2. 实验环境示意图

4.2. 定位算法实验及结果

随机生成四个锚点与一个靶点，通过改进后的四点定位法计算出靶点的位置，再将精确的靶点位置与计算的靶点位置进行比较，可以验证算法的准确性。为了实际性，对空间四点定位的误差进行分析，加入干扰，多次定位同一点，观察误差情况。下面介绍相关误差评定指标：

1) 平均偏差：定位点与待定位点距离的平均值，反映定位精确度。

2) 均方根误差：反映定位精确度。

$RMSE=\sqrt{\frac{{\displaystyle {\sum }_{i=1}^n{\left(X_{obs,i}-X_{\mathrm{mod}el,i}\right)}^2}}{n}}$ (37)

3) 标准差：反映定位精密度。

$S=\sqrt{\frac{{\displaystyle {\sum }_{i=1}^N{\left(X_i-\bar{X}\right)}^2}}{n}}$ (38)

由图3可知，加入随机扰动后将数据代入误差指标可以得到误差分析图。从误差分析图可以观察到，采用四点定位的方法，平均偏差接近于4 cm，均方根误差与标准差均接近于4.5 cm，误差较小，表明本文定位算法具有较高的定位精度。

Figure 3. Error analysis chart

图3. 误差分析图

4.3. 轨迹估计算法实验及结果

将锚点与靶点的信息数据带入轨迹估计算法中，获得靶点随时间运动的位置信息，将其可视化为轨迹。图4展示了原始定位散点图，图5展示了根据轨迹模型优化后的轨迹图。在具有信号干扰的情况下，本文算法仍然能够较为准确的重现出靶点的运动轨迹。

Figure 4. Original localization scatter plot

图4. 原始定位散点图

Figure 5. Motion trajectory chart

图5. 运动轨迹图