网上很多关于卡尔曼的讲解，自从看到Youtube上的这个视频，才焕然大悟！这才是真正的深入浅出呀！
以下内容摘录于视频，仅作学习记录。
卡尔曼公式一
整个过程如下图：
Screen Shot 2017-07-02 at 8.53.44 PM.png
Screen Shot 2017-07-04 at 10.38.49 PM.png
- x 表示当前时刻的状态
- p 表示位置
- v 表示速度
- u 表示对车的控制量。不踩油门，速度为0
如果已知上一时刻的状态X(t-1) ,那么X(t)会是什么呢？如下图：
Screen Shot 2017-07-04 at 10.41.01 PM.png
而速度如下图：
Screen Shot 2017-07-04 at 10.41.50 PM.png
通过观察这两个公式，发现输出变量是输入变量的线性组合。卡尔曼最佳的线性滤波器，它只能描述状态与状态之间的线性关系。上面两个公式是线性的关系，那么写成矩阵的形式就是这样：
Screen Shot 2017-07-04 at 10.47.21 PM.png
进一步把状态变换矩阵提取出来：
Screen Shot 2017-07-04 at 10.48.01 PM.png
F：状态转移矩阵，表示如何从上一时刻状态如何推测当前状态
B：控制矩阵，表示控制量u是如何作用于当前状态
最后公式可以简化成：
Screen Shot 2017-07-04 at 10.48.36 PM.png
这个就是卡尔曼滤波器的第一个公式：状态预测公式。所有的推测都是包含噪声的，噪声越大，不确定性就越大。如何表示这次推测带来了多少不确定性呢？就需要使用协方差矩阵来表示。
如何表示推测的不确定性呢？
卡尔曼公式二
Screen Shot 2017-07-02 at 8.59.37 PM.png
在卡尔曼滤波器中，所有不确定性的表述都需要用到协方差矩阵。
协方差用P表示，如何让不确定性在每个时刻里面传递呢？答案使用上面的状态转移矩阵。
Screen Shot 2017-07-02 at 9.03.14 PM.png
预测模型本身也不是百分百准确的，本身也包含噪声。
Screen Shot 2017-07-02 at 9.04.39 PM.png
Q ： 表示预测模型本身带来的噪声。
上面的公式表示不确定性在各个时刻的传递关系。
卡尔曼公式三
Screen Shot 2017-07-02 at 9.07.54 PM.png
- z 是汽车在某一时刻观察的值。 它们之间的变换关系是线性的。
- 因为观察值也不是百分百正确的，所有用v来表示观察的噪声。
- 噪声的协方差矩阵用R表示，由于观察值是一个一维的值，所有R不是矩阵，而是一个单独的值。
卡尔曼公式四
Screen Shot 2017-07-02 at 9.46.37 PM.png
Screen Shot 2017-07-02 at 9.40.09 PM.png
它表示实际观测值和预期观测值之间的残差。参数乘以系数K就可以用来修正Xt 的值。而K是什么？
Screen Shot 2017-07-02 at 9.41.26 PM.png
K 叫做卡尔曼系数。作用主要是：
权衡预测状态协方差P 和观察量协方差矩阵R的大小，来决定我们是相信预测模型多一点（残差权重小），还是观察模型多一点（残差权重大）。
把残差的表行形式，从观察域转换到状态域。
卡尔曼公式五
Screen Shot 2017-07-02 at 9.47.49 PM.png
更新最佳估计值的噪声分布，留给下一轮迭代使用。
Screen Shot 2017-07-02 at 9.49.54 PM.png
Demo
Screen Shot 2017-07-02 at 9.59.17 PM.png
Ref：
