一种基于消息近似的分布式无线协作定位方法

摘要

本发明公开了一种基于消息近似的分布式无线协作定位方法，涉及通信与网络技术领域，具体为：首先，搭建包括若干待定位节点和锚节点的分布式无线协作定位装置；各待定位节点分别广播发送协作定位请求，将各自通信范围内返回响应的节点作为协作定位节点保存；接着、每个待定位节点分别向各自列表中的节点发送测距请求，存储返回的响应消息；在任意时刻，每个待定位节点基于自身以及列表中各节点的位置随机向量的联合后验概率进行因子分解，构造局部时空因子图。最后，利用拓展卡尔曼滤波预测并更新各待定位节点状态的后验概率分布，估计各待定位节点在本时刻的位置，实现分布式无线协作定位。本发明降低了计算复杂度以及通信开销，具有更高的精度。

权利要求

1.一种基于消息近似的分布式无线协作定位方法，其特征在于，具体步骤如下：首先，搭建包括若干待定位节点和若干锚节点的分布式无线协作定位装置；各待定位节点分别广播发送协作定位请求，将各自通信范围内返回响应的待定位节点以及锚节点为协作定位节点，保存到各自的协作列表存储；

然后、针对每个待定位节点，分别向各自协作列表中的每个节点发送测距请求，列表中的各节点返回自身位置的先验信息、接收到测距请求的时刻以及发送测距响应消息的时刻并存储；

在任意时刻t，每个待定位节点基于各自列表中存储的协作定位节点，对包含自身以及各自协作定位节点的位置随机向量的联合后验概率进行因子分解；并根据因子分解的结果，构造各待定位节点的局部时间‑空间因子图；

最后、对于各待定位节点，利用拓展卡尔曼滤波的预测算法对本时刻节点状态的初始概率分布进行预测，使用经过无迹变换处理的邻居节点的消息获取本时刻节点状态的后验概率分布，并使用拓展卡尔曼滤波的更新算法对节点状态的后验概率分布进行更新，分别估计各待定位节点在本时刻的位置，实现分布式无线协作定位；

所述的利用拓展卡尔曼滤波预测并更新节点状态的后验概率分布，估计各待定位节点在本时刻的位置，实现分布式无线协作定位具体过程为：步骤601、针对待定位节点i，计算单元建立自身的运动模型以及观测模型；

待定位节点i在时刻t的运动模型为：

其中 表示待定位节点i在时刻t的状态信息，包括节点的位置随机向量以及速度随机向量； 表示待定位节点i在时刻t的位置， 表示待定位节点i在时刻t的速度；F表示待定位节点i的状态转移矩阵；B为待定位节点i的输入控制矩阵； 表示待定位节点i的控制向量； 表示过程噪声，服从均值为0，协方差为 的高斯分布；

待定位节点i在时刻t的观测模型为：

其中h(·)是待定位节点i的非线性观测函数； 表示观测噪声，服从均值为0，协方差为 的高斯分布；

步骤602、基于t‑1时刻，利用运动模型得到的待定位节点i状态的估计信息 利用拓展卡尔曼滤波预测待定位节点i在时刻t的状态均值以及协方差；

状态均值计算公式为：

待定位节点i在时刻t状态的预测协方差为：

其中 表示待定位节点i在时刻t‑1状态的协方差；

步骤603、将时刻t对待定位节点i的预测状态均值以及协方差，作为待定位节点i在时刻t的先验信息，利用各自局部因子图上各协作定位节点发送至待定位节点的信息，对消息中的非线性项进行无迹变换近似，计算待定位节点i在时刻t的位置随机向量的后验概率分布；

步骤604、利用拓展卡尔曼滤波，对因子图上的待定位节点i在时刻t的后验概率分布进行更新；

具体而言：

首先、基于待定位节点i在时刻t的位置随机变量的后验概率分布的均值 与位置随机变量的后验概率分布的协方差 计算待定位节点i的观测残差 以及观测残差的协方差计算公式如下：其中，观测矩阵H定义为：

然后、利用待定位节点i在时刻t状态的预测协方差和观测矩阵H计算卡尔曼增益；

表示为：

最后、利用卡尔曼增益，计算待定位节点i在时刻t的位置随机变量的后验概率分布更新后的均值 与更新后的协方差步骤605、各待定位节点利用各自在时刻t更新的位置随机向量的后验概率分布，分别估计各自的位置，实现分布式无线协作定位；

节点i在时刻t位置随机向量的估计值 表示成：

最后、将节点i在时刻t位置随机向量的估计值 作为节点i在时刻t的最终位置。

2.如权利要求1所述的一种基于消息近似的分布式无线协作定位方法，其特征在于，所述的分布式无线协作定位装置中，各待定位节点按任意拓扑分布于空间区域内，组成一个adhoc网络。

3.如权利要求1所述的一种基于消息近似的分布式无线协作定位方法，其特征在于，所述的列表中存储的节点包括已知实际位置的锚节点与未知实际位置的其余待定位节点。

4.如权利要求1所述的一种基于消息近似的分布式无线协作定位方法，其特征在于，所述的因子分解满足：t

其中 表示节点i及其协作定位节点在时刻t的位置随机向量；Z表示时刻t，节点i以及其所有协作节点对自身、以及上述节点之间互相观测的位置随机向量的观测值构成的矩t阵；Z由 和 组成，其中 表示时刻t,节点i以及其所有协作节点对自身位置随机向量的观测值构成的矩阵； 表示时刻t节点i以及其所有协作节点之间互相观测的位置随机向量的观测值构成的矩阵。

5.如权利要求1所述的一种基于消息近似的分布式无线协作定位方法，其特征在于，所述的局部时间‑空间因子图具体为：针对各待定位节点，当前待定位节点i对协作节点的观测似然函数，构成一个因子；每个节点在时刻t的位置先验分布，构成一个因子；其余待定位节点对该节点i的观测似然函数，构成一个因子；对于每一个因子，在因子图上用一个因子节点表示；每个节点在每个时刻的位置，都作为一个随机向量，在因子图上用一条边表示；

如果随机变量出现在因子中，将对应于随机变量的边与对应于因子的因子节点相连接；对于出现在多于两个因子中的随机变量，构造一个等号节点，连接该等号节点的每条边上的随机变量是相同的。

6.如权利要求1所述的一种基于消息近似的分布式无线协作定位方法，其特征在于，所述的步骤603具体而言：t|t‑1

首先、基于预测的待定位节点i在时刻t状态的均值 以及状态协方差Pi ，得到待定位节点i在时刻t的位置先验信息服从均值为 协方差为 的高斯分布：t|t‑1

其中 是均值 中的元素， 是协方差Pi 中的元素；

然后、待定位节点i计算所有与自身在时刻t的位置随机向量 相连接的因式节点，在时刻t向位置随机向量 传递消息，并对上述消息中的非线性项进行无迹变换，使用变换后的结果来近似上述消息；

传递消息包括：因子节点φj→i(·)向 传递的消息 以及因子节点φk→i(·)向 传递的消息因子节点φj→i(·)表示待定位节点i在时刻t对与其相连接的待定位节点j的测距似然函数；因子节点φk→i(·)表示待定位节点i在时刻t对与其相连接的锚节点k的测距似然函数；

其次、待定位节点i根据当前时刻t所有向 传递的消息来更新节点i的后验概率分布；

待定位节点i在时刻t的位置随机变量的后验概率分布等于：待定位节点i在时刻t的位置随机变量的先验概率和所有与 相连接的观测似然函数因子节点向 传递消息的积，即：其中， 表示在时刻t所有与待定位节点i相连接的锚节点集合， 表示在时刻t所有与待定位节点i相连接的待定位节点集合，待定位节点i在时刻t的位置随机变量的后验概率分布 服从均值为 协方差为 的高斯分布。

说明书

一种基于消息近似的分布式无线协作定位方法

技术领域

[0001] 本发明涉及通信与网络技术领域，具体是一种基于消息近似的分布式无线协作定位方法。

背景技术

[0002] 精准的位置信息对于很多现代的民用和军事应用都是非常重要的，比如基于位置的服务、应急救援、车联网、自动驾驶车辆以及群体感知等。这些服务也是未来物联网(Internet of Things,IoT)、智能交通系统(Intelligent Transportation System,ITS)以及5G通信等领域的重要发展方向。

[0003] 在大部分室外环境中，很多定位应用得益于全球导航卫星系统(Global Navigation Satellite System,GNSS)精度为米级的定位技术。然而，由于卫星信号的射频穿透能力较弱，在室内、城市峡谷、森林覆盖、隧道以及战场等GNSS拒止的环境下，卫星信号容易受到干扰或者阻断，导致基于GNSS的定位方式和定位精度下降甚至定位失败。

[0004] 基于无线网络的定位系统因为结构灵活、易于设置，已经成为恶劣环境下的重要定位解决方案。系统中一般包括两类节点：已知位置的锚节点和未知位置的待定位节点。整个定位过程可以分为两步：第一步，系统中的节点利用定位测量技术获得与邻居锚节点的测量信息；第二步，利用定位算法对测量信息进行融合，得到节点的位置估计信息。

[0005] 在无线定位系统中，每个节点对辅助定位的锚节点数量有最低要求(例如确定用户节点的二维位置至少需要3个锚节点)，否则无法确定用户节点位置的唯一性。在复杂环境条件下，由于节点分布和运动的随机性以及障碍物的遮挡，系统中部分节点在某些时刻无法获得足够的测量信息，导致定位失败。

[0006] 为了提高定位系统的可靠性，一种方法是增加锚节点的数量以及发射功率，但会导致成本的上升、系统资源的浪费以及复杂度的增长。协作定位是另一种能够有效提高系统定位精度和可靠性的方法。在协作定位系统中，用户节点在与锚节点进行测量的同时，还可以通过测量与相邻用户节点的距离或者角度信息来降低自身的定位误差。在锚节点无法完全覆盖系统中所有用户节点时，协作定位是完成节点位置估计的重要手段，进一步扩大了无线定位的服务范围。

[0007] 基于因子图的消息传递算法在分布式无线协作定位中有重要的应用前景。目前针对基于因子图的消息传递算法在协作定位方面的研究主要分为两个方向：

[0008] 其一，核心思想在于基于参数化消息的消息传递过程，待定位节点通过特定的相似度度量指标来寻找某个概率分布，用以近似该节点位置的后验概率分布，然后使用此近似分布的参数来替代原始分布的参数，最后通过因子图上随机变量间的消息传递，解算出网络中各个待定位节点的位置；

[0009] 其二，核心思想在于基于粒子化消息的消息传递过程，待定位节点通过对该节点位置的后验概率分布按特定的采样规则进行采样，使基于采样点(即粒子)重构的概率分布的参数等于原始后验概率分布的参数，最后通过因子图上随机变量间的消息传递，解算出网络中各个待定位节点的位置。

[0010] 参数类算法假设网络中节点的位置信息以及节点间的相对距离信息服从某种特定的概率分布(如高斯分布)，然后在节点间信息传递的过程中只传递这种分布的特征参数，减小了系统的通信负担和计算复杂度；但是在实际定位场景中，位置信息或者测量信息模型并不满足高斯分布，模型的失配会明显降低定位性能。

[0011] 非参数类算法利用带有权值的随机粒子来近似测量信息的概率分布，用这些粒子的均值和方差来描述节点的位置估计信息，因而不受非线性非高斯的限制，具有较高的定位精度。但是非参数类算法会导致较大的通信负担和能量消耗，这个问题在节点较多的场景下变得尤其严重。

发明内容

[0012] 为解决现有的消息传递协作定位技术研究中，面临的节点高移动性导致的性能退化问题，降低无线自组织网络中节点因邻居节点数量不足导致对定位性能的影响，更好地为网络中的节点提供可靠的定位保障，本发明提出了一种基于消息近似的分布式无线协作定位方法。

[0013] 所述的基于消息近似的分布式无线协作定位方法，具体步骤如下：

[0014] 步骤一、搭建包括若干待定位节点和若干锚节点的分布式无线协作定位装置；

[0015] 具体为：各待定位节点按任意拓扑分布于空间区域内，组成一个ad hoc网络；

[0016] 针对每个待定位节点，与各节点进行通信的其余待定位节点以及锚节点，统称为该节点的协作定位节点。

[0017] 步骤二、各待定位节点分别广播发送协作定位请求，将各自通信范围内返回响应的协作定位节点，保存到各自的协作列表存储；

[0018] 列表中存储的节点包括该待定位节点通信范围内响应的已知实际位置的锚节点与该待定位节点通信范围内响应的未知实际位置的其余待定位节点；

[0019] 步骤三、针对每个待定位节点，分别向各自协作列表中的每个节点发送测距请求，列表中的各节点返回自身位置的先验信息、接收到测距请求的时刻以及发送测距响应消息的时刻并存储；

[0020] 步骤四、在任意时刻t，每个待定位节点基于各自列表中存储的协作定位节点，对包含自身以及各自协作定位节点的位置随机向量的联合后验概率进行因子分解；

[0021] 因子分解满足：

[0022]

[0023] 其中 表示节点i及其协作定位节点在时刻t的位置随机向量；Zt表示时刻t，节点i以及其所有协作节点对自身、以及上述节点之间互相观测的位置随机向量的观测值构成t的矩阵；Z由 和 组成，其中 表示时刻t,节点i以及其所有协作节点对自身位置随机向量的观测值构成的矩阵； 表示时刻t节点i以及其所有协作节点之间互相观测的位置随机向量的观测值构成的矩阵；

[0024] 步骤五、每个待定位节点根据各自因子分解的结果，构造各自的局部时间‑空间因子图。

[0025] 针对各待定位节点，当前待定位节点i对协作节点的观测似然函数，构成一个因子；每个节点在时刻t的位置先验分布，构成一个因子；其余待定位节点对该节点i的观测似然函数，构成一个因子；对于每一个因子，在因子图上用一个因子节点表示；每个节点在每个时刻的位置，都作为一个随机向量，在因子图上用一条边表示；

[0026] 如果随机变量出现在因子中，将对应于随机变量的边与对应于因子的因子节点相连接；对于出现在多于两个因子中的随机变量，构造一个等号节点，连接该等号节点的每条边上的随机变量是相同的；

[0027] 步骤六、对于各待定位节点，利用拓展卡尔曼滤波的预测步骤对本时刻节点状态的初始概率分布进行预测，使用经过无迹变换处理的邻居节点的消息获取本时刻节点状态的后验概率分布，并使用拓展卡尔曼滤波的更新步骤对节点状态的后验概率分布进行更新，分别估计各待定位节点在本时刻的位置，实现分布式无线协作定位。

[0028] 具体过程为：

[0029] 步骤601、针对待定位节点i，计算单元建立自身的运动模型以及观测模型；

[0030] 待定位节点i在时刻t的运动模型为：

[0031]

[0032] 其中 表示待定位节点i在时刻t的状态信息，包括节点的位置随机向量以及速度随机向量； 表示待定位节点i在时刻t的位置， 表示待定位节点i在时刻t的速度；F表示待定位节点i的状态转移矩阵；B为待定位节点i的输入控制矩阵； 表示待定位节点i的控制向量； 表示过程噪声，服从均值为0，协方差为 的高斯分布。

[0033] 待定位节点i在时刻t的观测模型为：

[0034]

[0035] 其中h(·)是待定位节点i的非线性观测函数； 表示观测噪声，服从均值为0，协方差为 的高斯分布。

[0036] 步骤602、基于t‑1时刻，利用运动模型得到的待定位节点i状态的估计信息利用拓展卡尔曼滤波预测待定位节点i在时刻t的状态均值以及协方差；

[0037] 状态均值计算公式为：

[0038]

[0039] 待定位节点i在时刻t状态的预测协方差为：

[0040]

[0041] 其中 表示待定位节点i在时刻t‑1状态的协方差。

[0042] 步骤603、将时刻t对待定位节点i的预测状态均值以及协方差，作为待定位节点i在时刻t的先验信息，利用各自局部因子图上各协作定位节点发送至待定位节点的信息，对消息中的非线性项进行无迹变换近似，计算待定位节点i在时刻t的位置随机向量的后验概率分布。

[0043] 具体而言：

[0044] 首先、基于预测的待定位节点i在时刻t状态的均值 以及状态协方差得到待定位节点i在时刻t的位置先验信息服从均值为 协方差为 的高斯分布：

[0045]

[0046] 其中 是均值 中的元素， 是协方差 中的元素。

[0047] 然后、待定位节点i计算所有与自身在时刻t的位置随机向量 相连接的因式节点，在时刻t向位置随机向量 传递消息，并对上述消息中的非线性项进行无迹变换，使用变换后的结果来近似上述消息。

[0048] 传递消息包括：因子节点φj→i(·)向 传递的消息 以及因子节点φk→i(·)向 传递的消息

[0049] 因子节点φj→i(·)表示待定位节点i在时刻t对与其相连接的待定位节点j的测距似然函数；因子节点φk→i(·)表示待定位节点i在时刻t对与其相连接的锚节点k的测距似然函数；

[0050] 其次、待定位节点i根据当前时刻t所有向 传递的消息来更新节点i的后验概率分布；

[0051] 待定位节点i在时刻t的位置随机变量的后验概率分布等于：待定位节点i在时刻t的位置随机变量的先验概率和所有与 相连接的观测似然函数因子节点向 传递消息的积，即：

[0052]

[0053] 其中， 表示在时刻t所有与待定位节点i相连接的锚节点集合， 表示在时刻t所有与待定位节点i相连接的待定位节点集合，待定位节点i在时刻t的位置随机变量的后验概率分布 服从均值为 协方差为 的高斯分布。

[0054] 步骤604、利用拓展卡尔曼滤波，对因子图上的待定位节点i在时刻t的后验概率分布进行更新；

[0055] 具体而言：

[0056] 首先、基于待定位节点i在时刻t的位置随机变量的后验概率分布的均值 与位置随机变量的后验概率分布的协方差 计算待定位节点i的观测残差 以及观测残差的协方差

[0057] 计算公式如下：

[0058]

[0059]

[0060] 其中，观测矩阵H定义为：

[0061]

[0062] 然后、利用待定位节点i在时刻t状态的预测协方差和观测矩阵H计算卡尔曼增益；

[0063] 表示为：

[0064]

[0065] 最后、利用卡尔曼增益，计算待定位节点i在时刻t的位置随机变量的后验概率分布 更新后的均值 与更新后的协方差

[0066]

[0067]

[0068] 步骤605、各待定位节点利用各自在时刻t更新的位置随机向量的后验概率分布，分别估计各自的位置，实现分布式无线协作定位。

[0069] 节点i在时刻t位置随机向量的估计值 表示成：

[0070]

[0071] 最后、将节点i在时刻t位置随机向量的估计值 作为节点i在时刻t的最终位置。

[0072] 本发明的优点在于：

[0073] 本发明一种基于消息近似的分布式无线协作定位方法，通过对待定位节点构造局部因子图，设计了一种网络设备间的分布式无线协作定位方法，包含协作定位节点列表的获取、一套节点定位流程与对节点位置随机变量的后验概率中非线性项的无迹变换处理，降低了传统粒子化消息传递定位过程中的计算复杂度以及通信开销；由于通过拓展卡尔曼滤波模型对待定位节点的先验信息进行初步预测以及后验概率的更新，相比传统消息传递方法，本发明具有更高的精度，对于网络设备的无线协作定位具有重要意义。

附图说明

[0074] 图1为本发明一种节点间协作的分布式定位方法工作流程图；

[0075] 图2为本发明搭建的分布式无线协作定位装置中待定位节点的示意图；

[0076] 图3为本发明搭建的分布式无线协作定位装置中锚节点的示意图；

[0077] 图4为本发明目标节点与其协作定位节点间消息传递过程的局部因子图。

具体实施方式

[0078] 下面将结合附图和实施例对本发明作进一步的详细说明。

[0079] 本发明针对随机布置的分布式传感器或移动节点构成的系统与应用场景，制定一种基于消息近似的分布式无线协作定位方法，如图1所示，具体步骤如下：

[0080] 步骤一、搭建包括若干待定位节点和若干锚节点的分布式无线协作定位装置；

[0081] 具体为：各待定位节点按任意拓扑分布于空间区域内，组成一个ad hoc网络；

[0082] 如图2所示，每个待定位节点均包括第一无线通信单元，第一存储单元，第一控制单元和第一计算单元；第一控制单元操控各待定位节点的运动轨迹；

[0083] 如图3所示，每个锚节点包括第二无线通信单元，第二存储单元和第二控制单元；

[0084] 针对每个待定位节点，与各节点进行通信的其余待定位节点以及锚节点，统称为该节点的协作定位节点。

[0085] 针对当前要处理的待定位节点，标记为目标节点；首先通过第一无线通信单元广播发送协作定位请求；可以参与协作定位的节点返回给目标节点协作定位响应消息；目标节点根据协作定位响应消息生成协作定位节点列表，并存储到第一存储单元中；

[0086] 接着，目标节点向列表中的协作定位节点发送测距请求；协作定位节点将自身位置信息、本地接收到测距请求的时刻以及本地发送测距响应消息的时刻发送至目标节点；目标节点将上述信息存储至第一存储单元；目标节点的第一计算单元根据上述信息，完成目标节点与各协作定位节点之间距离的计算、目标节点的位置计算以及目标节点的位置更新，并存储到第一存储单元。

[0087] 目标节点的第一计算单元根据协作定位节点列表，以及位置后验概率密度的因子分解，生成目标节点的局部因子图；通过初始时刻局部因子图内所有节点位置随机向量的联合先验分布、位置随机向量的状态转移概率以及联合似然函数，计算目标节点在各个时刻位置随机向量的后验概率分布，并将其中的非线性项进行一阶泰勒展开；最后，估计并更新目标节点的位置坐标。

[0088] 第二无线通信单元获取所有与该锚节点进行通信的待定位节点，保存到待定位节点列表中，存储到第二存储单元中；同时，接收协作定位请求和测距请求，并发送自身位置信息给与本锚节点通信的待定位节点。

[0089] 第二存储单元用于存储锚节点的位置，并传输至第二无线通信单元。

[0090] 第二无线通信单元感知锚节点与列表中各节点的连接状态，若锚节点与列表中的某待定位节点断开通信，则在列表中删除断开该节点，并更新列表。

[0091] 步骤二、各待定位节点通过其第一无线通信单元分别广播发送协作定位请求，将各自通信范围内返回响应的协作定位节点，保存到各自的协作列表存储；

[0092] 列表中存储的节点包括该待定位节点通信范围内响应的已知实际位置的锚节点与该待定位节点通信范围内响应的未知实际位置的其余待定位节点；

[0093] 步骤三、针对每个待定位节点，分别向各自协作列表中的每个节点发送测距请求，列表中的各节点通过各自的第一无线通信单元返回自身位置的先验信息、接收到测距请求的时刻以及发送测距响应消息的时刻并存储在各节点的第一存储单元中；

[0094] 步骤四、在任意时刻t，每个待定位节点基于各自列表中存储的协作定位节点，对包含自身以及各自协作定位节点的位置随机向量的联合后验概率进行因子分解；

[0095] 因子分解满足：

[0096]

[0097] 其中“∝”表示“正比于”； 表示节点i及其协作定位节点在时刻t的位置随机向t量；Z表示时刻t，节点i以及其所有协作节点对自身、以及上述节点之间互相观测的位置随t
机向量的观测值构成的矩阵；Z由 和 组成，其中 表示时刻t,节点i以及其所有协作节点对自身位置随机向量的观测值构成的矩阵； 表示时刻t节点i以及其所有协作节点之间互相观测的位置随机向量的观测值构成的矩阵；

[0098] 步骤五、每个待定位节点根据各自因子分解的结果，构造各自的局部时间‑空间因子图。

[0099] 针对各待定位节点，当前待定位节点i对协作节点的观测似然函数，构成一个因子；每个节点在时刻t的位置先验分布，构成一个因子；其余待定位节点对该节点i的观测似然函数，构成一个因子；对于每一个因子，在因子图上用一个因子节点表示；每个节点在每个时刻的位置，都作为一个随机向量，在因子图上用一条边表示；

[0100] 如果随机变量出现在因子中，将对应于随机变量的边与对应于因子的因子节点相连接；对于出现在多于两个因子中的随机变量，构造一个等号节点，连接该等号节点的每条边上的随机变量是相同的；

[0101] 以待定位节点i为例，其计算单元构造出如图4所示的局部因子图；其中，因子节点表示待定位节点i在时刻t的位置先验信息；因子节点φj→i(·)表示待定位节点i在时刻t对与其相连接的待定位节点j测距似然函数；因子节点φk→i(·)表示待定位节点i在时刻t对与其相连接的锚节点k的测距似然函数；因子节点φi→j(·)表示与待定位节点i相连接的待定位节点j待在时刻t对待定位节点i的测距似然函数。

[0102] 步骤六、对于各待定位节点，利用拓展卡尔曼滤波的预测步骤对本时刻节点状态的初始概率分布进行预测，使用经过无迹变换处理的邻居节点的消息获取本时刻节点状态的后验概率分布，并使用拓展卡尔曼滤波的更新步骤对节点状态的后验概率分布进行更新，分别估计各待定位节点在本时刻的位置，实现分布式无线协作定位。

[0103] 首先，建立各待定位节点的运动模型以及观测模型；基于前一时刻各节点状态的估计信息，利用拓展卡尔曼滤波对当前时刻各对应节点的状态信息进行预测；所述节点状态信息包括节点的位置随机向量以及速度随机向量；

[0104] 然后，将待定位节点i的预测信息作为该待定位节点i在时刻t的先验信息，利用局部因子图上各协作定位节点发送至待定位节点i的信息，并对协作定位节点发送至待定位节点i的消息中的非线性项进行无迹变换近似，计算目标节点i本时刻的位置随机向量的后验概率分布；

[0105] 最后，利用拓展卡尔曼滤波对因子图上得到的节点状态的后验概率分布进行更新；利用更新后各待定位节点本时刻的位置随机向量的后验概率分布，分别估计各待定位节点在本时刻的位置，实现分布式无线协作定位。

[0106] 具体过程为：

[0107] 步骤601、针对待定位节点i，计算单元建立自身的运动模型以及观测模型；

[0108] 待定位节点i在时刻t的运动模型为：

[0109]

[0110] 其中 表示待定位节点i在时刻t的状态信息，包括节点的位置随机向量以及速度随机向量； 表示待定位节点i在时刻t的位置， 表示待定位节点i在时刻t的速度；F表示待定位节点i的状态转移矩阵；B为待定位节点i的输入控制矩阵； 表示待定位节点i的控制向量； 表示过程噪声，服从均值为0，协方差为 的高斯分布。

[0111] 状态转移矩阵F和输入控制矩阵B可以进一步表示为：

[0112]

[0113]

[0114] 其中，I3表示维度为3×3的单位矩阵，03表示维度为3×3的零矩阵，ΔT表示时间间隔。

[0115] 待定位节点i在时刻t的观测模型为：

[0116]

[0117] 其中h(·)是待定位节点i的非线性观测函数； 表示观测噪声，服从均值为0，协方差为 的高斯分布。

[0118] 步骤602、基于t‑1时刻，利用运动模型得到的待定位节点i状态的估计信息利用拓展卡尔曼滤波预测待定位节点i在时刻t的状态均值以及协方差；

[0119] 状态均值计算公式为：

[0120]

[0121] 待定位节点i在时刻t状态的预测协方差为：

[0122]

[0123] 其中 表示待定位节点i在时刻t‑1状态的协方差。

[0124] 步骤603、将步骤602在时刻t对待定位节点i的预测信息作为待定位节点i在时刻t的先验信息，利用局部因子图上各协作定位节点发送至待定位节点的信息，并对消息中的非线性项进行无迹变换近似，计算待定位节点i在时刻t的位置随机向量的后验概率分布。

[0125] 具体而言：

[0126] 首先、基于预测的待定位节点i在时刻t状态的均值 以及状态协方差得到待定位节点i在时刻t的位置先验信息服从均值为 协方差为 的高斯分布：

[0127]

[0128] 其中 是均值 中的元素， 是协方差 中的元素。

[0129] 然后、待定位节点i计算所有与自身在时刻t的位置随机向量 相连接的因式节点，在时刻t向位置随机向量 传递消息，并对上述消息中的非线性项进行无迹变换，使用变换后的结果来近似上述消息。

[0130] 传递消息包括：因子节点φj→i(·)向 传递的消息 以及因子节点φk→i(·)向 传递的消息

[0131] 因子节点φj→i(·)表示待定位节点i在时刻t对与其相连接的待定位节点j的测距似然函数；因子节点φk→i(·)表示待定位节点i在时刻t对与其相连接的锚节点k的测距似然函数；

