本节探讨信息矩阵、hessian矩阵与协方差矩阵的关系，阐明边缘化的原理。
一个简单的示例，如下：
来自 David Mackay. “The humble Gaussian distribution”. In: (2006). 以及手写vio第四节。

箭头代表了约束方程(或可以理解为观测方程)：
$$\begin{array}{c}{z}_1:\\ z_2:\\ z_3:\end{array}\begin{array}{c}{x}_2=v_2\\ x_1=w_1{x}_2+v_1\\ x_3=w_3{x}_2+v_3\end{array}$$

其中，$v_i$ 相互独立，且各自服从零均值，协方差为${\sigma }_i^2$的高斯分布。

协方差矩阵

协方差计算公式：
$$\begin{array}{rl}Cov(X,Y)& =E[(X-E[X])\ast (Y-E[Y])\\ & =E[XY]-2E[X]E[Y]+E[X]E[Y]\\ & =E[XY]-E[X]E[Y]\end{array}$$

或：$Cov(X,Y)=E[(X-{\mu }_x)(Y-{\mu }_y)]$

计算$x_1,x_2,x_3$之间的协方差矩阵：
$$\begin{array}{rl}{\mathrm{\Sigma }}_{11}& =E(x_1{x}_1)=E((w_1{v}_2+v_1)(w_1{v}_2+v_1))\\ & =w_1^{2}E(v_2^2)+2w_{1}E(v_1{v}_2)+E(v_1^2)\\ & =w_1^2{\sigma }_2^2+{\sigma }_1^2\\ {\mathrm{\Sigma }}_{22}& ={\sigma }_2^2,{\mathrm{\Sigma }}_{33}=w_3^2{\sigma }_2^2+{\sigma }_3^2\\ {\mathrm{\Sigma }}_{12}& =E(x_1{x}_2)=E((w_1{v}_2+v_1)v_2)=w_1{\sigma }_2^2\\ {\mathrm{\Sigma }}_{13}& =E((w_1{v}_2+v_1)(w_3{v}_2+v_3))=w_1{w}_3{\sigma }_2^2\end{array}$$

最后得到协方差矩阵：
$$\Sigma =\left[\begin{array}{ccc}{w}_1^2{\sigma }_2^2+{\sigma }_1^2& w_1{\sigma }_2^2& w_1{w}_3{\sigma }_2^2\\ w_1{\sigma }_2^2& {\sigma }_2^2& w_3{\sigma }_2^2\\ w_1{w}_3{\sigma }_2^2& w_3{\sigma }_2^2& w_3^2{\sigma }_2^2+{\sigma }_3^2\end{array}\right]$$

联合概率密度

$$\begin{array}{rl}p(x_1,x_2,x_3& |z_1,z_2,z_3)\\ & =\frac{1}{C}\exp (-\frac{x_2^2}{2{\sigma }_2^2}-\frac{{(x_1-w_1{x}_2)}^2}{2{\sigma }_1^2}-\frac{{(x_3-w_3{x}_2)}^2}{2{\sigma }_3^2})\\ & \\ & =\frac{1}{C}\exp (-x_2^2[\frac{1}{2{\sigma }_2^2}+\frac{w_1^2}{2{\sigma }_1^2}-\frac{w_3^2}{2{\sigma }_3^2}]-x_1^2\frac{1}{2{\sigma }_1^2}+2x_1{x}_2\frac{w_1}{2{\sigma }_1^2}-x_3^2\frac{1}{2{\sigma }_3^2}+2x_3{x}_2\frac{w_3}{2{\sigma }_3^2})\\ & \\ & =\frac{1}{C}\exp (-\frac{1}{2}\left[\begin{array}{ccc}{x}_1& x_2& x_3\end{array}\right]\left[\begin{array}{ccc}\frac{1}{{\sigma }_1^2}& -\frac{w_1}{{\sigma }_1^2}& 0\\ -\frac{w_1}{{\sigma }_1^2}& \frac{w_1^2}{{\sigma }_1^2}+\frac{1}{{\sigma }_2^2}+\frac{w_3^2}{{\sigma }_3^2}& -\frac{w_3}{{\sigma }_3^2}\\ 0& -\frac{w_3}{{\sigma }_3^2}& \frac{1}{{\sigma }_3^2}\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right])\\ & =\frac{1}{C}\exp (-\frac{1}{2}\left[\begin{array}{ccc}{x}_1& x_2& x_3\end{array}\right]{\Sigma }^{-1}\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right])\end{array}$$

