滤波算法(四)—— 卡尔曼滤波算法[通俗易懂]

滤波算法(四)—— 卡尔曼滤波算法[通俗易懂]一、算法介绍卡尔曼滤波是一个神奇的滤波算法,应用非常广泛,它是一种结合先验经验、测量更新的状态估计算法。1、状态估计首先,对于一个我们关心的物理量,我们假设它符合下面的规律其中,为该物理量本周期的实际值,为该物理量上一个周期的实际值,当然这个物理量可能不符合这个规律,我们只是做了一个假设。不同的物理量符合的规律不同,是我们的经验,我们根据这个规律…

大家好,又见面了,我是你们的朋友全栈君。

一、算法介绍

        卡尔曼滤波是一个神奇的滤波算法,应用非常广泛,它是一种结合先验经验、测量更新的状态估计算法。

1、状态估计

        首先,对于一个我们关心的物理量,我们假设它符合下面的规律

x_{k}=ax_{k-1}

其中,x_{k}为该物理量本周期的实际值,x_{k-1}为该物理量上一个周期的实际值,当然这个物理量可能不符合这个规律,我们只是做了一个假设。不同的物理量符合的规律不同,是我们的经验,我们根据这个规律可以预测我们关心的物理量。比如,我们关心的物理量是车速,如果车辆接近匀速运动时,则a的取值为1,也就是这个周期与上个周期的速度相同。

        下面我们再来看一下这个物理量的测量公式

z_{k}=x_{k}+v_{k}

其中,z_{k}是这个物理量的测量值,v_{k}是测量噪声。我们对一个物理进行预测,测量是一个必不可少的手段,虽然测量的不一定准,但是在很大程度上体现了物理量的实际值。这个公式体现的就是实际值与测量值的关系。还是以车速为例,z_{k}是通过车速传感器得到的测量值。

        实际中,物理量一般不会像我们上面的公式那样简单,一般我们用下面的公式来表示

x_{k}=ax_{k-1}+bu_{k}

其中,bu_{k} 代表了处理噪声,这个噪声是处理模型与实际情况的差异,比如车速,他会受到人为加速、减速、路面不平等外界因素的影响。

        卡尔曼滤波的基本思想是综合利用上一次的状态和测量值来对物理量的状态进行预测估计。我们用\hat{x_{k}}来表示x_{k}的估计值,则有下面的公式

\hat{x}_{k}=\hat{x}_{k-1}+g_{k}\left ( z_{k}-\hat{x}_{k-1} \right )

在这个公式中,综合利用了上一个周期的估计值和这个周期的测量值来对x_{k}进行估计。其中,g_{k}叫做卡尔曼增益,这个公式与一阶滤波很相似,只不过卡尔曼增益是会变的,每个周期都会更新,一阶滤波的系数则是固定值。考虑极端的情况来分析增益的作用,当g_{k}=0时,增益为0,这时\hat{x}_{k}=\hat{x}_{k-1},这表示我们这个周期的估计值与上个周期是相同的,不信任当前的测量值;当g_{k}=1时,增益为1,这时\hat{x}_{k}=z_{k},这表示我们这个周期的估计值与测量值是相同的,不信任上个周期的估计值,在实际应用时,g_{k}介于0~1之间,它代表了对测量值的信任程度。

2、卡尔曼增益

        上面我们通过卡尔曼增益来估计物理量的值,那卡尔曼增益又是如何取值的呢?我们通过下面两个公式来计算并在每个周期进行迭代更新。

g_{k}=p_{k-1}/\left ( p_{k-1}+r \right )

p_{k}=\left ( 1-g_{k} \right )p_{k-1}

在上述公式中,r是测量噪声v_{k}的平均值,测量噪声是符合高斯分布的,一般可以从传感器厂商那里获得测量噪声的均值,如果无法获得可以根据采集到的数据给出一个经验值。r的大小对最终滤波效果的影响是比较大的。p_{k} 为本周期的预测误差。我们采用分析卡尔曼增益的方法来分析预测误差的作用,即采用假设极端情况的方法。假设前一次的预测误差p_{k-1}=0,根据第一个公式则g_{k}=0,根据上面的分析,这种情况估计值为上个周期的估计值;如果前一次的预测误差p_{k-1}=1,则增益变为1/\left ( 1+r \right ),一般r取值很小,所以g_{k}\approx 1,这种情况以新测量的值作为估计值。

        对于第二个公式,当卡尔曼增益为0时,p_{k}=p_{k-1},即采用上一个周期的预测误差;当增益为1时,p_{k}=0

3、完整卡尔曼滤波算法

        有了上面的推导,我们在下面列出来完成卡尔曼滤波的公式,卡尔曼滤波分为预测过程和更新过程两个过程,在公式中,我们又引入了缩放系数h,和协方差q

预测过程:

\hat{x}_{k}=a\hat{x}_{k-1}+bu_{k}

p_{k}=ap_{k-1}a+q

更新过程:

g_{k}=p_{k}h/\left ( hp_{k}h+r \right )

\hat{x}_{k}=\hat{x}_{k}+g_{k}\left ( z_{k}-h\hat{x}_{k} \right )

p_{k}=\left ( 1-g_{k}h \right )p_{k}

        上面的公式适合一维变量的卡尔曼滤波,将变量扩展到多维,用向量和矩阵替换上面的变量,就可以实现多维变量的卡尔曼滤波,下面的公式适用于多维变量。

预测过程:

\hat{x}_{k}=A\hat{x}_{k-1}+Bu_{k}

P_{k}=AP_{k-1}A^{T}+Q

更新过程:

G_{k}=P_{k}H^{T}\left ( HP_{k}H^{T}+R \right )^{-1}

\hat{x}_{k}=\hat{x}_{k}+G_{k}\left ( z_{k}-H\hat{x}_{k} \right )

P_{k}=\left ( 1-G_{k}H \right )P_{k}

二、实现代码

        下面我们通过c++代码来实现卡尔曼滤波算法,所实现的算法为一维滤波算法。首先定义卡尔曼滤波的参数

typedef struct{
    float filterValue;//滤波后的值
    float kalmanGain;//Kalamn增益
    float A;//状态矩阵
    float H;//观测矩阵
    float Q;//状态矩阵的方差
    float R;//观测矩阵的方差
    float P;//预测误差
    float B;
    float u;
}KalmanInfo;

下面是卡尔曼滤波器的初始化函数,在这个函数中,info为卡尔曼滤波参数的指针。初始化的参数是针对一个车速滤波过程的设置。

void Kalm::initKalmanFilter(KalmanInfo *info)
{
    info->A = 1;
    info->H = 1;
    info->P = 0.1;
    info->Q = 0.05;
    info->R = 0.1;
    info->B = 0.1;
    info->u = 0;
    info->filterValue = 0;
}

卡尔曼滤波过程函数,函数的输入info为卡尔曼滤波参数的指针,new_value为新的测量值,函数返回滤波后的估计值。

float Kalm::kalmanFilterFun(KalmanInfo *info, float new_value)
{
    float predictValue = info->A*info->filterValue+info->B*info->u;//计算预测值
    info->P = info->A*info->A*info->P + info->Q;//求协方差
    info->kalmanGain = info->P * info->H /(info->P * info->H * info->H + info->R);//计算卡尔曼增益
    info->filterValue = predictValue + (new_value - predictValue)*info->kalmanGain;//计算输出的值
    info->P = (1 - info->kalmanGain* info->H)*info->P;//更新协方差
    return info->filterValue;
}

三、示例

        下面我们通过是一个车速滤波的示例来体验卡尔曼滤波的效果。通过上面的介绍,R对滤波效果的影响比较大,在这个示例中,我们分别将R取为0.1和0.5,来看一下车速的滤波效果。首先R取为0.1时,滤波效果如下图所示。其中,蓝色线为滤波前的车速,红色线为滤波后的车速。从图中可以看到滤波后的信号与滤波前的信号跟随很好,滞后很小。基本波动被滤掉了,但也带入了一些波动。

滤波算法(四)—— 卡尔曼滤波算法[通俗易懂]

下图为R取为0.5时的滤波效果,很明显,这张图信号的跟随效果比上图要差,滞后也多,但是滤波后曲线更平滑。

滤波算法(四)—— 卡尔曼滤波算法[通俗易懂]

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。

发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/130768.html原文链接:https://javaforall.cn

【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛

【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...

(0)


相关推荐

  • @RequestMapping拦截多个请求

    @RequestMapping拦截多个请求

  • CSS样式表的使用

    CSS样式表的使用在用html5写网页的时候,结合CSS能够让页面更美观。由于只在做课程设计和实习的时候使用过,并没有系统的学习过,使得自己对于CSS的使用一直处于能用,但是容易弄混的阶段。为了弄清楚CSS样式表的使用,特地看书系统学习,现总结如下:CSS是通过选择器对不同的HTML标签进行控制,从而实现各种效果。常用的CSS选择器有标签选择器、类别选择器、id选择器。标签选择器的使用HTML网页是由很…

  • w3c css规范_jquery是w3c标准吗

    w3c css规范_jquery是w3c标准吗2014年10月7日,W3CCEOJeffJaffe在W3C官方博客发表文章,讨论在Web技术标准制定过程中应当如何达成一致认同——是通过正式的认同流程(formalconsensusprocess)还是由标准编辑(informededitor)非正式的收集意见而达成认同。一致认同一直是W3C的核心价值,但WHATWG正在实行另外一种达成认同的方式,技术标准的编辑们通过他们的判断,决定…

  • ASSERT 和 ASSERT_VALID[通俗易懂]

    ASSERT 和 ASSERT_VALID[通俗易懂]这个宏都是MFC的调试宏.ASSERT_VALID宏用来在运行时检查一个对象的内部合法性,比如说现在有一个学生对象,我们知道每个学生的年龄一定大于零,若年龄小于零,则该学生对象肯定有问题。事实上,ASSERT_VALID宏就是转化为对象的成员函数AssertValid()的调用,只是这种方法更安全。它的参数是一个对象指针,通过这个指针来调用它的AssertValid()成员…

  • Linux系统结构详解

    Linux系统结构详解Linux系统一般有4个主要部分:内核、shell、文件系统和应用程序。内核、shell和文件系统一起形成了基本的操作系统结构,它们使得用户可以运行程序、管理文件并使用系统。部分层次结构如图1-1所示。1.linux内核Linux内核是世界上最大的开源项目之一,内核是与计算机硬件接口的易替换软件的最低级别。它负责将所有以“用户模式”运行的应用程…

  • React Native(四)——顶部以及底部导航栏实现方式

    React Native(四)——顶部以及底部导航栏实现方式

发表回复

您的电子邮箱地址不会被公开。

关注全栈程序员社区公众号