《机器人学的状态估计》是入行SLAM的经典书籍之一，其中有大量的公式相关的内容，看起来还是比较艰涩的。最近重新读一遍，顺便将其中的一些内容记录下来，方便以后回看。

概率密度函数

定义

定义$x$为区间$[a.b]$上的随机变量，服从某个概率密度函数$p(x)$，那么这个非负函数必须满足：

$${\int }_a^{b}p(x)dx=1$$

这个积分等干1的条件，实际上是为了满足全概率公理。请注意公式里面的$p(x)$是概率密度而不是概率。概率是指概率密度在区间上的面积积分。比如，计算$x$落在区间$[c,d]$上的概率$Pr(c\le x\le d)$，即用概率密度函数在该区间上积分，公式如下：

$$Pr(c\le x\le d)={\int }_c^{d}p(x)dx$$

接下来，引入条件概率。假设$p(x|y)$表示自变量$x\in [a,b]$在条件$y\in [r,s]$下的概率密度函数，那么它满足：

$$(\forall y){\int }_a^{b}p(x|y)dx=1$$

$N$维连续型随机变量的联合概率密度函数也可以用之前的方法表示为$p(x)$，其中$x=(x_0,x_1,...,x_n)$。对于每个$x_i$，满足$x_i\in [a_i,b_i]$。实际上也可以用：

$$p(x_1,x_2,...,x_n)$$

代替$p(x)$。

在$N$维情况下，全概率公理要求：

$${\int }_a^{b}p(x)dx={\int }_{a_N}^{b_N}...{\int }_{a_2}^{b_2}{\int }_{a_1}^{b_1}p(x_1,x_2,...,x_N)d{x}_{1}d{x}_2...d{x}_N=1$$

贝叶斯公式及推断

一个联合概率密度可以分解成一个条件概率密度和一个非条件概率密度的乘积：

$$p(x,y)=p(x|y)p(y)=p(y|x)p(x)$$

重新整理，可以得到贝叶斯公式：

$$p(x|y)=\frac{p(y|x)p(x)}{p(y)}$$

如果有了状态的先验概率密度函数$p(x)$和传感器模型$p(y|x)$，就可以推断状态的后验概率密度函数。因此将分母展开：

$$p(y)=p(y)\int p(x|y)dx=\int p(x|y)p(y)dx=\int p(x,y)dx=\int p(y|x)p(x)dx$$

$$p(x|y)=\frac{p(y|x)p(x)}{\int p(y|x)p(x)dx}$$

也就是说，可以通过边缘化的方式计算分母$p(y)$。

在贝叶斯推断中，$p(x)$称为先验密度，$p(x|y)$称为后验密度。因此，所有的先验信息被包含在$p(x)$中。同样的，所有的后验信息被包含在$p(x|y)$中。

矩

在概率密度中，通常使用矩来描述其简单的特性。概率密度的零阶矩正好是整个全事件的概率，因此恒等于1。概率一阶矩称为期望，用$\mu$表示。

$$\mu =E[x]=\int xp(x)dx$$

这里的$E[]$称为期望算子。对于一般的矩阵函数$F(x)$，这个算子定义为：

$$E[F(x)]=\int F(x)p(x)dx$$

注意它的展开式为：

$$E[F(x)]=E[E[f_{ij}(x)]]=[\int f_{ij}(x)p(x)dx]$$

概率二阶矩是协方差矩阵$\Sigma$：

$$\Sigma =E[(x-\mu )(x-\mu {)}^T]$$

接下来的三阶矩和四阶矩分别称为偏度和峰度。但是在多变量情况下形式很复杂，这里就不研究了。

样本均值和样本方差

假设一个随机变量$x$，它的概率密度函数为$p(x)$。所谓样本，也就是随机变量的一次实现，可以直观地理解为一次测量值。如果想要通过采样得到$N$个样本来估计随机变量的期望和方差，可以直接使用样本均值和样本方差去近似。公式如下：

