卡尔曼滤波器
参考资料：https://www.bilibili.com/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−=Ftx^t−1+Btut
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−1FT+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是平衡预测和估计的最终状态
卡尔曼滤波的理解：
