卡尔曼滤波笔记

0. 参考文献

【1】kalman and bayesian filter in python

1. g-h filter

1.1 通过试验来看直观效果

想看看体重
拿来一个称，一称，172lbs
过了一会儿，一称，160lbs
卖家说，正常，没有称没有误差
为什么有这本书：传感器不准
这时候拿来另一个称，用A称，体重170lbs，用B称，体重160lbs，这个时候体重是多少？有几个选择：
a. 相信A称：170lbs
b. 相信B称：160lbs
c. 比A称还大的体重
d. 比B称还小的体重
e. 在A称和B称之间的数
还是e靠谱，或许直接平均是个好办法：165lbs

我们假设测量可能有正负8lbs的误差，则有下图

如果选161，则不再B称的范围之内，如果选169，则不在A称的范围之内，所以要选在两个之间的区域

进一步，如果A称比B称准，比如正负3，而B称正负九，则有下图：

考虑极端情况：

因此可以有个结论，两个称比一个称好，尽管可能不准。
如果我们在一个称上测量很多很多次，最好求个平均就行。

但是这有一个问题，如果我们在好几天内称的，那么我们的体重到底变还是没变，如下两张图：


但是这样也太奇怪了，以线性的速度，两周增了13磅，还是比较奇怪的，我们再增加一步，“预测”，因为称是“测量”，加了预测应该能平衡一下。我们预测的是，我们的体重每天多重1lbs，配合上测量，怎么进行计算呢：如下图：

我们有了一个预测值，有了一个测量值，但是两者不匹配，怎么办呢？
将预测值与测量值“平均一下”
两个值平均就有了上面讨论过的问题：更相信哪一个？怎么平均？

接着这个例子，我们取平均的时候，用一个数4/10，有如下式子：

得到结果如图所示：

如果我们用的预测模型很奇怪，比如每天体重会减少1lbs，就会得到下图：

下面我们进一步考虑，每天增加1lbs这个数能不能改进。我们考虑测量值-预测值，其实是一个gain，每天增加1lbs也是一个gain。这个时候用哪个gain好呢，又用到平均了，采用如下式子：

代码如下：

1.2 g-h filter

g-h的含义：g是称体重例子中测量的权重，h是测量变化情况的权重。

g-hfilter是很多filter的基础，包括kalman filter

正本书就是对g-h问“what if”引出的，也就是讨论不同情况下的g和h的情况

整个流程就是下面这个图：

几个注意事项：
（1）不要扔掉点，是点就有用
（2）在两个点之间的范围内选择一个数，会更好（求平均）
（3）通过当的估计值，以及我们认为它会怎么变化，来预测一个新的测量，以及变化率
（4）新的估计值，在新的测量值以及估计值之间的范围内选择（求平均）

本书的核心观点：将不准确的测量值，和不准确的模型预测值，进行平均，来估计一个比较好的状态

1.4 给体重秤写一个g-h filter

1.5 关于g-h的选择

可以是固定，可以是函数，有一本书来写，kalman filter就是从概率的角度去自动的获得g和h

1.6 生成带噪声的曲线

1.7 初始化选择非常差的情况

1.8 特别多噪声的情况

这个g-h filter仿佛并不能很好的应对

1.9 带有加速度的情况

在有加速度时，g-h filter会持续落后，这不是改变g和h能改变的，是system error

1.10 改变g

1.11 改变h

2. Discrete Bayes Filter

kalman filter 是贝叶斯filter的一种

2.1 跟踪一个狗

利用声呐作为测量，假设有所有可能的位置有10个，初始情况下，在每个位置的概率是1/10.

然后讲了一些频率学派和贝叶斯学派的区别。

回到例子，我们建一个地图，用1表示door 用0 表示wall

然后，我听到一个传感器回声，说是在door处，但此时无法判断哪个door，只能每个1/3.

上面是一个分类的分布，可以计算如下

2.2 从传感器度读数中获取信息

假设我们从传感器中得到如下数据：
（1）door
（2）move right
（3）door
可以得到如下分布：

2.3 有噪声的传感器

一旦读到door，就把对应的概率乘以3：？得到如下代码和图

但这不是一个概率分布，应该标准化

利用scale，代替上面的3倍准确率：

代码写成下面这个：


总结：
一开始的分布是先验priori，中间乘的是likelihood，再nomalize后得到posterior

总结成代码：

完整总结：

2.4 加入运动

代码如下：

效果如下图：

就是将整体的循环右移

2.5 词汇

system：狗
state：狗的位置
系统在evolution或propagation
我们用process model来建模系统，建模过程中有system error 或 process error
我们的预测（prediction）是新的prior

例子：
狗现在在17m处，速度15m/s，两秒后，预测狗在17+（15*2）=47

2.6 在prediction中加入uncertainty

我们的预测是不准的，假设预测的过程中80%的概率是对的，10%的概率可能落在位置左侧，10%可能落在位置右侧，比如一开始在3位置，向右移动一格，则10%可能在3，10%可能在5,80%可能在4.

有如下代码：

效果如下：

如果一开始有多个可能：

可以看到，其实每次运动预测都会损失信息，如果一直不停的预测，就会得到一个平均的分布，每个位置都是0.1，这样所有信息都损失掉了。

2.7 通过卷积计算来一般化：

上面的运算其实就是卷积：用一个kernal（0.1,0.8,0.1）在一个array上进行滑动

2.8 将测量和运动估计融合

下面看一个过程：
首先，我们的prior是所有位置都有可能，然后通过一个观测，增加了某些信息


然后经过了一次运动预测，信息移动并且减少了

然后进行一次测量update

然后再进行移动更新

继续移动更新：

2.9 离散贝叶斯filter

伪代码：
Initialization：

1. 初始化对状态的belief
   Predict：
2. 基于系统行为，预测下一时刻状态
3. 调整belief，加入预测过程的uncertainty
   Update：
4. 获得一个测量值，以及它的精度
5. 计算这个测量值符合每个state的程度
6. 利用likelihood更新每个state的belief.