$${\mu }_{meas}=\frac{1}{N}\sum _{i=1}^N{x}_{i,meas}$$

$${\Sigma }_{means}=\frac{1}{N-1}\sum _{i=1}^N(x_{i,meas}-{\mu }_{meas})(x_{i,meas}-{\mu }_{meas}{)}^T$$

注意：样本方差的归一化参数分母是$N-1$而不是$N$，这称为贝塞尔修正。直观上说，样本方差使用了测量值和样本均值的差，而样本均值本身又是通过相同的测量值得到的，它们之间存在一个轻微的相关性，于是就出现了这样的一个轻微的修正量。可以证明，样本方差其实正好是随机变量方差的无偏估计。

统计独立性与不相关性

如果两个随机变量$x$和$y$的联合概率密度函数可以用以下的因式分解，那么称这两个随机变量是统计独立的：

$$p(x,y)=p(x)p(y)$$

同样的，如果两个变量的期望运算满足下式，则称它们是不相关的：

$$E[x{y}^T]=E[x]E[y{]}^T$$

不相关比独立更弱。如果两个随机变量是统计独立的，也就意味着它们也是不相关的，但是不相关的两个随机变量不一定是独立的。出于简化计算的目的，一般都会假设变量之间是统计独立的。

归一化积

对于同一个随机变量的两个不同的概率密度函数，可以计算它们的归一化积。一个随机变量怎么会有两个不同的概率密度函数呢？这可以理解为一个随机变量有两个不同的观测，每个观测都表示一个概率密度函数。

如果$p_1(x)$和$p_2(x)$是随机变量$x$的两个不同的概率密度函数，那么它们的归一化积$p(x)$定义为：

$$p(x)=\eta p_1(x)p_2(x)$$

其中：

$$\eta =(\int p_1(x)p_2(x)dx{)}^{-1}$$

$\eta$是一个常值的归一化因子，用于确保$p(x)$满足全概率公理。

根据贝叶斯理论，归一化积可用于融合对同一个随机变量的多次估计（以概率密度函数形式表示），只要假设其先验为均匀分布即可。设$x$为待估计变量，$y_1$、$y_2$为两次独立测量，那么：

$$p(x|y_1,y_2)=\frac{p(y_1,y_2|x)p(x)}{p(y_1,y_2)}$$

该公式的理解：

$$p(y_1,y_2|x)p(x)=p(x,y_1,y_2)=p(x|y_1,y_2)p(y_1,y_2)$$

给定$x$，假设$y_1$、$y_2$是统计独立的，则：

$$\begin{array}{rl}p(y_1,y_2|x)& =p(y_1|x)p(y_2|x)\\ & =\frac{p(x|y_1)p(y_1)}{p(x)}\frac{p(x|y_2)p(y_2)}{p(x)}\\ & =\frac{p(x|y_1)p(x|y_2)p(y_1,y_2)}{p(x)p(x)}\end{array}$$

带入，最终得到：

$$p(x|y_1,y_2)=\frac{p(x|y_1)p(x|y_2)}{p(x)}=\eta p(x|y_1)p(x|y_2)$$

其中：

$$\eta =\frac{1}{p(x)}$$

如果先验$p(x)$为均匀分布，那么$\eta$也是一个常数。

高斯概率密度函数

定义

一维情况下，高斯概率密度函数表示为：

$$p(x|\mu ,\sigma )=\frac{1}{\sqrt{2\pi {\sigma }^2}}exp(-\frac{1}{2}\frac{(x-\mu {)}^2}{{\sigma }^2})$$

其中，$\mu$为均值，${\sigma }^2$为方差，$\sigma$为标准差。

多维变量的高斯分布是说，随机变量$x\in R^N$，服从的分布$p(x|\mu ,\Sigma )$可以写为：

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

其中，$\mu \in R^N$为均值，$\Sigma \in R^{N\ast N}$为协方差矩阵（对称正定矩阵），它们可以通过以下方式计算：

