京东秒杀
国美-超级5
索尼Xperia旗舰
限时优惠页 - 京东
自营热卖

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

夏槿凉安 1年前   阅读数 77 0

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

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 0.8 0.7 + 0.8 \frac{0.8}{0.7+0.8} \ast 10m + 0.7 0.7 + 0.8 \frac{0.7}{0.7+0.8} \ast 9m
    行驶距离x=(1- 0.7 0.7 + 0.8 \frac{0.7}{0.7+0.8} ) \ast 10m + 0.7 0.7 + 0.8 \frac{0.7}{0.7+0.8} \ast 9m //记住这个式子后面会用到
    0.7 0.7 + 0.8 \frac{0.7}{0.7+0.8} 这个就是我们要求的卡尔曼增益K。
    现实的使用中两种测量方式的精度我们是不知道的,卡尔曼滤波做的主要工作就是计算两种测量方式的精度(权重) 。

2、卡尔曼滤波的公式

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

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

最后一个公式
第六个公式: 计算估计值与真值的估计协方差矩阵 P k 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和司南牧。

发布了3 篇原创文章 · 获赞 4 · 访问量 307

注意:本文归作者所有,未经作者允许,不得转载

全部评论: 0

    我有话说: