SLAM基础——聊一聊信息矩阵

前言

本文主要谈哪些问题？本文主要以信息矩阵为核心，发散畅谈~

• 谈一谈信息矩阵的作用，以及与Hessian矩阵的关系与作用。
• 谈一谈舒尔补的边缘化原理与本质
• 在舒尔补下信息矩阵的更新，先验的定义与本质。
• 信息矩阵更新牵涉到的零空间问题，以及FEJ的引入~
  （参考资料较多，吸取了知乎等大佬的回答，也包含一家之言，欢迎批评指正）

1 信息矩阵

1-1 信息矩阵是什么？有什么作用？

本小节参考: 信息矩阵在图优化slam里面的作用 表示感谢

信息矩阵是一个scalar 表达不确定性
$$e_i(\mathbf{x})={\mathbf{e}}_i(\mathbf{x}{)}^T{\mathbf{\Omega }}_i{\mathbf{e}}_i(\mathbf{x})$$
Described by information matrix $\Omega$
$$\Omega ={\Sigma }^{-1}$$
and information vector $\xi$
$$\xi ={\Sigma }^{-1}\mu$$
那为什么需要信息矩阵呢？
$$\begin{array}{rl}{\mathbf{x}}^{\ast }& =\underset{\mathrm{x}}{\mathrm{argmin}}F(\mathrm{x})-----\text{ global error (scalar) }\\ & =\underset{\mathrm{x}}{\mathrm{argmin}}\sum _i{e}_i(\mathrm{x})-----\text{ squared error terms (scalar) }\\ & =\underset{\mathrm{x}}{\mathrm{argmin}}\sum _i{\mathrm{e}}_i^T(\mathrm{x}){\Omega }_i{\mathrm{e}}_i(\mathrm{x})---\text{error terms (vector) }\end{array}$$

系统可能有很多传感器，传感器精度越高，对应的information matrix里面的系数会很大（这里是越大越好，因为它是协方差矩阵的逆矩阵），系数越大代表权重越高，表达的信息越多，在优化的过程中就越会被重视。用一个形象的数学表达式表达就是：

const int INT_MAX=1e9;
argmin( INT_MAX*(x-3)^2+1/INT_MAX*(x-1)^2)) 

那么INT MAX就代表我们的精确传感器，那么优化的结果肯定是 x=3;也就是说，我们更加相信我们好的传感器

1-2 信息矩阵与Hessian矩阵的关系

本小节参考链接：
文章
[SLAM的滑动窗口算法中，在边缘化时，高斯牛顿法的信息矩阵为什么是 优化变量协方差的逆？]

[Why is the observed Fisher information defined as the Hessian of the log-likelihood?]
PDF
Maximum Likelihood Estimation (MLE).pdf

先放出结论：

Hessian矩阵在最大似然（MLE）问题中被认为约等于信息矩阵，所以一般也会将Hessian矩阵直接当做信息矩阵对待。

协方差的逆=信息矩阵，这个结论是建立在假设分布是高斯分布这个前提下，计算最大后验概率（MAP）优化问题得出来的

1-2-1 Hessian矩阵和H矩阵的关系

Hessian矩阵平时接触的可能不多, 但是Hessian矩阵的近似矩阵H矩阵就比较多了, 因为总是在求解优化问题，必不可少的就会接触到优化问题的H矩阵, 通常我们见到的都是最小二乘问题中的H矩 阵, 如下有：
$$\begin{array}{rl}E& =\Vert z-f(x){\Vert }_w^2=\Vert z-f(x)+J\delta x{\Vert }_w^2\\ & =(e+J\delta x{)}^{T}W(e+J\delta x)\\ & =e^{T}we+\delta x^T{J}^{T}We+e^{T}WJ\delta x+\delta x^T{J}^{T}WJ\delta x\end{array}$$
其中 $J^{T}WJ$ 就称为 $\mathrm{H}$ 矩阵。
Hessian矩阵其实说白了就是 $E$ 对于状态变量 $x$ 的二阶偏导数。而H矩阵是对Hessian矩阵的近似, 主要是为了加速计算。

1-2-2 Hessian或H矩阵和信息矩阵的关系

结论：在最大似然估计问题中，Hessian矩阵通常被用来表示Information矩阵。(结论所参考的资料如下)