$$\mu =E[x]={\int }_{-\infty }^{\infty }x\frac{1}{\sqrt{(2\pi {)}^{N}det(\Sigma )}}exp(-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu ))dx$$

以及：

$$\begin{array}{rl}\Sigma & =E[(x-\mu )(x-\mu {)}^T]\\ & ={\int }_{-\infty }^{\infty }(x-\mu )(x-\mu {)}^T\frac{1}{\sqrt{(2\pi {)}^{N}det(\Sigma )}}exp(-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu ))dx\end{array}$$

习惯上，正态分布（即高斯分布）记为：

$$x\sim N(\mu ,\Sigma )$$

如果随机变量$x$满足：

$$x\sim N(0,1)$$

其中，$1$为$N\ast N$的单位矩阵，那么该随机变量服从标准正太分布。

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

假设有两个服从多元正态分布的变量$(x,y)$，可以写出它们的联合概率密度函数：

$$p(x,y)=N(\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xy}\\ {\Sigma }_{yx}& {\Sigma }_{yy}\end{array}\right])$$

其中：${\Sigma }_{yx}={\Sigma }_{xy}^T$。

由于联合概率密度，总可以将其分解成两个因子的乘积（条件概率乘以边缘概率），$p(x,y)=p(x|y)p(y)$。对于联合高斯概率密度，这两个因子也都是高斯分布。

特别的，对于高斯分布，可以利用舒尔补详细地推演出分解的过程。所谓舒尔补，本质上就是高斯消元法的总结，可以参考舒尔补理论Schur Compliment。即：

$$\left[\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xy}\\ {\Sigma }_{yx}& {\Sigma }_{yy}\end{array}\right]=\left[\begin{array}{cc}1& {\Sigma }_{xy}{\Sigma }_{yy}^{-1}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}& 0\\ 0& {\Sigma }_{yy}\end{array}\right]\left[\begin{array}{cc}1& 0\\ {\Sigma }_{yy}^{-1}{\Sigma }_{yx}& 1\end{array}\right]$$

两边求逆：

$${\left[\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xy}\\ {\Sigma }_{yx}& {\Sigma }_{yy}\end{array}\right]}^{-1}=\left[\begin{array}{cc}1& 0\\ -{\Sigma }_{yy}^{-1}{\Sigma }_{yx}& 1\end{array}\right]\left[\begin{array}{cc}({\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{)}^{-1}& 0\\ 0& {\Sigma }_{yy}^{-1}\end{array}\right]\left[\begin{array}{cc}1& -{\Sigma }_{xy}{\Sigma }_{yy}^{-1}\\ 0& 1\end{array}\right]$$

看一下联合概率密度函数$p(x,y)$的指数部分：

$$\begin{array}{rl}& (\left[\begin{array}{c}x\\ y\end{array}\right]-\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right]{)}^T{\left[\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xy}\\ {\Sigma }_{yx}& {\Sigma }_{yy}\end{array}\right]}^{-1}(\left[\begin{array}{c}x\\ y\end{array}\right]-\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right])\\ \stackrel{1}{=}& (\left[\begin{array}{c}x\\ y\end{array}\right]-\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right]{)}^T\left[\begin{array}{cc}1& 0\\ -{\Sigma }_{yy}^{-1}{\Sigma }_{yx}& 1\end{array}\right]\left[\begin{array}{cc}({\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{)}^{-1}& 0\\ 0& {\Sigma }_{yy}^{-1}\end{array}\right]\left[\begin{array}{cc}1& -{\Sigma }_{xy}{\Sigma }_{yy}^{-1}\\ 0& 1\end{array}\right](\left[\begin{array}{c}x\\ y\end{array}\right]-\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right])\\ \stackrel{2}{=}& ({\left[\begin{array}{c}x-{\mu }_x-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}(y-{\mu }_y)\\ y-{\mu }_y\end{array}\right]}^T\left[\begin{array}{cc}({\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{)}^{-1}& 0\\ 0& {\Sigma }_{yy}^{-1}\end{array}\right]\left[\begin{array}{c}x-{\mu }_x-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}(y-{\mu }_y)\\ y-{\mu }_y\end{array}\right])\\ \stackrel{3}{=}& (x-{\mu }_x-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}(y-{\mu }_y){)}^T({\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx}{)}^{-1}(x-{\mu }_x-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}(y-{\mu }_y))+(y-{\mu }_y{)}^T{\Sigma }_{yy}^{-1}(y-{\mu }_y)\end{array}$$

