高斯分布的表示

高斯分布有两种表达方式

协方差矩阵+均值
信息矩阵+信息矢量

协方差矩阵+均值的方式比较常见，如下

$p(x)=\eta \exp\{-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)\}$

其中对称正定矩阵 $\Sigma$ 为随机变量 $x$ 的协方差矩阵， $\mu$ 为 $x$ 的均值，简记为

$p(x) = N(\mu, \Sigma)$

信息矩阵+信息矢量的形式可以由上式推导而来

$\begin{aligned} p(x)&=\eta \exp\{-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)\} \\ &=\eta\exp\{-\frac{1}{2}x^T\Sigma^{-1}x+x^T\Sigma^{-1}\mu\} \end{aligned}$

运算中产生的常数项都全部吸收到了 $\eta$ 中.

现在定义信息矩阵 $\Omega=\Sigma^{-1}$ ，信息矢量 $\xi=\Sigma^{-1}\mu=\Omega\mu$ ，则

$p(x)=\eta\exp\{-\frac{1}{2}x^T\Omega x+x^T\xi\}$

可记为

$p(x) = N^{-1}(\xi, \Omega)$

联合高斯分布的分解

设随机变量 $x_a, x_b$ 满足联合高斯分布 $p(x_a, x_b)$

由条件概率公式可知

$p(x_a, x_b)=p(x_a)p(x_b|x_a)$

联合高斯函数的分解就是根据 $p(x_a, x_b)$ 求出上式中的 $p(x_a)$ 和 $p(x_b\vert x_a)$ 。

下面根据不同的高斯分布表示形式分别推导。

协方差矩阵+均值

$p(x_a, x_b)$ 以协方差矩阵+均值的形式给出，即

$p(x_a, x_b) = N\Bigg(\begin{pmatrix} \mu_a \\ \mu_b \end{pmatrix}, \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\ \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}\Bigg)$

其密度函数可写为

$p(x_a, x_b)=\eta \exp\Bigg\{-\frac{1}{2}\begin{pmatrix} x_a-\mu_a \\ x_b-\mu_b \end{pmatrix}^T\begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\ \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}^{-1}\begin{pmatrix} x_a-\mu_a \\ x_b-\mu_b \end{pmatrix}\Bigg\}$

为了求出 $p(x_a)$ 和 $p(x_b\vert x_a)$ 的表达式，需要用到舒尔补（Schur Complement），即

$\begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\ \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}= \begin{pmatrix} I & 0\\ \Sigma_{ba}\Sigma_{aa}^{-1} & I \end{pmatrix} \begin{pmatrix} \Sigma_{aa} & 0 \\ 0 & \Sigma_{bb}-\Sigma_{ba}\Sigma_{aa}^{-1}\Sigma_{ab} \end{pmatrix} \begin{pmatrix} I & \Sigma_{aa}^{-1}\Sigma_{ab} \\ 0 & I \end{pmatrix}$

将上式带入 $p(x_a, x_b)$ 的概率密度函数，并注意到对任意矩阵 $K$ ，有

$\begin{pmatrix} I & 0 \\ K & I \end{pmatrix}^{-1}= \begin{pmatrix} I & 0 \\ -K & I \end{pmatrix},\ \ \begin{pmatrix} I & K \\ 0 & I \end{pmatrix}^{-1}= \begin{pmatrix} I & -K \\ 0 & I \end{pmatrix}$

可以得到

$p(x_a, x_b)=\eta \exp\{-\frac{1}{2}(x_a-\mu_a)^T\Sigma_{aa}^{-1}(x_a-\mu_a)-\frac{1}{2}[x_b-(\mu_b+\Sigma_{ba}\Sigma_{aa}^{-1}(x_a-\mu_a))]^T\Theta_{bb}[x_b-(\mu_b+\Sigma_{ba}\Sigma_{aa}^{-1}(x_a-\mu_a))]\}$ $=\underbrace{\eta_1 \exp\{-\frac{1}{2}(x_a-\mu_a)^T\Sigma_{aa}^{-1}(x_a-\mu_a)}_{p(x_a)\}}\underbrace{\eta_2\exp\{-\frac{1}{2}[x_b-(\mu_b+\Sigma_{ba}\Sigma_{aa}^{-1}(x_a-\mu_a))]^T\Theta_{bb}^{-1}[x_b-(\mu_b+\Sigma_{ba}\Sigma_{aa}^{-1}(x_a-\mu_a))]\}}_{p(x_b|x_a)}$

