由bayes滤波导出标准Kalman滤波

来源：互联网发布：smt贴片机编程编辑：程序博客网时间：2024/06/04 00:53

bayes滤波

考虑一般的离散时间动态系统：

x k + 1 y k = f k (x k, v k) = h k (x k, w k)

假设
（1）初始状态

x0 的概率密度

p(x0) 已知；
（2）过程噪声

vk 和观测噪声

wk 的统计性质已知；
（3）状态

x0，过程噪声

{v1,v2,...} 和观测噪声

{w1,w2,...} 三者独立.

我们知道在观测结果为 y1:k 的条件下，状态 xk 的最小均方误差估计为：

x k | k = E (x k | y 1 : k)

下面给出 bayes 滤波的公式：

p (x k + 1 | y 1 : k) = \int p (x k + 1, x k | y 1 : k) = \int p (x k + 1 | x k) p (x k | y 1 : k) d x k (1)

p (y k + 1 | y 1 : k) = \int p (y k + 1, x k + 1 | y 1 : k) d x k + 1 = \int p (y k + 1 | x k + 1) p (x k + 1 | y 1 : k) (3)

p (x k + 1 | y 1 : k + 1) = p ( x k + 1 , y k + 1 , y 1 : k ) p ( y 1 : k + 1 ) = p ( y k + 1 | x k + 1 ) p ( x k + 1 | y 1 : k ) p ( y 1 : k + 1 ) (3)

于是

x^k + 1 | k y^k + 1 | k x^k + 1 = \int x k + 1 p (x k + 1 | y 1 : k) d x k + 1 (4) = \int y k + 1 p (y k + 1 | y 1 : k) d y k + 1 (5) = \int x k + 1 p (x k + 1 | y 1 : k + 1) d x k + 1 (6)

标准Kalman滤波

考虑离散时间动态系统：

x k + 1 y k = A k x k + f k u k + w k = C k x k + g k u k + v k

设系统满足上述三个条件，

x0 服从一个已知的正态分布，且过程噪声

vk 和观测噪声

wk 是零均值的高斯分布，协方差分别为

Qk,Rk.

k=1,2,3...

x^k + 1 | k = \int x k + 1 p (x k + 1 | y 1 : k) d x k + 1 = \int x k + 1 \int p (x k + 1 | x k) p (x k | y 1 : k) d x k d x k + 1 = \int \int x k + 1 p (x k + 1 | x k) p (x k | y 1 : k) d x k + 1 d x k (f u b i n i 定 理) = \int (A k x k + f k u k) p (x k | y 1 : k) d x k = A k x^k + f k u k (7)

Σ k + 1 | k = E (x^k + 1 | k - x k + 1) (x^k + 1 | k - x k + 1) T = E (A k (x^k - x k) - w k) (A k (x^k - x k) - w k) T = A k E (x^k - x k) (x^k - x k) T A T k + Q k = A k Σ k | k A T k + Q k (8)

y^k + 1 | k = \int y k + 1 p (y k + 1 | y 1 : k) d y k + 1 = \int y k + 1 \int p (y k + 1 | x k + 1) p (x k + 1 | y 1 : k) d x k + 1 d y k + 1 = \int \int y k + 1 p (y k + 1 | x k + 1) p (x k + 1 | y 1 : k) d y k + 1 d x k + 1 (f u b i n i 定 理) = \int (C k + 1 x k + 1 + f k + 1 u k + 1) p (x k + 1 | y 1 : k) d x k = C k + 1 x^k + 1 | k + f k + 1 u k + 1 (9)

S k + 1 = E (y^k + 1 | k - y k + 1) (y^k + 1 | k - y k + 1) T = E (C k + 1 (x^k + 1 | k - x k + 1) - v k + 1) (C k + 1 (x^k + 1 | k - x k + 1) - v k + 1) T = C k + 1 E (x^k + 1 | k - x k + 1) (x^k + 1 | k - x k + 1) T C T k + 1 + R k + 1 = C k + 1 Σ k + 1 | k C T k + 1 + R k + 1 (10)

一个有用的引理：
若 X 和 Y|X 都服从正态分布，且Y|X∼N(Cx+D,R),则 (X,Y) 也服从正态分布.

命题1：在标准Kalman滤波中，p(xk+1|y1:k)∼N(x^k+1|k,Σk+1|k).

证明：首先证明 p(xk+1|y1:k) 服从正态分布.
使用数学归纳法.
由于 p(x1) 和 p(y1|x1) 都服从正态分布，由上述引理可知
p(x1,y1) 是正态密度函数。
假设 p(xk,y1:k) 是正态的，则

p (x k + 1, x k, y 1 : k) = p (x k + 1 | x k, y 1 : k) p (x k, y 1 : k) = p (x k + 1 | x k) p (x k, y 1 : k) (M a r k o v 性)

是正态的.从而它的边际分布

p (x k + 1, y 1 : k) = \int p (x k + 1, x k, y 1 : k) d x k

是正态的.于是得到它的条件分布

p (x k + 1 | y 1 : k)

是正态的.
同理可以证明

p (x k + 1, y 1 : k + 1) = p (y k + 1 | x k + 1, y 1 : k) p (x k + 1, y 1 : k) = p (y k + 1 | x k + 1) p (x k + 1, y 1 : k) (M a r k o v 性)

是正态的.再由（7）、（8）即可证明此命题.

命题2(Swiss-Army-Knife for Gaussians)：

$N (y; C x + D, R) N (x; μ, P) = N (y; C μ + D, C P C T + R) N (x; m, P - K C P)$
其中，
$K = P C T (C P C T + R) - 1$
$m = μ + K (y - C μ - D)$
命题2的证明可以由正态分布的条件密度函数直接得到.

x^k + 1 = \int x k + 1 p ( y k + 1 | x k + 1 ) p ( x k + 1 | y 1 : k ) p ( y 1 : k ) d x k + 1 p ( y 1 : k + 1 ) = \int x k + 1 p ( y k + 1 | x k + 1 ) p ( x k + 1 | y 1 : k ) d x k + 1 \int p ( y k + 1 | x k + 1 ) p ( x k + 1 | y 1 : k ) d x k + 1 = \int x k + 1 p ˜ ( x k + 1 ) N ( y k + 1 ; y k + 1 | k , S k + 1 ) d x k + 1 \int p ˜ ( x k + 1 ) N ( y k + 1 ; y k + 1 | k , S k + 1 ) d x k + 1 (命 题 2) = x^k + 1 | k + Σ k + 1 | k C T k + 1 (R k + 1 + C k + 1 Σ k + 1 | k C T k + 1) - 1 (y k + 1 - y^k + 1 | k) (11)

其中，

p ˜ (x k + 1) = N (x k + 1; x^k + 1 | k + Σ k + 1 | k C T k + 1 (R k + 1 + C k + 1 Σ k + 1 | k C T k + 1) - 1 (y k + 1 - y^k + 1 | k), Σ k + 1 | k - Σ k + 1 | k C T k + 1 (R k + 1 + C k + 1 Σ k + 1 | k C T k + 1) - 1 C k + 1 Σ k + 1 | k)

由此可直接得出

Σ k + 1 = Σ k + 1 | k - Σ k + 1 | k C T k + 1 (R k + 1 + C k + 1 Σ k + 1 | k C T k + 1) - 1 C k + 1 Σ k + 1 | k (12)

至此，标准Kalman滤波公式就已经完全由bayes滤波导出了.

1 0