其中，1处带入协方差矩阵的舒尔补的逆，2处两边的矩阵乘法进行结合律运算，3处对整个公式进行相乘。

由于指数相加，反映到幂运算上，就是相乘。并且联合概率密度分解成条件概率乘以边缘概率：

$$p(x,y)=p(x|y)p(y)$$

可以通过$p(x,y)$的指数部分，可以得到：
$$p(x|y)=N({\mu }_x+{\Sigma }_{xy}{\Sigma }_{yy}^{-1}(y-{\mu }_y),{\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx})$$

$$p(y)=N({\mu }_y,{\Sigma }_{yy})$$

可以发现：因子$p(x|y)$、$p(y)$都是高斯概率密度函数。这实际上就是高斯推断。

统计独立性，不相关性

在高斯概率密度函数的情况下，统计独立性和不相关性是等价的，即统计独立的两个高斯变量是不相关的（对于一般的概率密度函数也成立），不相关的两个高斯变量也是统计独立的（并不是所有的概率密度函数都成立）。通过高斯推断的结论就能够证明这一点。

假设$x$、$y$是统计独立的高斯概率密度函数，即$p(x,y)=p(x)p(y)$，那么$p(x|y)=p(x)=N({\mu }_x,{\Sigma }_{xx})$。那么：

$${\Sigma }_{xy}{\Sigma }_{yy}^{-1}(y-{\mu }_y)=0$$

$${\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx}=0$$

可以得到${\Sigma }_{xy}=0$，既然：

$${\Sigma }_{xy}=E[(x-{\mu }_x)(y-{\mu }_y{)}^T]=E[x{y}^T]-E[x]E[y{]}^T$$

那么就可以得到不相关成立的条件：

$$E[x{y}^T]=E[x]E[y{]}^T$$

假设$x$、$y$是不相关的高斯概率密度函数，那么${\Sigma }_{xy}=0$，于是它们是统计独立的。

高斯分布随机变量的线性变换

假设高斯随机变量$x\in R^N\sim N({\mu }_x,{\Sigma }_{xx})$，与$x$线性相关的随机变量$y\in R^M=Gx$。其中$G\in R^{M\ast N}$是一个常量矩阵。现在需要研究$y$的统计特性，最简单的方法就是直接计算期望和方差：

$${\mu }_y=E[y]=E[Gx]=GE[x]=G{\mu }_x$$

$$\begin{array}{rl}{\Sigma }_y& =E[(y-{\mu }_y)(y-{\mu }_y{)}^T]\\ & =E[(G(x-{\mu }_x))(G(x-{\mu }_x){)}^T]\\ & =GE[(x-{\mu }_x)(x-{\mu }_x{)}^T]G^T\\ & =G{\Sigma }_{xx}{G}^T\end{array}$$

因此，$y\sim N({\mu }_y,{\Sigma }_{yy})=N(G{\mu }_x,G{\Sigma }_{xx}{G}^T)$。

高斯概率密度函数的归一化积

现在来讨论高斯概率密度函数中一个有用的性质，即$K$个高斯概率密度函数的归一化积仍然是高斯概率密度函数：

$$exp(-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu ))=\eta \prod _{k=1}^{K}exp(-\frac{1}{2}(x-{\mu }_k{)}^T{\Sigma }_k^{-1}(x-{\mu }_k))$$

其中，

