码迷,mamicode.com
首页 > 其他好文 > 详细

Kalman Filter的数学推导

时间:2017-03-19 10:53:01      阅读:189      评论:0      收藏:0      [点我收藏+]

标签:mem   sigma   线性变换   com   数学   lte   exp   cts   sys   

关于Kalman Filter,有一篇很好的入门文章,建议先通读:

Understanding the Basis of the Kalman Filter Via a Simple and ...

不过这篇文章只推导了一元分布的情况,对多元分布的情况只是简单地写出结论。因此这里给出推导过程。

 

预备知识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:正态分布的乘积

有两个人,一个人说$x$服从$N(\mu_1,\Sigma_1)$的正态分布,另一个人说$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 ... - TINA vision system

 

上面两个式子表达很简洁,但是为了引入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)$

 

上面两个式子都出现了$\Sigma_1(\Sigma_1+\Sigma_2)^{-1}$,所以令$J=\Sigma_1(\Sigma_1+\Sigma_2)^{-1}$,有:

$\Sigma=\Sigma_1-J\Sigma_1$

$\mu=\mu_1+J(\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^{-1}z_t=x_t+H^{-1}v_t$

$x_t=H^{-1}z_t-H^{-1}v_t$

正态分布在线性变换后依然是正态分布,$H^{-1}v_t\sim N(0, H^{-1}R_tH^{-T})$

因此,测量过程表明:$x_t\sim N(H^{-1}z_t, H^{-1}R_tH^{-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^{-1}R_tH^{-T})^{-1}(H^{-1}z_t-\hat x_{t|t-1})$

$=

$P_{t|t}=$

Kalman Filter的数学推导

标签:mem   sigma   线性变换   com   数学   lte   exp   cts   sys   

原文地址:http://www.cnblogs.com/milaohu/p/6577341.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!