A Belief Propagation Algorithm for Multipath-Based SLAM IEEE TWC2019阅读

Abstract

提出了一种基于无线信号与带几何特征的镜面反射的MPC（多径分量）结合的SLAM算法，在室内场景中，鲁棒的无线SLAM因为复杂的多径效应、MPC和特征的关联未知、特征的可见性差而具有很大的挑战性。在我们的方法中，平面上的镜面反射可以为我们提供virtual anchor(VA，虚拟节点)，也就是physical anchors(PA，物理节点)的镜像。而在一开始，这些VA和PA的位置很可能是未知的。我们把这个SLAM问题建模为贝叶斯模型，并用因子图表示，这样就可以使用belief propagation(BP)算法来进行有效的边缘化联合后验分布。由此而产生的BP-SLAM检测和PA相关的VA，然后同时估计移动体的时变的位置、VA的位置和PA的位置，从而使用MPC来提高物体定位的准确性。

Intro

在以前，多径分量经常被认为是无线SLAM的一个干扰因素，而当我们把MPC和本地的几何特征联系起来时这一干扰因素就变成了一个很好的辅助因素。本文中的信号是从移动agent传输到base station，也就是我们之前说过的PA。算法可以很好地应对恶劣的无线多径环境。虽然MPC分量可能由于很多不同的原因产生，比如镜面反射、散射和衍射，但是由于算法注重于和PA/VA对相关的MPC分量，所以只关心镜面反射的MPC分量，其余的MPC分量都被建模成干扰，顺便提一句，有一篇文章把和散射点相关的MPC分量用了起来(Multipath Assisted Positioning with Simultaneous Localization and Mapping)

基于特征的SLAM

这个算法是基于SLAM问题中的基于特征的思想而产生的，地图是由一堆特征组成的，而这些特征的位置随着时间而被逐渐精确地估计，在我们的模型中，这些特征就是被PA和VA给出来的。比较突出的基于特征的SLAM算法包括Extended Kalman filter SLAM (EKF-SLAM) , Rao-Blackwellized(RB)-SLAM (dubbed FastSLAM) , variational inference-based SLAM [21], [22], set-based SLAM，这些方法的主要原理是操作和MPC相关的估计变量，比如距离（和延迟成比例），到达角（AOA）或者是出发角（angles of departure，AOD），这些参数在预处理阶段从信号中得到估计，我们认为它们是SLAM方法的“measurements”，值得研究的是这些测量值和PA或者VA之间的数据联系方式（DA）
基于特征的SLAM和多物体追踪(MTT)有着紧密的关系，很多多物体追踪的思想也被融入了SLAM，比如是joint probabilistic DA(JPDA)滤波器，joint integrated probabilistic DA（JIPDA）滤波器，有一些现有的论文已经采用了JIPDA的方法，最近在MTT领域belief propagation的方法很流行。具体来讲，BP算法是基于因子图来描述多传感器的MTT问题，计算代价只是被检测物体的二次方，和传感器的个数是线性的。也有一些算法是基于RFS（随机有限集）并嵌入BP算法来做probabilistic DA的。最后要指出，我们的方法也和未知传感器位置情况下的多传感器物体贝叶斯追踪有关

文章的贡献和组织结构