$${\Sigma }^{-1}=\sum _{k=1}^K{\Sigma }_k^{-1}$$

$${\Sigma }^{-1}\mu =\sum _{k=1}^K{\Sigma }_k^{-1}{\mu }_k$$

其中，$\eta$是一个归一化常量，用于确保概率密度函数满足全概率公理。当需要将多个高斯估计融合在一起的时候，就需要使用到高斯归一化乘积。

对于高斯分布随机变量的线性变换，也有类似的结果：

$$exp(-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu ))=\eta \prod _{k=1}^{K}exp(-\frac{1}{2}(G_{k}x-{\mu }_k{)}^T{\Sigma }_k^{-1}(G_{k}x-{\mu }_k))$$

其中，

$${\Sigma }^{-1}=\sum _{k=1}^K{G}_k^T{\Sigma }_k^{-1}{G}_k$$

$${\Sigma }^{-1}\mu =\sum _{k=1}^K{G}_k^T{\Sigma }_k^{-1}{\mu }_k$$

Sherman-Morrison-Woodbury等式

Sherman-Morrison-Woodbury恒等式，有时也称为矩阵求逆引理，可以缩写成SMW恒等式。

对于可逆矩阵， 可以将它分解成一下三角一对角一上三角（lower-diagonal-upper,LDU）形式或者一上三角一对角一下三角（upper-diagonal-lower,UDL）形式，如下所示：

$$\begin{array}{rlr}& \left[\begin{array}{cc}{A}^{-1}& B\\ C& D\end{array}\right]& \\ =& \left[\begin{array}{cc}1& 0\\ CA& 1\end{array}\right]\left[\begin{array}{cc}{A}^{-1}& 0\\ 0& D+CAB\end{array}\right]\left[\begin{array}{cc}1& -AB\\ 0& 1\end{array}\right]& (LDU)\\ =& \left[\begin{array}{cc}1& -B{D}^{-1}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{A}^{-1}+B{D}^{-1}C& 0\\ 0& D\end{array}\right]\left[\begin{array}{cc}1& 0\\ D^{-1}C& 1\end{array}\right]& (UDL)\end{array}$$

接着对等式两侧求逆：

对于LDU，有：

$$\begin{array}{rl}{\left[\begin{array}{cc}{A}^{-1}& B\\ C& D\end{array}\right]}^{-1}& =\left[\begin{array}{cc}1& AB\\ 0& 1\end{array}\right]\left[\begin{array}{cc}A& 0\\ 0& (D+CAB{)}^{-1}\end{array}\right]\left[\begin{array}{cc}1& 0\\ -CA& 1\end{array}\right]\\ & =\left[\begin{array}{cc}A-AB(D+CAB{)}^{-1}CA& AB(D+CAB{)}^{-1}\\ -(D+CAB{)}^{-1}CA& (D+CAB{)}^{-1}\end{array}\right]\end{array}$$

对于UDL，有：

$$\begin{array}{rl}{\left[\begin{array}{cc}{A}^{-1}& B\\ C& D\end{array}\right]}^{-1}& =\left[\begin{array}{cc}1& 0\\ -D^{-1}C& 1\end{array}\right]\left[\begin{array}{cc}(A^{-1}+B{D}^{-1}C{)}^{-1}& 0\\ 0& D^{-1}\end{array}\right]\left[\begin{array}{cc}1& B{D}^{-1}\\ 0& 1\end{array}\right]\\ & =\left[\begin{array}{cc}(A^{-1}+B{D}^{-1}C{)}^{-1}& (A^{-1}+B{D}^{-1}C{)}^{-1}B{D}^{-1}\\ -D^{-1}C(A^{-1}+B{D}^{-1}C{)}^{-1}& D^{-1}-D^{-1}C(A^{-1}+B{D}^{-1}C{)}^{-1}B{D}^{-1}\end{array}\right]\end{array}$$

这两者计算出来的结果应该是相等的，于是有：

