SAM推导(2)

Micheal Kaess,Ananth Ranganathan,Frank Dellaert, iSAM:Incremental Smoothing and Mapping,IEEE

QR分解和Cholesky分解

  之前的推导将SLAM问题的最大后验估计通过取负对数，转化成最小二乘问题，后来又通过线性化转换成线性最小二乘问题，整理变形后得到下面的形式$${\theta }^{\ast }=arg\underset{\theta }{\max }\Vert A\theta -b{\Vert }_2^2$$如果采用Cholesky分解，就是对上式求导，令导数为0，得到$A^{T}A\theta =A^{T}b$,对$A^{T}A$进行Cholesky分解。这样求解，会对矩阵条件数$A^{T}A$进行开方。
  为了避免开方，采用对量测雅可比矩阵$A$进行QR分解，即$$A=Q\left[\begin{array}{c}R\\ 0\end{array}\right]$$其中，$R\in {\mathbb{R}}^{n\times n}$是上三角平方根信息矩阵(存在$R^{T}R=A^{T}A$)，$Q\in {\mathbb{R}}^{m\times m}$是正交矩阵。将这个QR分解代入上面的最小二乘问题，得到$$\begin{array}{rl}\Vert A\theta -b{\Vert }_2^2=& \Vert Q\left[\begin{array}{c}R\\ 0\end{array}\right]\theta -b{\Vert }_2^2\\ =& \Vert Q^{T}Q\left[\begin{array}{c}R\\ 0\end{array}\right]\theta -Q^{T}b{\Vert }_2^2\\ =& \Vert \left[\begin{array}{c}R\\ 0\end{array}\right]\theta -\left[\begin{array}{c}d\\ e\end{array}\right]{\Vert }_2^2\\ =& \Vert R\theta -d{\Vert }^2+\Vert e{\Vert }^2\end{array}$$其中，$[d,e{]}^T=Q^{T}b$，$d\in {\mathbb{R}}^n$，$e\in {\mathbb{R}}^{m-n}$。对上式取最小值，当且仅当$R\theta =d$时上式取得最小值$\Vert e{\Vert }^2$。
  通过QR分解将最小二乘问题转化成具有唯一解的线性问题$R{\theta }^{\ast }=d$。

Incremental Smoothing and Mapping

  矩阵$A$的QR分解可以使用Givens Rotations将对角线下的元素转化成0，从而得到上三角矩阵$R$，不断左乘的正交矩阵或者说Givens Rotations的累乘得到矩阵$Q$，$Q$一般是稠密的。
  当新的量测到来时，比起更新、再分解矩阵$A$，通过QR-updating直接修正之前的分解是更有效的。假设增加一行新的量测$$\left[\begin{array}{cc}{Q}^T& \\ & 1\end{array}\right]\left[\begin{array}{c}A\\ w^T\end{array}\right]=\left[\begin{array}{c}R\\ w^T\end{array}\right],newRHS:\left[\begin{array}{c}d\\ \gamma \end{array}\right]$$其中，$A=QR\to Q^{T}A=Q^{T}QR=R$，$w^T$是新增的量测，$\gamma$是新增的RHS。对于上式加入一行新的量测后，仍然可以等式两端左乘Givens Rotations，将矩阵$R$转化为上三角矩阵，这样就是实现了新的矩阵$A^{\prime }$的QR分解。