>

## 使用概率对 SLAM 问题建模

$$p(\boldsymbol{\xi}|\boldsymbol{r}) = \frac{p(\boldsymbol{r}|\boldsymbol{\xi})p(\boldsymbol{\xi})}{p(\boldsymbol{r})}$$


$$p(\boldsymbol{\xi}|\boldsymbol{r}) = \frac{p(\boldsymbol{r}|\boldsymbol{\xi})p(\boldsymbol{\xi})}{\int p(\boldsymbol{r}|\boldsymbol{\xi})p(\boldsymbol{\xi})d\boldsymbol{\xi}}$$


SLAM 要解决的问题是我们要估计出一个状态量 $\boldsymbol{\xi}^*$, 使得 $p(\boldsymbol{\xi}|\boldsymbol{r})$ 最大化，即求解以下式子：

$$\boldsymbol{\xi}^* = \arg\max_{\boldsymbol{\xi}}p(\boldsymbol{\xi}|\boldsymbol{r})$$


\begin{aligned} \boldsymbol{\xi}^* &= \arg\max_{\boldsymbol{\xi}}p(\boldsymbol{\xi}|\boldsymbol{r})\\ &= \arg\max_{\boldsymbol{\xi}}\frac{p(\boldsymbol{r}|\boldsymbol{\xi})p(\boldsymbol{\xi})}{p(\boldsymbol{r})}\\ &= \arg\max_{\boldsymbol{\xi}}p(\boldsymbol{r}|\boldsymbol{\xi})p(\boldsymbol{\xi})\\ &= \arg\max_{\boldsymbol{\xi}}(\log{p(\boldsymbol{r}|\boldsymbol{\xi})} + \log{p(\boldsymbol{\xi})})\ \end{aligned}


\begin{aligned} \boldsymbol{x} &\sim N(\boldsymbol{u}, \boldsymbol{\Sigma})\\ p(\boldsymbol{x}) &= \frac{1}{\sqrt{(2\pi)^N\det{\boldsymbol{\Sigma}}}}\exp{\left(-\frac{1}{2}(\boldsymbol{x}-\boldsymbol{\mu})^T\boldsymbol{\Sigma}^{-1}(\boldsymbol{x}-\boldsymbol{\mu})\right)} \end{aligned}


\begin{aligned} \boldsymbol{\xi}^* &= \arg\max_{\boldsymbol{\xi}}p(\boldsymbol{r}|\boldsymbol{\xi})p(\boldsymbol{\xi})\\ &= \arg\max_{\boldsymbol{\xi}}{\left(\exp{\left(-\frac{1}{2}(\boldsymbol{r}-\boldsymbol{\mu}_r)^T\boldsymbol{\Sigma}_r^{-1}(\boldsymbol{r}-\boldsymbol{\mu}_r)\right)}\exp{\left(-\frac{1}{2}(\boldsymbol{\xi}-\boldsymbol{\mu}_{\xi})^T\boldsymbol{\Sigma}_{\xi}^{-1}(\boldsymbol{\xi}-\boldsymbol{\mu}_{\xi})\right)}\right)}\\ &= \arg\max_{\boldsymbol{\xi}}(\log{p(\boldsymbol{r}|\boldsymbol{\xi})} + \log{p(\boldsymbol{\xi})})\\ &= \arg\max_{\boldsymbol{\xi}}{\left({\left(-\frac{1}{2}(\boldsymbol{r}-\boldsymbol{\mu}_r)^T\boldsymbol{\Sigma}_r^{-1}(\boldsymbol{r}-\boldsymbol{\mu}_r)\right)}+ {\left(-\frac{1}{2}(\boldsymbol{\xi}-\boldsymbol{\mu}_{\xi})^T\boldsymbol{\Sigma}_{\xi}^{-1}(\boldsymbol{\xi}-\boldsymbol{\mu}_{\xi})\right)}\right)}\\ &= -\frac{1}{2}\arg\min_{\boldsymbol{\xi}}{\left({\left((\boldsymbol{r}-\boldsymbol{\mu}_r)^T\boldsymbol{\Sigma}_r^{-1}(\boldsymbol{r}-\boldsymbol{\mu}_r)\right)}+ {\left((\boldsymbol{\xi}-\boldsymbol{\mu}_{\xi})^T\boldsymbol{\Sigma}_{\xi}^{-1}(\boldsymbol{\xi}-\boldsymbol{\mu}_{\xi})\right)}\right)}\\ &= \arg\min_{\boldsymbol{\xi}}\left(||\boldsymbol{r}-\boldsymbol{\mu}_r||^2_{\boldsymbol{\Sigma}_r} + ||\boldsymbol{\xi}-\boldsymbol{\mu}_{\xi}||^2_{\boldsymbol{\Sigma}_{\xi}}\right)\\ \end{aligned}


\begin{aligned} \boldsymbol{\xi}^* &= \arg\max_{\boldsymbol{\xi}}p(\boldsymbol{r}_1, \boldsymbol{r}_2, ..., \boldsymbol{r}_n |\boldsymbol{\xi})p(\boldsymbol{\xi})\\ &= \arg\max_{\boldsymbol{\xi}}\prod_ip(\boldsymbol{r_i}|\boldsymbol{\xi})p(\boldsymbol{\xi})\\ &= \arg\min_{\boldsymbol{\xi}}\left(\sum_i||\boldsymbol{r}_i-\boldsymbol{\mu}_{r_i}||^2_{\boldsymbol{\Sigma}_{r_i}} + ||\boldsymbol{\xi}-\boldsymbol{\mu}_{\xi}||^2_{\boldsymbol{\Sigma}_{\xi}}\right)\\ \end{aligned}


## 舒尔补在状态估计中的应用

### 问题引入

$$p(\boldsymbol{\xi}_0 | \boldsymbol{r}_0)$$


$$p(\boldsymbol{\xi}_0, \boldsymbol{\xi}_1, ..., \boldsymbol{\xi}_n | \boldsymbol{r}_0, \boldsymbol{r}_1, ..., \boldsymbol{r}_m)$$


\begin{aligned} \boldsymbol{J}^T\Sigma^{-1}\boldsymbol{J}\delta\boldsymbol{\xi} &= -\boldsymbol{J}^T\boldsymbol{\Sigma}^{-1}\boldsymbol{r}\\ \Rightarrow \boldsymbol{H}\delta\boldsymbol{\xi} &= \boldsymbol{b} \end{aligned}


\begin{aligned} \boldsymbol{H}\delta\boldsymbol{\xi} &= \boldsymbol{b}\\ \begin{bmatrix} \boldsymbol{H}_{ii} & \boldsymbol{H}_{ij}\\ \boldsymbol{H}_{ji} & \boldsymbol{H}_{jj} \end{bmatrix} \begin{bmatrix} \delta\boldsymbol{\xi}_i\\ \delta\boldsymbol{\xi}_j \end{bmatrix} &= \begin{bmatrix} \boldsymbol{b}_i\\ \boldsymbol{b}_j \end{bmatrix} \end{aligned}


### 舒尔补和边缘化原理

$$\boldsymbol{M} = \begin{bmatrix} \boldsymbol{A} & \boldsymbol{B} \\ \boldsymbol{C} & \boldsymbol{D} \end{bmatrix}$$


\begin{aligned} \begin{bmatrix} \boldsymbol{I} & \boldsymbol{0}\\ -\boldsymbol{CA}^{-1} & \boldsymbol{I} \end{bmatrix} \begin{bmatrix} \boldsymbol{A} & \boldsymbol{B} \\ \boldsymbol{C} & \boldsymbol{D} \end{bmatrix} &= \begin{bmatrix} \boldsymbol{A} & \boldsymbol{B}\\ \boldsymbol{0} & \Delta_{\boldsymbol{A}} \end{bmatrix}\\ \begin{bmatrix} \boldsymbol{A} & \boldsymbol{B} \\ \boldsymbol{C} & \boldsymbol{D} \end{bmatrix} \begin{bmatrix} \boldsymbol{I} & \boldsymbol{0}\\ -\boldsymbol{D}^{-1}\boldsymbol{C} & \boldsymbol{I} \end{bmatrix} &= \begin{bmatrix} \Delta_{D} & \boldsymbol{B}\\ \boldsymbol{0} & \boldsymbol{D} \end{bmatrix} \end{aligned}


\begin{aligned} \begin{bmatrix} \boldsymbol{H}_{ii} & \boldsymbol{H}_{ij}\\ \boldsymbol{H}_{ji} & \boldsymbol{H}_{jj} \end{bmatrix} \begin{bmatrix} \delta\boldsymbol{\xi}_i\\ \delta\boldsymbol{\xi}_j \end{bmatrix} &= \begin{bmatrix} \boldsymbol{b}_i\\ \boldsymbol{b}_j \end{bmatrix}\\ \begin{bmatrix} \boldsymbol{I} & \boldsymbol{0}\\ -\boldsymbol{H}_{ji}\boldsymbol{H}_{ii}^{-1} & \boldsymbol{I} \end{bmatrix} \begin{bmatrix} \boldsymbol{H}_{ii} & \boldsymbol{H}_{ij}\\ \boldsymbol{H}_{ji} & \boldsymbol{H}_{jj} \end{bmatrix} \begin{bmatrix} \delta\boldsymbol{\xi}_i\\ \delta\boldsymbol{\xi}_j \end{bmatrix} &= \begin{bmatrix} \boldsymbol{I} & \boldsymbol{0}\\ -\boldsymbol{H}_{ji}\boldsymbol{H}_{ii}^{-1} & \boldsymbol{I} \end{bmatrix} \begin{bmatrix} \boldsymbol{b}_i\\ \boldsymbol{b}_j \end{bmatrix}\\ \begin{bmatrix} \boldsymbol{H}_{ii} & \boldsymbol{H}_{ij}\\ \boldsymbol{0} & \boldsymbol{H}_{jj} - \boldsymbol{H}_{ji}\boldsymbol{H}_{\boldsymbol{ii}}^{-1}\boldsymbol{H}_{ij} \end{bmatrix} \begin{bmatrix} \delta\boldsymbol{\xi}_i\\ \delta\boldsymbol{\xi}_j \end{bmatrix} &= \begin{bmatrix} \boldsymbol{b}_i\\ -\boldsymbol{H}_{ji}\boldsymbol{H}^{-1}_{ii}\boldsymbol{b}_i + \boldsymbol{b}_j \end{bmatrix} \end{aligned}


\begin{aligned} \boldsymbol{H}^*\Delta\boldsymbol{\xi}_j &= \boldsymbol{b}^*\\ (\boldsymbol{H}_{jj} - \boldsymbol{H}_{ji}\boldsymbol{H}_{\boldsymbol{ii}}^{-1})\Delta\boldsymbol{\xi}_j &= -\boldsymbol{H}_{ji}\boldsymbol{H}^{-1}_{ii}\boldsymbol{b}_i + \boldsymbol{b}_j \end{aligned}


## 舒尔补在多元高斯分布中的应用

\begin{aligned} p(\boldsymbol{x}) = \mathcal{N}(\boldsymbol{\mu}_x, \boldsymbol{\Sigma}_{xx})\\ p(\boldsymbol{y}) = \mathcal{N}(\boldsymbol{\mu}_y, \boldsymbol{\Sigma}_{yy})\\ \end{aligned}


$$p(\boldsymbol{x}, \boldsymbol{y}) = \mathcal{N}\left( \begin{bmatrix} \boldsymbol{\mu}_x\\ \boldsymbol{\mu}_y \end{bmatrix}, \begin{bmatrix} \boldsymbol{\Sigma}_{xx} & \boldsymbol{\Sigma}_{xy}\\ \boldsymbol{\Sigma}_{yx} & \boldsymbol{\Sigma}_{yy} \end{bmatrix} \right)$$


$$p(\boldsymbol{x}, \boldsymbol{y}) = p(\boldsymbol{x}|\boldsymbol{y})p(\boldsymbol{y})$$


$$\begin{bmatrix} \boldsymbol{\Sigma}_{xx} & \boldsymbol{\Sigma}_{xy}\\ \boldsymbol{\Sigma}_{yx} & \boldsymbol{\Sigma}_{yy} \end{bmatrix} = \begin{bmatrix} \boldsymbol{I} & \boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}\\ \boldsymbol{0} & \boldsymbol{I} \end{bmatrix} \begin{bmatrix} \boldsymbol{\Sigma}_{xx} - \boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}\boldsymbol{\Sigma}_{yx} & \boldsymbol{0}\\ \boldsymbol{0} & \boldsymbol{\Sigma}_{yy} \end{bmatrix} \begin{bmatrix} \boldsymbol{I} & \boldsymbol{0}\\ \boldsymbol{\Sigma}_{yy}^{-1}\boldsymbol{\Sigma}_{yx} & \boldsymbol{I} \end{bmatrix}$$


$$\begin{bmatrix} \boldsymbol{\Sigma}_{xx} & \boldsymbol{\Sigma}_{xy}\\ \boldsymbol{\Sigma}_{yx} & \boldsymbol{\Sigma}_{yy} \end{bmatrix} ^{-1} = \begin{bmatrix} \boldsymbol{I} & \boldsymbol{0}\\ -\boldsymbol{\Sigma}_{yy}^{-1}\boldsymbol{\Sigma}_{yx} & \boldsymbol{I} \end{bmatrix} \begin{bmatrix} (\boldsymbol{\Sigma}_{xx} - \boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}\boldsymbol{\Sigma}_{yx})^{-1} & \boldsymbol{0}\\ \boldsymbol{0} & \boldsymbol{\Sigma}_{yy}^{-1} \end{bmatrix} \begin{bmatrix} \boldsymbol{I} & -\boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}\\ \boldsymbol{0} & \boldsymbol{I} \end{bmatrix}$$


$$p(\boldsymbol{x}) = \frac{1}{(\sqrt{(2\pi)^{N}\det{\boldsymbol{\Sigma}})}}\exp{\left(-\frac{1}{2}(\boldsymbol{x}-\boldsymbol{\mu})^{T}\boldsymbol{\Sigma}^{-1}(\boldsymbol{x}-\boldsymbol{\mu})\right)}$$


\begin{aligned} &(\boldsymbol{x}-\boldsymbol{\mu})^{T}\boldsymbol{\Sigma}^{-1}(\boldsymbol{x}-\boldsymbol{\mu})\\ = & \left(\begin{bmatrix} \boldsymbol{x}\\ \boldsymbol{y} \end{bmatrix} - \begin{bmatrix} \boldsymbol{\mu}_x\\ \boldsymbol{\mu}_y \end{bmatrix}\right)^{T} \begin{bmatrix} \boldsymbol{\Sigma}_{xx} & \boldsymbol{\Sigma}_{xy}\\ \boldsymbol{\Sigma}_{yx} & \boldsymbol{\Sigma}_{yy} \end{bmatrix} \left(\begin{bmatrix} \boldsymbol{x}\\ \boldsymbol{y} \end{bmatrix} - \begin{bmatrix} \boldsymbol{\mu}_x\\ \boldsymbol{\mu}_y \end{bmatrix}\right)\\ =& \left(\begin{bmatrix} \boldsymbol{x}\\ \boldsymbol{y} \end{bmatrix} - \begin{bmatrix} \boldsymbol{\mu}_x\\ \boldsymbol{\mu}_y \end{bmatrix}\right)^{T} \begin{bmatrix} \boldsymbol{I} & \boldsymbol{0}\\ -\boldsymbol{\Sigma}_{yy}^{-1}\boldsymbol{\Sigma}_{yx} & \boldsymbol{I} \end{bmatrix} \begin{bmatrix} (\boldsymbol{\Sigma}_{xx} - \boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}\boldsymbol{\Sigma}_{yx})^{-1} & \boldsymbol{0}\\ \boldsymbol{0} & \boldsymbol{\Sigma}_{yy}^{-1} \end{bmatrix} \begin{bmatrix} \boldsymbol{I} & -\boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}\\ \boldsymbol{0} & \boldsymbol{I} \end{bmatrix} \left(\begin{bmatrix} \boldsymbol{x}\\ \boldsymbol{y} \end{bmatrix} - \begin{bmatrix} \boldsymbol{\mu}_x\\ \boldsymbol{\mu}_y \end{bmatrix}\right)\\ =& \left(\begin{bmatrix} \boldsymbol{x} - \boldsymbol{\mu}_x\\ \boldsymbol{y} - \boldsymbol{\mu}_y \end{bmatrix}\right)^{T} \begin{bmatrix} \boldsymbol{I} & \boldsymbol{0}\\ -\boldsymbol{\Sigma}_{yy}^{-1}\boldsymbol{\Sigma}_{yx} & \boldsymbol{I} \end{bmatrix} \begin{bmatrix} (\boldsymbol{\Sigma}_{xx} - \boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}\boldsymbol{\Sigma}_{yx})^{-1} & \boldsymbol{0}\\ \boldsymbol{0} & \boldsymbol{\Sigma}_{yy}^{-1} \end{bmatrix} \begin{bmatrix} \boldsymbol{I} & -\boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}\\ \boldsymbol{0} & \boldsymbol{I} \end{bmatrix} \left(\begin{bmatrix} \boldsymbol{x} - \boldsymbol{\mu}_x\\ \boldsymbol{y} - \boldsymbol{\mu}_y \end{bmatrix}\right)\\ =& \begin{bmatrix} \boldsymbol{x} - \boldsymbol{\mu}_x -\boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}(\boldsymbol{y} - \boldsymbol{\mu}_y)\\ \boldsymbol{y} - \boldsymbol{\mu}_y \end{bmatrix}^T \begin{bmatrix} (\boldsymbol{\Sigma}_{xx} - \boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}\boldsymbol{\Sigma}_{yx})^{-1} & \boldsymbol{0}\\ \boldsymbol{0} & \boldsymbol{\Sigma}_{yy}^{-1} \end{bmatrix} \begin{bmatrix} \boldsymbol{x} - \boldsymbol{\mu}_x -\boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}(\boldsymbol{y} - \boldsymbol{\mu}_y)\\ \boldsymbol{y} - \boldsymbol{\mu}_y \end{bmatrix}\\ =& (\boldsymbol{x} - (\boldsymbol{\mu}_x +\boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}(\boldsymbol{y} - \boldsymbol{\mu}_y)))^T(\boldsymbol{\Sigma}_{xx} - \boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}\boldsymbol{\Sigma}_{yx})^{-1}(\boldsymbol{x} - (\boldsymbol{\mu}_x +\boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}(\boldsymbol{y} - \boldsymbol{\mu}_y))) + (\boldsymbol{y} - \boldsymbol{\mu}_y)^T\boldsymbol{\Sigma}_{yy}^{-1}(\boldsymbol{y} - \boldsymbol{\mu}_y) \end{aligned}


$$p(\boldsymbol{x}|\boldsymbol{y}) = \mathcal{N}(\boldsymbol{\mu}_x + \boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}(\boldsymbol{y} - \boldsymbol{\mu}_y), \boldsymbol{\Sigma}_{xx} - \boldsymbol{\Sigma}_{xy}\boldsymbol{\Sigma}_{yy}^{-1}\boldsymbol{\Sigma}_{yx})$$


## 边缘化在 SLAM 中的应用

$$\boldsymbol{\xi} = \arg\min_{\boldsymbol{\xi}}\frac{1}{2}\sum_i|||\boldsymbol{r}_i||_{\boldsymbol{\Sigma}_i}^2$$


\begin{aligned} \boldsymbol{\xi} &= \begin{bmatrix} \boldsymbol{\xi}_1 & \boldsymbol{\xi}_2 & \boldsymbol{\xi}_3 & \boldsymbol{\xi}_4 & \boldsymbol{\xi}_5 & \boldsymbol{\xi}_6 \end{bmatrix}^T\\ \boldsymbol{r} &= \begin{bmatrix} \boldsymbol{r}_{12} & \boldsymbol{r}_{13} & \boldsymbol{r}_{14} & \boldsymbol{r}_{15} & \boldsymbol{r}_{56} \end{bmatrix}^T \end{aligned}


\begin{aligned} \boldsymbol{J}^T\Sigma^{-1}\boldsymbol{J}\delta\boldsymbol{\xi} &= -\boldsymbol{J}^T\boldsymbol{\Sigma}^{-1}\boldsymbol{r}\\ \Rightarrow \boldsymbol{H}\delta\boldsymbol{\xi} &= \boldsymbol{b} \end{aligned}


$$\boldsymbol{\Sigma} = \begin{bmatrix} \boldsymbol{\Sigma}_1 & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0}\\ \boldsymbol{0} & \boldsymbol{\Sigma}_2 & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0}\\ \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{\Sigma}_3 & \boldsymbol{0} & \boldsymbol{0}\\ \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{\Sigma}_4 & \boldsymbol{0}\\ \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{\Sigma}_5\\ \end{bmatrix}$$


$\boldsymbol{J}$ 为残差对各状态量的雅可比，对其展开有：

\begin{aligned} \boldsymbol{J} = \frac{\partial\boldsymbol{r}}{\partial\boldsymbol{\xi}} = \begin{bmatrix} \frac{\boldsymbol{r}_{12}}{\partial\boldsymbol{\xi}}\\ \frac{\boldsymbol{r}_{13}}{\partial\boldsymbol{\xi}}\\ \frac{\boldsymbol{r}_{14}}{\partial\boldsymbol{\xi}}\\ \frac{\boldsymbol{r}_{15}}{\partial\boldsymbol{\xi}}\\ \frac{\boldsymbol{r}_{56}}{\partial\boldsymbol{\xi}}\\ \end{bmatrix} = \begin{bmatrix} \boldsymbol{J}_1 \boldsymbol{J}_2 \boldsymbol{J}_3 \boldsymbol{J}_4 \boldsymbol{J}_5 \end{bmatrix} \end{aligned}


\begin{aligned} \boldsymbol{H}\delta\boldsymbol{\xi} &= \boldsymbol{b}\\ \boldsymbol{J}^T\Sigma^{-1}\boldsymbol{J}\delta\boldsymbol{\xi} &= -\boldsymbol{J}^T\boldsymbol{\Sigma}^{-1}\boldsymbol{r}\\ \sum_{i=1}^5\boldsymbol{J}_i^T\boldsymbol{\Sigma}_i^{-1}\boldsymbol{J}_i\delta\boldsymbol{\xi} &= -\sum_{i=1}^5\boldsymbol{J}_i^T\boldsymbol{\Sigma}_i^{-1}\boldsymbol{r}_i\\ \Rightarrow \boldsymbol{H} &= \sum_{i=1}^5\boldsymbol{J}_i^T\boldsymbol{\Sigma}_i^{-1}\boldsymbol{J}_i = \sum_i\boldsymbol{H}_i\\ \Rightarrow \boldsymbol{b} &= -\sum_{i=1}^5\boldsymbol{J}_i^T\boldsymbol{\Sigma}_i^{-1}\boldsymbol{r}_i = \sum_i\boldsymbol{b}_i \end{aligned}


\begin{aligned} \boldsymbol{H} = \begin{bmatrix} \boldsymbol{H}_{\alpha\alpha} & \boldsymbol{H}_{\beta\alpha} \\ \boldsymbol{H}_{\alpha\beta} & \boldsymbol{H}_{\beta\beta} \end{bmatrix} \end{aligned}


\begin{aligned} \boldsymbol{H}^* = \boldsymbol{H}_{\beta\beta} - \boldsymbol{H}_{\alpha\beta}\boldsymbol{H}_{\alpha\alpha}^{-1}\boldsymbol{H}_{\beta\alpha} \end{aligned}


\begin{aligned} &(\boldsymbol{H}_{\beta\beta} - \boldsymbol{H}_{\alpha\beta}\boldsymbol{H}_{\alpha\alpha}^{-1}\boldsymbol{H}_{\beta\alpha})\delta\boldsymbol{\xi} = \boldsymbol{b}_{\beta} - \boldsymbol{H}_{\alpha\beta}\boldsymbol{H}_{\alpha\alpha}^{-1}\boldsymbol{b}_{\alpha}\\ \Rightarrow &\left\{ \begin{aligned} \boldsymbol{H}_{\beta\beta}\delta\boldsymbol{\xi}&= \boldsymbol{b}_{\beta}\\ - \boldsymbol{H}_{\alpha\beta}\boldsymbol{H}_{\alpha\alpha}^{-1}\boldsymbol{H}_{\beta\alpha}\delta\boldsymbol{\xi} &= - \boldsymbol{H}_{\alpha\beta}\boldsymbol{H}_{\alpha\alpha}^{-1}\boldsymbol{b}_{\alpha} \end{aligned} \right. \end{aligned}\\


• $t$ 时刻，有 $\boldsymbol{\xi}_{1:6}$ 6 个状态量以及 $\boldsymbol{r}_{1:5}$ 5 个观测量（以及对应残差）
• 在完成最小二乘优化之后，将 $\boldsymbol{\xi}_1$ 及其相关的残差 $\boldsymbol{r}_{1:4}$ 通过边缘化去掉，更新先验信息 $\boldsymbol{r}_p$，剩余状态量有 $\boldsymbol{\xi}_{2:6}$， 与 $\boldsymbol{\xi}_1$ 相关的信息通过边缘化传递给了 $\boldsymbol{\xi}_{2:5}$$\boldsymbol{\xi}_6$ 不受影响
• $t+1$ 时刻，将新的状态量 $\boldsymbol{\xi}_7$ 以及对应的观测 $\boldsymbol{r}_6$ 和上一时刻剩下的 $\boldsymbol{\xi}_{2:6}$ ，相应的观测 $\boldsymbol{r}_5$ 以及先验信息 $\boldsymbol{r}_p$ 加入图中，进入新一轮优化。

## 边缘化可能引起的问题

### 新旧信息融合时产生的问题

$$\boldsymbol{r}_p^* = \boldsymbol{r}_p + \frac{\partial\boldsymbol{r}_p}{\partial\boldsymbol{x}_p}\delta\boldsymbol{x} = \boldsymbol{r}_p + \boldsymbol{J}_p\delta\boldsymbol{x}$$


$\boldsymbol{b}_p$ 也是类似原理：

\begin{aligned} \boldsymbol{b}_p^* &= \boldsymbol{b}_p + \frac{\partial\boldsymbol{b}_p}{\partial\boldsymbol{x}_p}\delta\boldsymbol{x}\\ &= \boldsymbol{b}_p + \frac{\partial(-\boldsymbol{J}_p^T\boldsymbol{\Sigma}^{-1}\boldsymbol{r})}{\partial\boldsymbol{x}_p}\delta\boldsymbol{x}\\ &= \boldsymbol{b}_p + (-\boldsymbol{J}_p^T\boldsymbol{\Sigma}^{-1}\boldsymbol{J}_p)\delta\boldsymbol{x}\\ &= \boldsymbol{b}_p - \boldsymbol{H}_p\delta\boldsymbol{x} \end{aligned}


## 参考资料

Built with Hugo
Theme Stack designed by Jimmy