[0132] 其次、待定位节点i根据当前时刻t所有向 传递的消息来更新节点i的后验概率分布；

[0133] 待定位节点i在时刻t的位置随机变量的后验概率分布等于：待定位节点i在时刻t的位置随机变量的先验概率和所有与 相连接的观测似然函数因子节点向 传递消息的积，即：

[0134]

[0135] 其中， 表示在时刻t所有与待定位节点i相连接的锚节点集合， 表示在时刻t所有与待定位节点i相连接的待定位节点集合，待定位节点i在时刻t的位置随机变量的后验概率分布 服从均值为 协方差为 的高斯分布。

[0136] 以消息 为例，表示为：

[0137]

[0138] 其中， 表示与目标节点i相连接的锚节点k在时刻t的位置随机变量向因式节点φk→i(·)传递的消息，服从：

[0139]

[0140]

[0141] 其中，δ(·)表示狄拉克δ函数， 表示与节点i相连接的锚节点k在时刻t位置随机向量的均值向量， 表示与节点i相连接的锚节点k在时刻t对节点i的观测， 表示 的方差，exp(·)表示以自然常数e为底的指数函数，||·||表示欧氏距离。

[0142] 因式节点φk→i(·)向 传递的消息 进一步表示为：

[0143]

[0144] 目标节点i的计算单元其中的 进行无迹变换。

[0145] 无迹变换具体而言：

[0146] 首先，根据 的均值 与协方差矩阵 通过一定的采样方法得到2n+1采样点集 采样点集服从：

[0147]

[0148]

[0149]

[0150] λ＝α2(n+k)‑n

[0151] 其中，n表示 的维度数， 表示矩阵 的第i列，λ表示一个缩放比例参数，α表示控制采样点分布状态的参数，k是控制矩阵，用于控制矩阵为半正定矩阵。

[0152] 然后，计算采样点的权值：

[0153]

[0154]

[0155]

[0156] 其中， 表示第a个采样点的均值向量， 表示第a个采样点的协方差矩阵；

[0157] 对采样得到的点集 中的每个采样点通过非线性变换,得到采样后的点集服从：

[0158]

[0159] 对变换后的采样点集 进行加权处理，得到输出变量Ya的均值E{Ya}与协方差[0160]

[0161]

[0162] 即因式节点φk→i(·)向 传递的消息 服从：

[0163]

[0164] 根据所有与目标节点i在时刻t的位置随机向量 相连接的因式节点向 传递的消息来计算目标节点i在时刻t的后验概率分布，即：

[0165]

[0166] 其中，目标节点i在时刻t位置随机向量的均值向量 可以表示为：

[0167]

[0168] 目标节点i在时刻t位置随机向量的协方差矩阵 可以表示成：

[0169]

[0170] 步骤604、利用拓展卡尔曼滤波，对因子图上的待定位节点i在时刻t的状态的后验概率分布进行更新；

[0171] 具体而言：

[0172] 首先、基于待定位节点i在时刻t的位置随机变量的后验概率分布的均值 与位置随机变量的后验概率分布的协方差 计算待定位节点i的观测残差 以及观测残差的协方差

[0173] 计算公式如下：

[0174]

[0175]

[0176] 其中，观测矩阵H定义为：

[0177]

[0178] 然后、利用待定位节点i在时刻t状态的预测协方差和观测矩阵H计算卡尔曼增益；

[0179] 卡尔曼增益表示为：

[0180]

[0181] 最后、利用卡尔曼增益，计算待定位节点i在时刻t的位置随机变量的后验概率分布 更新后的均值 与更新后的协方差

[0182]

[0183]

[0184] 至此，待定位节点i的计算单元更新了在时刻t待定位节点i的位置后验概率分布并将其传输至待定位节点i的第一存储单元。

[0185] 步骤605、各待定位节点利用各自在时刻t更新的位置随机向量的后验概率分布，分别估计各自的位置，实现分布式无线协作定位。

[0186] 节点i在时刻t位置随机向量的估计值 表示成：

[0187]

[0188] 最后、将节点i在时刻t位置随机向量的估计值 作为节点i在时刻t的最终位置。