基于BP的，贝叶斯检测和估计SLAM算法（简称BP-SLAM），算法用了概率DA和对于移动个体以及potential features（PF）的时序估计。每一个PF状态被一个二元的存在变量描述（？），并和一个同样被估计的probability of existence（专有名词？）相关联，概率数据联系和状态估计通过在因子图上运行BP来获得，因子图表示了SLAM问题的概率结构，BP方法利用条件统计的独立性来获得低复杂度和高的可扩展性，和前人的算法比起来，BP-SLAM假定每一步中agent状态和所有的特征状态是先验地独立的。
从纯距离的传感器上做SLAM是困难的，因为1)当新的特征被初始化时，PA或者VA的概率分布是环状的，所以无法很好地用高斯分布来表示；2)相对于传统的SLAM问题来说，数据关联（DA）是更困难的，因为PA和VA位于相当不同的位置，很可能产生几乎互相无关的测量结果。
主要的贡献如下 ：
1）为基于特征的SLAM建立了一个贝叶斯模型，使用从无线信号中提取出的MPC参数，把PA、VA的出现与消失和DA不确定性共同用概率的方式建模
2）基于该模型的因子图表示，建立了一个可扩展的 BP算法，用于估计移动agent的状态以及PA/VA的位置
BP-SLAM算法能够很好地运用方位信息(MPC的AOA，AOD)以及从惯性测量单元得到的信息。为了方便，假设PA和agent是时钟同步的，不过即使是非同步也有文章在处理，所以并不是一个多么强的假设。此外，我们假设初始信号分析阶段（产生测量）检测无线电信号中特征的概率是已知的；然而，也有文章在处理最初的概率未知的情况，所以这也不是一个很强的假设

无线信号和MPC参数

基于无线信号的SLAM将从接收信号中估计出的镜面MPC参数和在几何上期待的参数（比如距离，AOA，AOD）相关联。这些参数描述的是agent和PA或VA的相对位置。VA位置是PA关于平表面（比如墙）的镜像。对于每一个反射路径，从PA到agent的距离和VA到agent的距离是相等的。即使agent在动，只要PA和反射面是静止的，VA就是静止的。假设agent有着未知的时变位置$p_n\in {\mathcal{R}}^2$，J个PA有着可能未知的位置$a_1^{(j)}\in {\mathcal{R}}^2$，这里的J是提前知道的。和第j个PA相关的VA有$L_n^{(j)}-1$个，位置分别是$a_l^{(j)}$，我们也称所有的 PA和VA为特征，特征的个数，也就是$L_n^{(j)}$个，取决于agent的位置$p_n$，每一时刻，agent向PA发射一个无线信号s(t)，PA接收下这个信号，当然我们的算法也可以很容易改成PA发射，agent接收的类型，第j个PA接收到的基带信号可以用以下方式建模：

右侧的第一项表示$L_n^{(j)}$个有着复数幅度和延时的镜面MPC，这些MPC和特征相关。延时和距离是成正比的
，c是光速，第二项表示漫反射多径成分，会干涉镜面MPC分量。第三项是高斯噪声。再次强调，虽然我们的算法看起来很简单，但是可以扩展成时间不同步的，也可以扩展成MPC参数中包含AOA或AOD的。
对于每个时间点n和每个PA，一个参数的无线信道估计器处理这个信号，然后给出$M_N^j$个MPC参数估计以及相关的复数幅度（用来在channel estimator没有工作的时候估计MPC参数的方差），随后要把他们和镜面MPC集合关联起来，有可能一些镜面MPC没有被无线信道估计器检测到，所以没有产生MPC参数估计，也有可能一些估计不对应任何MPC，也就是说这两个集合的大小关系是未知的。

系统模型和统计公式

agent状态和PF状态

agent在时间n的状态是$x_n=[p_n,v_n]$，也就是位置和速度，potential feature用一个元组(j,k)来区分表示，j代表和第J个PA相关联，k是从每个PA具有的PF的个数取值的、注意这个PF的集合也包括PA自己，PA的个数是已知的，但是PF是未知的且随机的，每个PF是否在当前视野中存在用一个布尔变量r表示，PF的描述参数是位置a，加上描述子r，定义y=[a,r]为PF的状态向量，而不在视野中的PF的值是无关变量，因此如果对这种状态向量进行一个probability density function（PDF)来进行描述的话，要满足$f(a,0)=f_{k,n}\ast f_D(a)$,f_D是一个随机的假PDF，$f_{k,n}$可以看做是PF不存在的概率。另外，联合的增强PF状态表示，也可以用一个多伯努利的随机有限集来表示，但是RFS的无序特征会让我们的因子图和BP算法难度增加，所以不采用。
在任意一个时刻n，每个PF要么是一个遗留PF（过去已经看到过），或者是一个新的PF，首次建立，他们的状态一个用上波浪线标识，一个用下凹线标识。新的PF的个数和测量数是相等的，遗留PF的个数的更新方式为

