数学小抄: 概率角度推导Kalman Filter

复习

---

参考: [机器人学中的状态估计]
联合概率密度指数部分:

( [ x y ] − [ μ x μ y ] ) ⊤ [ Σ x x Σ x y Σ y x Σ y y ] − 1 ( [ x y ] − [ μ x μ y ] ) = ( [ x y ] − [ μ x μ y ] ) ⊤ [ 1 0 − Σ y y − 1 Σ y x 1 ] [ ( Σ x x − Σ x y Σ y y − 1 Σ y x ) − 1 0 0 Σ y y − 1 ] × [ 1 − Σ x y Σ y y − 1 0 1 ] ( [ x y ] − [ μ x μ y ] ) = ( x − μ x − Σ x y Σ y y − 1 ( y − μ y ) ) ⊤ ( Σ x x − Σ x y Σ y y − 1 Σ y x ) − 1 × ( x − μ x − Σ x y Σ y y − 1 ( y − μ y ) ) + ( y − μ y ) ⊤ Σ y y − 1 ( y − μ y ) ([xy]−[μxμy])⊤[ΣxxΣxyΣyxΣyy]−1([xy]−[μxμy])=([xy]−[μxμy])⊤[10−Σ−1yyΣyx1][(Σxx−ΣxyΣ−1yyΣyx)−100Σ−1yy]×[1−ΣxyΣ−1yy01]([xy]−[μxμy])=(x−μx−ΣxyΣ−1yy(y−μy))⊤(Σxx−ΣxyΣ−1yyΣyx)−1×(x−μx−ΣxyΣ−1yy(y−μy))+(y−μy)⊤Σ−1yy(y−μy)

​([xy​]−[μx​μy​​])⊤[Σxx​Σyx​​Σxy​Σyy​​]−1([xy​]−[μx​μy​​])=([xy​]−[μx​μy​​])⊤[1−Σyy−1​Σyx​​01​][(Σxx​−Σxy​Σyy−1​Σyx​)−10​0Σyy−1​​]×[10​−Σxy​Σyy−1​1​]([xy​]−[μx​μy​​])=(x−μx​−Σxy​Σyy−1​(y−μy​))⊤(Σxx​−Σxy​Σyy−1​Σyx​)−1×(x−μx​−Σxy​Σyy−1​(y−μy​))+(y−μy​)⊤Σyy−1​(y−μy​)​

p ( x , y ) = p ( x ∣ y ) p ( y ) p ( x ∣ y ) = N ( μ x + Σ x y Σ y y − 1 ( y − μ y ) , Σ x x − Σ x y Σ y y − 1 Σ y x ) p ( y ) = N ( μ y , Σ y y ) p(x,y)=p(x|y)p(y)p(x|y)=N(μx+ΣxyΣ−1yy(y−μy),Σxx−ΣxyΣ−1yyΣyx)p(y)=N(μy,Σyy)

p(x,y)p(x∣y)p(y)​=p(x∣y)p(y)=N(μx​+Σxy​Σyy−1​(y−μy​),Σxx​−Σxy​Σyy−1​Σyx​)=N(μy​,Σyy​)​
高斯分布为指数形式, 指数的乘积为等于幂次项的相加

$(\hat{\cdot })$表示后验, $(\check{\cdot })$表示先验, 无上标表示真值

k-1时刻的高斯后验为:
$$p(x_{k-1}|{\check{x}}_0,v_{1:k-1},y_{0:k-1})=\mathcal{N}({\hat{x}}_{k-1},{\hat{P}}_{k-1})$$
考虑最近时刻的输入$v_k$, 计算k时刻的高斯先验:
$$p(x_k|{\check{x}}_0,v_{1:k},y_{0:k-1})=\mathcal{N}({\check{x}}_k,{\check{P}}_k)$$

其中
P ˇ k = A k − 1 P ^ k − 1 A k − 1 ⊤ + Q k x ˇ k = A k − 1 x ^ k − 1 + v k ˇPk=Ak−1ˆPk−1A⊤k−1+Qkˇxk=Ak−1ˆxk−1+vk

Pˇk​xˇk​​=Ak−1​P^k−1​Ak−1⊤​+Qk​=Ak−1​x^k−1​+vk​​

