Intro
最近在学习伟大的Kalman Filter，这篇笔记主要是对国外的两篇介绍Kalman Filter的博客的翻译和一些个人理解，博客链接：Kalman Filter For Dummies， How a Kalman filter works。两篇博客的侧重的层次面不太一样，第一篇讲的层次比较浅显但是缺失了一些细节，第二篇讲的很深入但是有些抽象，这篇笔记将两篇博客结合起来，希望可以由浅入深的理解Kalman Filter。
Kalman Filter的简介和应用
首先先介绍一下Kalman Filter（以后简称kf）。kf是一种线性二次估计算法，由Rudolf Kalman于1960年提出，在随后的阿波罗计划中的导航系统就主要应用了kf算法。kf用对系统在时域的测量来预测一些未知的变量在下一个时间点的状态，测量的对象包括噪音干扰和其他的不确定因素。kf相比于其它的预测算法有更高的精准度。
kf可以用于任何包含未知信息的动态系统，然后自适地预测系统下一步的状态。哪怕环境的干扰很多很复杂，kf一般也可以得出很准确的结果。kf在连续变化的系统有很好的效果。它的优点是在预测时并不需要记录很多系统过去的状态，只需要知道上一状态的预测结果的误差协方差矩阵就可以了，所以kf可以算得很快，可以用于实时的动态系统。
一个kf的小例子
首先我们创造一个场景来引入kf算法。假设我们有一个机器人可以在丛林中穿梭，机器人的控制算法需要知道机器人的位置和速度然后才可以导航机器人的运动。我们用一个状态Xk向量来表示：
这个例子为了简化所以只考虑两个状态变量，在现实中可能会有更多的变量，更多的变量就意味着更多的信息，这更有利于kf的计算精准度。
同时，这个机器人还有一个GPS传感器来测量距离和速度。这个GPS的精度在10米左右，但这是远远不够的，因为如果在丛林里有一个悬崖的话10米的精度并不能保证机器人可以及时的避免。所以单单依靠GPS的数据来控制是不够精准的。而且传感器的也会被干扰，所以传感器的数据并不是完全可依靠的。
机器人的运动可以分为主动因素和外界干扰两个方面。主动因素是控制程序给电机发送的运动指令，它可以是向前向后，但这并不能保证机器人一定会沿着指定的方向运动，因为还有外界干扰的存在。比如说机器人遇到障碍了，被风吹了等等。所以外界干扰也是不可被忽视的一部分。
在这里就可以引入kf算法中的两个概念，主观预测和客观测量。我们的主观预测就是基于我们对机器人发出的命令，如果是向前5米，那我们对下一个时间点的的主观预测就是向前5米。这个预测当然是不准的，因为我们没有考虑到外界的干扰。客观测量就是我们通过GPS得到的数据，首先这个数据是间接的，因为传感器的得到的数据只是一些电平信号，我们还需要将它们对应转换到现实世界的数据。其次这个数据也是不可靠的，理由见上。
我们对机器人下一个状态的最优预测只能基于主观预测和客观测量这两个信息，但是哪一个更可靠一点呢？我们加入权重来让这个预测结果更公平一点，这里就引入了我们最优预测的权重公式：
其中Zk是我们得到的客观测量值，X（k－1）就是我们上一次做出的主观的预测，Xk就是我们最终的预测值，而K就是我们要通过kf找到的权重值，又称Kalman Gain。这样得到的估计相比于独立的主观估计和客观估计都要精确的多。
但是注意，这个最优预测并不是用于实际的使用，实际中还是使用主观预测为估计，但是当到下一个时间点得到客观测量后，我们可以修正我们的估计，得到最优估计，这个估计是作为参数传递给下一次估计，这样一次次修正，我们的估计会更加精确。
分布函数与协方差矩阵
在我们的问题情景里，我们只有两个需要追踪的状态，位置和速度。我们可以假设位置和速度都服从于高斯分布，kf算法中，都假设要追踪的变量服从于高斯分布。一维高斯分布的分布函数为：
中间的μ是这个高斯分布的期望，σ是标准差，σ^2是方差，在这个分布里只有一个变量，这个函数的积分为1。现在我们把高斯分布扩展到二维，则分布函数为：
在这个图像中，X轴Y轴分别代表一个变量，Z轴是概率，所以中间最高点对应的X，Y为这两个变量的期望，整个凸起部分的体积积分为1。用2维的图来表示则为：
颜色越白的地方概率越大，所以期望在中心点，周围黑色的部分概率为0。注意上图中所标注的方差的大小不是所标注的范围，此处有误解。
在以上的分布的例子里，位置和速度的相关性不是很强，但是在现实中，位置一般是和速度有很大相关性的，比如以下的例子：
在这个例子中，速度与位置成正相关，当速度大的时候，往往位置也会大，速度小的时候，位置也会小。这样的相关性信息也是kf算法所需要的。多维变量的相关性的信息是用协方差矩阵进行储存的。协方差矩阵可以储存一个一维列向量的变量之间相互的相关性信息。在这个例子中一维的变量向量和协方差矩阵为：
自己和自己的相关性是1，所以Σpp和Σvv都是1。相关性与顺序无关，所以Σpv等于Σvp。所以协方差矩阵Pk是一个对称矩阵。
主观预测与状态空间方程的构建
首先要构建的是状态空间方程，所以变量为位置和速度。k是时间变量，我们的主观预测需要从（k－1）时的状态来预测k时的状态：
蓝色的区域是（k－1）时的速度与位置服从的分布，紫色的是k时速度与位置服从的分布。这个图假设不同的时间点速度与位置服从的分布不同，然而在大部分时候，速度与位置的分布是不随时间变化而变化的。
在这里，我们做的是主观预测，我们的预测是给予一个经验公式，这个公式可以来自于控制的命令，或者其它的一些我们已知的信息。我们的预测需要将（k－1）的状态的分布转移到k的状态的分布，每一个（k－1）的点都可对应到K时的一个点。假设我们状态转移矩阵是Fk则：
注意，一般情况下，这个Fk就是1，因为分布的关系不会随时间变化而变化。
我们可以把这个state space 方程写下来：
Δt是k－1到k的时间变化，在这里假设机器人匀速运动。这样我们就得到了我们的初步的主观预测方程。我们可以从（k－1）的状态预测下一个时间点时位置和速度的值。同时根据协方差矩阵的性质：
我们可以得到一个完整的预测信息：
但是这个状态方程只是一个很基础的，我们还需要考虑其他因素对系统的影响，比如说电机对机器人的控制，外界干扰等等。
我们首先考虑电机对机器人的控制，如果加上电机的加速度，则空间状态方程为：
Bk就是控制矩阵，uk就是加速度向量。
此外还要考虑噪声的影响，如果加上噪声，则每一个（k－1）时的点不会对应到K时的一个点，而是对应到一片区域，所以对应关系如下：
因为每一个点都对应到一片区域，所有的对应叠加在一起，得到的新的K的分布范围应该比以前要大，如下图所示：
面积明显比上图要大，同时我们假设噪声是高斯白噪声，所以期望为0，所以加上噪声后K的期望不变。假设噪声所造成的那个小绿圈的速度与位置的协方差矩阵为Qk，则新的主观预测和协方差矩阵为：
通过这个方程我们可以在X（k－1）时主观的预测出Xk时的状态。下一步就是得到客观测量的结果。
客观测量
我们的机器人上有GPS传感器可以得到位置和速度的信息，假设sensor1测的是速度，sensor2测的是位置：
但是注意，因为传感器得到的只是电平值，所以得到的分布函数的范围与现实中的速度和位置的范围不一样。我们需要用一个转移矩阵把现实世界的速度位置数据的map到传感器的电平数据单位，每一个点对应传感器坐标的一个点，然后在相同的单位下进一步计算权重Kalman gain。这里我们引入转移矩阵Hk：
对应的期望与协方差被转换为：
这里的μ expected就是我们的主观估计在传感器的单位下的值，Σ expected则是我们的主观估计的协方差矩阵在传感器单位的转换后的矩阵。
当然，我们也不能忽略传感器的噪声影响，噪声会造成我们在把现实世界的值转换为传感器单位时一个点对应传感器坐标系的一片区域：
所以当传感器坐标系中每一个点都被这样的一个绿色的小椭圆代替，新的传感器坐标系下的数据分布将变为：
形状变成这样是因为我们假设的传感器噪音是这样的一个椭圆形，原来的紫色的小圆形被扩张成了这样的绿色椭圆。
则绿色椭圆分布的期望（中心点）就是我们传感器读到的客观测量数据，但是包含误差。
得出最优修正预测
得到我们的主观预测在传感器坐标系的分布和客观测量得到的数据的分布后，就可以计算权重了。首先我们先将两个分布重叠在一起，得到如下分布图：
紫色的是主观估计，绿色的是客观测量。所以我们就有了两个概率：
1，我们的主观估计是我们最优估计的概率
2，客观测量是我们最优估计的概率
在这里我们直接将两个概率分布函数相乘，得到一个新的分布。因为黑的地方概率为0，所以相乘后紫色和绿色的非重叠面积都变为0了。我们只得到中心重叠的区域为我们最优的预测的分布：
这个分布的期望就是我们对下一个时间点K的最优预测。这个预测的特点是，在这个点上，主观估计和客观测量的概率最大。
一个一维更直观的图示为：
那个小蓝色分布的期望就是红色分布和绿色分布同时预测时的最佳预测值。
计算权重Kalman Gain
我们首先考虑只有一个变量时的情况。这时分布的波动情况用方差表示。
因为我们是直接将两个分布函数相乘得到一个新的分布，所以用公式表达为：
如果两个分布都是服从高斯分布：
则相乘以后得到的新的分布的期望和方差为：
我们可以在期望的计算公式中提出一个K，作为我们的权重Kalman Gain：
下面我们考虑多个变量的情况，这时数据的波动情况就不能用方差表示了，应该用协方差矩阵来表示，所以直接替换得到的新的分布和新的K：
这样，我们就得到多变量时的Kalman Gain。这个分布期望就是我们最终的预测。协方差矩阵就是我们要传给下一次预测的信息。
推导公式
我们的主观预测的分布是：
客观测量的分布是：
所以代入刚才得到的最佳预测的分布公式得到：
K‘就是我们的最佳预测的权重Kalman Gain，Xk’就是我们的最佳预测，Pk‘就是我们要传给下一次预测的协方差矩阵。至此一次预测已经完成。
迭代流程图
我们首先给以个起始估计，然后在（k－1）时先做主观估计，用主观估计值作为我们的估计，然后等到K时，用得到客观测量进行矫正我们的估计，再把矫正后的新的值作为这次的最佳估计和新的协方差矩阵传递给下一次估计。
Time Update是在（k－1）阶段，Measurement Update是在K阶段。等到K时我们才可以得到客观测量值，所以是用客观测量进行矫正之后的最佳估计值再用来下一次估计。但这个最佳估计并没有被用在实际中，之前实际用到的估计还是Time Update的估计值。