[Why is the observed Fisher information defined as the Hessian of the log-likelihood?]
注意：我们的最小二乘就是建立在最大似然估计的基础上的，这也是为什么在一些SLAM框架中，直接将Hessian矩阵当做了信息矩阵。而在最大似然估计中，就是将Hessian矩阵近似为了信息矩阵

Maximum Likelihood Estimation (MLE).pdf
里面的公式(68)， The Information matrix is the negative of the expectation of the Hessian. 信息矩阵是Hessian期望的负值。

根据参考资料得出：

1. 对于似然分布$p(y|x)$而言，Information矩阵就是负对数似然问题的Hessian矩阵的期望；
2. 对于分布$p(x)$而言，负对数似然问题的协方差矩阵的逆就是Hessian矩阵；

VINS-mono应该就是采用这样的思路，直接将hessian矩阵作为了信息矩阵(information)（原因写在下面了）

问题：信息矩阵=协方差的逆，但是为什么有的还说Hessian矩阵在高斯牛顿中被近似认为是信息矩阵？哪一个是对的？

答：都对，信息矩阵就是协方差的逆，这个不是非线性优化推导出的，是假设分布是高斯分布，计算最大后验估计得到的一个优化问题。（参考后面信息矩阵与最小二乘的关系，就会发现推导过程中，假设分布属于高斯分布）

2 信息矩阵与最小二乘的关联

2-1 先谈一谈常规的最小二乘

残差函数 $\mathbf{f}(\mathbf{x})$ 为非线性函数，对其一阶泰勒近似有:
$$\mathbf{f}(\mathbf{x}+\Delta \mathbf{x})\approx \ell (\Delta \mathbf{x})\equiv \mathbf{f}(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}$$
请特别注意, 这里的 $\mathrm{J}$ 是残差函数 $\mathrm{f}$ 的雅克比矩阵。代入损失函数：
$$\begin{array}{rl}F(\mathbf{x}+\Delta \mathbf{x})\approx L(\Delta \mathbf{x})& \equiv \frac{1}{2}\ell (\Delta \mathbf{x}{)}^{\top }\ell (\Delta \mathbf{x})\\ & =\frac{1}{2}{\mathbf{f}}^{\top }\mathbf{f}+\Delta {\mathbf{x}}^{\top }{\mathbf{J}}^{\top }\mathbf{f}+\frac{1}{2}\Delta {\mathbf{x}}^{\top }{\mathbf{J}}^{\top }\mathbf{J}\Delta \mathbf{x}\\ & =F(\mathbf{x})+\Delta {\mathbf{x}}^{\top }{\mathbf{J}}^{\top }\mathbf{f}+\frac{1}{2}\Delta {\mathbf{x}}^{\top }{\mathbf{J}}^{\top }\mathbf{J}\Delta \mathbf{x}\end{array}$$
这样损失函数就近似成了一个二次函数，并且如果雅克比是满秩的， 则 ${\mathbf{J}}^{\top }\mathbf{J}$ 正定，损失函数有最小值。

另外, 易得： $F^{\prime }(\mathbf{x})={\left({\mathbf{J}}^{\top }\mathbf{f}\right)}^{\top }$, 以及 $F^{\prime \prime }(\mathbf{x})\approx {\mathbf{J}}^{\top }\mathbf{J}$.

---

2-2 包含信息矩阵的最小二乘​

本小节主要回答下面问题：

有时候我们不写中间的协方差的逆，为什么？？多出来的协方差的逆是怎么多出来的？？

没有写那是假设其为1了。

SLAM问题建模