其中 $\Theta_{bb}=\Sigma_{bb}-\Sigma_{ba}\Sigma_{aa}^{-1}\Sigma_{ab}$ 。

由此可看出， $p(x_a)$ 是均值为 $\mu_a$ ，协方差矩阵为 $\Sigma_{aa}$ 的高斯分布，记为

$p(x_a)=N(\mu_a,\ \Sigma_{aa})$

同时， $p(x_b\vert x_a)$ 是均值为 $\mu_b+\Sigma_{ba}\Sigma_{aa}^{-1}(x_a-\mu_a)$ ，协方差矩阵为 $\Theta_{bb}$ 的高斯分布，记为

$p(x_b\vert x_a)=N(\mu_b+\Sigma_{ba}\Sigma_{aa}^{-1}(x_a-\mu_a),\ \Theta_{bb})=N(\mu_b+\Sigma_{ba}\Sigma_{aa}^{-1}(x_a-\mu_a), \ \Sigma_{bb}-\Sigma_{ba}\Sigma_{aa}^{-1}\Sigma_{ab})$

信息矩阵+信息矢量

$p(x_a, x_b)$ 以信息矩阵+信息矢量的形式给出，即

$p(x_a, x_b) = N^{-1}\Bigg(\begin{pmatrix} \xi_a \\ \xi_b \end{pmatrix}, \begin{pmatrix} \Omega_{aa} & \Omega_{ab} \\ \Omega_{ba} & \Omega_{bb} \end{pmatrix}\Bigg)$

通过信息矢量与信息矩阵，可以计算出该分布的均值

$\begin{pmatrix} \mu_a \\ \mu_b \end{pmatrix}=\begin{pmatrix} \Omega_{aa} & \Omega_{ab} \\ \Omega_{ba} & \Omega_{bb} \end{pmatrix}^{-1}\begin{pmatrix} \xi_a \\ \xi_b \end{pmatrix}$

因此该分布的概率密度函数可写为（注意到信息矩阵与协方差矩阵为互逆关系）

$p(x_a, x_b)=\eta \exp\Bigg\{-\frac{1}{2}\begin{pmatrix} x_a-\mu_a \\ x_b-\mu_b \end{pmatrix}^T\begin{pmatrix} \Omega_{aa} & \Omega_{ab} \\ \Omega_{ba} & \Omega_{bb} \end{pmatrix}\begin{pmatrix} x_a-\mu_a \\ x_b-\mu_b \end{pmatrix}\Bigg\}$

为了求出 $p(x_a)$ 和 $p(x_b\vert x_a)$ 的表达式，需要再次用到舒尔补（Schur Complement），不过作用对象与之前不同，即

$\begin{pmatrix} \Omega_{aa} & \Omega_{ab} \\ \Omega_{ba} & \Omega_{bb} \end{pmatrix}= \begin{pmatrix} I & \Omega_{ab}\Omega_{bb}^{-1} \\ 0 & I \end{pmatrix} \begin{pmatrix} \Omega_{aa}-\Omega_{ab}\Omega_{bb}^{-1}\Omega_{ba} & 0 \\ 0 & \Omega_{bb} \end{pmatrix} \begin{pmatrix} I & 0 \\ \Omega_{bb}^{-1}\Omega_{ba} & I \end{pmatrix}$

将上式带入 $p(x_a, x_b)$ 的密度函数中，并令 $\Omega_{aa}-\Omega_{ab}\Omega_{bb}^{-1}\Omega_{ba} = \Lambda_{aa}$ ，可得