$$A-AB(D+CAB{)}^{-1}CA=(A^{-1}+B{D}^{-1}C{)}^{-1}$$

$$AB(D+CAB{)}^{-1}=(A^{-1}+B{D}^{-1}C{)}^{-1}B{D}^{-1}$$

$$-(D+CAB{)}^{-1}CA=-D^{-1}C(A^{-1}+B{D}^{-1}C{)}^{-1}$$

$$(D+CAB{)}^{-1}=D^{-1}-D^{-1}C(A^{-1}+B{D}^{-1}C{)}^{-1}B{D}^{-1}$$

高斯分布随机变量的非线性变换

接下来研究高斯分布经过一个随机非线性变换之后的情况，即计算：

$$p(y)={\int }_{-\infty }^{\infty }p(y|x)p(x)dx$$

其中，

$$p(y|x)\sim N(g(x),R)$$

$$p(x)\sim N({\mu }_x,{\Sigma }_{xx})$$

这里的$g()$是一个非线性映射，它受到均值为0，协方差为$R$的高斯噪声的干扰。将高斯分布传递进非线性变换中是很有必要的，当需要对传感器进行建模时，这类随机非线性映射就很适合。

标量情况下的非线性映射

假设已知$x\in R^1\sim N(0,{\sigma }^2)$，且存在非线性变换$y=exp(x)$，那么$y$的分布还是高斯分布么？

根据$x$和$y$的转换关系：

$$x=ln(y)$$

$$dx=\frac{1}{y}dy$$

根据全概率公式：

$$\begin{array}{rl}1& ={\int }_{-\infty }^{\infty }p(x)dx\\ & ={\int }_{-\infty }^{\infty }\frac{1}{\sqrt{2\pi {\sigma }^2}}exp(-\frac{x^2}{2{\sigma }^2})dx\\ & ={\int }_0^{\infty }\frac{1}{\sqrt{2\pi {\sigma }^2}}exp(-\frac{(ln(y){)}^2}{2{\sigma }^2})\frac{1}{y}dy\\ & ={\int }_0^{\infty }p(y)dy\end{array}$$

即：

$$p(y)=\frac{1}{\sqrt{2\pi {\sigma }^2}}exp(-\frac{(ln(y){)}^2}{2{\sigma }^2})\frac{1}{y}$$

可以指定${\sigma }^2$的值，对$p(y)$进行绘制。可以看出，经过非线性变换之后，$p(y)$不再服从高斯分布。

一般情况下的非线性处理

对非线性变换进行线性化后，得到：

$$g(x)\approx {\mu }_y+G(x-{\mu }_x)$$

其中：

$${\mu }_y=g({\mu }_x)$$

$$G=\frac{\partial g(x)}{\partial x}{|}_{x={\mu }_x}$$

其中，$G$为$g()$关于$x$的雅可比矩阵。在线性化后，就可以得到非线性问题的线性近似解，当这个映射的非线性不强的时候，该近似解才成立。

对于之前提到的$p(y)={\int }_{-\infty }^{\infty }p(y|x)p(x)dx$，及$p(y|x)$、$p(x)$的分布形式，可以得到：

$$\begin{array}{rl}p(y)=& {\int }_{-\infty }^{\infty }p(y|x)p(x)dx\\ =& \eta {\int }_{-\infty }^{\infty }exp(-\frac{1}{2}(y-g(x){)}^T{R}^{-1}(y-g(x)))\times exp(-\frac{1}{2}(x-{\mu }_x{)}^T{\Sigma }_{xx}^{-1}(x-{\mu }_x))dx\\ =& \eta {\int }_{-\infty }^{\infty }exp(-\frac{1}{2}(y-({\mu }_y+G(x-{\mu }_x)){)}^T{R}^{-1}(y-({\mu }_y+G(x-{\mu }_x))))\times exp(-\frac{1}{2}(x-{\mu }_x{)}^T{\Sigma }_{xx}^{-1}(x-{\mu }_x))dx\\ =& \eta exp(-\frac{1}{2}(y-{\mu }_y{)}^T{R}^{-1}(y-{\mu }_y))\times \\ & {\int }_{-\infty }^{\infty }exp(-\frac{1}{2}(x-{\mu }_x{)}^T({\Sigma }_{xx}^{-1}+G^T{R}^{-1}G)(x-{\mu }_x))\times exp(-\frac{1}{2}(y-{\mu }_y{)}^T{R}^{-1}G(x-{\mu }_x))dx\end{array}$$