x ˇ k = E [ x k ] = E [ A k − 1 x k − 1 + v k + w k ] = A k − 1 E [ x k − 1 ] + v k + E [ w k ] = A k − 1 x ^ k − 1 + v k ˇxk=E[xk]=E[Ak−1xk−1+vk+wk]=Ak−1E[xk−1]+vk+E[wk]=Ak−1ˆxk−1+vk

xˇk​=E[xk​]​=E[Ak−1​xk−1​+vk​+wk​]=Ak−1​E[xk−1​]+vk​+E[wk​]=Ak−1​x^k−1​+vk​​

对于协方差有:
P ˇ k = E [ ( x k − E [ x k ] ) ( x k − E [ x k ] ) ⊤ ] = E [ ( A k − 1 x k − 1 + v k + w k − A k − 1 x ^ k − 1 − v k ) ( A k − 1 x k − 1 + v k + w k − A k − 1 x ^ k − 1 − v k ) ⊤ ] = A k − 1 E [ ( x k − 1 − x ^ k − 1 ) ( x k − 1 − x ^ k − 1 ) ⊤ ] A k − 1 ⊤ + E [ w k w k ⊤ ] = A k − 1 P ^ k − 1 A k − 1 ⊤ + Q k ˇPk=E[(xk−E[xk])(xk−E[xk])⊤]=E[(Ak−1xk−1+vk+wk−Ak−1ˆxk−1−vk)(Ak−1xk−1+vk+wk−Ak−1ˆxk−1−vk)⊤]=Ak−1E[(xk−1−ˆxk−1)(xk−1−ˆxk−1)⊤]A⊤k−1+E[wkw⊤k]=Ak−1ˆPk−1A⊤k−1+Qk

Pˇk​​=E[(xk​−E[xk​])(xk​−E[xk​])⊤]=E[(Ak−1​xk−1​+vk​+wk​−Ak−1​x^k−1​−vk​)(Ak−1​xk−1​+vk​+wk​−Ak−1​x^k−1​−vk​)⊤]=Ak−1​E[(xk−1​−x^k−1​)(xk−1​−x^k−1​)⊤]Ak−1⊤​+E[wk​wk⊤​]=Ak−1​P^k−1​Ak−1⊤​+Qk​​

对于更新部分(状态与最近一次测量(即k时刻)):
p ( x k , y k ∣ x ˇ 0 , v 1 : k , y 0 : k − 1 ) = N ( [ μ x μ y ] , [ Σ x x Σ x y Σ y x Σ y y ] ) = N ( [ x ˇ k C k x ˇ k ] , [ P ˇ k P ˇ k C k ⊤ C k P ˇ k C k P ˇ k C k ⊤ + R k ] ) p(xk,yk|ˇx0,v1:k,y0:k−1)=N([μxμy],[ΣxxΣxyΣyxΣyy])=N([ˇxkCkˇxk],[ˇPkˇPkC⊤kCkˇPkCkˇPkC⊤k+Rk])

p(xk​,yk​∣xˇ0​,v1:k​,y0:k−1​)​=N([μx​μy​​],[Σxx​Σyx​​Σxy​Σyy​​])=N([xˇk​Ck​xˇk​​],[Pˇk​Ck​Pˇk​​Pˇk​Ck⊤​Ck​Pˇk​Ck⊤​+Rk​​])​

结合高维高斯分布的性质
$$p(x_k|{\check{x}}_k,v_{1:k},y_{0:k})=\mathcal{N}({\mu }_x+{\Sigma }_{xy}{\Sigma }_{yy}^{-1}(y_k-{\mu }_y),{\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx})$$

${\hat{x}}_k$作为均值, ${\hat{P}}_k$作为协方差:
K k = P ˇ k C k ⊤ ( C k P ˇ k C k ⊤ + R k ) − 1 P ^ k = ( 1 − K k C k ) P ˇ k x ^ k = x ˇ k + K k ( y k − C k x ˇ k ) Kk=ˇPkC⊤k(CkˇPkC⊤k+Rk)−1ˆPk=(1−KkCk)ˇPkˆxk=ˇxk+Kk(yk−Ckˇxk)

Kk​P^k​x^k​​=Pˇk​Ck⊤​(Ck​Pˇk​Ck⊤​+Rk​)−1=(1−Kk​Ck​)Pˇk​=xˇk​+Kk​(yk​−Ck​xˇk​)​