卡尔曼滤波器学习笔记

网上很多关于卡尔曼的讲解，自从看到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：

卡尔曼滤波的简单实现(Matlab & OC)

说说卡尔曼滤波