>

## The GraphSLAM algorithm with applications to large scale mapping of urban structures: Part 3

### 前言

GraphSLAM 的数学推导分为以下几步：

• 推导出一个关于 SLAM 全后验概率的递归公式
• 对每一个后验概率进行单独分析其对 SLAM 问题的影响
• 推导回复机器人位姿和地图特征点必要的公式

### SLAM 后验概率

\begin{aligned} y_{0:t} &= [x_0, x_1, ..., x_t, m]^T \\ y_t &= [x_t, m]^T \\ \end{aligned}


$$p(y_{0:t} | z_{1:t}, u_{1:t}, c_{1:t})$$


• $z_{1:t}$ 是观测值
• $c_{1:t}$ 是观测值观测到特征点的序号
• $u_{1:t}$ 是控制输入量

$$p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) = \eta p(z_t | y_{0:t}, z_{1:t-1}, u_{1:t}, c_{1:t}) p(y_{0:t}| z_{1:t-1}, u_{1:t}, c_{1:t})$$


$$p(z_t | y_{0:t}, z_{1:t-1}, u_{1:t}, c_{1:t}) = p(z_t | y_t, c_t)$$


\begin{aligned} p(y_{0:t}| z_{1:t-1}, u_{1:t}, c_{1:t}) &= p(x_t | y_{0:t-1}, z_{1:t-1}, u_{1:t}, c_{1:t}) p(y_{0:t-1}| z_{1:t-1}, u_{1:t}, c_{1:t})\\ &= p(x_t|x_{t-1}, u_t)p(y_{0:t-1}| z_{1:t-1}, u_{1:t}, c_{1:t}) \end{aligned}


$$p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) = \eta p(z_t | y_t, c_t) p(x_t|x_{t-1}, u_t)p(y_{0:t-1}| z_{1:t-1}, u_{1:t}, c_{1:t})$$


\begin{aligned} p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) &= \eta p(y_0)\prod_tp(x_t|x_{t-1}, u_t)p(z_t| y_t, c_t) \\ &= \eta p(y_0)\prod_t \Big[p(x_t|x_{t-1}, u_t)\prod_ip(z_t^i| y_t, c_t^i)\Big] \end{aligned}


### 负对数后验概率

$$\log{p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t})} = \text{const.} + \log{p(x_0)} + \sum_t\Big[\log{p(x_t|x_{t-1}, u_t)} + \sum_i\log{p(z_t^i| y_t, c_t^i)} \Big]$$


• 机器人运动模型的概率服从 $\mathcal{N}(g(u_t, x_{t-1}), R_t)$，其中 $g$ 是运动模型函数，是确定性的；$R_t$ 则是运动误差的协方差矩阵
• 观测模型服从 $\mathcal{N}(h(x_t, c_t^i, i), W_t)$，其中 $h$ 是观测模型函数，是确定性的；$Q_t$ 则是观测误差的协方差矩阵

\begin{aligned} p(x_t|x_{t-1}, u_t) &= \eta \exp\Big\{-\frac{1}{2}(x_t - g(u_t, x_{t-1}))^TR_t^{-1}(x_t - g(u_t, x_{t01}))\Big\} \\ p(z_t^i|y_t, c_t^i) &= \eta \exp\Big\{-\frac{1}{2}(z_t^i - h(y_t, c_t^i, i))^TQ_t^{-1}(z_t^i - h(y_t, c_t^i, i))\Big\} \end{aligned}


$$p(x_0) = \eta \exp \Big\{ -\frac{1}{2}x_0^T\Omega_0x_0\Big\}$$


\begin{aligned} x_0 &= \begin{bmatrix} 0 & 0 & 0 \end{bmatrix}^T \\ \\ \Omega_0 &= \begin{bmatrix} \infty & 0 & 0 \\ 0 & \infty & 0 \\ 0 & 0 & \infty \end{bmatrix} \end{aligned}


\begin{aligned} -\log{p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t})} = \text{const.} + \frac{1}{2} \Big[ x_0^T\Omega_0x_0 &+ \sum_t(x_t - g(u_t, x_{t-1}))^TR_t^{-1}(x_t - g(u_t, x_{t01})) \\ +& \sum_t\sum_i(z_t^i - h(y_t, c_t^i, i))^TQ_t^{-1}(z_t^i - h(y_t, c_t^i, i))\Big] \end{aligned}


### 泰勒展开

\begin{aligned} g(u_t, x_{t - 1}) &\approx g(u_t, \mu_{t-1}) + \underbrace{g'(u_t, \mu_{t-1})}_{=: G_t}(x_{t -1 - \mu_{t-1}})\\ h(y_t, c_t^i, i) &\approx h(\mu_t, c_t^i, i) + \underbrace{h'(\mu_t)}_{=:H_t^i}(y_t - \mu_t) \end{aligned}


\begin{aligned} \log{p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t})} = \text{const.} - \frac{1}{2} \Big[ x_0^T\Omega_0x_0 &+ \sum_t(x_t - g(u_t, \mu_{t-1}) - G_t(x_{t-1-\mu_{t-1}}))^TR_t^{-1}(x_t - g(u_t, \mu_{t-1}) - G_t(x_{t-1-\mu_{t-1}})) \\ +& \sum_i(z_t^i - h(\mu_t, c_t^i, i) - H_t^i(y_t - \mu_t))^TQ_t^{-1}(z_t^i - h(\mu_t, c_t^i, i) - H_t^i(y_t - \mu_t))\Big] \end{aligned}


\begin{aligned} \log{p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t})} &= \text{const.} - \frac{1}{2} \underbrace{x_0^T\Omega_0x_0}_{\text{quadratic in }x_0} \\ &- \frac{1}{2} \sum_t \underbrace{x_{t-1:t}^T\begin{bmatrix} 1 \\ -G_t \end{bmatrix} R_t^{-1}\begin{bmatrix} 1 & -G_t \end{bmatrix}x_{t-1:t}}_{\text{quadratic in } x_{t-1:t}} \\ &+ \underbrace{x_{t-1:t}^T\begin{bmatrix} 1 \\ -G_t \end{bmatrix}R_t^{-1}\begin{bmatrix} g(u_t, \mu_{t-1}) + G_t & \mu_{t-1} \end{bmatrix}}_{\text{linear in } x_{t-1:t}} \\ &- \frac{1}{2} \sum_i \underbrace{y_t^TH_t^{iT}Q_t^{-1}H_t^iy_t}_{\text{quadratic in } y_t}\\ &+ \underbrace{y_t^TH_t^{iT}Q_t^{-1}\Big[z_t^i - h(\mu_t, c_t^i, i) - H_t^i\mu_t\Big]}_{\text{linear in } y_t} \end{aligned}


$$\log{p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t})} = \text{const.} - \frac{1}{2} y_{0:t}^T\Omega y_{0:t} + y_{0:t}^T\xi$$


### 构建信息矩阵

$$\Omega \leftarrow \Omega_0$$


\begin{aligned} \Omega &\leftarrow \Omega + \begin{bmatrix} 1 \\ -G_t \end{bmatrix}R_t^{-1}\begin{bmatrix} 1 & -G_t \end{bmatrix} \\ \xi &\leftarrow \xi + \begin{bmatrix} 1 \\ -G_t \end{bmatrix}R_t^{-1}\begin{bmatrix} g(u_t, \mu_{t-1}) + G_t & \mu_{t-1} \end{bmatrix} \end{aligned}


\begin{aligned} \Omega &\leftarrow \Omega + H_t^{iT}Q_t^{-1}H_t^i \\ \xi &\leftarrow H_t^{iT}Q_t^{-1}\begin{bmatrix} z_t^i - h(\mu_t, c_t^i, i) - H_t^i\mu_t \end{bmatrix} \end{aligned}


### 信息矩阵简化

$$p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) = p(x_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) p(m | z_{1:t}, y_{1:t}, c_{1:t})$$


$$p(x_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) = \int p(y_{0:t} | z_{1:t}, y_{1:t}, c_{1:t}) dm$$


\begin{aligned} \Omega &= \begin{bmatrix} \Omega_{x_{0:t}, x_{0:t}} & \Omega_{x_{0:t}, m} \\ \Omega_{m, x_{0:t}} & \Omega_{m, m} \end{bmatrix} \\ \xi &= \begin{bmatrix} \xi_{x_{0:t}} \\ \xi_m \end{bmatrix} \end{aligned}


\begin{aligned} \tilde{\Omega} &= \Omega_{x_{0:t}, x_{0:t}} - \Omega_{x_{0:t}, m}\Omega_{m, m}^{-1}\Omega_{m, x_{0:t}} \\ \tilde{\xi} &= \xi_{x_{0:t}} - \Omega_{x_{0:t}, m}\Omega_{m, m}^{-1}\xi_m \end{aligned}


$$\Omega_{m, m}^{-1} = \sum_jF_j^T\Omega_{j,j}^{-1}F_j$$


$$F_j = \begin{bmatrix} 0\cdots0 & 1 \quad 0 & 0\cdots 0 \\ 0\cdots0 & \underbrace{0 \quad 1}_{j\text{-th feature}} & 0\cdots 0 \end{bmatrix}$$


\begin{aligned} \tilde{\Omega} &= \Omega_{x_{0:t}, x_{0:t}} - \sum_j\Omega_{x_{0:t}, j}\Omega_{j, j}^{-1}\Omega_{j, x_{0:t}} \\ \tilde{\xi} &= \xi_{x_{0:t}} - \sum_j\Omega_{x_{0:t}, j}\Omega_{j, j}^{-1}\xi_j \end{aligned}


### 求解机器人路径和地图

\begin{aligned} \tilde{\Sigma} &= \tilde{\Omega}^{-1}\\ \tilde{\mu} &= \tilde{\Sigma}\tilde{\xi} \end{aligned}


\begin{aligned} \Sigma_m &= \Omega_{m,m}^{-1} \\ \mu_m &= \Sigma_m(\xi_m + \Omega_{m, x_{0:t}}\tilde{\xi}) \end{aligned}


$$p(m | z_{1:t}, y_{1:t}, c_{1:t}) = \prod_jp(m_j | z_{1:t}, y_{1:t}, c_{1:t})$$


\begin{aligned} \Sigma_j &= \Omega_{j,j}^{-1} \\ \mu_j &= \Sigma_j(\xi_j + \Omega_{j, x_{0:t}}\tilde{\mu}) = \Sigma_j(\xi_j + \Omega_{j, \tau(j)}\tilde{\mu}_{\tau(j)}) \end{aligned}


### 结语

Built with Hugo
Theme Stack designed by Jimmy