$p(x_a, x_b)=\eta \exp\{-\frac{1}{2}(x_a-\mu_a)^T\Lambda_{aa}(x_a-\mu_a)-\frac{1}{2}[x_b-(\mu_b-\Omega_{bb}^{-1}\Omega_{ba}(x_a-\mu_a))]^T\Omega_{bb}[x_b-(\mu_b-\Omega_{bb}^{-1}\Omega_{ba}(x_a-\mu_a))]\}$ $= \underbrace{\eta_1\exp\{-\frac{1}{2}(x_a-\mu_a)^T\Lambda_{aa}(x_a-\mu_a)\}}_{p(x_a)} \underbrace{\eta_2\exp\{-\frac{1}{2}[x_b-(\mu_b-\Omega_{bb}^{-1}\Omega_{ba}(x_a-\mu_a))]^T\Omega_{bb}[x_b-(\mu_b-\Omega_{bb}^{-1}\Omega_{ba}(x_a-\mu_a))]\}}_{p(x_b|x_a)}$

由此可见， $p(x_a)$ 是一个均值为 $\mu_a$ ，协方差矩阵为 $\Lambda_{aa}^{-1}$ 的高斯分布。

同时 $p(x_b\vert x_a)$ 是一个均值为 $\mu_b-\Omega_{bb}^{-1}\Omega_{ba}(x_a-\mu_a)$ ，协方差矩阵为 $\Omega_{bb}^{-1}$ 的高斯分布。

由于最初的 $p(x_a, x_b)$ 是由信息矩阵+信息矢量表示的，因此我们希望 $p(x_a)$ 和 $p(x_b\vert x_a)$ 也用同样形式表示，不希望引入额外的均值量。

首先，信息矩阵已经得到，分别为 $\Lambda_{aa}$ 和 $\Omega_{bb}$ 。

然后， $p(x_a)$ 的均值为 $\mu_a$ ，则对应的新的信息矢量为

$\bar{\xi}_a=\Lambda_{aa}\mu_a= \begin{pmatrix} \Lambda_{aa} & 0 \\ 0 & 0 \end{pmatrix} \begin{pmatrix} \mu_a\\ \mu_b \end{pmatrix}= \begin{pmatrix} \Lambda_{aa} & 0 \\ 0 & 0 \end{pmatrix} \begin{pmatrix} \Omega_{aa} & \Omega_{ab} \\ \Omega_{ba} & \Omega_{bb} \end{pmatrix}^{-1} \begin{pmatrix} \xi_a\\ \xi_b \end{pmatrix}$ $=\Bigg[\begin{pmatrix} \Omega_{aa} & \Omega_{ab} \\ \Omega_{ba} & \Omega_{bb} \end{pmatrix}- \begin{pmatrix} 0 & \Omega_{ab}\Omega_{bb}^{-1} \\ 0 & I \end{pmatrix} \begin{pmatrix} \Omega_{aa} & \Omega_{ab} \\ \Omega_{ba} & \Omega_{bb} \end{pmatrix}\Bigg] \begin{pmatrix} \Omega_{aa} & \Omega_{ab} \\ \Omega_{ba} & \Omega_{bb} \end{pmatrix}^{-1} \begin{pmatrix} \xi_a\\ \xi_b \end{pmatrix}$ $=\Bigg[ \begin{pmatrix} I & 0 \\ 0 & I \end{pmatrix}- \begin{pmatrix} 0 & \Omega_{ab}\Omega_{bb}^{-1} \\ 0 & I \end{pmatrix}\Bigg] \begin{pmatrix} \xi_a\\ \xi_b \end{pmatrix}= \xi_a-\Omega_{ab}\Omega_{bb}^{-1}\xi_b$

所以 $p(x_a)$ 完全使用信息矩阵+信息矢量的形式可记为

$p(x_a) = N^{-1}(\xi_a-\Omega_{ab}\Omega_{bb}^{-1}\xi_b,\ \Lambda_{aa})=N^{-1}(\xi_a-\Omega_{ab}\Omega_{bb}^{-1}\xi_b,\ \Omega_{aa}-\Omega_{ab}\Omega_{bb}^{-1}\Omega_{ba})$

同理， $p(x_b\vert x_a)$ 的均值为 $\mu_b-\Omega_{bb}^{-1}\Omega_{ba}(x_a-\mu_a)$ ，对应的新的信息矢量为

