数学小抄: 概率角度推导Kalman Filter
复习
参考: [机器人学中的状态估计]
联合概率密度指数部分:
([xy]−[μxμy])⊤[ΣxxΣxyΣyxΣyy]−1([xy]−[μxμy])=([xy]−[μxμy])⊤[10−Σyy−1Σyx1][(Σxx−ΣxyΣyy−1Σyx)−100Σyy−1]×[1−ΣxyΣyy−101]([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)\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+ΣxyΣyy−1(y−μy),Σxx−ΣxyΣyy−1Σyx)p(y)=N(μy,Σyy)\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(xk−1∣xˇ0,v1:k−1,y0: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)
考虑最近时刻的输入vkv_kvk, 计算k时刻的高斯先验:
p(xk∣xˇ0,v1:k,y0: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=Ak−1P^k−1Ak−1⊤+Qkxˇk=Ak−1x^k−1+vk\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[xk]=E[Ak−1xk−1+vk+wk]=Ak−1E[xk−1]+vk+E[wk]=Ak−1x^k−1+vk\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[(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\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(xk,yk∣xˇ0,v1:k,y0:k−1)=N([μxμy],[ΣxxΣxyΣyxΣyy])=N([xˇkCkxˇk],[PˇkPˇkCk⊤CkPˇkCkPˇkCk⊤+Rk])\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(xk∣xˇk,v1:k,y0:k)=N(μx+ΣxyΣyy−1(yk−μy),Σxx−ΣxyΣyy−1Σyx)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}_kx^k作为均值, P^k\hat{P}_kP^k作为协方差:
Kk=PˇkCk⊤(CkPˇkCk⊤+Rk)−1P^k=(1−KkCk)Pˇkx^k=xˇk+Kk(yk−Ckxˇ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)