从而我们可以得到协方差的逆矩阵，即信息矩阵：
$${\Sigma }^{-1}=\left[\begin{array}{ccc}\frac{1}{{\sigma }_1^2}& -\frac{w_1}{{\sigma }_1^2}& 0\\ -\frac{w_1}{{\sigma }_1^2}& \frac{w_1^2}{{\sigma }_1^2}+\frac{1}{{\sigma }_2^2}+\frac{w_3^2}{{\sigma }_3^2}& -\frac{w_3}{{\sigma }_3^2}\\ 0& -\frac{w_3}{{\sigma }_3^2}& \frac{1}{{\sigma }_3^2}\end{array}\right]$$

求最大似然估计：$\underset{x_1,x_2,x_3}{\mathrm{argmax}}p(x_1,x_2,x_3|z_1,z_2,z_3)$

可以转化为求
$$\begin{gathered} \underset{x_1,x_2,x_3}{\mathrm{argmax}}\log (p(x_1,x_2,x_3|z_1,z_2,z_3)) \\ \propto -\frac{1}{2}\left[\begin{array}{ccc}{x}_1& x_2& x_3\end{array}\right]\left[\begin{array}{ccc}\frac{1}{{\sigma }_1^2}& -\frac{w_1}{{\sigma }_1^2}& 0\\ -\frac{w_1}{{\sigma }_1^2}& \frac{w_1^2}{{\sigma }_1^2}+\frac{1}{{\sigma }_2^2}+\frac{w_3^2}{{\sigma }_3^2}& -\frac{w_3}{{\sigma }_3^2}\\ 0& -\frac{w_3}{{\sigma }_3^2}& \frac{1}{{\sigma }_3^2}\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right] \end{gathered}$$

即求：$$\underset{x_1,x_2,x_3}{\mathrm{argmin}}\frac{1}{2}\left[\begin{array}{ccc}{x}_1& x_2& x_3\end{array}\right]{\Sigma }^{-1}\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]$$

如此我们可以将问题转化为一个最小二乘问题，同时我们看出信息矩阵与协方差的数学意义。

hessian矩阵

根据约束方程创建最小二乘问题：
$\begin{array}{rl}e& =\sum _{i=1}^3\parallel z_i{\parallel }_2\\ H& =\sum _{i=1}^3{J}_{zi}^T{J}_{zi}\\ & =\left[\begin{array}{c}-1\\ w_1\\ 0\end{array}\right]\left[\begin{array}{ccc}-1& w_1& 0\end{array}\right]+\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]\left[\begin{array}{ccc}0& 1& 0\end{array}\right]+\left[\begin{array}{c}0\\ w_3\\ -1\end{array}\right]\left[\begin{array}{ccc}0& w_3& -1\end{array}\right]\\ & =\left[\begin{array}{ccc}1& -w_1& 0\\ -w_1& w_1^2+1+w_3^2& -w_3\\ 0& -w_3& 1\end{array}\right]\end{array}$

