Review of Factor Graphs for Robust GNSS

Cite : Das S, Watson R, Gross J. Review of factor graphs for robust GNSS applications. arxiv preprint arxiv:2112.07794. 2021 Dec 14.

本文是该文的翻译版适合大概的因子图综述阅读

什么是因子图

概率建模是任何一种估计问题的一个非常重要的工具。这导致了图形模型[1]的发展，该模型使用图中的顶点对系统的已知变量和未知变量进行建模，并定义它们之间的边来表示它们之间的关系。由于系统建模中固有的不确定性，这些关系是概率性的。

一个因子图连接了这些图形化的许多类型的模型，如马尔可夫随机场，贝叶斯网络，坦纳图[2]。因子图的主要动机是将许多变量的全局函数表示为具有变量的较小子集的局部函数的乘积。因子图不是一种方法，而是一种使用其局部性结构建模任何系统的框架，即每个变量只依赖于其他几个局部变量，并且独立于其他局部变量。正如在[3]中所解释的那样，正是这种局部性特性使它在建模各种问题中非常有用，包括映射、视觉惯性测程法、运动规划、轨迹估计和深度学习。因子图被定义为一个二部图有两种类型的顶点，一个是变量（即状态向量）顶点是估计和另一个是因子顶点编码约束（例如，一组GNSS观察）应用于变量的顶点。一条边只能存在于因子顶点和变量顶点之间。因子顶点表示局部函数，它依赖于与它的变量顶点共享它的边。

机器人技术中一个常见的估计问题是利用因子图框架来估计未知机器人姿态以及根据问题的其他参数。这是通过求解最大后验（MAP）问题来实现的，该问题使状态和测量值之间的概率约束因子的乘积最大化。 [4]给出了一个严格的数学描述，如何解决这个最大化问题在机器人感知领域。

因子图已被视为解决估计问题的替代框架，并已被证明是非常有效的特定应用，在克隆同步定位和映射（SLAM）。由于该方法的灵活性和通过提供GTSAM [5]、g2o [6]、Ceres [7]等开源图优化库而易于实现，该框架得到了广泛的使用。虽然因子图框架已被证明对许多应用程序是有益的，但应该注意的是，框架可以等同于（即，它是一种遗传化）现有的状态估计实现（例如，卡尔曼滤波器和它的许多变体）。为了开始这个比较，我们将注意到因子图最终编码了一个目标函数，该函数通过非线性优化程序（例如，高斯-牛顿，莱夫-恩伯格-马夸特）重复重新线性化来求解。先前的工作已经证明，对扩展卡尔曼滤波的测量更新迭代和迭代之间的系统模型等价于高斯-牛顿优化[8]。结果表明，在一定的约束条件下，批处理模式下的因子图等价于反向平滑扩展卡尔曼滤波器（EKF）[9]，使系统和观测模型重新线性化并迭代。

使用因子图有什么优势

因子图优化比标准的非迭代卡尔曼滤波器具有一些优势，对某些应用具有一定的价值。

首先，像任何优化问题一样，它使用多次迭代来最小化成本，而不是像标准卡尔曼滤波器那样对每个状态只进行一次迭代。其次，它还将标准卡尔曼滤波器的非线性测量模型线性化，而不是用标准卡尔曼滤波器进行的单线性化。因子图也被证明可以更好地利用过去和当前时代之间的时间相关性，这被归因于估计方法的批处理性质。特别是，当在批处理模式下操作时，一个因子图将相当于一个前向滤波器和向后平滑器。

对于GNSS/INS应用，这些好处得到了[10]实验结果的支持，其中因子图已被证明在城市环境中比EKF表现更好。随着较长时间内新测量值的积累，批估计可能会失去实时性能。滑动窗口方法也可以使用类似于[11]来减轻计算成本。窗口大小已被发现对良好的优化结果至关重要，并可取决于环境条件[10]。因子图通过利用雅可比矩阵和信息矩阵的稀疏性来实现有效的计算。这有助于快速的矩阵分解和反替换。

