卡尔曼滤波器（Kalman filtering）

卡尔曼滤波器

本文主要参考该视频，在此对vic_wu表示感谢。

卡尔曼滤波器，又叫做最佳线性滤波器，它的好处有很多，比如说实现简单，而且又是一个纯时域的滤波器，不需要进行频域变换，所以在工程上有很多应用。

假设有一辆汽车在路上行驶，我们用它的位置和速度来表示他当前的状态，写成矩阵形式：

就是X(t)等于1个二维的列向量。这个向量里面的两个元素，就是汽车的两个状态。一个是位置P和速度V。

另外驾驶员也可以踩油门和刹车，让车有一个向前或者向后的加速度U，U表示对车的控制量，如果他没有踩油门，也没有踩刹车，那U就等于0，车就会做匀速直线运动，

如果我们已知上一时刻的状态X(t-1)，那么当前时刻的状态X(t)会是什么呢

位置P(t)等于上一时刻的位置P(t-1)加上上一时刻的速度为V(t-1)乘以（的塔T），（的塔T）就是两个时刻的间隔的时间，如果有加速度的话，还要加上加速度U(t)乘以1/2的（的塔T)平方，

而速度的呢，就等于上一次的速度，加上加速度U（t）乘以（的塔T），我们观察这两个公式，会发现他们的输出变量都只是输入变量的线性组合，这就是为什么我们说卡尔曼滤波器是最佳的线性滤波器。

因为它只能描述状态与状态之间的线性关系，竟然是线性关系，那我们就可以把它写成矩阵的形式，就变成这样了，

我们再进一步把两个状态变换矩阵提取出来，变成了F和B，

公式就可以简化成，这样。

这个公式就是卡尔曼滤波器中第一个公式状态预测公式，

其中：

F就叫做状态转移矩阵，他表示我们如何从上一时刻的状态来推测当前时刻的状态，

B叫做控制矩阵，他表示控制量U如何作用于当前状态。

---

我发现这个公式里面的X有点不太一样，都带了一个尖帽子，表示这是对X的估计量，而不是X的真实值，因为汽车的真实状态，我们是永远无法知道的，我们只能根据我们的观测，来，尽可能的估计X的值。
 

等号左边的X还加上了一个减号的上标，表示这个值是根据上一时刻的状态推测而来的，等会我们还要根据我们的观测量的去修正，这个X的值，修正了之后才可以算是最佳的估计值，也就是没有减号上标的这个X。
有了状态预测公式，我们就可以推测当前时刻的状态，但是我们知道所有的推测都是包含噪声的，噪声越大，不确定性就越大，我们如何来表示这一次推测带来了多少不确定性，这时候我们就要用协方差矩阵来表示。

那么什么是协方差矩阵呢？我们还是从一维的简单情况来解释，

假设我们有一个一维的包含噪声的数据，每次测量的值都不同的，但都是围绕在一个中心指的周围，那我们表示它的分布状况的最简单的方法就是计下它的中心值和方差，这实际上是假设了，它是一个高斯的分布。

那么现在我们来看二维的情况，一个二维包含噪声的数据，它看起来是这个样子的，

分别对两个坐标轴进行投影，在两个轴上都是高斯分布，那我们在表示它的分布的时候是不是，分别记下了两个高斯分布的中心值和方差就可以了呢？

如果两个维度的噪声是独立的时候，是可以这么表示，但是如果两个维度有相关性的情况呢，比如说在一个维度上，噪声增大的时候，另一个维度上噪声也增大，这个图形就会变成这个样子。

如果一个维度增大的时候，另一个维度减小，就会变成这样。

这时候在两个坐标轴上投影，跟第一个是完全没有变化，仍然是高斯分布，所以为了表示这两个维度之间的相关性率，除了记录两个维度的方差之外，还要有一个协方差来表示两个维度之间的相关程度，把它们写成一个矩阵的形式呢，就是这样。

对角线上的两个值是两个维度的方差。反对角线上的两个值是相等的。是他们的协方差，在这三种情况中，左边的斜方差等于0；中间的协方差为正；右边的，呈负相关的趋势，协方差为负。在卡尔曼滤波器中，所有关于不确定性的表述都要用到这个协方差矩阵。

---

在我们的小汽车的例子中，每一个时刻的状态的不确定性，都是由协方差矩阵P来表示的，

那么下一个问题就是我们如何让这种不确定性在每个时刻之间传递呢？其实答案就是乘上状态转换矩阵F。

这次是要在左右两边各乘一次，左边乘以F，右边乘以F的转制，具体一点就是这样的。