考虑某个状态 $\mathbf{\xi }$, 以及一次与该变量相关的观测 ${\mathbf{r}}_{i\circ }$ 由于噪声的存在, 观测服从概率分布 $p\left({\mathbf{r}}_i\mid \mathbf{\xi }\right)$ 。 多次观测时，各个测量值相互独立, 则多个测量相互独立，则多个测量 $\mathbf{r}={\left({\mathbf{r}}_1,\dots ,{\mathbf{r}}_n\right)}^{\top }$ 构建的似然概率为：
$$p(\mathbf{r}\mid \mathbf{\xi })=\prod _{i}p\left({\mathbf{r}}_i\mid \mathbf{\xi }\right)$$
如果知道机器人状态的先验信息 $p(\mathbf{\xi })$, 如 GPS, 车轮码盘信息等, 则 根据 Bayes 法则，有后验概率：
$$p(\mathbf{\xi }\mid \mathbf{r})=\frac{p(\mathbf{r}\mid \mathbf{\xi })p(\mathbf{\xi })}{p(\mathbf{r})}$$
通过最大后验估计，获得系统状态的最优估计：
$${\mathbf{\xi }}_{\mathrm{M}\mathrm{A}\mathrm{P}}=\arg \underset{\xi }{\max }p(\mathbf{\xi }\mid \mathbf{r})$$
后验公式中分母跟状态量无关，舍弃。最大后验变成了：
$${\mathbf{\xi }}_{\mathrm{M}\mathrm{A}\mathrm{P}}=\arg \underset{\xi }{\max }\prod _{i}p\left({\mathbf{r}}_i\mid \mathbf{\xi }\right)p(\mathbf{\xi })$$
即
$${\mathbf{\xi }}_{\mathrm{M}\mathrm{A}\mathrm{P}}=\arg \underset{\xi }{\min }\left[-\sum _i\log p\left({\mathbf{r}}_i\mid \mathbf{\xi }\right)-\log p(\mathbf{\xi })\right]$$
如果假设观测值服从多元高斯分布：
$$p\left({\mathbf{r}}_i\mid \mathbf{\xi }\right)=\mathcal{N}\left({\mathbf{\mu }}_i,{\mathbf{\Sigma }}_i\right),p(\mathbf{\xi })=\mathcal{N}\left({\mathbf{\mu }}_{\xi },{\mathbf{\Sigma }}_{\xi }\right)$$
则有：
$${\mathbf{\xi }}_{\mathrm{M}\mathrm{A}\mathrm{P}}=\underset{\mathbf{\xi }}{\mathrm{argmin}}\sum _i{\Vert {\mathbf{r}}_i-{\mathbf{\mu }}_i\Vert }_{{\mathbf{\Sigma }}_i}^2+{\Vert \mathbf{\xi }-{\mathbf{\mu }}_{\xi }\Vert }_{{\Sigma }_{\xi }}^2$$

注意右下角的$\Vert ..{\Vert }_{{\Sigma }_{\xi }}^2$表示$\Vert ..{\Vert }_{}^2$需要除以${\Sigma }_{\xi }$，这也是为什么高斯牛顿中出现中间的协方差的逆的原因。不过有时候我们假设其为1了，也就是省略不写

这个最小二乘的求解为：（下面结果如何推导出的，看下面的细节说明）
$${\mathbf{J}}^T{\mathbf{\Sigma }}^{-1}\mathbf{J}\delta \mathbf{\xi }=-{\mathbf{J}}^T{\mathbf{\Sigma }}^{-1}\mathbf{r}$$

这里对上面省略掉的推到细节进行说明：

前期基础：

考虑一个任意的高维高斯分布$\mathbf{x}\sim N(\mathbf{\mu },\mathbf{\Sigma })$它的概率密度函数展开形式为:
$$P(x)=\frac{1}{\sqrt{(2\pi {)}^N\det (\mathbf{\Sigma })}}\exp \left(-\frac{1}{2}(x-\mu {)}^T{\mathbf{\Sigma }}^{-1}(x-\mu )\right)$$
取它的负对数, 则变为:
$$-\ln (P(x))=\frac{1}{2}\ln \left((2\pi {)}^N\det (\mathbf{\Sigma })\right)+\frac{1}{2}(x-\mathbf{\mu }{)}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu }).$$
对原分布求最大化相当于对负对数求最小化。在最小化上式的 x 时，第一项与 x 无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。相当于在求：
$${\mathbf{x}}^{\ast }=\arg \min \left((x-\mathbf{\mu }{)}^T{\Sigma }^{-1}(x-\mathbf{\mu })\right).$$
推导