直接从图形中删除早期的姿态可能会导致信息丢失。这可以避免使用平方根信息形式中的边缘化来删除变量从贝叶斯网络从因子图使用消除算法。 由于图中的稀疏性，增量QR分解也可以有效地实现[5]。除了固定滞后平滑之外，isam2公式[12]在添加一个新的约束时，通过将因子图转换为贝叶斯树[13]来实现实时性能。对于高度非线性的测量模型，这是一种更精确的增量和平滑的方法。贝叶斯树的顶点代表了在因子分解过程中由因子图得到的贝叶斯网中的团系。只有包含在新约束中的状态和贝叶斯树中的状态以及它们在贝叶斯树中的前一个状态需要更新。Watson和Gross [14]在GNSS因子图中使用了isam2，显示出比传统的EKF-精确点定位（PPP）方法有更好的定位性能。Wen等人[15]将因子图优化应用于GNSS和GNSS-实时运动学（RTK）定位问题，显示出比EKF更好的性能。

因子图如何运用于GNSS

在[14]中详细描述了用GNSS观察结果产生的因素。在GNSS因子图中通常估计的状态是重新收发器的位置、对流层延迟、载波相位偏置和接收机时钟偏置。图1提供了GNSS因子图的可视化表示，其中ψ表示状态和测量值之间可能存在的任何概率约束。

在这个特定的实现中，ψ是对每个状态的先验信念，这取决于特定的数据集和环境心理特征。$ψ^p$是沿轨迹的两个连续状态之间的运动约束，例如，可以合并来自惯性测量单元（IMU）或车轮里程表的运动数据。在GNSS/IMU导航中使用的$ψ^b$的一个常见例子是使用IMU预积分[16]来计算两个因子图位置之间的位移，它们之间有多个IMU测量值的积分。最后，在一个状态和从该状态感知到的测量值之间的$ψ^m$测量约束，例如GNSS伪距或载波相位测量值。为了找到GNSS因子图的MAP估计值，我们可以找到使因子乘积最大化的状态集。 然而，在实践中，这个优化问题可以大大简化采用高斯噪声假设，使问题的转换最大化因素的乘积非线性最小二乘问题的每个组件是一个马氏成本，代表归一化残差的平方和，如等式提供1，其中f（∗）在不同时期的状态映射和h（∗）之间的映射是从状态空间到观测空间的映射。

$\begin{aligned}\hat{X}&=\underset{x}{\operatorname*{argmin}}\left[\sum_{i=1}^I\left\|\psi_i^p\right\|_\Sigma^2+\sum_{j=1}^J\left\|\psi_j^b\right\|_\Lambda^2+\sum_{k=1}^K\left\|\psi_k^m\right\|_\Xi^2\right]\\&=\underset{x}{\operatorname*{argmin}}\left[\sum_{i=1}^I\left\|x_0-x_i\right\|_\Sigma^2+\sum_{j=1}^J\left\|x_j-f_j(x_{j-1})\right\|_\Lambda^2+\sum_{k=1}^K\left\|y_k-h_k\left(x_k\right)\right\|_\Xi^2\right]\\&&\text{(1)}\end{aligned}$

在GNSS中使用因子图进行鲁棒估计的方法是什么？

