卡尔曼滤波器原理分析

描述

原理

卡尔曼滤波器是一种基础预测定位算法。原理非常简单易懂。核心过程可以用一个图说明:

滤波器

本质上就是这两个状态过程的迭代,来逐步的准确定位。

预测:当前状态环境下,对下一个时间段t的位置估计计算的值。

更新:更具传感器获取到比较准确的位置信息后来更新当前的预测问位置,也就是纠正预测的错误。

 你可能要问为什么有传感器的数据了还要进行更新?因为在现实世界中传感器是存在很多噪声干扰的,所以也不能完全相信传感器数据。卡尔曼算法依赖于线性计算,高斯分布,我们以一维定位来介绍算法的实现。

滤波器

滤波器

接下来我们开更新,预测后我们获取到传感器数据,表示目前传感器发现小车的位置应该是在26这个位置,在这种情况下,我们肯定是觉得传感器的准确度比我之前的预测瞎猜要来的准确。
 
所以方差自然会比较小,最终我们觉得真是的小车位置应该是更靠近传感器数据的,而且方差会缩小,以至于,想想也很清楚,我猜了一个预测值,现在有个专家告诉了我相对比较靠谱的数据,那我对小车的位置的自信度肯定会上升啊。
 
最终小车的位置经过这个时间段t的更新就是下图红色的高斯图:

滤波器

就这样不停的移动更新,最终小车的位置就会越来越准确。

一维模型下的Kalman公式:

预测   滤波器

更新 滤波器

滤波器

参考代码:

 

#include 
#include 
#include 


using namespace std;


double new_mean, new_var;


tuple measurement_update(double mean1, double var1, double mean2, double var2)
{
    new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2);
    new_var = 1 / (1 / var1 + 1 / var2);
    return make_tuple(new_mean, new_var);
}


tuple state_prediction(double mean1, double var1, double mean2, double var2)
{
    new_mean = mean1 + mean2;
    new_var = var1 + var2;
    return make_tuple(new_mean, new_var);
}


int main()
{
    //Measurements and measurement variance
    double measurements[5] = { 5, 6, 7, 9, 10 };
    double measurement_sig = 4;
    
    //Motions and motion variance
    double motion[5] = { 1, 1, 2, 1, 1 };
    double motion_sig = 2;
    
    //Initial state
    double mu = 0;
    double sig = 1000;


    for (int i = 0; i < sizeof(measurements) / sizeof(measurements[0]); i++) {
        tie(mu, sig) = measurement_update(mu, sig, measurements[i], measurement_sig);
        printf("update:  [%f, %f]
", mu, sig);
        tie(mu, sig) = state_prediction(mu, sig, motion[i], motion_sig);
        printf("predict: [%f, %f]
", mu, sig);
    }


    return 0;
}
 

 

审核编辑:汤梓红

打开APP阅读更多精彩内容
声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉
评论(0)
发评论
jf_92591268 2023-07-14
0 回复 举报
感觉还可以吧 收起回复

全部0条评论

快来发表一下你的评论吧 !

×
20
完善资料,
赚取积分