对于某一次观测：
$$z_{k,j}=h\left(y_j,x_k\right)+v_{k,j}$$
我们假设了噪声项 ${\mathbf{v}}_k\sim N\left(0,{\mathbf{\Sigma }}_{k,j}\right)$, 所以观测数据的条件概率为:
$$P\left(z_{j,k}\mid x_k,y_j\right)=N\left(h\left(y_j,x_k\right),{\Sigma }_{k,j}\right).$$
根据上面【前期基础】知识，我们相当于在求：
$${\mathbf{x}}^{\ast }=\arg \min \left({\left({\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\right)}^T{\mathbf{\Sigma }}_{k,j}^{-1}\left({\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\right)\right)$$
最终最小二乘F(x)为：
$$\begin{array}{rl}F(x+\Delta x)& =\Vert z-h(x){\Vert }_w^2=\Vert z-h(x)+J\delta x{\Vert }_w^2\\ & =(e+J\delta x{)}^T{\Sigma }^{-1}(e+J\delta x)\\ & =e^T{\Sigma }^{-1}e+2\delta x^T{J}^T{\Sigma }^{-1}e+\delta x^T{J}^T{\Sigma }^{-1}J\delta x\\ & =F(x)+2\delta x^T{J}^T{\Sigma }^{-1}e+\delta x^T{J}^T{\Sigma }^{-1}J\delta x\end{array}$$
根据解析求导，容易得出：
$${\mathbf{J}}^T{\mathbf{\Sigma }}^{-1}\mathbf{J}\delta \mathbf{\xi }=-{\mathbf{J}}^T{\mathbf{\Sigma }}^{-1}\mathbf{e}$$

3 信息矩阵如何更新的？

3-1 舒尔补介绍

参考我的博文
SLAM基础——舒尔补介绍

3-2 在滑窗中信息矩阵的更新（以及矩阵块的变化）

• 为什么 SLAM 需要滑动窗口算法?

  • 随着 VSLAM 系统不断往新环境探索，就会有新的相机姿态以及看到新的环境特征，最小二乘残差就会越来越多，信息矩阵越来越大，计算量将不断增加。
  • 为了保持优化变量的个数在一定范围内，需要使用滑动窗口算法动态增加或移除优化变量。

• 滑动窗口算法大致流程？

  • 增加新的变量进入最小二乘系统优化
  • 如果变量数目达到了一定的维度，则移除老的变量。
  • SLAM 系统不断循环前面两步

• 怎么移除老的变量？直接丢弃这些变量吗？（参考下面几小结）

3-2-1 回顾附录A的结论

这里先拿出舒尔补里面的部分结论（在滑窗中计算的是边际概率的信息矩阵）

由舒尔补矩阵求逆公式可知，协方差矩阵各块和信息矩阵之间有：
$${\left[\begin{array}{cc}A& C^{\top }\\ C& D\end{array}\right]}^{-1}=\left[\begin{array}{cc}{A}^{-1}+A^{-1}{C}^{\top }{\Delta }_{\mathrm{A}}^{-1}C{A}^{-1}& -A^{-1}{C}^{\top }{\Delta }_{\mathrm{A}}^{-1}\\ -{\Delta }_{\mathrm{A}}^{-1}C{A}^{-1}& {\Delta }_{\mathrm{A}}^{-1}\end{array}\right]\triangleq \left[\begin{array}{cc}{\Lambda }_{aa}& {\Lambda }_{ab}\\ {\Lambda }_{ba}& {\Lambda }_{bb}\end{array}\right]$$
由条件概率 $P(b\mid a)$ 的协方差为 ${\Delta }_A$ 以及公式, 易得其信息矩阵为
$${\Delta }_A^{-1}={\Lambda }_{bb}$$
由边际概率 $P(a)$ 的协方差为 $A$ 以及公式 , 易得其信息矩阵为：
$$A^{-1}={\Lambda }_{aa}-{\Lambda }_{ab}{\Lambda }_{bb}^{-1}{\Lambda }_{ba}$$
或者：
$$D^{-1}={\Lambda }_{bb}-{\Lambda }_{ba}{\Lambda }_{aa}^{-1}{\Lambda }_{ab}$$
这里的$A^{-1}$或者$D^{-1}$就是在下一次优化会使用的先验信息矩阵（又名：边际概率的信息矩阵）。

3-2-2 划窗过程中的信息矩阵变化

使用边际概率移除变量 ${\xi }_1$, 信息矩阵的变化过程如下

注意这里的矩阵要保留的是右下角

思考：如果是直接丢弃，信息矩阵如何变化？用边际概率来操作又会带来什么问题？

如下图优化系统中，随着 $x_{t+1}$ 的进入，变量 $x_t$ 被移除

3-2-3 步骤详解

总体流程是：先移除旧的变量，再加入新变量

最开始的状态：

• 红色为被 marg 变量以及测量约束。
• 绿色为跟 marg 变量有关的保留变量。
• 蓝色为和 marg 变量无关联的变量。

marg老变量之后的状态：

• STEP 1 新的变量${\xi }_7$跟老的变量${\xi }_2$之间存在观测信息，能构建残差${\mathbf{r}}_{27}$。
• STEP 2 新的残差加上之前marg留下的信息，构建新的最小二乘系统，对应的信息矩阵变化如下图所示

注意：

这里的${\xi }_2$自身的信息矩阵由两部分组成（与${\xi }_1$有关并在marg之后形成的，第二部分是新添加观测后形成的），这会使得系统存在潜在风险。

这两部分计算雅克比时的线性化点不同。这可能会导致信息矩阵的零空间发生变化，从而在求解时引入错误信息。

步骤总结：

• (1) 在 $t\in [0,k]\mathrm{s}$ 时刻, 系统中状态 量为 ${\xi }_i,i\in [1,6]$ 。第 $k^{\prime }$ 时刻，加入新的观 测和状态量 ${\xi }_7$

• (2) 在第 $\mathrm{k}$ 时刻，最小二乘优化完以后, marg 掉变量 ${\xi }_{1\circ }$ 被 $\mathrm{marg}$ 的状态量记为 ${\mathbf{x}}_m$, 剩余 的变量 ${\xi }_i,i\in [2,5]$ 记为 ${\mathbf{x}}_r$.

• (3) marg 发生以后， ${\mathbf{x}}_m$ 所有的变量以及对应的 测量将被丟弃。同时，这部分信息通过 $\mathrm{marg}$ 操作传递给了保留变量 ${\mathrm{x}}_r$。 marg 变量的信息跟 ${\xi }_6$ 不相关。

• (4) 第 $k^{\prime }$ 时刻，加入新的状态量 ${\xi }_7($ 记作 ${\mathbf{x}}_n$ ) 以 及对应的观测，开始新一轮最小二乘优化。

谈一谈新测量信息和旧测量信息构建新的系统的细节

在 $k^{\prime }$ 时刻， 新残差 ${\mathbf{r}}_{27}$ 和先验信息 ${\mathbf{b}}_p(k),{\mathbf{\Lambda }}_p(k)$ 以及残差 ${\mathbf{r}}_{56}$ 构建新的最小二乘问题：
$$\begin{array}{c}\mathbf{b}\left(k^{\prime }\right)={\mathbf{\Pi }}^{\top }{\mathbf{b}}_p(k)-{\sum }_{(i,j)\in {\mathcal{S}}_a\left(k^{\prime }\right)}{\mathbf{J}}_{ij}^{\top }\left(k^{\prime }\right){\mathbf{\Sigma }}_{ij}^{-1}{\mathbf{r}}_{ij}\left(k^{\prime }\right)\\ \mathbf{\Lambda }\left(k^{\prime }\right)={\mathbf{\Pi }}^{\top }{\mathbf{\Lambda }}_p(k)\mathbf{\Pi }+{\sum }_{(i,j)\in {\mathcal{S}}_a\left(k^{\prime }\right)}{\mathbf{J}}_{ij}^{\top }\left(k^{\prime }\right){\mathbf{\Sigma }}_{ij}^{-1}{\mathbf{J}}_{ij}\left(k^{\prime }\right)\end{array}$$

• $\mathbf{\Pi }=\left[\begin{array}{ll}{\mathbf{I}}_{\dim }{\mathbf{x}}_r& 0\end{array}\right]$ ：用来将矩阵的维度进行扩张。
• ${\mathcal{S}}_a$：用来表示除被marg 掉的测量以外的其他测量，如 $r_{56},r_{27}$。

其他

说一说关于先验理解的一家之言：

并不能简简单单的理解为把新的信息矩阵传递到下一帧作为先验约束。从更深层面理解，是相互之间的约束关系变了，新的信息矩阵在图的表示上改变了，也就是说新的信息矩阵表达着一种新的图的关系（如上面图最后两个信息矩阵关系图的变化），这种新的关系是把本来不相关的两个变量关联起来了，这种新的关联，就是所谓的先验约束（一家之言）

3-2-4 marg之后留下信息介绍（先验）

变量命名声明

• ${\mathrm{x}}_r$：marg之后的保留变量
• ${\mathbf{x}}_m$：被 $\mathrm{marg}$ 的变量

marg 前，变量 ${\mathbf{x}}_m$ 以及对应测量 ${\mathcal{S}}_m$ 构建的最小二乘信息矩阵为:
$$\begin{array}{rl}{\mathbf{b}}_m(k)& =\left[\begin{array}{c}{\mathbf{b}}_{mm}(k)\\ {\mathbf{b}}_{mr}(k)\end{array}\right]=-\sum _{(i,j)\in {\mathcal{S}}_m}{\mathbf{J}}_{ij}^{\top }(k){\mathbf{\Sigma }}_{ij}^{-1}{\mathbf{r}}_{ij}\\ {\mathbf{\Lambda }}_m(k)& =\left[\begin{array}{cc}{\mathbf{\Lambda }}_{mm}(k)& {\mathbf{\Lambda }}_{mr}(k)\\ {\mathbf{\Lambda }}_{rm}(k)& {\mathbf{\Lambda }}_{rr}(k)\end{array}\right]=\sum _{(i,j)\in {\mathcal{S}}_m}{\mathbf{J}}_{ij}^{\top }(k){\mathbf{\Sigma }}_{ij}^{-1}{\mathbf{J}}_{ij}(k)\end{array}$$
marg 后, 变量 ${\mathbf{x}}_m$ 的测量信息传递给了变量 ${\mathbf{x}}_r$ :
$${\mathbf{b}}_p(k)={\mathbf{b}}_{mr}(k)-{\mathbf{\Lambda }}_{rm}(k){\mathbf{\Lambda }}_{mm}^{-1}(k){\mathbf{b}}_{mm}(k)$$

$${\mathbf{\Lambda }}_p(k)={\mathbf{\Lambda }}_{rr}(k)-{\mathbf{\Lambda }}_{rm}(k){\mathbf{\Lambda }}_{mm}^{-1}(k){\mathbf{\Lambda }}_{mr}(k)$$

下标 $p$ 表示 prior. 即这些信息将构建一个关于 ${\mathrm{x}}_r$ 的先验信息。

谈一谈先验

我们可以从 ${\mathbf{b}}_p(k),{\mathbf{\Lambda }}_p(k)$ 中反解出一个残差 ${\mathbf{r}}_p(k)$ 和对应的雅克比矩 阵 ${\mathbf{J}}_p(k)$. 需要注意的是，随着变量 ${\mathbf{x}}_r(k)$ 的后续不断优化变化, 残差 ${\mathbf{r}}_p(k)$ 或者 ${\mathbf{b}}_p(k)$ 也将跟着变化, 但雅克比 ${\mathbf{J}}_p(k)$ 则固定不变了。

说一说关于先验理解的一家之言：

并不能简简单单的理解为把新的信息矩阵传递到下一帧作为先验约束。从更深层面理解，是相互之间的约束关系变了，新的信息矩阵在图的表示上改变了，也就是说新的信息矩阵表达着一种新的图的关系（如上面图最后两个信息矩阵关系图的变化），这种新的关系是把本来不相关的两个变量关联起来了，这种新的关联，就是所谓的先验约束（一家之言）

3-3 滑窗带来的问题，以及信息矩阵的零空间变化

回顾上节

谈一谈新测量信息和旧测量信息构建新的系统的细节

在 $k^{\prime }$ 时刻， 新残差 ${\mathbf{r}}_{27}$ 和先验信息 ${\mathbf{b}}_p(k),{\mathbf{\Lambda }}_p(k)$ 以及残差 ${\mathbf{r}}_{56}$ 构建新的最小二乘问题：
$$\begin{array}{c}\mathbf{b}\left(k^{\prime }\right)={\mathbf{\Pi }}^{\top }{\mathbf{b}}_p(k)-{\sum }_{(i,j)\in {\mathcal{S}}_a\left(k^{\prime }\right)}{\mathbf{J}}_{ij}^{\top }\left(k^{\prime }\right){\mathbf{\Sigma }}_{ij}^{-1}{\mathbf{r}}_{ij}\left(k^{\prime }\right)\\ \mathbf{\Lambda }\left(k^{\prime }\right)={\mathbf{\Pi }}^{\top }{\mathbf{\Lambda }}_p(k)\mathbf{\Pi }+{\sum }_{(i,j)\in {\mathcal{S}}_a\left(k^{\prime }\right)}{\mathbf{J}}_{ij}^{\top }\left(k^{\prime }\right){\mathbf{\Sigma }}_{ij}^{-1}{\mathbf{J}}_{ij}\left(k^{\prime }\right)\end{array}$$

• $\mathbf{\Pi }=\left[\begin{array}{ll}{\mathbf{I}}_{\dim }{\mathbf{x}}_r& 0\end{array}\right]$ ：用来将矩阵的维度进行扩张。
• ${\mathcal{S}}_a$：用来表示除被marg 掉的测量以外的其他测量，如 $r_{56},r_{27}$。

出现的问题

• 由于被 $\mathrm{marg}$ 的变量以及对应的测量已被丢弃, 先验信息 ${\mathbf{\Lambda }}_p(k)$ 中关于 ${\mathrm{x}}_r$ 的雅克比在后续求解中没法更新。
• 但 ${\mathbf{x}}_r$ 中的部分变量还跟其他残差有联系, 如 ${\xi }_2,{\xi }_5$ 。 这些残差如 ${\mathbf{r}}_{27}$ 对 ${\xi }_2$ 的雅克比会随着 ${\xi }_2$ 的迭代更新而不断在最新的线性化 点处计算。

滑动窗口算法优化的时候，信息矩阵如上述公式变成了两部分，且这两部分计算雅克比时的线性化点不同。这可能会导致信息矩阵的零空间发生变化，从而在求解时引入错误信息。

比如: 求解单目 SLAM 进行 Bundle Adjustment 优化时，问题对应的 信息矩阵 $\mathbf{\Lambda }$ 不满秩，对应的零空间为 $\mathbf{N}$, 用高斯牛顿求解时有
$$\begin{array}{rl}\mathbf{\Lambda }\delta \mathbf{x}& =\mathbf{b}\\ \mathbf{\Lambda }\delta \mathbf{x}+\mathbf{N}\delta \mathbf{x}& =\mathbf{b}\end{array}$$
增量 $\delta x$ 在零空间维度下变化，并不会改变残差。这意味着系统可以有多个满足最小化损失函数的解 ${\mathbf{x}}_{\circ }$

如下图所示，多个解的问题，变成了一个确定的解。不可观的量，变成了可观的。

我们的观测不是一下子全部到齐的，有着先后顺序的，在第一个观测到达的时候我们在(0.5,1.4)周围进行雅可比求解析如图2的$E_1^{\prime }$，第二个观测再来的时候我们这次是在(1.2,0.5)周围求解雅可比如图3的$E_2^{\prime }$，将新的和老的观测求解合并成图4.对比图4和图1，不可观的问题变得可观了。

3-4 滑窗问题的解决（FEJ）

滑动窗口中的问题

滑动窗口算法中，对于同一个变量，不同残差对其计算雅克比矩阵时线性化点可能不一致，导致信息矩阵可以分成两部分，相当于在信息矩阵中多加了一些信息，使得其零空间出现了变化

解决办法： First Estimated Jacobian

FEJ 算法：不同残差对同一个状态求雅克比时，线性化点必须一致。这样就能避免零空间退化而使得不可观变量变得可观。

比如: 计算 ${\mathbf{r}}_{27}$ 对 ${\xi }_2$ 的雅克比时， ${\xi }_2$ 的线性话点必须和 ${\mathbf{r}}_{12}$ 对其求导时一致。

所谓线性化点一致，也就是新老观测在求导的时候，求导变量数值不变（即使新观测来的时候，求导变量被优化了，也要使用变量优化之前的值求解雅可比）。

在边缘化的时候，FEJ的表现就是，一旦完成了marg，那么旧的雅可比矩阵就被固定住了，不能再变了。至于新来的观测残差球星求导雅可比就无所谓了。（简言之，同意误差对同一变量的雅可比求导必须保证线性化点一致。不能误差，或者不同变量之间没有要求线性化点必须一致。）

0 参考资料

信息矩阵在图优化slam里面的作用 【推荐】

OrbSLAM的Optimizer函数用到的信息矩阵如何理解？

SLAM的滑动窗口算法中，在边缘化时，高斯牛顿法的信息矩阵为什么是 优化变量协方差的逆？

[Why is the observed Fisher information defined as the Hessian of the log-likelihood?]

Maximum Likelihood Estimation (MLE).pdf

公式(68)， The Information matrix is the negative of the expectation of the Hessian.

信息矩阵是Hessian期望的负值。

深蓝学院–手写vio课程

《视觉SLAM14讲》