卡尔曼滤波器（to be continued）

下述来自Vic在Youtube上的视频

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



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



图中的式子为状态预测公式，其中的 F t F_t是状态转移矩阵，B t B_t是控制矩阵。观察到x x上都有一个倒三角，说明这是对于x x的估计，因为x x的真实值是永远也无法知道的，只能通过观测尽可能地推测这个值；等号左边的x x还有一个减号，说明这是通过上一时刻的值推导得到的。待会还要通过观测量来修正这个值，最后得到的没有减号的x x才是最佳的估计值。

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

一维情况：



二维情况：



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

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



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



上面就是卡尔曼滤波器的第二个公式，它表示不确定性在各个时刻之间的传递关系。

假设有一个激光器用于实时测量汽车的位置：



此处，R只是一个标量，因为z t z_t此处只是一个标量（表示位置），如果我们除了激光测距仪之外还有别的方法可以观测到汽车的某项特征，那么z t z_t就会变成一个多维的列向量，它会包含每一种测量方式的测量值，而每一个测量值都只是真实状态的一种不完全表现，我们可以从几种不完整的表述中推断出真实的状态，而卡尔曼滤波器的数据融合功能正是在测量矩阵中体现出来的。

我们已经有了观测值z t z_t以及噪声的协方差矩阵R，那么我们怎么把他们整合进对状态x x的估计呢？我们在前面已经得到了带减号的x − t x_t^-，我们只需要在它后面再加上一项作为修正，就可以得到最佳的估计了。



修正项就是真实的观测值减去预期的观测值得到残差，再乘以一个系数K t K_t就可以了，其中K t K_t十分关键，它叫做卡尔曼系数，它也是一个矩阵，具体的推导十分麻烦。只是定性的分析一下，具体有两方面作用：

1.权衡预测状态矩阵协方差P和观测量的协方差矩阵的大小来决定更加信赖预测模型还是观测模型，相信预测模型多一点，那么这个残差的权重就小一点，相信观测模型多一点那么残差权重就会大一点。

2.把残差的表现形式从观测域转换到状态域（注意到z t z_t只是一个位置标量，但是却用去更新x x，x x是一个二维列向量，这样成立的原因是K t K_t已经包含了位置与速度的协方差矩阵，所以可以根据位置的残差推测速度的残差，进而更新x x中的速度。）。

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



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



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

matlab实现（上面的小车）：



结果：



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

课程到此结束，但是还有一些没搞懂，具体是卡尔曼滤波器那五个公式的推导与理解，尽快完善。