本文详细介绍了如何通过贝叶斯推断推导卡尔曼滤波，从已知的k-1时刻后验概率出发，经过运动方程计算k时刻的先验概率，再结合观测方程求得联合概率密度函数，最终得出卡尔曼滤波的预测和更新公式，涉及线性高斯系统和自动驾驶中的状态估计。
1. 通过贝叶斯推断推导卡尔曼滤波
（1）首先，已知 k − 1 k-1 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)
（2）根据运动方程求 k k 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)中的 x ˇ k \check{x}_k xˇk与 P ˇ k \check{P}_k Pˇk，如下所示：
运动方程 : x k = A k − 1 x k − 1 + v k + w 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 P ˇ k = E [ ( x k − E [ x k ] ) ( x k − E [ x k ] ) T ] = E [ ( A k − 1 ( x K − 1 − x ^ k − 1 ) + w k ) ( A k − 1 ( x K − 1 − x ^ k − 1 ) + w k ) T ] = A k − 1 E [ ( x k − 1 − x ^ k − 1 ) ( x k − 1 − x ^ k − 1 ) T ] A k − 1 T + E [ w k ( x k − 1 − x ^ k − 1 ) T ] A k − 1 T + A k − 1 E [ ( x k − 1 − x ^ k − 1 ) w k T ] + E [ w k w k T ] = A k − 1 P ^ k − 1 A k − 1 T + 0 + 0 + Q k = A k − 1 P ^ k − 1 A k − 1 T + Q k 运动方程:x_k = A_{k-1}x_{k-1}+v_k+w_k \\ \check{x}_k = E[x_k] = E[A_{k-1}x_{k-1}+v_k+w_k] = A_{k-1}\hat{x}_{k-1} + v_k \\ \check{P}_k = E[(x_k-E[x_k])(x_k-E[x_k])^T] = E[(A_{k-1}(x_{K-1} - \hat{x}_{k-1})+w_k)(A_{k-1}(x_{K-1} - \hat{x}_{k-1})+w_k)^T] \\ = A_{k-1}E[(x_{k-1}-\hat{x}_{k-1})(x_{k-1}-\hat{x}_{k-1})^T]A_{k-1}^T + E[w_k(x_{k-1}-\hat{x}_{k-1})^T]A_{k-1}^T + A_{k-1}E[(x_{k-1} - \hat{x}_{k-1})w_k^T] + E[w_kw_k^T] \\ = A_{k-1}\hat{P}_{k-1}A_{k-1}^T+0+0+Q_k = A_{k-1}\hat{P}_{k-1}A_{k-1}^T+Q_k 运动方程:xk=Ak−1xk−1+vk+wkxˇk=E[xk]=E[Ak−1xk−1+vk+wk]=Ak−1x^k−1+vkPˇk=E[(xk−E[xk])(xk−E[xk])T]=E[(Ak−1(xK−1−x^k−1)+wk)(Ak−1(xK−1−x^k−1)+wk)T]=Ak−1E[(xk−1−x^k−1)(xk−1−x^k−1)T]Ak−1T+E[wk
最低0.47元/天 解锁文章
扫一扫
1320
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
