iSAM1论文推导学习--第二节QR部分

iSAM1论文链接

一.基于QR分解的增量平滑优化

A.一个SLAM的概率图模型

用下图的网络表示SLAM问题：

其中， $x_{i}$ 是机器人在时刻i的状态， $l_{j}$ 是地标j的位置， $u_{i}$ 是时刻i的控制输入， $z_{k}$ 是第k个地标测量。

所有变量和测量值的联合概率由以下公式表示：

其中 $P_{(x0)}$ 是初始状态上的先验值， $P(x_{i}|x_{i-1},u_{i})$ 是运动学模型，由控制输入ui参数化， $P(z_{k}|x_{ik},l_{jk})$ 是地标性测量模型。这里假设每个测量值 $z_{k}$ 都有已知的对应关系 $(i_{k},j_{k})$ 。

假设为高斯测量模型。则“过程模型”如下：

该方程描述了测程传感器或扫描匹配的过程，其中 $w_{i}$ 是正态分布的零均值过程噪声与协方差矩阵 $\Lambda _{i}$ 。

而高斯测量模型如下:

对机器人的地标传感器进行建模，其中 $v_{k}$ 是正态分布的零均值测量噪声与协方差 $\Gamma _{k}$ 。

B.把SLAM化为最小二乘问题

当执行平滑而不是滤波操作时，注意的是给定控制输入u和地标测量值z时，整个轨迹x和地标l的地图估计。轨迹和映射的映射估计 $X^{*}$ ， $L^{*}$ 是通过最小化来自（1）的联合概率的负对数得到的：

结合过程和测量模型，则有以下非线性最小二乘问题：

其中，这里使用符号 $||e||_{\Sigma }=e^{T}\Sigma ^{-1}e$ 代替平方马氏距离与协方差矩阵Σ

这里附录回顾了如何将测量函数线性化，并将非线性最小二乘目标函数（5）的所有分量收集成一个一般的最小二乘公式，遵循。通过泰勒展开式将（5）中的测量函数线性化，假设要么有一个很好的线性化点可用，或者正在进行一个非线性优化方法的一次迭代。在以上任何一种情况下，（5）中过程模型的一阶线性化给出为：

其中， $F_{i}^{i-1}$ 是过程模型 $f_{i(.)}$ 在线性化点 $x_{i-1}^{0}$ 处的雅可比矩阵，这里定义为：

并且， $a_{i}=x_{i}^{0}-f_{i}(x_{i-1}^{0},u_{i})$ 是测程预测误差(注意这里的ui是给出的，因此是常数)。而方程（5）中测量模型的一阶线性化如下：

这里， $H_{k}^{ik}$ 和 $J_{k}^{jk}$ 分别是测量函数 $h_{k(.)}$ 关于在线性化点 $(x_{ik}^{0},l_{jk}^{0})$ 上计算的 $x_{ik}$ 和 $l_{jk}$ 变化的雅可比矩阵:

并且 $c_{k}=z_{k}-h_{k}(x_{ik}^{0},l_{jk}^{0})$ 是测量预测误差。

分别使用线性化过程和测量模型（24）和（26），则非线性最小二乘问题（5）变成：

也就是说，这里得到了δθ中一个需要有效求解的线性最小二乘问题。为了避免以一种特殊的方式处理 $\delta x_{i}$ ，我们引入了矩阵 $G_{i}^{i}=-I_{d_{x}\times{d_{x} }$ ,

通过简单地改变变量，可以去掉协方差矩阵 $\Lambda _{i}$ 和 $\Gamma _{k}$ 。用 $\Lambda ^{-1/2}$ 作为Λ的矩阵平方根，可以将马氏范数重写如下:

也就是说，总是可以通过将每个项中的 $F_{i}^{i-1}$ 、 $G_{i}^{i}$ 和 $a_{i}$ 预乘来从（29）中消除Λi，同样从测量项中消除 $\Gamma _{k}$ 。对于标量测量，这仅仅意味着将每一项除以测量的标准差。下面假设已经这样做了，然后去掉马氏符号:

最后，将雅可比矩阵收集到一个大矩阵A，将向量 $a_{i}$ 和 $c_{k}$ 收集到一个右侧向量b，得到以下标准最小二乘问题:

为了简单起见，iSAM在附录之外去掉了δ·符号,则上式变成：

其中，向量θ∈ $R^{n}$ 包含所有的姿态和地标变量，矩阵A∈ $R^{m\times n }$ 是一个大但稀疏的测量雅可比矩阵，b∈ $R^{m}$ 是右侧(RHS)向量。

通过将 $||A_{\theta }-b||^{2}$ 导数设为0，将该稀疏最小二乘系统转化为普通的线性方程组，得到所谓的正规方程 $A^{T}A\theta =A^{T}b$ 。该方程系统可以用 $A^{T}A$ 的 Cholesky 分解来求解。

C.用QR分解法求解

将标准QR矩阵分解应用于测量雅可比矩阵A来求解最小二乘问题（6）。与Cholesky分解相比，这避免了必须用矩阵条件数的相关平方来计算信息矩阵 $A^{T}A$ 。测量值雅可比矩阵A的QR分解得到：

其中，R∈ $R^{n\times n}$ 为上三角平方根信息矩阵(注意，信息矩阵由 $R^{T}R=A^{T}A$ 给出)，Q∈ $R^{m\times m}$ 为正交矩阵。我们将这个因子分解应用于最小二乘问题（6）：

其中，定义 $[d,e]^{T}=Q^{T}b$ 与d∈ $R^{n}$ 和e∈ $R^{m-n}$ .（8）当且仅当Rθ=d时成为最小，留下第二项 $||e||^{2}$ 作为最小二乘问题的残差。因此，QR分解将最小二乘问题简化为具有单一唯一解 $\theta ^{*}$ 的线性系统：

求解这个方程组的大部分工作已经通过QR分解完成了，因为R是上三角的，所以可以使用简单的反替换。结果是基于所有测量条件的完整机器人轨迹和地图的最小二乘估计 $\theta ^{*}$ 。