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

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

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和司南牧。