Kalman Filter (Bayesian Derivation)

假设有两个随机变量 $X_1,X_2$ 分别服从高斯分布。即：
$$\begin{array}{rl}{X}_1\sim N({\mu }_1,{\sigma }_1^2)& ⟹p_1(x)=\frac{1}{\sqrt{2\pi {\sigma }_1^2}}\exp \{-\frac{1}{2}\frac{(x-{\mu }_1{)}^2}{{\sigma }_1^2}\}\\ X_2\sim N({\mu }_2,{\sigma }_2^2)& ⟹p_2(x)=\frac{1}{\sqrt{2\pi {\sigma }_2^2}}\exp \{-\frac{1}{2}\frac{(x-{\mu }_2{)}^2}{{\sigma }_2^2}\}\end{array}$$

那么他们的乘积 $\hat{p}(x)=p_1(x)\cdot p_2(x)$ 的分布是什么呢？
我们计算：
$$p_1(x)\cdot p_2(x)=\eta \exp \{-\frac{1}{2}(\frac{(x-{\mu }_1{)}^2}{{\sigma }_1^2}+\frac{(x-{\mu }_2{)}^2}{{\sigma }_2^2})\}$$
上式中， $\eta$ 是归一化因子（确保 $p_1(x)\cdot p_2(x)$ 对 $x$ 积分为 1 即可）。
我们接着把上式中指数部分写为 $x$ 的降幂形式：
$$\begin{array}{rl}\frac{(x-{\mu }_1{)}^2}{{\sigma }_1^2}+\frac{(x-{\mu }_2{)}^2}{{\sigma }_2^2}& =\frac{{\sigma }_2^2\cdot (x-{\mu }_1{)}^2+{\sigma }_1^2\cdot (x-{\mu }_2{)}^2}{{\sigma }_1^2\cdot {\sigma }_2^2}\\ & =\frac{{\sigma }_1^2+{\sigma }_2^2}{{\sigma }_1^2\cdot {\sigma }_2^2}{x}^2-2\frac{{\sigma }_2^2{\mu }_1^2+{\sigma }_1^2{\mu }_2^2}{{\sigma }_1^2\cdot {\sigma }_2^2}x+\frac{{\sigma }_2^2{\mu }_1^2+{\sigma }_1^2{\mu }_2^2}{{\sigma }_1^2\cdot {\sigma }_2^2}\end{array}$$
我们考虑一个均值为 $\hat{\mu }$ ，方差为 ${\hat{\sigma }}^2$ 的高斯分布，其密度函数中对应指数部分也写为 $x$ 的降幂形式：
$$\frac{(x-\hat{\mu }{)}^2}{{\hat{\sigma }}^2}=\frac{1}{{\hat{\sigma }}^2}{x}^2-2\frac{\hat{\mu }}{{\hat{\sigma }}^2}x+\frac{{\hat{\mu }}^2}{{\hat{\sigma }}^2}$$
如果把上面两个式子中，对应部分划等号后求解 $\hat{\mu },\hat{\sigma }$ ：
$$\{\begin{array}{l}\frac{1}{{\hat{\sigma }}^2}=\frac{{\sigma }_1^2+{\sigma }_2^2}{{\sigma }_1^2\cdot {\sigma }_2^2}\\ \frac{\hat{\mu }}{{\hat{\sigma }}^2}=\frac{{\sigma }_2^2{\mu }_1^2+{\sigma }_1^2{\mu }_2^2}{{\sigma }_1^2\cdot {\sigma }_2^2}\\ \frac{{\hat{\mu }}^2}{{\hat{\sigma }}^2}=\frac{{\sigma }_2^2{\mu }_1^2+{\sigma }_1^2{\mu }_2^2}{{\sigma }_1^2\cdot {\sigma }_2^2}\end{array}$$
上面方程组是无解的，其实我们可以不考虑和 $x$ 无关的常数项部分（即方程组中第 3 个方程）。因为这个部分可以计入到归一化因子 $\eta$ 中。即只需求解下面的方程组：
$$\{\begin{array}{l}\frac{1}{{\hat{\sigma }}^2}=\frac{{\sigma }_1^2+{\sigma }_2^2}{{\sigma }_1^2\cdot {\sigma }_2^2}\\ \frac{\hat{\mu }}{{\hat{\sigma }}^2}=\frac{{\sigma }_2^2{\mu }_1^2+{\sigma }_1^2{\mu }_2^2}{{\sigma }_1^2\cdot {\sigma }_2^2}\end{array}$$
求解得：
$$\{\begin{array}{l}\hat{\mu }=\frac{{\sigma }_2^2{\mu }_1^2+{\sigma }_1^2{\mu }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\\ {\hat{\sigma }}^2=\frac{{\sigma }_1^2\cdot {\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\end{array}$$
所以：两个一维高斯分布的乘积还是高斯分布，它的参数如上式所述。

假设有两个随机变量 $X,Y$ 分别满足下面约束：
$$\begin{array}{rl}X\sim N({\mu }_X,{\sigma }_X^2)& ⟹p_X(x)=\frac{1}{\sqrt{2\pi {\sigma }_X^2}}\exp \{-\frac{1}{2}\frac{(x-{\mu }_X{)}^2}{{\sigma }_X^2}\}\\ Y\mid x\sim N(ax+b,{\sigma }_r^2)& ⟹p_{Y\mid X}(x)=\frac{1}{\sqrt{2\pi {\sigma }_r^2}}\exp \{-\frac{1}{2}\frac{(y-ax-b{)}^2}{{\sigma }_r^2}\}\end{array}$$
那么随机变量 $Y$ 满足什么分布呢？
由全概率公式有：
$$p_Y(y)={\int }_{-\mathrm{\infty }}^{\mathrm{\infty }}{p}_{Y\mid X}(y\mid x)p_X(x)\mathrm{d}x$$
所以 $Y$ 可以看作是两个高斯分布的卷积（说明：卷积有不同形式的定义）。
结论是： $Y$ 也是高斯分布，其均值和方差计算如下：
$$\{\begin{array}{l}{\mu }_Y=a{\mu }_X+b\\ {\sigma }_Y^2={\sigma }_r^2+a^2{\sigma }_X^2\end{array}$$
其证明略。证明过程可参考：http://ais.informatik.uni-freiburg.de/teaching/ss15/robotics/etc/gaussian_notes.pdf

Bayes 滤波器的预测阶段为：
$$\overline{bel}(x_t)={\int }_{-\mathrm{\infty }}^{\mathrm{\infty }}p(x_t\mid x_{t-1},u_t)bel(x_{t-1})\mathrm{d}{x}_{t-1}$$
由于 Kalman 滤波器中假设“运动方程是线性的，且存在高斯噪声”，所以有：
$$p(x_t\mid u_t,x_{t-1})\sim N(a_t{x}_{t-1}+b_t{u}_t,{\sigma }_{Q,t}^2)$$
又由于 Kalman 滤波器中假设“初始分布是高斯分布”，所以有：
$$bel(x_{t-1})\sim N({\mu }_{t-1},{\sigma }_{t-1}^2)$$
从而 $\overline{bel}(x_t)$ 是两个一维高斯分布的卷积，利用之前介绍过的“两个一维高斯分布的卷积”一节给出的结论可知 $\overline{bel}(x_t)$ 也服从高斯分布，且：
$$\begin{array}{rl}\overline{bel}(x_t)& \sim N({\bar{\mu }}_t,{\bar{\sigma }}_t^2)\\ {\bar{\mu }}_t& =a_t{\bar{\mu }}_{t-1}+b_t{u}_t\\ {\bar{\sigma }}_t^2& =a_t^2{\bar{\sigma }}_{t-1}^2+{\bar{\sigma }}_{Q,t}^2\end{array}$$

至此，一维 Kalman 滤波器算法中预测阶段两公式推导完毕。

Bayes 滤波器的更新阶段为：
$$bel(x_t)=\eta p(z_t\mid x_t)\overline{bel}(x_t)$$
由于 Kalman 滤波器中假设“测量方程是线性的，且存在高斯噪声”，所以有：
$$p(z_t\mid x_t)\sim N(c_t{x}_t,{\sigma }_{R,t}^2)$$
在前一节中，已经推导出：
$$\overline{bel}(x_t)\sim N({\bar{\mu }}_t,{\bar{\sigma }}_t^2)$$
从而 $bel(x_t)$ 是两个一维高斯分布的乘积，但我们却无法直接利用之前介绍过的“两个一维高斯分布的乘积”一节给出的结论。因为 $p(z_t\mid x_t)\sim N(c_t{x}_t,{\sigma }_{R,t}^2)$ 中 $x_t$ 出现在高斯分布的参数中。不过，我们可以利用介绍“两个一维高斯分布的乘积”时类似的思路来推导 $bel(x_t)$ 的分布。

$$bel(x_t)=\eta \exp \{-\frac{1}{2}(\frac{(z_t-c_t{x}_t{)}^2}{{\sigma }_{R,t}^2}+\frac{(x_t-{\bar{\mu }}_t{)}^2}{{\bar{\sigma }}_t^2})\}$$
把上式中指数部分写为 $x_t$ 的降幂形式：
$$\frac{(z_t-c_t{x}_t{)}^2}{{\sigma }_{R,t}^2}+\frac{(x_t-{\bar{\mu }}_t{)}^2}{{\bar{\sigma }}_t^2}=\frac{{\bar{\sigma }}_t^2{c}_t^2+{\sigma }_{R,t}^2}{{\bar{\sigma }}_t^2\cdot {\sigma }_{R,t}^2}{x}_t^2-2\frac{{\sigma }_{R,t}^2{\bar{\mu }}_t+{\bar{\sigma }}_t^2{c}_t{z}_t}{{\bar{\sigma }}_t^2\cdot {\sigma }_{R,t}^2}{x}_t+\frac{{\sigma }_{R,t}^2{\bar{\mu }}_t^2+{\bar{\sigma }}_t^2{z}_t^2}{{\bar{\sigma }}_t^2\cdot {\sigma }_{R,t}^2}$$

我们考虑一个均值为 $\mu$ ，方差为 ${\sigma }^2$ 的高斯分布，其密度函数中对应指数部分也写为 $x$ 的降幂形式：
$$\frac{(x-\mu {)}^2}{{\sigma }^2}=\frac{1}{{\sigma }^2}{x}^2-2\frac{\mu }{{\sigma }^2}x+\frac{{\mu }^2}{{\sigma }^2}$$
和“两个一维高斯分布的乘积”一节类似，联立下面方程组：
$$\{\begin{array}{ll}\frac{1}{{\sigma }^2}& =\frac{{\bar{\sigma }}_t^2{c}_t^2+{\sigma }_{R,t}^2}{{\bar{\sigma }}_t^2\cdot {\sigma }_{R,t}^2}\\ \frac{\mu }{{\sigma }^2}& =\frac{{\sigma }_{R,t}^2{\bar{\mu }}_t+{\bar{\sigma }}_t^2{c}_t{z}_t}{{\bar{\sigma }}_t^2\cdot {\sigma }_{R,t}^2}\end{array}$$
求解后，得：
$$\{\begin{array}{ll}{\mu }_t& =\frac{{\sigma }_{R,t}^2{\bar{\mu }}_t+{\bar{\sigma }}_t^2{c}_t{z}_t}{{\bar{\sigma }}_t^2{c}_t^2+{\sigma }_{R,t}^2}\\ {\sigma }_t^2& =\frac{{\bar{\sigma }}_t^2\cdot {\sigma }_{R,t}^2}{{\bar{\sigma }}_t^2{c}_t^2+{\sigma }_{R,t}^2}\end{array}$$
即 $bel(x_t)$ 服从均值和方差如上式所示的高斯分布， $bel(x_t)\sim N({\mu }_t,{\sigma }_t^2)$ 。

至此，我们已经计算出了 $bel(x_t)$ 的均值和方差。但是，它们和一维 Kalman 滤波器算法中更新阶段的三个公式是等价的吗？
答案是肯定的。因为，令：
$$K_t=\frac{{\bar{\sigma }}_t^2{c}_t}{{\bar{\sigma }}_t^2{c}_t^2+{\sigma }_{R,t}^2}$$
容易验证 ${\bar{\mu }}_t+K_t(z_t-c_t{\bar{\mu }}_t)$ 可以化为 $\frac{{\sigma }_{R,t}^2{\bar{\mu }}_t+{\bar{\sigma }}_t^2{c}_t{z}_t}{{\bar{\sigma }}_t^2{c}_t^2+{\sigma }_{R,t}^2}$ （即 ${\mu }_t$ ）。类似地，容易验证 $(1-K_t{c}_t){\bar{\sigma }}_t^2$ 可以化为 $\frac{{\bar{\sigma }}_t^2\cdot {\sigma }_{R,t}^2}{{\bar{\sigma }}_t^2{c}_t^2+{\sigma }_{R,t}^2}$ （即 ${\sigma }_t^2$ ）。

至此，一维 Kalman 滤波器算法中更新阶段三公式推导完毕。