卡尔曼滤波器 bilibili视频版

卡尔曼滤波器又叫最佳线性滤波器，好处有实现简单，纯时域（无须频域变换）所以在工程上又很广泛的应用。

因为它只能描述状态与状态之间的线性关系。 既然是线性关系，所以就可以用矩阵表示出来（如下图） 。

图中的式子为状态预测公式，其中的 FtFt才是最佳的估计值。

有了状态预测公式就可以推测下一时刻的状态，但是推测总是含有噪声的，噪声越大，不确定性就越大。可以用协方差矩阵来衡量一次推测的不确定性

一维情况：

二维情况：

在卡尔曼滤波器中，所有关于不确定性的表述都要用到协方差矩阵。

在我们的小车例子中，每一个时刻状态的不确定性都是由协方差矩阵P来表示的，下一个问题就是我们如何让这种不确定性在每个时刻之间传递呢，答案就是乘上状态转移矩阵F，左右各乘（这是利用了协方差的性质）

此外，还要额外加上协方差矩阵Q来表示预测模型本身带来的不确定性

上面就是卡尔曼滤波器的第二个公式，它表示不确定性在各个时刻之间的传递关系。
假设有一个激光器用于实时测量汽车的位置：

此处，R只是一个标量，因为ztzt就会变成一个多维的列向量，它会包含每一种测量方式的测量值，而每一个测量值都只是真实状态的一种不完全表现，我们可以从几种不完整的表述中推断出真实的状态，而卡尔曼滤波器的数据融合功能正是在测量矩阵中体现出来的。

我们已经有了观测值ztzt，我们只需要在它后面再加上一项作为修正，就可以得到最佳的估计了。

修正项就是真实的观测值减去预期的观测值得到残差，再乘以一个系数KtKt中的速度。）。

好了只剩最后一步了，最后一步就是更新最佳估计值的噪声分布，这个值是留给下一轮迭代时用的，在这一步里，状态的不确定性是减小的，而在下一轮迭代里，由于传递噪声的引入，不确定性又会增大。卡尔曼滤波器就是在这种不确定性的变化中寻求一种平衡的。

到现在为止，我们已经有了卡尔曼滤波器的所有5个公式：

左边两个式子是用来预测，右边三个式子是使用观测值来更新预测值以得到最佳估计值。

matlab实现（上面的小车）：

结果：

可见只用了很少的几次迭代，图像就稳定在1附近，可见卡尔曼滤波器的功能还是很强大的。

---

课程到此结束，但是还有一些没搞懂，具体是卡尔曼滤波器那五个公式的推导与理解，尽快完善。 本文非原创，为转载，后期更改分类时误选了原创后转自何处找不到了，反正，本文转载，感谢原创