$\bar\xi_{a|b}=\Omega_{bb}[\mu_b-\Omega_{bb}^{-1}\Omega_{ba}(x_a-\mu_a)]= \Omega_{bb}\mu_b+\Omega_{ba}\mu_a-\Omega_{ba}x_a =\begin{pmatrix} \Omega_{ba} & \Omega_{bb} \end{pmatrix} \begin{pmatrix} \mu_a \\ \mu_b \end{pmatrix} - \Omega_{ba}x_a$ $=\begin{pmatrix} 0 & I \end{pmatrix} \begin{pmatrix} \Omega_{aa} & \Omega_{ab} \\ \Omega_{ba} & \Omega_{bb} \end{pmatrix} \begin{pmatrix} \Omega_{aa} & \Omega_{ab} \\ \Omega_{ba} & \Omega_{bb} \end{pmatrix}^{-1} \begin{pmatrix} \xi_a \\ \xi_b \end{pmatrix}-\Omega_{ba}x_a = \xi_b - \Omega_{ba}x_a$

所以 $p(x_b\vert x_a)$ 完全使用信息矩阵+信息矢量的形式可以记为

$p(x_b\vert x_a) = N^{-1}(\xi_b - \Omega_{ba}x_a, \ \Omega_{bb})$

总结对比

已知 $p(x_a, x_b)$ 满足联合高斯分布，用协方差矩阵+均值，以及信息矩阵+信息矢量的方式可分别表示为如下

$p(x_a, x_b)=N\Bigg(\begin{pmatrix} \mu_a \\ \mu_b \end{pmatrix}, \begin{pmatrix} \Sigma_{aa} & \Sigma_{ab} \\ \Sigma_{ba} & \Sigma_{bb} \end{pmatrix}\Bigg)= N^{-1}\Bigg(\begin{pmatrix} \xi_a \\ \xi_b \end{pmatrix}, \begin{pmatrix} \Omega_{aa} & \Omega_{ab} \\ \Omega_{ba} & \Omega_{bb} \end{pmatrix}\Bigg)$

不同表示方式下，将联合高斯分布的分解为 $p(x_a)p(x_b\vert x_a)$ ，有下表的结果

概率分布	协方差矩阵+均值	信息矩阵+信息矢量
$p(x_a)$	$N(\mu_a,\ \Sigma_{aa})$	$N^{-1}(\xi_a-\Omega_{ab}\Omega_{bb}^{-1}\xi_b,\ \Omega_{aa}-\Omega_{ab}\Omega_{bb}^{-1}\Omega_{ba})$
$p(x_b\vert x_a)$	$N(\mu_b+\Sigma_{ba}\Sigma_{aa}^{-1}(x_a-\mu_a), \ \Sigma_{bb}-\Sigma_{ba}\Sigma_{aa}^{-1}\Sigma_{ab})$	$N^{-1}(\xi_b - \Omega_{ba}x_a, \ \Omega_{bb})$

根据协方差矩阵与信息矩阵的互逆关系，从上表还可以得出如下一组关系

$\Sigma_{aa}^{-1}=\Omega_{aa}-\Omega_{ab}\Omega_{bb}^{-1}\Omega_{ba}$ $\Omega_{bb}^{-1}=\Sigma_{bb}-\Sigma_{ba}\Sigma_{aa}^{-1}\Sigma_{ab}$

边缘化与条件化

所谓边缘化，就是求某个联合概率分布的边缘分布。比如对于联合概率 $p(x_a, x_b)$ ，对 $x_b$ 进行边缘化，就是对 $x_b$ 在整个空间中积分，即

$\int{p(x_a, x_b)dx_b}$

由贝叶斯公式可知

$\int{p(x_a, x_b)dx_b}=\int{p(x_a)p(x_b|x_a)dx_b}=p(x_a)\int{p(x_b|x_a)dx_b}=p(x_a)$

因此，对联合高斯分布而言，对 $x_b$ 边缘化的结果就是上一节求出的 $p(x_a)$ ，仍然是一个高斯函数。伴随着边缘化， $p(x_b|x_a)$ 就是 $p(x_a, x_b)$ 对 $x_a$ 的条件化。

在信息矩阵+信息矢量的表示方式下，边缘化和条件化与最小二乘法有密切关系。在许多基于最小二乘的优化问题中，常有如下形式的优化目标：

$\min_x\Vert e(x)\Vert_W^2=\min_x~e(x)^TW^{-1}e(x)$