从上一次和推测当前时刻的协方差，就等于上一时刻的协方差两边，乘以状态转移矩阵，至于为什么要乘两边，这是协方差矩阵的一个性质，X的协方差是P，想计算，F乘以X协方差呢，就可以把F从里面提到两边来。

这时候我们还要考虑一件事情，就是我们的预测模型并不是百分之百准确的，它本身也是包含噪声的，所以我们要在后面加上一个协方差矩阵q，来表示预测模型本身带来的噪声。

好啦，这个公式呢，就是卡尔曼滤波器的第二个公式，他表示不确定性在各个时刻之间的传递关系。
回到我们那个小汽车的模型，假如我们在公路的一端放置了一个激光测距仪，在每个时刻都可以观测到汽车的位置。

观测到的值，我们称为Z(t)，那个从汽车本身的状态，X(t)到了观测状态Z(t)之间有一个变换关系我们记为H，当然这个变换关系也只能是线性关系，因为卡尔曼滤波器是线性滤波器，所以我们理所当然的要把H写成矩阵的形式，也就是所谓的观测矩阵，X和Z的维度是不一定相同的。

在我们的例子里，X是一个二维的列向量，而Z只是一个标量的值，所以H应该是一个一行两列的一个矩阵，里面的元素分别是一和零，

这样呢，h和x相乘的时候，就可以得到一个标量的值Z。而Z也就是汽车的位置，它和X的第一个元素是相等的，那为什么后面还要加了一个小写V呢？

因为我们的观测值也不是百分之百可靠的，所以我们后面还要加上一个V来表示观测噪声，而这个噪声的协方差矩阵呢，我们又用R来表示。由于在我们这个例子里观测值是一个一维的值，所以这个R的形式也不是一个矩阵，而是一个单独的值，仅仅表示Z的方差。

假设我们除了激光测距仪之外，还有别的测量方法可以观测到汽车的某项特征，那么Z呢就会变成一个多维的列向量，它会包含每一种测量方式的测量值？而每一个测量值呢，都只是真实状态的一种不完全的表现，我们可以从几种不完整的表述里面推断出真实的状态，而卡尔曼滤波器的数据融合的功能呢，就正是在这个测量矩阵中体现出来的。

我们已经有了观测Z和他的噪声的协方差矩阵R，那么我们如何把它们整合进我们对状态的估计呢？

我们在前面已经得到了带减号上标的X(t)，现在我们只要在它后面再加上一项来修正它的值，就可以达到我们的最佳估计值了，那么加上的这一项是什么东西呢？

先看括号里面的部分，Z(t)减去H乘以X(t)上标减号，这表示实际的观测值和我们预期的观测值之间的残差，这个残差了，乘上一个系数K，就可以用来修正X(t)的值，这K可以十分关键，它叫做卡尔曼系数，实际上它也是一个矩阵，它的公式是这样的。

这个公式的推导比较复杂，所以我们只是定性的来分析一下，这个卡尔曼系数K了，它的作用主要有两方面，

第一个作用是权衡预测状态协方差P，和观察量的协方差矩阵R的大小，来决定，我们是相信预测模型多一点呢？还是相信观察模型多的一点，如果相信预测模型多一点，那么这个残差的权重就会小一点；如果相信观察模型多一点，那个残差的权重就会大一点。
第二个作用是把残差的表现形式，从观察域转换的状态域，这什么意思呢？我们刚才讲到，观察值z值是一个一维的向量，状态值是一个二维的向量，他们所用的单位，甚至描述的特征都有可能是不同的，那我们怎么可以用观察值的残差去更新状态值呢？实际上这个卡面系数k的，就是在替我们做这样的状况。

在我们的这个例子里，我们只观察到了汽车的位置，但K里面，已经包含了协方差矩阵P的信息，所以它利用位置和速度这两个维度的相关性，从位置的残差里面推算出了速度的残差，对状态X的两个维度同时进行修正。

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

好了，到现在为止，我们已经有了卡尔曼滤波器的所有的五个公式，把它们完整的列出来看一下。

前两个是通过上一时刻的状态来预测当前时刻的状态，通过这两条公式，我们得到是带减号上标的X和P，这表示，这并不是最佳的估计值，减号的上标表示他们还欠缺点什么东西？这欠缺的东西，就是从观测诊断里面带来的信息，因为我们还没有考虑当前时刻的观测值。

后面三个公式呢，就是用当前的观测值来更新X和P，经过更新后的值，就是最佳的观测值，所以他们是不带减号上标的。

---

---

---

这是别人的视频，感觉讲的特别清晰明了，特别好，所以转换成文字，分享给大家。

好东西不能独享，是这样吗？哈哈。。。。。