其中，$\eta$是归一化常量。$exp(-\frac{1}{2}(y-{\mu }_y{)}^T{R}^{-1}(y-{\mu }_y))$只与$y$相关，可以纳入$\eta$中。定义$F$，使得：

$$F^T({\Sigma }_{xx}^{-1}+G^T{R}^{-1}G)=R^{-1}G$$

对于积分内的乘积，可以补全里面的平方项（此处类似于$a^2-2ab=(a-b{)}^2-b^2$）：

$$\begin{array}{rl}& exp(-\frac{1}{2}(x-{\mu }_x{)}^T({\Sigma }_{xx}^{-1}+G^T{R}^{-1}G)(x-{\mu }_x))\times exp(-\frac{1}{2}(y-{\mu }_y{)}^T{R}^{-1}G(x-{\mu }_x))\\ =& exp(-\frac{1}{2}[(x-{\mu }_x{)}^T({\Sigma }_{xx}^{-1}+G^T{R}^{-1}G)(x-{\mu }_x))-(y-{\mu }_y{)}^T{R}^{-1}G(x-{\mu }_x)])\\ =& exp(-\frac{1}{2}[(x-{\mu }_x{)}^T({\Sigma }_{xx}^{-1}+G^T{R}^{-1}G)(x-{\mu }_x))-(y-{\mu }_y{)}^T{F}^T({\Sigma }_{xx}^{-1}+G^T{R}^{-1}G)(x-{\mu }_x)])\\ =& exp(-\frac{1}{2}((x-{\mu }_x)-F(y-{\mu }_y){)}^T({\Sigma }_{xx}^{-1}+G^T{R}^{-1}G)((x-{\mu }_x)-F(y-{\mu }_y)))\times \\ & exp(-\frac{1}{2}(y-{\mu }_y{)}^T{F}^T({\Sigma }_{xx}^{-1}+G^T{R}^{-1}G)F(y-{\mu }_y))\end{array}$$

其中，第二个因子与$x$无关，又可以纳入$\eta$中。最终剩下的就是$x$的高斯分布：

$$\begin{array}{rl}p(y)& =\eta exp(-\frac{1}{2}((x-{\mu }_x)-F(y-{\mu }_y){)}^T({\Sigma }_{xx}^{-1}+G^T{R}^{-1}G)((x-{\mu }_x)-F(y-{\mu }_y)))\\ & =\eta exp(-\frac{1}{2}(y-{\mu }_y{)}^T(R^{-1}-R^{-1}G(G^T{R}^{-1}G+{\Sigma }_{xx}^{-1}{)}^{-1}{G}^T{R}^{-1})(y-{\mu }_y))\end{array}$$

其中，根据SMW公式：

$$R^{-1}-R^{-1}G(G^T{R}^{-1}G+{\Sigma }_{xx}^{-1}{)}^{-1}{G}^T{R}^{-1}=(R+G{\Sigma }_{xx}{G}^T{)}^{-1}$$

因此：

$$p(y)=\eta exp(-\frac{1}{2}(y-{\mu }_y{)}^T(R+G{\Sigma }_{xx}{G}^T{)}^{-1}(y-{\mu }_y))$$

即：

$$y\sim N({\mu }_y,{\Sigma }_{yy})=N(g({\mu }_x),R+G{\Sigma }_{xx}{G}^T)$$

该式即为经典的离散时间（扩展）卡尔曼滤波的更新步骤。