如前所述，因子图框架也便于添加现有的和新的鲁棒估计方法，有助于减少欺骗攻击时的定位误差或多路径或大气效应的大噪声。下面的讨论列举了一些应用于GNSS的健壮方法。桑德豪夫等人[17]定义了开关约束（SC），这是一种提升优化[18]方法，定义了观察加权函数$Ψ（）$，它是开关变量的函数，与感兴趣的状态参数一起估计。SC方法最初是用于SLAM中的鲁棒环闭合检测的，然后扩展到GNSS用于多路径缓解[19]。当使用开关约束时，伪距因子代价函数表示为预测和实际测量之间的马氏成本的比例版本。 \[ \left\|\mathbf{e}_k^\mathrm{switch}\right\|_{\boldsymbol{\Sigma}_k}^2=\left\|\Psi\left(s_k\right)\cdot\left(y_k-h_k(x_k)\right)\right\|_{\boldsymbol{\Sigma}_k}^2 \] 其中，函数 $Ψ$ 是开关变量的线性函数。为这些开关变量添加先验因子，以阻止优化使所有$s_k$变成零。还可以添加一个过渡因子来模拟$s_{k−1}$和在下一个时间步观测到的同一卫星之间的变化。这些开关功能有助于自动减轻错误测量（例如，可疑的多路径测量），并被认为比计算昂贵的射线追踪方法[19]表现更好。在[20]中推导出了SC的一个扩展，称为动态协方差尺度（DCS），其中开关变量从优化方法中提取，并使用残差、电流测量不确定度和先验开关不确定度分别进行计算。在计算$s_k$后，与GNSS观测因子相关的信息矩阵用$ Ψ(s_k)^2$进行缩放。 最大混合（MM）[21]也使用高斯混合模型（GMM）来解决假循环闭包，但不是使用多模态不确定模型时不适合最大似然的和算子，目标函数转换为使用最大算子，如等式所示 3 \[ p\left(y_i\mid x\right)=\max_kw_kN\left(\mu_k,\Lambda_k^{-1}\right) \] SC、DSC和MM的好处已经在[22,23]中评估了具有真实世界数据的GNSS因子图应用。这两项研究都表明，在对退化的GNSS观测值进行优化时，通过使用鲁棒估计技术，可以实现实质性的定位改进。

为了扩展最大混合工作，Watson等人[24]提出基于观测残差的聚类在运行时学习GMM。最初，这项工作是是在一个批处理框架中实现的.然而，后来通过一种逐步合并gmm的有效方法，它被扩展到增量[25]工作，通过一种有效的方法来逐步合并gmm。

m-估计器[26]最近也在GNSS框架中以批处理形式的[27]进行了测试，并发现它比非鲁棒估计器性能更好。m-估计器假设有一个不同于平方损失函数的损失函数。平方损失函数对离群值高度敏感，因为它在较大的残差值下积极增长。因此，引入了一组损失函数，它们的增长侵略性小于平方损失函数。Huber成本函数[28]，如等式中提供的4就是这样一个函数。 \[ \rho(z)=\begin{cases}z^2/2&|z|\leq\Delta\\\Delta|z|-\Delta^2/2&|z|>\Delta\end{cases} \] 当目标函数被修改为利用m-估计量时，优化问题的形式如等式中所示 5. \[ \hat{X}=\arg\min_x\sum_i\rho\left(\frac{r_i(x)}\sigma\right)\quad(5) \] 式中，$r_i (x)$ 为每次测量的残差，$σ$ 为尺度参数。

增加∆参数会使该函数更接近平方损失函数。方程5可以用加权最小二乘法迭代求解方法[27,26]。选择一个合适的∆参数并不简单，因为它依赖于测量噪声的统计量。阿加门诺尼等人[29]利用这样一个事实，即一些m估计量，如胡贝尔，柯西，拉普拉斯有一个对应的椭圆分布来估计期望最大化（EM）框架下的∆和状态。Barron [30]联合优化了计算机视觉应用程序的状态和参数。因子图在m估计器应用中提供了更大的灵活性，因为它不仅可以帮助降低当前测量的权重，而且还可以改变过去测量的权重。如果后来发现一些过去的测量值，那么它是一个异常值，而在卡尔曼滤波器中，过去的测量值的贡献不能以实时的方式改变。大多数图形操作时间化库还内置了使用健壮的成本函数的功能，这也很有帮助。

