1.卡尔曼公式详解 - 重点！！！

预测当前的值也叫先验估计

---

\(\hat x_t^- = F\hat x_{t-1}+B\mu_{t-1}\)
基于之前的最优估计来推出当阶段的先验估计

---

协方差公式
\(P_t^-=FP_{t-1}F^T+Q\)
\(P_t^- 是\hat x_t^时刻的协方差矩阵\)

---

\(K_t ,卡尔曼增益, 很重要!!!\)
\(\hat x_t 最优估计-\color{red}{这是我们最后要得到的东西}\)

---

\(\hat x_t = \hat x_t^- + K_t (z_t - H \hat x_t^-)\)
\(z_t是观测值,\hat x_t是最优估计 = \hat x_t^-(估计值) + K_t (z_t - H \hat x_t^-)(观测的修正)\)

---

协方差矩阵的更新
\(P_t=(I-K_tH)P_t^-\)

2.举例说明

预测

有一个二维矢量,一个维度是p-position，小车的位置，还有一个是v，小车的速度

\(\begin{bmatrix} p\\ v\\ \end{bmatrix}\)

假设小车是匀加速直线运动，则根据高中的物理模型，有如下预测模型

\(这里的表达式就是预测中的\color{red}{第一个公式},先验估计公式\)

---

然后计算先验估计对应的协方差矩阵

这里应用了几个协方差公式

\(cov(x,x)=Var(x)\)
\(cov(Ax,Ax)=Acov(x,x)A^T\)
\(cov(Ax+k,Ax+k)=Acov(x,x)A^T\)
\(所以有\)


这里可以看出和原来的2式就差了后面的Q，是因为上面的1式中的先验估计少了噪声\(w_t\)

测量

给小车加个gps观察

现在有测量模型
\(\Delta p是观测器的噪声，误差\)

这里注意一点，测量变量的维数不一定很状态变量的维度向量，这个例子中很明显，速度这个维度是不观测的，但是速度变量又参与了预测模型的计算

状态更新

这三个公式的推导没有讲，就讲了应用，其实就是一个循环迭代的过程，类似梯度下降

这里H举了一个一维的例子，主要为了告诉我们，K是关于Q和R的，可以通过Q，R这两个超参数来调整K,告知模型是更加信任预测值还是更加信任观测值

最后就是一个循环迭代的过程

2.调整超参数

\(图片里面的化简指的是F,H取一维的化简\)

3.卡尔曼滤波的使用过程