SAM推导

参考文献

Frank Dellaert，Michael Kaess，The International Journal of Robotics Research，2006，Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing

模型

SLAM问题可以表示成置信网(belief net)，因子图(factor graph)，马尔可夫随机场(markov random field)。首先从置信网(也就是贝叶斯网络)的角度分析。
假设第$i$时刻机器人的状态为$x_i$，$i\in 0$$\dots$$M$，路标为$l_j$，$j\in 1$$\dots$$N$，量测为$z_k$，$k\in 1$$\dots$$K$。
对应上图的联合概率密度模型为$$P(X,L,Z,U)=P(x_0)\prod _{i=1}^{M}P(x_i|x_{i-1},u_i)\prod _{k=1}^{K}P(z_k|x_{ik},l_{jk})$$其中，$P(x_0)$表示初始状态，$P(x_i|x_{i-1},u_i)$表示运动模型，$P(z|x,l)$表示路标量测模型。引入标准的高斯模型，$$x_i=f_i(x_{i-1},u_i)+w_i⟺P(x_i|x_{i-1},u_i)\propto exp(-\frac{1}{2}\Vert f_i(x_{i-1},u_i)-x_i){\Vert }_{{\Lambda }_i}^2)$$其中$f_i()$是一个process model，$w_i$是零均值高斯白噪声，协方差为${\Lambda }_i$。量测模型为$$z_k=h_k(x_{ik},l_{jk})+v_k⟺P(z_k|x_{ik},ljk)\propto exp(-\frac{1}{2}\Vert h_k(x_{ik},l_{jk})-z_k{\Vert }_{{\Sigma }_k}^2)$$其中$h_k()$是观测方程，$v_k$同样是零均值高斯白噪声，协方差为${\Sigma }_k$。$\Vert e{\Vert }_{\Sigma }^2=e^T{\Sigma }^{-1}e$定义为平方马氏距离。

最小二乘问题

将最大后验估计取负对数，转化成最小二乘问题$${\Theta }^{\ast }=\arg \underset{\Theta }{\max }P(X,L|Z)=\arg \underset{\Theta }{\max }P(X,L,Z)=\arg \underset{\Theta }{\min }-log(P(X,L|Z))$$代入前面的几个式子，将$P(x_0)$视为常数省略，得到非线性最小二乘问题如下：
Θ ∗ = arg ⁡ min ⁡ Θ { ∑ i = 1 M ∥ f i ( x i − 1 , u i ) − x i ∥ Λ i 2 + ∑ k = 1 K ∥ h k ( x i k , l j k ) − z k ∥ Σ k 2 } \Theta^* = \arg\min_{\Theta}\lbrace\sum_{i=1}^M {\Vert f_i(x_{i-1},u_i)-x_i\Vert ^2_{\Lambda_i}+\sum_{k=1}^K {\Vert h_k(x_{ik},l_{jk})-z_k}\Vert^2_{\Sigma_k}}\rbrace Θ∗=argΘmin​{i=1∑M​∥fi​(xi−1​,ui​)−xi​∥Λi​2​+k=1∑K​∥hk​(xik​,ljk​)−zk​∥Σk​2​}实际中一般加以线性化， f i ( x i − 1 , u i ) − x i ≈ { f i ( x i − 1 0 , u i ) + F i i − 1 δ x i − 1 } − { x i 0 + δ x i } = { F i i − 1 δ x i − 1 − δ x i } − a i f_i(x_{i-1},u_i) -x_i \approx \lbrace f_i(x^0_{i-1},u_i) + F^{i-1}_i\delta x_{i-1}\rbrace-\lbrace x^0_i+\delta x_i\rbrace = \lbrace F^{i-1}_i\delta x_{i-1} - \delta x_i\rbrace - a_i fi​(xi−1​,ui​)−xi​≈{fi​(xi−10​,ui​)+Fii−1​δxi−1​}−{xi0​+δxi​}={Fii−1​δxi−1​−δxi​}−ai​其中$a_i=x_i^0-f_i(x_{i-1}^0,u_i)$表示里程计预测误差，这里$u_i$是给定的，被认为是常数。而$F_i^{i-1}$是$f_i(.)$在线性化点$x_{i-1}^0$的雅可比矩阵，定义为$$F_i^{i-1}={\frac{\partial f_i(x_{i-1},u_i)}{\partial x_{i-1}}|}_{x_{i-1}^0}$$对于量测模型 h k ( x i k , l j k ) − z k ≈ { h k ( x i k 0 , l j k 0 ) + H k i k δ x i k + J k j k δ l j k } − z k = { H k i k δ x i k + J k j k δ l j k } − c k h_k(x_{ik},l_{jk}) - z_k \approx \lbrace h_k(x_{ik}^0,l_{jk}^0) + H_k^{ik}\delta x_{ik} + J_k^{jk}\delta l_{jk}\rbrace - z_k = \lbrace H_k^{ik} \delta x_{ik} + J_{k}^{jk} \delta l_{jk}\rbrace -c_k hk​(xik​,ljk​)−zk​≈{hk​(xik0​,ljk0​)+Hkik​δxik​+Jkjk​δljk​}−zk​={Hkik​δxik​+Jkjk​δljk​}−ck​其中$H_k^{ik}$和$J_k^{jk}$分别是$h_k(.)$在线性化点$(x_{ik}^0,l_{jk}^0)$处对$x_{ik}$和$l_{jk}$的雅可比矩阵，即$$H_k^{ik}={\frac{\partial h_k(x_{ik},l_{jk})}{\partial x_{ik}}|}_{(x_{ik}^0,l_{jk}^0)}$$$$J_k^{jk}={\frac{\partial h_k(x_{ik},l_{jk})}{\partial l_{jk}}|}_{(x_{ik}^0,l_{jk}^0)}$$而$c_k=z_k-h_k(x_{ik}^0,l_{jk}^0)$是量测误差。
那么非线性最小二乘问题就可以转化成线性最小二乘问题，即 δ ∗ = a r g min ⁡ δ { ∑ i = 1 M ∥ F i i − 1 δ x i − 1 + G i i δ x i − a i ∥ Λ i 2 + ∑ k = 1 K ∥ H k i k δ x i k + J k j k δ l j k − c k ∥ Σ k 2 } \delta^* = arg\min_\delta \lbrace\sum_{i=1}^M \Vert F_i^{i-1}\delta x_{i-1} + G_i^i\delta x_i -a_i\Vert _{\Lambda_i}^2 + \sum_{k=1}^K\Vert H_k^{ik}\delta x_{ik} + J_k^{jk}\delta l_{jk} -c_k\Vert ^2_{\Sigma_k}\rbrace δ∗=argδmin​{i=1∑M​∥Fii−1​δxi−1​+Gii​δxi​−ai​∥Λi​2​+k=1∑K​∥Hkik​δxik​+Jkjk​δljk​−ck​∥Σk​2​}其中，$G_i^i=-I_{d\ast d}$为单位矩阵，$d$是$x_i$的维度。

