卡尔曼滤波公式及原理的简易详解（新手入门）

1、卡尔曼滤波怎么滤波

网上介绍卡尔曼滤波的案例很多，但是看完之后总有种云里雾里的感觉。
这里参考知乎上一篇答主的例子：
卡尔曼滤波是贝叶斯滤波在特殊情况下的一种算法（服从高斯分布且为线性）
（1）贝叶斯公式，举个例子来说。假如你想判断一个人是男性还是女性，
在无外界其它条件的情况下，这个人为女性的可能性是50%，这时候有两个
新的信息，信息A：这个人为长头发；
… … … . . 信息B：这个人穿高跟鞋。
在假设我们知道 1 女性为长头发的概率为75%
. … … … … … … 2 女性穿着高跟鞋的概率为70%

有了上面的信息以后，我们再次判断这个人的性别就更为精确了。(通过分子判断即可)

（2)卡尔曼滤波。个人理解，贝叶斯滤波通过输入不同的信息，来刷新需要计算目标的概率。P（x|a,b）我们通过掌握a,b信息来计算出x的概率值（行驶距离为11米概率为60%，行驶距离为10米概率为80%…等等）
卡尔曼滤波呢，卡尔曼滤波其中求得也是概率分布函数，但是这个分布函数并不直观，而且还非常占用计算机的内存，所以卡尔曼滤波最后把这个分布函数取均值处理，比如说，我们想要获取一辆车的实时位置信息，这辆车上有两种方式来计算自身的位置<1>距离传感器<2>编码器。
距离传感器计算的行驶距离为10m，精度为80%。
编码器计算的行驶距离为9m，精度为70%。
行驶距离x $\frac{0.8}{0.7+0.8}$ $\ast$ 10m + $\frac{0.7}{0.7+0.8}$ $\ast$ 9m
行驶距离x=(1- $\frac{0.7}{0.7+0.8}$ ) $\ast$ 10m + $\frac{0.7}{0.7+0.8}$ $\ast$ 9m //记住这个式子后面会用到
$\frac{0.7}{0.7+0.8}$ 这个就是我们要求的卡尔曼增益K。
现实的使用中两种测量方式的精度我们是不知道的，卡尔曼滤波做的主要工作就是计算两种测量方式的精度（权重）。

2、卡尔曼滤波的公式

卡尔曼滤波公式及原理的简易详解（新手入门）
为了简单起见，下边用温度模型考虑
（1）第一个公式： $x_k^ ,$ 为我们的预测量， $x_{k-1}^ ,$ 为上次我们的预测量（这个初始值需要我们设定）， $u_k$ 是我们的控制量（简单来说，比如一个正在运动的车来说，t-1时刻位置是5m，下一秒你让车前进1m，此时 $u_k$ 就为1m。在小车运动模型中上述公式1中还应有一个噪声误差v，因为你让小车前进1m，真实情况肯定是有误差的）。
但是我们是温度模型，所以系数A看做1（大多数情况下都是这样，个人理解），因为温度不会突然变化，所以控制量 $u_k$ 就为0，噪声v便于理解就不考虑了也为0.公式1变为 $x_k^ ,$ = $x_{k-1}^ ,$

（2）第二个公式： $P_k^’$ 为预测协方差矩阵（指的是公式一中预测量 $x_k^ ,$ 与真值 $x_k$ ）。因为上面我们将A设为1，所以此时公式二变为 $P_k^’$ = $P_{k-1}$ +Q。（这里的Q我也不清楚叫什么，他需要我们预先设定，Q会影响预测值和测量值所占的权重，这个后面会说）现在我们只需要知道Q增大 $P_k^’$ 也会增大，Q减小 $P_k^’$ 也会减小。
（3）第三个公式： 个人感觉，这个公式很坑害初学者
大家看上面这个公式吧卡尔曼滤波公式及原理的简易详解（新手入门）
这个公式也有坑， $z_k$ 是我们处理以后的测量值，记得这个公式里的 $x_k$ 是我们传感器返回的数值，前边的H学名叫转换矩阵，在这里作用可以理解为统一单位，假如传感器返回的是cm，但是你预测值是m，所以你的预测值 $z_k$ 也应该为m，这时候H就起到一个单位转换的作用，然后 $v_k$ 为噪声（服从高斯分布），因为传感器不准确嘛。（为了方便理解卡尔曼滤波，关于转换矩阵H这里只做最简单的理解，匀速运动模型暂时不考虑了）然后把H也看做1处理， $v_k$ 应该看做均值为0，方差为R的密度函数**（至于R是多少，我也不知道，自己写就行。在这就当0.1了）**，。此时公式三变为 $z_k$ = $x_k$ +0.1（此处 $x_k$ 为传感器测量数据，切记不要混淆）
（3）第四个公式： 此时我们可以由我们第二个公式计算的 $P_k^’$ 来计算卡尔曼增益K，我们先把我们之前假设的系数带进去看一下，K= $\frac {P_k^’} {P_k^’+0.1}$ 。记得公式二时候咱们说过吧，Q越大 $P_k^’$ 越大，然后在这里我们会发现 $P_k^’$ 增大，K也会增大。记住！
（4）第五个公式： 这个我们要算我们的估计值了，也就是最终的结果。此处我们还是不看他这个公式。卡尔曼滤波公式及原理的简易详解（新手入门）
我们看这个
这个公式里 $X_k$ 就是我们的估计值（就是最终需要的结果，切记别与那个传感器测量值弄混了，他俩没关系）。 $x_{k^-}$ 就是我们公式一算的预测值* $x_k^ ,$ ，K 为卡尔曼增益。
好了，我们把这个式子乘开看一下 $X_k$ =（1-K） $\ast$ $x_{k^-}$ +K $\ast$ $z_k$ .这个式子是不是我们最开始的那个。
行驶距离x=(1- $\frac{0.7}{0.7+0.8}$ ) $\ast$ 10m + $\frac{0.7}{0.7+0.8}$ $\ast$ 9m
卡尔曼滤波做的就是根据判断测量值和预测值与真值的偏离程度，不断更新测量值和预测值所占的权重。

最后一个公式
第六个公式： 计算估计值与真值的估计协方差矩阵 $P_k$ 。计算它来用于下一次我们公式二的迭代，因为这个权重是不断变化的，所以这个P的值要不断迭代才能实现动态滤波。 注意的是这里公式中的I是1别看错了。

卡尔曼滤波的的精华在于P、K之间的迭代，我下边给大家推荐两个链接，最好自己推导一下。
https://blog.csdn.net/heyijia0327/article/details/17487467
https://blog.csdn.net/victor_zy/article/details/82862904
由于作者水平有限，上述内容仅作为参考，如有问题希望各位大佬不吝赐教。
谢谢
上述问题有借鉴知乎答主DBinary和司南牧。