卡尔曼滤波器

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

卡尔曼滤波器是线性滤波器
在这里插入图片描述

状态预测公式：
$${\hat{x}}_t^{-}=F_t{\hat{x}}_{t-1}+B_t{u}_t$$
$F_t$是状态转移矩阵，表示如何从上一时刻的状态来推测这一时刻的状态；
$B_t$表示控制矩阵，表示控制量$ u_t$如何作用于当前状态；
$\hat{x}$表示对$x$的估计量而不是真实值，因为汽车的真实状态我们是不知道的，智能观测后估计

估计会带来不确定性，用协方差矩阵来表示：

$$\mathrm{cov}(x,x)=\left[\begin{array}{ll}{\sigma }_{11}& {\sigma }_{12}\end{array}\right]$$


最终的噪声协方差阵的传递：
$$P_t^{-}=F{P}_{t-1}{F}^T+Q$$
其中，$Q$表示预测模型本身带来的噪声，上面这个公式表示不确定性在各个时刻之间的传递关系

上图的红色$v$表示的是$z_t$的观测噪声

其中，$K_t$是卡尔曼滤波的系数。


${\hat{x}}_t^{-}$是先验估计，是预测状态
${\hat{x}}_t$是平衡预测和估计的最终状态
卡尔曼滤波的理解：