其中 $W$ 是 $e(x)$ 的协方差矩阵。

为了寻找上式的最小值，常使用迭代优化的方法，每一次迭代都会寻找一个增量 $\Delta x$ 使目标函数减小。为了求增量，往往会将 $e(x)$ 在当前 $x$ 处展开为一阶近似（这种处理方式即Gauss-Newton Method），即

$e(x+\Delta x)\simeq e(x)+J(x)\Delta x$

其中 $J(x)=\partial e(x)/\partial x$ 。则优化的目标变为：

$\min_{\Delta x}~[e(x)+J(x)\Delta x]^TW^{-1}[e(x)+J(x)\Delta x]$

这是关于 $\Delta x$ 的二次函数，对 $\Delta x$ 求导，并令导数等于0，有

$J(x)^TW^{-1}J(x)\Delta x+J(x)^TW^{-1}e(x)=0$

即

$J(x)^TW^{-1}J(x)\Delta x = -J(x)^TW^{-1}e(x)$

令 $J(x)^TW^{-1}J(x) = \Omega$ ，表示变量 $\Delta x$ 的信息矩阵，令 $-J(x)^TW^{-1}e(x)=\xi$ ，表示信息矢量，则有

$\Omega\Delta x = \xi$

以上就是非线性优化时，每次都要求解的线性方程。

在很多优化问题中，待优化的变量有明确意义，比如在SLAM或者SfM问题中，要优化的是所有相机的位姿 $p$ 以及地图中所有三维点的坐标 $m$ ，设 $\Delta x$ 由这两个分量的增量构成，即

$\Delta x = \begin{pmatrix} \Delta p \\ \Delta m \end{pmatrix}$

同时设

$\Omega=\begin{pmatrix} \Omega_{pp} & \Omega_{pm} \\ \Omega_{mp} & \Omega_{mm} \end{pmatrix}, ~~~ \xi = \begin{pmatrix} \xi_p \\ \xi_m \end{pmatrix}$

则有

$\begin{pmatrix} \Omega_{pp} & \Omega_{pm} \\ \Omega_{mp} & \Omega_{mm} \end{pmatrix} \begin{pmatrix} \Delta p \\ \Delta m \end{pmatrix}= \begin{pmatrix} \xi_p \\ \xi_m \end{pmatrix}$

为了简化以上方程的求解，往往使用高斯消元法，具体的，对以上方程等式两边左乘

$\begin{pmatrix} I & -\Omega_{pm}\Omega_{mm}^{-1} \\ 0 & I \end{pmatrix}$

可得

$\begin{pmatrix} \Omega_{pp}-\Omega_{pm}\Omega_{mm}^{-1}\Omega_{mp} & 0 \\ \Omega_{mp} & \Omega_{mm} \end{pmatrix} \begin{pmatrix} \Delta p \\ \Delta m \end{pmatrix}= \begin{pmatrix} \xi_p-\Omega_{pm}\Omega_{mm}^{-1}\xi \\ \xi_m \end{pmatrix}$

于是原方程可以转换为两个独立方程

$\begin{aligned} (\Omega_{pp}-\Omega_{pm}\Omega_{mm}^{-1}\Omega_{mp})\Delta p &= \xi_p-\Omega_{pm}\Omega_{mm}^{-1}\xi_m \\ \Omega_{mm}\Delta m &= \xi_m-\Omega_{mp}\Delta p \end{aligned}$

可以发现， $\Delta p$ 的系数矩阵和等号右边的结果，与上文中高斯分布 $p(x_a)$ 的信息矩阵和信息矢量有相同的形式。而 $\Delta m$ 的系数矩阵和等号右边的结果，则与高斯分布 $p(x_b\vert x_a)$ 的信息矩阵和信息向量有相同形式。

也就是说，这里的高斯消元法，等价于对变量 $\Delta x$ 做了边缘化，先将 $\Delta m$ 边缘化掉，单独求 $\Delta p$ ，然后再在 $\Delta p$ 已知的情况下求 $\Delta m$ 。

Reference

State Estimate for Robotics.
Probabilistic Robotics.

高斯分布与边缘化

Gaussian Distribution and Marginalization