当我们考虑变量方差${\sigma }_i^2$时，问题变为：$\underset{x_1,x_2,x_3}{\mathrm{argmin}}\sum _{i=1}^3\parallel z_i{\parallel }_{{\sigma }_i^2}$
我们得到加入方差的hessian矩阵，即为信息矩阵：
$$\begin{array}{rl}H& =J^T\left[\begin{array}{ccc}{\sigma }_1^2& 0& 0\\ 0& {\sigma }_2^2& 0\\ 0& 0& {\sigma }_3^2\end{array}\right]J\\ & =\sum _{i=1}^3{J}_{zi}^T{\sigma }_{\mathrm{i}}^2{J}_{zi}\\ & =\left[\begin{array}{ccc}\frac{1}{{\sigma }_1^2}& -\frac{w_1}{{\sigma }_1^2}& 0\\ -\frac{w_1}{{\sigma }_1^2}& \frac{w_1^2}{{\sigma }_1^2}+\frac{1}{{\sigma }_2^2}+\frac{w_3^2}{{\sigma }_3^2}& -\frac{w_3}{{\sigma }_3^2}\\ 0& -\frac{w_3}{{\sigma }_3^2}& \frac{1}{{\sigma }_3^2}\end{array}\right]\end{array}$$

由最大似然得到的最小二乘问题与使用观测约束建立的最小二乘问题等价：

$$\underset{x_1,x_2,x_3}{\mathrm{argmin}}\frac{1}{2}\left[\begin{array}{ccc}{x}_1& x_2& x_3\end{array}\right]{\Sigma }^{-1}\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]\to \underset{x_1,x_2,x_3}{\mathrm{argmin}}\sum _{i=1}^3\parallel z_i{\parallel }_{{\sigma }_i^2}$$

由此，我们便可以具体看出hessian矩阵与协方差矩阵之间的联系，当我们需要边缘化marginalize一个变量时，可以将信息矩阵求逆转化为相关的协方差矩阵，然后剔除掉变量后，再次求逆得到新的信息矩阵。

marginalize

通过schur补，我们可以将marginalize的过程公式化：
以Marg b为例：需要先将信息矩阵(此例中为$\mathrm{\Lambda }$)求逆，得协方差矩阵$\mathrm{\Sigma }$，提取与b无关的矩阵A，再对A求逆，即得到marg 后的信息矩阵。
设协方差矩阵如下：

$$\mathrm{\Sigma }=\left[\begin{array}{cc}A& C^T\\ C& D\end{array}\right]=\left[\begin{array}{cc}{\Sigma }_{aa}& {\Sigma }_{ab}\\ {\Sigma }_{ba}& {\Sigma }_{bb}\end{array}\right]$$
信息矩阵如下：
$$\mathrm{\Lambda }={\mathrm{\Sigma }}^{-1}=\left[\begin{array}{cc}{\mathrm{\Lambda }}_{aa}& {\mathrm{\Lambda }}_{ab}\\ {\mathrm{\Lambda }}_{ba}& {\mathrm{\Lambda }}_{bb}\end{array}\right]$$
通过schur补，可得：

即：
$${\Lambda }_p=A^{-1}={\mathrm{\Sigma }}_{aa}^{-1}={\mathrm{\Lambda }}_{aa}-{\mathrm{\Lambda }}_{ab}{{\mathrm{\Lambda }}_{bb}}^{-1}{\mathrm{\Lambda }}_{ba}$$
对应的偏差:
$${\eta }_p={\eta }_a-{\mathrm{\Lambda }}_{ab}{\mathrm{\Lambda }}_{bb}^{-1}{\eta }_b$$
若没有marg时，关于a的更新值为：$\mathrm{updat}{\mathrm{e}}_a{=\mathrm{\Lambda }}_{aa}^{-1}\cdot {\eta }_a$.
加入marg后，关于a的更新值为：
$$\mathrm{updat}{\mathrm{e}}_a=\left({\mathrm{\Lambda }}_{aa}-{\mathrm{\Lambda }}_{ab}{{\mathrm{\Lambda }}_{bb}}^{-1}{\mathrm{\Lambda }}_{ba}\right){}^{-1}\cdot \left({\eta }_a-{\mathrm{\Lambda }}_{ab}{\mathrm{\Lambda }}_{bb}^{-1}{\eta }_b\right)={\Sigma }_{aa}{\mu }_a$$

详细推导见《手写VIO》第四讲，以及相关博客