求解

由于$$\Vert e{\Vert }_{\Sigma }^2=e^T{\Sigma }^{-1}e=({\Sigma }^{-\frac{T}{2}}e{)}^T({\Sigma }^{-\frac{T}{2}}e)=\Vert {\Sigma }^{-\frac{T}{2}}e{\Vert }_2^2$$那么将马氏距离转化成几何的平方距离形式(二范数) δ ∗ = a r g min ⁡ δ { ∑ i = 1 M ∥ Λ i − T 2 ( F i i − 1 δ x i − 1 + G i i δ x i − a i ) ∥ 2 2 + ∑ k = 1 K ∥ Σ k − T 2 ( H k i k δ x i k + J k j k δ l j k − c k ) ∥ 2 2 } \delta^* = arg\min_{\delta} \lbrace\sum_{i=1}^M \Vert \Lambda_i^{-\frac T{2}}(F_i^{i-1}\delta x_{i-1} + G_i^i\delta x_i -a_i)\Vert _2^2 + \sum_{k=1}^K \Vert \Sigma_k^{-\frac T{2}}(H_k^{ik}\delta x_{ik} + J_k^{jk}\delta l_{jk} -c_k)\Vert _2^2\rbrace δ∗=argδmin​{i=1∑M​∥Λi−2T​​(Fii−1​δxi−1​+Gii​δxi​−ai​)∥22​+k=1∑K​∥Σk−2T​​(Hkik​δxik​+Jkjk​δljk​−ck​)∥22​}这其中，$F_i^{i-1}$、$H_k^{ik}$、$J_k^{jk}$都是雅可比矩阵，将所有的雅可比矩阵合并到一个矩阵$A$，而向量$a_i$、$c_k$放入RHS(right-hand side)向量$b$中，得到最小二乘问题的标准形式为$${\delta }^{\ast }=arg\underset{\delta }{\min }\Vert A\delta -b{\Vert }_2^2$$.其中，$A$可能维度增长到比较大，但是是保持稀疏的。向量$\delta \in R^n$包含所有的位姿和landmark变量，矩阵$A\in R^{m\times n}$是一个大但是稀疏的量测雅可比矩阵，$m$表示量测的数目，$b\in R^m$是RHS向量。
  对上面的式子取最小值，就是对$\Vert A\delta -b{\Vert }_2^2$求导，令导数为0，得到$A^{T}A\delta =A^{T}b$。可以通过对$A^{T}A$的Cholesky分解进行求解。
后续QR分解或者Cholesky分解，理解了再补充。