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}$