高斯推断 Gaussian inference

高斯推断

在滑动窗口的过程中,添加新的路标点,选择剔除旧的路标点,这个过程需要对窗口维护内的信息矩阵进行剔除与增加.

在这个过程当中,所需要的重要知识其实就是怎么样从变量的联合概率分布得到单变量的概率分布,这个过程本质上就是高斯推断

因此,这里主要摘录了<机器人学中的状态估计>一书中的第2.2.3小节中的内容

联合高斯概率密度函数,分解与推断

假设我们有一对服从多元分布的变量\((x,y)\),其联合概率密度函数为: \[ p(\boldsymbol{x}, \boldsymbol{y})=\mathcal{N}\left(\left[\begin{array}{l} \boldsymbol{\mu}_{x} \\ \boldsymbol{\mu}_{y} \end{array}\right],\left[\begin{array}{ll} \boldsymbol{\Sigma}_{x x} & \boldsymbol{\Sigma}_{x y} \\ \boldsymbol{\Sigma}_{y x} & \boldsymbol{\Sigma}_{y y} \end{array}\right]\right) \] 可以使用联合分布可以表示成一个条件概率和一个边缘概率的乘积,\(p(\boldsymbol{x}, \boldsymbol{y})=p(\boldsymbol{x} \mid \boldsymbol{y}) p(\boldsymbol{y})\).

这对于滑动窗口而言,也就是目前窗口内函数的联合概率分布是已知的,但是要去掉一个因素,所以也就是从联合概率分布中获得边缘概率分布

若联合概率密度函数满足高斯分布,也就是上式所表示的,那么可以使用舒尔补的方式来获得分解之后条件概率的高斯分布以及边缘概率的高斯分布

联合分布的方差舒尔补分解如下: \[ \left[\begin{array}{cc} \boldsymbol{\Sigma}_{x x} & \boldsymbol{\Sigma}_{x y} \\ \boldsymbol{\Sigma}_{y x} & \boldsymbol{\Sigma}_{y y} \end{array}\right]=\left[\begin{array}{cc} \mathbf{1} & \boldsymbol{\Sigma}_{x y} \boldsymbol{\Sigma}_{y y}^{-1} \\ \boldsymbol{0} & \mathbf{1} \end{array}\right]\left[\begin{array}{cc} \boldsymbol{\Sigma}_{x x}-\boldsymbol{\Sigma}_{x y} \boldsymbol{\Sigma}_{y y}^{-1} \boldsymbol{\Sigma}_{y x} & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{\Sigma}_{y y} \end{array}\right]\left[\begin{array}{cc} \boldsymbol{1} & \boldsymbol{0} \\ \boldsymbol{\Sigma}_{y y}^{-1} \boldsymbol{\Sigma}_{y x} & \mathbf{1} \end{array}\right] \] 求逆就可以获得信息矩阵的形式 \[ \begin{array}{cc} {\left[\begin{array}{cc} \Sigma_{x x} & \Sigma_{x y} \\ \Sigma_{y x} & \Sigma_{y y} \end{array}\right]^{-1}=\left[\begin{array}{cc} 1 & 0 \\ -\Sigma_{y y}^{-1} \Sigma_{y x} & 1 \end{array}\right]} \\ \times\left[\begin{array}{cc} \left(\Sigma_{x x}-\Sigma_{x y} \Sigma_{y y}^{-1} \Sigma_{y x}\right)^{-1} & 0 \\ 0 & \Sigma_{y y}^{-1} \end{array}\right]\left[\begin{array}{cc} 1 & -\Sigma_{x y} \Sigma_{y y}^{-1} \\ 0 & 1 \end{array}\right] \end{array} \] 将联合概率密度函数\(p(x,y)\)指数部分的二次项写出,可以得到: \[ \left(\left[\begin{array}{l} \boldsymbol{x} \\ \boldsymbol{y} \end{array}\right]-\left[\begin{array}{l} \boldsymbol{\mu}_{x} \\ \boldsymbol{\mu}_{y} \end{array}\right]\right)^{\mathrm{T}}\left[\begin{array}{ll} \boldsymbol{\Sigma}_{x x} & \boldsymbol{\Sigma}_{x y} \\ \boldsymbol{\Sigma}_{y x} & \boldsymbol{\Sigma}_{y y} \end{array}\right]^{-1}\left(\left[\begin{array}{l} \boldsymbol{x} \\ \boldsymbol{y} \end{array}\right]-\left[\begin{array}{l} \boldsymbol{\mu}_{x} \\ \boldsymbol{\mu}_{y} \end{array}\right]\right) \] 将之前信息矩阵也就是方差的逆的舒尔补带入得到 \[ \begin{array}{r} \left(\left[\begin{array}{l} x \\ y \end{array}\right]-\left[\begin{array}{l} \mu_{x} \\ \mu_{y} \end{array}\right]\right)^{T}\left[\begin{array}{cc} 1 & 0 \\ -\Sigma_{y y}^{-1} \Sigma_{y x} & 1 \end{array}\right]\left[\begin{array}{cc} \left(\Sigma_{x x}-\Sigma_{x y} \Sigma_{y y}^{-1} \Sigma_{y x}\right)^{-1} & 0 \\ 0 & \Sigma_{y y}^{-1} \end{array}\right] \\ \times\left[\begin{array}{cc} 1 & -\Sigma_{x y} \Sigma_{y y}^{-1} \\ 0 & 1 \end{array}\right]\left(\left[\begin{array}{c} x \\ y \end{array}\right]-\left[\begin{array}{c} \mu_{x} \\ \mu_{y} \end{array}\right]\right) \end{array} \] 按照212进行合并,同时将x和y分别列写,可以得到 \[ \begin{aligned} \left(\boldsymbol{x}-\boldsymbol{\mu}_{x}-\boldsymbol{\Sigma}_{x y} \boldsymbol{\Sigma}_{y y}^{-1}\right.&\left.\left(\boldsymbol{y}-\boldsymbol{\mu}_{y}\right)\right)^{\mathrm{T}}\left(\boldsymbol{\Sigma}_{x x}-\boldsymbol{\Sigma}_{x y} \boldsymbol{\Sigma}_{y y}^{-1} \boldsymbol{\Sigma}_{y x}\right)^{-1} \\ & \times\left(\boldsymbol{x}-\boldsymbol{\mu}_{x}-\boldsymbol{\Sigma}_{x y} \boldsymbol{\Sigma}_{y y}^{-1}\left(\boldsymbol{y}-\boldsymbol{\mu}_{y}\right)\right)+\left(\boldsymbol{y}-\boldsymbol{\mu}_{y}\right)^{\mathrm{T}} \boldsymbol{\Sigma}_{y y}^{-1}\left(\boldsymbol{y}-\boldsymbol{\mu}_{y}\right) \end{aligned} \] 这也就是两个部分的列写,也就是条件概率和边缘概率. \[ \begin{aligned} p(\boldsymbol{x}, \boldsymbol{y}) &=p(\boldsymbol{x} \mid \boldsymbol{y}) p(\boldsymbol{y}) \\ p(\boldsymbol{x} \mid \boldsymbol{y}) &=\mathcal{N}\left(\boldsymbol{\mu}_{x}+\boldsymbol{\Sigma}_{x y} \boldsymbol{\Sigma}_{y y}^{-1}\left(\boldsymbol{y}-\boldsymbol{\mu}_{y}\right), \boldsymbol{\Sigma}_{x x}-\boldsymbol{\Sigma}_{x y} \boldsymbol{\Sigma}_{y y}^{-1} \boldsymbol{\Sigma}_{y x}\right) \\ p(\boldsymbol{y}) &=\mathcal{N}\left(\boldsymbol{\mu}_{y}, \boldsymbol{\Sigma}_{y y}\right) \end{aligned} \]

