Kalman Filter的数学推导
前言
关于Kalman Filter，有一篇很好的入门文章，建议先通读：
Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation
不过这篇文章只推导了一元分布的情况，对多元分布的情况只是简单地写出结论。因此这里给出推导过程。
预备知识1：多元正态分布
多元正态分布$N(\mu,\Sigma)$的分布密度函数为：
$p(x)=\frac{1}{(2\pi)^{\frac{k}{2}}|\Sigma|^{\frac{1}{2}}}\exp(-\frac{(x-\mu)^T\Sigma^{-1}(x-\mu)}{2})$
对于没有涉及过多元统计的读者来说，重点在于理解协方差矩阵$\Sigma$。
具体介绍参看：The Multivariate Gaussian Distribution - CS 229
分布函数中还出现了协方差行列式$|\Sigma|$，这是一个Jacobian行列式，具体用途请复习多变量积分。
预备知识2：正态分布的乘积
对于同一个随机变量有两个预测：1、$x$服从$N(\mu_1,\Sigma_1)$的正态分布，2、$x$服从$N(\mu_2,\Sigma_2)$的正态分布。
作为一个中庸主义者，希望对$x$估算一个值，使得同时符合两种预测的似然最大。因此最直接的方式是将两个概率分布相乘。结果依然是正态分布，且
$\Sigma=(\Sigma_1^{-1}+\Sigma_2{-1})^{-1}$
$\mu=(\Sigma_1^{-1}+\Sigma_2{-1})^{-1}(\Sigma_1^{-1}\mu_1+\Sigma_2^{-1}\mu_2)$
具体推导参看： Products and Convolutions of Gaussian Probability Density Functions
上面两个式子表达很简洁，但是为了引入Kalman Gain这个概念，我们要对结果作进一步的推导。如果不熟悉矩阵运算尤其是逆矩阵的变换技巧，这个推导过程会显得晦涩，可以把整个过程使用单变量（而不是多变量）来演算一遍，再对照下面这个过程，会豁然开朗。
$\Sigma=(\Sigma_1^{-1}+\Sigma_2^{-1})^{-1}$
$=\Sigma_1\Sigma_1^{-1}(\Sigma_1^{-1}+\Sigma_2^{-1})^{-1}\Sigma_2^{-1}\Sigma_2$
$=\Sigma_1(\Sigma_1^{-1}(\Sigma_1^{-1}+\Sigma_2^{-1})^{-1}\Sigma_2^{-1})\Sigma_2$
$=\Sigma_1(\Sigma_1+\Sigma_2)^{-1}\Sigma_2$
$=\Sigma_1-\Sigma_1(\Sigma_1+\Sigma_2)^{-1}\Sigma_1$
$\mu=(\Sigma_1^{-1}+\Sigma_2^{-1})^{-1}(\Sigma_1^{-1}\mu_1+\Sigma_2^{-1}\mu_2)$
$=(\Sigma_1^{-1}+\Sigma_2^{-1})^{-1}\Sigma_1^{-1}\mu_1+(\Sigma_1^{-1}+\Sigma_2{-1})^{-1}\Sigma_2^{-1}\mu_2$
$=\Sigma_2(\Sigma_1+\Sigma_2)^{-1}\Sigma_1\Sigma_1^{-1}\mu_1+\Sigma_1(\Sigma_1+\Sigma_2)^{-1}\Sigma_2\Sigma_2^{-1}\mu_2$
$=\Sigma_2(\Sigma_1+\Sigma_2)^{-1}\mu_1+\Sigma_1(\Sigma_1+\Sigma_2)^{-1}\mu_2$
$=(\Sigma_1+\Sigma_2)(\Sigma_1+\Sigma_2)^{-1}\mu_1-\Sigma_1(\Sigma_1+\Sigma_2)^{-1}\mu_1+\Sigma_1(\Sigma_1+\Sigma_2)^{-1}\mu_2$
$=\mu_1+\Sigma_1(\Sigma_1+\Sigma_2)^{-1}(\mu_2-\mu_1)$
Kalman Filter的推导：
1、状态转移：
$x_t=F_tx_{t-1}+B_tu_t+w_t$
$w_t\sim N(0, Q_t)$
2、测量过程：
$z_t=H_tx_t+v_t$
$v_t\sim N(0, R_t)$
将测量值转换到状态空间中：
$H_t^{-1}z_t=x_t+H_t^{-1}v_t$
$x_t=H_t^{-1}z_t-H_t^{-1}v_t$
正态分布在线性变换后依然是正态分布，$H_t^{-1}v_t\sim N(0, H_t^{-1}R_tH_t^{-T})$
因此，测量过程表明：$x_t\sim N(H_t^{-1}z_t, H_t^{-1}R_tH_t^{-T})$
3、估计过程：
$\hat x_{t|t-1}=F_t \hat x_{t-1|t-1}+B_tu_t$
估计误差的协方差矩阵：
$P_{t|t-1}=Cov(x_t-\hat x_{t|t-1})=E[x_t-\hat x_{t|t-1})(x_t-\hat x_{t|t-1})^T]=FP_{t-1|t-1}F^T+Q_t$
因此估计过程表明：$x_t\sim N(\hat x_{t|t-1}, P_{t|t-1})$
4、组合
测量过程和估计过程都对$x_t$作了阐述，因此，引入预备知识二中的公式将两者组合到一起，
$x_t\sim N(\hat x_{t|t}, P_{t|t})$
其中
$\hat x_{t|t}=\mu_1+\Sigma_1(\Sigma_1+\Sigma_2)^{-1}(\mu_2-\mu_1)$
$=\hat x_{t|t-1}+P_{t|t-1}(P_{t|t-1}+H_t^{-1}R_tH_t^{-T})^{-1}(H_t^{-1}z_t-\hat x_{t|t-1})$
$=\hat x_{t|t-1}+P_{t|t-1}H_t^TH_t^{-T}(P_{t|t-1}+H_t^{-1}R_tH_t^{-T})^{-1}H_t^{-1}H_t(H_t^{-1}z_t-\hat x_{t|t-1})$
$=\hat x_{t|t-1}+P_{t|t-1}H_t^T(H_tP_{t|t-1}H_t^T+R_t)^{-1}(z_t-H_t\hat x_{t|t-1})$
$P_{t|t}=\Sigma_1-\Sigma_1(\Sigma_1+\Sigma_2)^{-1}\Sigma_1$
$=P_{t|t-1}-P_{t|t-1}(P_{t|t-1}+H_t^{-1}R_tH_t^{-T})^{-1}P_{t|t-1}$
$=P_{t|t-1}-P_{t|t-1}H_t^TH_t^{-T}(P_{t|t-1}+H_t^{-1}R_tH_t^{-T})^{-1}H_t^{-1}H_tP_{t|t-1}$
$=P_{t|t-1}-P_{t|t-1}H_t^T(H_tP_{t|t-1}H_t^T+R_t)^{-1}H_tP_{t|t-1}$
令$K=P_{t|t-1}H_t^T(H_tP_{t|t-1}H_t^T+R_t)^{-1}$，代入上述两式，有：
$\hat x_{t|t}=\hat x_{t|t-1}+K(z_t-H_t\hat x_{t|t-1})$
$P_{t|t}=P_{t|t-1}-KH_tP_{t|t-1}$