实际上PF的增长量不到M，因为PF集要被修剪，也就是说低出现概率的会被丢弃掉
在时间n的新的PF的个数只有在当前测量完成后才能得到，第一次探测到的特征叫做newly detected features，在当前的测量被看到之前，关于NDF我们只有先验知识。
agent的状态$x_n$和已有PF的增强表达式应当是独立的，由于马尔科夫状态动力学

关联向量

对于每一个PA，它的测量服从一个不确定性，也称DA不确定性，也就是说我们不知道每个测量和哪个PF相对应，或者说一个测量和任何一个PF无关（也称false alarm或clutter），抑或说一个PF没有得到任何一个测量（也称missed detection），PF在参数估计阶段产生一个测量，也称被“detected”的概率用$P_d$表示，false alarm measurements的分布可以用PDF $f_{FA}$表示，这两个函数是预先知道的，我们认为每个时间点n，每个PF至多只能产生一个measurement，每个measurement至多只能被一个PF产生。
measurement和已有PF状态之间的关系可以用$K_{n-1}^{(j)}$维的面向特征的DA向量c表示，第k个分量的取值，如果已有PF产生了测量z，就定义成m，如果没有产生测量，就定义成0。另外我们考虑$M_n^{(j)}$维的面向特征的DA向量b，如果测量时有已有PF产生的，第m个分量定义成k，否则定义成0，这两个DA向量是未知的且被建模成随机的，它们是等价的，因为从一个就可以推出另一个，然而 这种冗余性正是使BP算法具有scalability的关键之所在，它有助于建立一个因子图，用于联合推断agent状态以及已有PFs和新PFs的状态

先验分布

对于一个给定的PA，在时间点n，有M个新的PF，我们认为false alarm和新检测到的特征数符合泊松分布，均值分别是${\mu }_{FA}$和${\mu }_{n,n}$，这样，只要给定agent的状态$x_n$以及legacy PF的增强向量，就可以写出来一个关于DA向量，r以及测量数（等同于新的PF）的联合条件先验概率质量函数

似然函数

为z和x与a建立起联系的似然函数

利用独立关系进行化简后变为

联合后验概率密度函数和因子图

排除约束的冗余公式

我们的BP-SLAM算法依赖于一个关于DA的冗余表示，为了得到一个概率描述，继而得到因子图，为了表示方便，令

随后把似然函数、先验PDf以及调整过的先验PMF乘在一起，得到

再整理一下就可以得到

联合后验PDF

用贝叶斯法则，和状态转移pdf，先验pdf和似然模型之间的独立性，可以写一个联合后验pdf，如下

将上个方程代入其中，这个方程可重写成

这样就可以用因子图来表示

五、BP-SLAM算法

我们的BP-SLAM算法对相关状态做了贝叶斯的检测和估计，后验分布通过因子图的方式进行了快速递归计算

检测与估计

目标是从总的测量向量中估计agent状态x_n，并检测与估计PF状态a。为了估计x，做一个最小均方误差（MMSE）的估计计算，
，估计器用到了后验pdf，另外确定在n时刻，PF是否能被探测到可以用一个后验PDF的边缘化来计算，如下

，如果这个结果大于一个概率阈值，我们认为就探测到了，最终关于a的状态估计就可以用这个表示：

为了优化，关于PF有个剪枝，如果概率没有超过某个剪枝阈值，就会被剪掉

信息传递算法