对于滤波方面的结论

对于滤波以及状态传递,这里主要关注于方差的迭代,所以会关注于变量y是怎么影响条件概率的.

也就是说,这里要关注的点就是,从上个时刻的状态\(x_{k-1}\)获得得到这个时刻状态的先验之后,如何使用观测值y来修正这个先验(从而得到后验).这也就是卡尔曼滤波中所重点讲的.简而言之就是: \[ p(\hat{x_{k}}),p(y) ->p(\hat{x_{k}}|y) = p(\check{x_{k}}) \] 先验到后验的过程,就是一个高斯推断

同时,这也就是之后2.2.3小节书上的解释:

如果我们知道变量y的值(比如一个观测,一个测量),就可以使用上述等式来计算\(p(x,y)\)得到在获得测量值也就是y值之后状态量x的一个似然值.

这也就是高斯推断中一个重要的部分:从状态的先验概率出发.然后基于一些观测值来缩小这个范围,所获得的效果就是可以看到,x的协方差明显缩小从\(\boldsymbol{\Sigma}_{x x}\)变到\(\boldsymbol{\Sigma}_{x x}-\boldsymbol{\Sigma}_{x y} \boldsymbol{\Sigma}_{y y}^{-1} \boldsymbol{\Sigma}_{y x}\),,也就是如下图

对于滑动窗口理论的结论

在基于优化的SLAM问题中,相比于之前的卡尔曼滤波,其更为注重并且直接操作的是信息矩阵,而不是协方差矩阵

所以在之前等式中的\(\boldsymbol{\Sigma}\)都将得换成\(\boldsymbol{\Sigma}^{-1}\)使用,也就是信息矩阵的形式.

在优化过程中总是会计算获得高斯牛顿方程 \(J^{T}{\Sigma}J{\delta}x = b ->H\delta x=b\),其中H就是信息矩阵,所以我们要知道少一个维度,少一个元素对信息矩阵H有什么变化以及影响

也就是要知道从联合概率函数到边缘概率的信息矩阵的变化 (a,b)到(a), 也就是从ab信息矩阵marg掉了b \[ P(\boldsymbol{a}, \boldsymbol{b})=\mathcal{N}\left(\left[\begin{array}{c} \boldsymbol{\mu}_{a} \\ \boldsymbol{\mu}_{b} \end{array}\right],\left[\begin{array}{cc} \boldsymbol{\Sigma}_{a a} & \boldsymbol{\Sigma}_{a b} \\ \boldsymbol{\Sigma}_{b a} & \boldsymbol{\Sigma}_{b b} \end{array}\right]\right)=\mathcal{N}^{-1}\left(\left[\begin{array}{c} \boldsymbol{\eta}_{a} \\ \boldsymbol{\eta}_{b} \end{array}\right],\left[\begin{array}{cc} \boldsymbol{\Lambda}_{a a} & \boldsymbol{\Lambda}_{a a} \\ \boldsymbol{\Lambda}_{b a} & \boldsymbol{\Lambda}_{b b} \end{array}\right]\right) \] 边际概率\(p(\boldsymbol{a})=\int p(\boldsymbol{a}, \boldsymbol{b}) d \boldsymbol{b}\): \[ \begin{array}{c} \boldsymbol{\eta}=\boldsymbol{\eta}_{a}-\Lambda_{a \beta} \Lambda_{b b}^{-1} \boldsymbol{\eta}_{b} \\ \Lambda=\Lambda_{a a}-\Lambda_{a b} \Lambda_{b b}^{-1} \Lambda_{b a} \end{array} \] 一整个marg的过程就是如下

所以高斯推断还是很有用的.