卡尔曼滤波器

卡尔曼滤波器 参考资料： .bilibili /video/BV12P411V7pc/?spm_id_from=333.337.search-card.all.click&vd_source=2f16c81b2e6b252c304116c646e6512c

卡尔曼滤波器是线性滤波器 在这里插入图片描述 状态预测公式： x ^ t − = F t x ^ t − 1 + B t u t \hat{x}_t^{-}=F_t \hat{x}_{t-1}+B_t u_t x^t−​=Ft​x^t−1​+Bt​ut​ F t F_t Ft​是状态转移矩阵，表示如何从上一时刻的状态来推测这一时刻的状态； B t B_t Bt​表示控制矩阵，表示控制量$ u_t$如何作用于当前状态； x ^ \hat{x} x^表示对 x x x的估计量而不是真实值，因为汽车的真实状态我们是不知道的，智能观测后估计

估计会带来不确定性，用协方差矩阵来表示： cov ⁡ ( x , x ) = [ σ 11 σ 12 ] \operatorname{cov}(x, x)=\left[\begin{array}{ll} \sigma_{11} & \sigma_{12} \end{array}\right] cov(x,x)=[σ11​​σ12​​] 最终的噪声协方差阵的传递： P t − = F P t − 1 F T + Q P_t^{-}=F P_{t-1} F^T+Q Pt−​=FPt−1​FT+Q 其中， Q Q Q表示预测模型本身带来的噪声，上面这个公式表示不确定性在各个时刻之间的传递关系 上图的红色 v v v表示的是 z t z_t zt​的观测噪声 其中， K t K_t Kt​是卡尔曼滤波的系数。 x ^ t − \hat{x}_t^{-} x^t−​是先验估计，是预测状态 x ^ t \hat{x}_t x^t​是平衡预测和估计的最终状态 卡尔曼滤波的理解：