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

滤波算法(四)—— 卡尔曼滤波算法[通俗易懂]一、算法介绍卡尔曼滤波是一个神奇的滤波算法,应用非常广泛,它是一种结合先验经验、测量更新的状态估计算法。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)
blank

相关推荐

  • STUN协议详解

    STUN协议详解   基于RFC3489标准的stun协议,无法穿越TCP类型NAT,只是适用于在现有NAT类型下的UDP穿越,另一种特殊情况NAT也无法进行穿越,就是对称型NAT,在很多企业中就很多属于对称型NAT,后面会讲到。STUN的发现过程是基于UDP的NAT处理的假设;随着新的NAT设备的部署,这些假设可能会被证明是无效的,当STUN被用来获取一个地址来与位于其在同一NAT后面的对等体通信时,它就不起作用了。当stun服务器的部署不在公共共享地址域范围内时,stun就不起作用。1.定义STUN客户端:产生

  • SpringCloud-Alibaba-Sentinel-服务降级-热点限流-服务熔断

    SpringCloud-Alibaba-Sentinel-服务降级-热点限流-服务熔断前言:除了流量控制以外,对调用链路中不稳定的资源进行熔断降级也是保障高可用的重要措施之一。一个服务常常会调用别的模块,可能是另外的一个远程服务、数据库,或者第三方API等。例如,支付的时候,可能需要远程调用银联提供的API;查询某个商品的价格,可能需要进行数据库查询。然而,这个被依赖服务的稳定性是不能保证的。如果依赖的服务出现了不稳定的情况,请求的响应时间变长,那么调用服务的方法的响应时间也会变长,线程会产生堆积,最终可能耗尽业务自身的线程池,服务本身也变得不可用熔断策略Sentinel提供

  • 云计算(1)—基础知识

    云计算(1)—基础知识一、云计算概述   云计算到底是什么呢?在这个问题上,可谓众说纷纭。比如,在维基百科上的定义是“云计算是一种基于互联网的计算新方式,通过互联网上异构、自治的服务为个人和企业用户提供按需即取的计算”;著名咨询机构Gartner将云计算定义为“云计算是利用互联网技术来将庞大且可伸缩的IT能力集合起来作为服务提供给多个客户的技术”;而IBM则认为“云计算是一种新兴的IT服务交付方式,应用、数据…

  • acwing-1080. 骑士(基环树dp)[通俗易懂]

    acwing-1080. 骑士(基环树dp)[通俗易懂]Z 国的骑士团是一个很有势力的组织,帮会中聚集了来自各地的精英。他们劫富济贫,惩恶扬善,受到了社会各界的赞扬。可是,最近发生了一件很可怕的事情:邪恶的 Y 国发起了一场针对 Z 国的侵略战争。战火绵延五百里,在和平环境中安逸了数百年的 Z 国又怎能抵挡得住 Y 国的军队。于是人们把所有希望都寄托在了骑士团身上,就像期待有一个真龙天子的降生,带领正义打败邪恶。骑士团是肯定具备打败邪恶势力的能力的,但是骑士们互相之间往往有一些矛盾。每个骑士有且仅有一个他自己最厌恶的骑士(当然不是他自己),他是绝对不

  • eclipse安装android_安卓studio怎么打包apk

    eclipse安装android_安卓studio怎么打包apkEclipseandroid开发更改apk名字有以下几步:第一步,修改工程包名在eclipse里,找到项目包和java包(原则上都一样),就可以按”F2″修改名字,随之,源.java也会得到相应的修改;然而每个.java文件都需要把如下图内容,再额外修改一下第二步,修改AndroidManifest.xml文件AndroidManifest.xml里的内容是android工程的相关配置文件。工程文件的解析具体可参考:某鸟教程.第三步,修改Strings.xml文件然后是…

  • pycharm安装配置环境_如何在pycharm中配置anaconda

    pycharm安装配置环境_如何在pycharm中配置anacondapycharm安装及配置(anaconda)pycharm的下载与安装jetbrains官网开发者工具找到pycharm下载专业版等待下载完成anaconda的下载与安装anaconda官网 滑到页面最下方选择合适的版本,开始下载双击开始安装这里选择所有用户或者仅自己都行这里勾选上添加环境变量,就不用自己配置了查看开始菜单安装成功安装配置pycharm双击以后再重启,直接finish启动pycharm这里需要进行一下激活,我这里就用

发表回复

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

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