数学小抄: 概率角度推导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 ) \begin{split} &(\begin{bmatrix} x\\y \end{bmatrix}-\begin{bmatrix} \mu_x\\ \mu_y \end{bmatrix})^{\top}\begin{bmatrix} \Sigma_{xx} & \Sigma_{xy}\\ \Sigma_{yx} & \Sigma_{yy}\\ \end{bmatrix}^{-1}(\begin{bmatrix} x\\y \end{bmatrix}-\begin{bmatrix} \mu_x \\ \mu_y \end{bmatrix}) \\ &= (\begin{bmatrix} x\\y \end{bmatrix}-\begin{bmatrix} \mu_x\\ \mu_y \end{bmatrix})^{\top} \begin{bmatrix} 1 & 0\\ -\Sigma^{-1}_{yy}\Sigma_{yx} & 1\\ \end{bmatrix} \begin{bmatrix} (\Sigma_{xx}-\Sigma_{xy}\Sigma^{-1}_{yy}\Sigma_{yx})^{-1} & 0\\ 0&\Sigma^{-1}_{yy}\\ \end{bmatrix} \\ & \times \begin{bmatrix} 1 & -\Sigma_{xy}\Sigma^{-1}_{yy}\\ 0 & 1\\ \end{bmatrix} (\begin{bmatrix}x\\y\end{bmatrix} - \begin{bmatrix}\mu_x\\ \mu_y\end{bmatrix})\\ &=(x-\mu_x-\Sigma_{xy}\Sigma^{-1}_{yy}(y-\mu_y))^{\top}(\Sigma_{xx}-\Sigma_{xy}\Sigma^{-1}_{yy}\Sigma_{yx})^{-1}\\ & \times (x-\mu_x-\Sigma_{xy} \Sigma^{-1}_{yy}(y-\mu_y))+(y-\mu_y)^{\top}\Sigma^{-1}_{yy}(y-\mu_y) \end{split} ([xy]−[μxμy])⊤[ΣxxΣyxΣxyΣyy]−1([xy]−[μxμy])=([xy]−[μxμy])⊤[1−Σyy−1Σyx01][(Σxx−ΣxyΣyy−1Σyx)−100Σyy−1]×[10−ΣxyΣyy−11]([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 ) \begin{split} p(x,y) &= p(x|y)p(y) \\ p(x|y) &= \mathcal{N}(\mu_x+\Sigma_{xy}\Sigma^{-1}_{yy}(y-\mu_y),\Sigma_{xx}-\Sigma_{xy}\Sigma^{-1}_{yy}\Sigma_{yx})\\ p(y) &= \mathcal{N}(\mu_y,\Sigma_{yy}) \end{split} 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 ∣ x ˇ 0 , v 1 : k − 1 , y 0 : k − 1 ) = N ( x ^ k − 1 , P ^ 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}) p(xk−1∣xˇ0,v1:k−1,y0:k−1)=N(x^k−1,P^k−1)
考虑最近时刻的输入 v k v_k vk, 计算k时刻的高斯先验:
p ( x k ∣ x ˇ 0 , v 1 : k , y 0 : k − 1 ) = N ( x ˇ k , P ˇ k ) p(x_k|\check{x}_0,v_{1:k},y_{0:k-1})=\mathcal{N}(\check{x}_k,\check{P}_k) p(xk∣xˇ0,v1:k,y0:k−1)=N(xˇk,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 \begin{split} \check{P}_k&= A_{k-1}\hat{P}_{k-1}A^{\top}_{k-1}+Q_k\\ \check{x}_k&=A_{k-1}\hat{x}_{k-1}+v_k \end{split} Pˇkxˇk=Ak−1P^k−1Ak−1⊤+Qk=Ak−1x^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 \begin{split} \check{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}\hat{x}_{k-1}+v_k \end{split} xˇk=E[xk]=E[Ak−1xk−1+vk+wk]=Ak−1E[xk−1]+vk+E[wk]=Ak−1x^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 \begin{split} \check{P}_k &= E[(x_k-E[x_k])(x_k-E[x_k])^{\top}]\\ &=E[(A_{k-1}x_{k-1}+v_k+w_k-A_{k-1}\hat{x}_{k-1}-v_k)(A_{k-1}x_{k-1}+v_k+w_k-A_{k-1}\hat{x}_{k-1}-v_k)^{\top}]\\ &=A_{k-1}E[(x_{k-1}-\hat{x}_{k-1})(x_{k-1}-\hat{x}_{k-1})^{\top}]A^{\top}_{k-1}+E[w_kw^{\top}_k]\\ &=A_{k-1}\hat{P}_{k-1}A^{\top}_{k-1}+Q_k \end{split} Pˇk=E[(xk−E[xk])(xk−E[xk])⊤]=E[(Ak−1xk−1+vk+wk−Ak−1x^k−1−vk)(Ak−1xk−1+vk+wk−Ak−1x^k−1−vk)⊤]=Ak−1E[(xk−1−x^k−1)(xk−1−x^k−1)⊤]Ak−1⊤+E[wkwk⊤]=Ak−1P^k−1Ak−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 ] ) \begin{split} p(x_k,y_k|\check{x}_0, v_{1:k},y_{0:k-1})&=\mathcal{N}(\begin{bmatrix} \mu_x \\ \mu_y \end{bmatrix}, \begin{bmatrix} \Sigma_{xx} & \Sigma_{xy}\\ \Sigma_{yx} & \Sigma_{yy} \end{bmatrix})\\ &=\mathcal{N}(\begin{bmatrix} \check{x}_k\\ C_k\check{x}_k \end{bmatrix},\begin{bmatrix} \check{P}_k & \check{P}_kC^{\top}_k\\ C_k\check{P}_k & C_k\check{P}_kC^{\top}_k+R_k \end{bmatrix}) \end{split} p(xk,yk∣xˇ0,v1:k,y0:k−1)=N([μxμy],[ΣxxΣyxΣxyΣyy])=N([xˇkCkxˇk],[PˇkCkPˇkPˇkCk⊤CkPˇkCk⊤+Rk])
结合高维高斯分布的性质
p ( x k ∣ x ˇ k , v 1 : k , y 0 : k ) = N ( μ x + Σ x y Σ y y − 1 ( y k − μ y ) , Σ x x − Σ x y Σ y y − 1 Σ y x ) p(x_k|\check{x}_k,v_{1:k},y_{0:k})=\mathcal{N}(\mu_x+\Sigma_{xy}\Sigma^{-1}_{yy}(y_k-\mu_y),\Sigma_{xx}-\Sigma_{xy}\Sigma^{-1}_{yy}\Sigma_{yx}) p(xk∣xˇk,v1:k,y0:k)=N(μx+ΣxyΣyy−1(yk−μy),Σxx−ΣxyΣyy−1Σyx)
x ^ k \hat{x}_k x^k作为均值, P ^ k \hat{P}_k 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 ) \begin{split} K_k &= \check{P}_kC^{\top}_k(C_k\check{P}_kC^{\top}_k+R_k)^{-1}\\ \hat{P}_k &= (1-K_kC_k)\check{P}_k\\ \hat{x}_k&=\check{x}_k+K_k(y_k-C_k\check{x}_k) \end{split} KkP^kx^k=PˇkCk⊤(CkPˇkCk⊤+Rk)−1=(1−KkCk)Pˇk=xˇk+Kk(yk−Ckxˇk)