我们上面用到的后验pdf实际上是联合pdf
的边缘化，但是我们无法直接进行边缘化，所以用一个在因子图上的迭代的BP信息传递方式，因为因子图有环，得到的belief只是后验的一种估计，而且没有order的很好地表示，所以我们依据如下方式来挑选order：信息不会在时间上反向传播，迭代信息传递只对DA使用，对于连接着不同PA的环路只计算一次信息传递迭代，沿着连接代理状态变量节点和新PF状态变量节点的边缘，消息只从前者发送到后者
首先我们做一个估计的步骤，对于agent的状态的估计为（q代表belief）

对于已有PF的预测信息为：

然后把r=0和1时的f(a,r|a,r)分别代入，得到当r为1时

当r为0时

在估计之后，后续的计算将对所有的已有PF和所有新PF以及所有的PA来实施

1）对于已有PF的测量估计：
传递给面向特征的DA变量c的信息$\beta$可以用如下方式计算

2）对新PF的测量估计：
传递给面向测量的DA变量b的信息$\xi$可以用以下方式计算：

可以简化为：

3）迭代的数据关联：
现在有了$\beta$和$\xi$，我们可以用迭代BP的方式来求解$\eta$和$\iota$。
首先可以迭代求解$\nu$和$\zeta$

在迭代收敛后，$\eta$和$\iota$用如下方式计算：

4）对于agent的测量的更新：
有了$\eta$，${\alpha }_k()$和${\alpha }_{k,n}$之后，和agent有关的信息${\gamma }_k$可以这样计算

5）对于已有PF的测量更新：

6）对于新PF的测量更新：

在这些信息都得到之后，近似于目标的边缘后验PDF的beliefs就可以拿到了如下：

这里的q就给出了一个边缘后验pdf的估计，就被用作f(xn|z1:n)的替代品。
对于已有PF的增强状态的估计函数可以这样计算：

对于新PF的增强状态的估计函数可以这样计算：

整体的伪代码如下：

实验

在每个agent位置，agent发送一个超宽带信号，由两个静态PA接收。该信号使用频率范围为3-10GHz的M序列相关信道探测仪和方位面辐射方向图近似均匀、地板和天花板方向为零的天线进行测量。在测量频带内，实际信号频带由滚降系数为0.5、双边3db带宽为2GHz、中心频率为7GHz的升余弦脉冲响应滤波器选择。从测量的信号中，通过基于快照的SISO SAGE算法[28]导出了构成所提算法输入的距离测量值z，该算法提供了mpc延迟的估计值$\tau$和相关复振幅的估计值。在该方法中，每个PA 的估计mpc数固定为20,方差就是从复振幅中算出来的

Conclusion

提出了一种基于概率DA的无线信号SLAM算法。基本的系统模型描述了镜面反射MPCs的VAs的未知和可能的时变位置。为了解决DA问题，即MPCs与VAs之间的未知关联，我们在Bayesian框架下对包括概率DA在内的整个SLAM问题进行了建模。然后，我们用因子图表示联合后验分布的因子分解，并应用BP算法近似边缘化联合后验分布。这种方法允许将最初为MTT提出的概率DA的有效BP算法结合起来。我们的因子图扩展了[33]中新的潜在特征的状态。
仿真结果表明，该算法即使在强杂波和低检测概率的情况下，也具有较高的精度和鲁棒性。此外，在室内环境中使用真实的超宽带无线电信号进行的实验分析表明，BP-SLAM算法在现实世界的场景中表现同样良好；观察到的agent位置误差100%低于0.2m，90%的测量结果低于0.083m。未来的研究方向是进一步开发MPC参数，如AOA和AOD，包括附加类型的特征，如散射点，并重新定义特征作为扩展对象。最后，研究在非同步传感器网络和分布式（分散）操作模式下的操作将是有趣的。
他还给了个flowchart，就不贴上来了，见链接https://gitlab.com/erikleitinger/BP-Multipath-basedSLAM/-/blob/master/FlowChartAlgorithm.pdf