最后，Yang等人[31]导出的鲁棒估计技术结合了计算机视觉的两个著名思想，黑-朗加拉扬对偶性[32]和分级非凸性[33]，利用鲁棒代价函数迭代解决点云重构问题。根据黑-朗加拉扬对偶性，方程5可以重写为 \[ \hat{X},\mathbf{w}=\underset{x,w_i\in[0,1]}{\operatorname*{\arg\min}}\sum_i\left[w_ir_i^2\left(x\right)+\Phi_\rho\left(w_i\right)\right], \] 其中 $w_i$ 是该测量的权重，$Φ_ρ$ 是一个惩罚项，它取决于权重和稳健代价ρ。分级非凸性（GNC）是一种不面临局部极小值问题的非凸函数f最小化的方法。其思想是代替函数用$f_µ$ 替代f，$f_µ$的凸性由参数µ控制。优化从$f_µ$和$µ$的凸形式开始，迭代地改变，使非凸性增加。[31]将这两种方法结合起来，解决了方程5中的两个问题，

1)在优化鲁棒代价函数的同时避免局部最小值

2)将方程5转换为一个总是更容易求解的加权最小二乘问题。这个问题与GNSS的相似性对读者来说应该是显而易见的。[34]显示了在GNSS因子图中应用这种稳健估计技术在减轻城市峡谷多路径效应中的重要性。大多数这些鲁棒估计方法的批处理性质使它适合用于因子图，而不是EKF。

因子图对GNSS和无线电导航社区的潜在用途是什么？

EKF一直是基于GNSS的状态估计的首选，因为其简单性、计算效率，以及GNSS观测模型可以用线性近似很好地建模，并且经常通过高斯误差进行很好的描述。尽管如此，但在某些情况下，无线电导航社区可能可以受益于因子图优化。

首先，最近的工作强调了机会信号（SOP）在无线电导航应用中的潜在用途和好处。SOP可能包括作为手机信号的[35,36,37]和近地轨道卫星。sop的使用通常包括对解决一个未知的或非常不确定的发射机位置和时钟偏移量的需要。因此，这类问题与SLAM有许多相似之处，因此可以从使用因子图中获得类似的好处，如基于视觉或激光雷达的姿态图SLAM所公认的那样。

其次，正如在GNSS的背景下所讨论的，许多鲁棒估计技术已经被开发出来用于因子图。对于使用容易出现多路径错误的GNSS城市环境，使用这些鲁棒因子图可能是有益的。例如，谷歌智能手机解密器挑战的获胜解决方案，包括收集到不同环境设置的各种数据集，确实是一个因子图实现[38]。

最后，因子图的采用可能是有益的，因为因子图框架已经成为机器人和自治社区中的标准状态估计范式（即，除了GNSS之外，几乎所有的传感器模态都利用并显示了因子图框架的好处）。采用GNSS因子图可以使GNSS和其他传感器模式之间更无缝集成，多个信息源的集成是任何关键导航系统的关键。

其他相关问题

关于本文提出的反向平滑EKF [9]

OURNAL OFGUIDANCE, CONTROL,ANDDYNAMICS Vol. 28, No. 5, September–October 2005

推广了迭代扩展卡尔曼滤波器的原理，提出了一种新的在估计问题存在严重非线性时具有优越性能的滤波器。当非噪声可能会显著降低其他滤波器的精度或收敛可靠性时，新的滤波器是有用的。该新的滤波器利用迭代数值技术解决了当前和过去样本区间的非线性平滑问题。这种方法保留了在感兴趣的阶段之前的固定数量的阶段的非线性，并以一种近似的方式处理来自早期阶段的信息。该算法已经在一个困难的航天器姿态估计问题上进行了仿真测试，该问题包括少于三个轴的感知和显著的动态模型不确定性。该滤波器通过同时估计惯性矩参数来补偿这种不确定性。对于初始姿态或姿态率误差较大的估计问题，该滤波器和无迹卡尔曼滤波器具有更好的收敛可靠性和精度。

反向平滑扩展卡尔曼滤波器（BSEKF）比其他过滤器更直接地处理更多的非线性。EKF使用了非线性动力学和测量函数的一项或两项泰勒级数近似。展开是对后验或先验均值展开的，以可用的为准。利用这些展开式和线性卡尔曼方程动态传播，更新状态状态及其估计误差协方差。1 IEKF试图通过改变一个关于一个基于先前线性化的后验状态估计来改进其测量方程的线性近似。在IEKF的一个选项中，可以实现几种重新线性化。1 UKF通过非线性动力学和测度函数传播点的分布。这些点被称为西格玛点，它们被用来估计非线性函数输出分布的高斯近似值的均值和协方差。如果你要注意如何选择UKF的西格玛点，那么它将保留二阶非线性效应，这些效应被EKF，2,3的某些细节所遗漏，这就是为什么与许多EKF相比，它可以提高收敛性和准确性。

BSEKF改进的改进来自于它处理现在和过去的动力学和测量函数的非线性的方法。该方法是对IEKF原理的一种扩展。其想法是重新线化化当前和过去的测量函数，以及当前和过去的动态函数，以改进对当前和过去状态和过程噪声向量的猜测。在一个结束的时间间隔内，通过迭代的非线性平滑来选择适当的再线性化点在当前的采样时间内。这个过程产生了一个状态估计算法，该算法处理多个阶段上的所有非线性，而没有任何近似。

BSEKF在本质上与参考文献中描述的过滤器参考文献4. 目前的公式的主要区别是，它适用于一般的离散时间问题，而参考文献4的滤波器直接工作于为特定应用程序导出的采样数据连续时间问题。目前的离散时间形式简化了数学如下：它既消除了指定状态向量的确定性和非确定性部分的需要，也消除了开发不包括过程噪声向量的联合概率分布的需要。

BSEKF可以被归类为一种特殊的因果批处理过滤器。一个仔细的研究人员可以从文献中关于批过滤的各种想法中综合BSEKF方法， 但没有一个参考文献将所有必要的组件组合在一个地方。在参考文献5中，使用移动窗口批处理过滤器nonlin- ear高斯-牛顿更新是提到执行因果非线性滤波，但公式不包括过程噪声，也没有解释重要细节如何最好地将数据从批处理窗口的影响。在参考文献6提到了一个具有类似缺点的移动窗口批处理过滤器，但给出的细节更少。在参考文献7定义了一种解决排除过程噪声的离散时间批估计问题的方法，但没有提到移动窗口实现作为因果非线性滤波的手段。在参考文献1，概述了非线性最大后验（MAP）平滑及其与动态规划的关系，但没有给出算法的解，也没有讨论用于因果滤波的移动窗口实现。

本文没有将BSEKF与使用蒙特卡罗技术的粒子滤波（PF）方法进行比较。理论上，粒子滤波器能够计算出比BSEKF或任何竞争的非线性滤波器更好的估计，但由于在状态空间中选择相关点的差异，它对具有挑战性的问题有很大的计算代价。BSEKF在精神上更类似于EKF、IEKF和UKF，因为每个算法都是一种使用有界计算数的近似方法。将这种方法与在无限次计算的极限下精确的方法进行比较似乎没有什么意义。另一方面，对于极端非线性问题，一个有用的方法可能是将BSEKF和PF技术技术结合起来。前者可用于指导粒子（状态空间中的点）的选择，以有效地实现后者。本文的其余部分分为三个主要部分，以及一些结论。在第二部分，非线性离散时间滤波问题被定义为一个地图估计问题，以及EKF、IEKF和UKF如何使用近似来解决它的各个方面谈论。本文描述了BSEKF如何比任何其他任何过滤器更少地逼近MAP问题，并详细描述了如何实现该算法。在摘要给出了一个同时进行参数计算的姿态估计问题。IV部分是BSEKF、EKF和UKF滤波结果。本文的结论在V.部分