卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用

卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用文章目录卡尔曼滤波一、滤波效果展示二、简介三、组成1.预测状态方程(1)目的:(2)方程:(3)备注2.预测协方差方程(1)目的(2)方程(3)备注3.卡尔曼增益方程(1)目的(2)方程(3)备注4.跟新最优值方程(卡尔曼滤波的输出)(1)目的(2)方程(3)备注5.更新协方差方程(1)目的(2)方程(3)备注四、C程序代码实现1.参数列表2.代码实现(一维数据滤波)五、发送波形到…

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

卡尔曼滤波

一、滤波效果展示

  蓝色的波形是实际测得的数据,红色的波形是经 Kalman 滤波后的数据波形。
注:这里是实际应用激光测距传感器(TOF)vl53l0x 测得的距离数据。
卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用

二、简介

  采用递归的方法解决线性滤波问题,只需要当前的测量值和前一个采样周期的估计值就能进行状态估计,需要的存储空间小,每一步的计算量小。

三、组成

1. 预测状态方程

(1)目的:

  由 系统状态变量k-1时刻的最优值系统输入 计算出k时刻的 系统预测值

(2)方程:

卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用

(3)备注

  ①. X k-1|k-1 为k-1时刻的输出。
  ②. 当X为一维数据时,Fk的值是1。
  ③. 一维数据下(uk=0时):系统预测值 = 系统状态变量k-1时刻的最优值。

2. 预测协方差方程

(1)目的

  根据 k-1时刻的系统协方差 预测 k时刻系统协方差

(2)方程

卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用

(3)备注

  ①. 当X为一维数据时,Fk的值是1。

3. 卡尔曼增益方程

(1)目的

  根据(k时刻) 协方差矩阵的预测值 计算 卡尔曼增益

(2)方程

卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用

(3)备注

  ①. 当 Pk|k-1 为一个一维矩阵时,Hk 是1。

4. 跟新最优值方程(卡尔曼滤波的输出)

(1)目的

  根据 状态变量的预测值系统测量值 计算出 k时刻状态变量的最优值

(2)方程

卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用

(3)备注

  ①. 当 Pk|k-1 为一个一维矩阵时,Hk 是1。

5. 更新协方差方程

(1)目的

  为了求 k时刻的协方差矩阵。(为得到k+1时刻的卡尔曼输出值做准备)

(2)方程

卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用

(3)备注

  ①. 当 Pk|k-1 为一个一维矩阵时,Hk 是1。

四、C 程序代码实现

1. 参数列表

卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用

2. 代码实现(一维数据滤波)

  实际参数是参照别人已经选好的参数,不过也可以自己改变参数,去观察波形的效果,体会每个参数对于滤波效果的影响,这里不详细介绍

//1. 结构体类型定义
typedef struct 
{ 
   
    float LastP;//上次估算协方差 初始化值为0.02
    float Now_P;//当前估算协方差 初始化值为0
    float out;//卡尔曼滤波器输出 初始化值为0
    float Kg;//卡尔曼增益 初始化值为0
    float Q;//过程噪声协方差 初始化值为0.001
    float R;//观测噪声协方差 初始化值为0.543
}KFP;//Kalman Filter parameter

//2. 以高度为例 定义卡尔曼结构体并初始化参数
KFP KFP_height={ 
   0.02,0,0,0,0.001,0.543};

/** *卡尔曼滤波器 *@param KFP *kfp 卡尔曼结构体参数 * float input 需要滤波的参数的测量值(即传感器的采集值) *@return 滤波后的参数(最优值) */
 float kalmanFilter(KFP *kfp,float input)
 { 
   
     //预测协方差方程:k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
     kfp->Now_P = kfp->LastP + kfp->Q;
     //卡尔曼增益方程:卡尔曼增益 = k时刻系统估算协方差 / (k时刻系统估算协方差 + 观测噪声协方差)
     kfp->Kg = kfp->Now_P / (kfp->NOw_P + kfp->R);
     //更新最优值方程:k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * (测量值 - 状态变量的预测值)
     kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
     //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
     kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
     return kfp->out;
 }

/** *调用卡尔曼滤波器 实践 */
int height;
int kalman_height=0;
kalman_height = kalmanFilter(&KFP_height,(float)height);

五、发送波形到上位机显示

  这里使用的是匿名的上位机 V65 版本,具体如何使用可以参考茶大的博客,并且茶大博客里面有上位机的下载地址。茶大博客地址:https://blog.csdn.net/wangjt1988/article/details/83684188
  
  
注:文章方程截图及参数来源于中科浩电。
  
  
Author : Beyonderwei
Email : Beyonderwei@163.com
Website : http://beyonderwei.com
WeChat:
微信公众平台

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

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

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

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

(0)
blank

相关推荐

  • SAP WebIDE里OData service catalog的实现原理「建议收藏」

    SAP WebIDE里OData service catalog的实现原理「建议收藏」我们在SAPWebIDE里创建UI5应用时,可以从Servicecatalog里选择需要的OData服务,如下图所示:这个ag3-backend是什么意思?是我在SAPCloudPlatform的Destination标签页里维护的一个Destination:这个destination指向了一个OnpremiseABAPNetweaver系统,AG3,通过SAPCloud…

    2022年10月18日
  • nextSibling 和nextElementSibling的区别[通俗易懂]

    nextSibling 和nextElementSibling的区别[通俗易懂]使用nextSibling属性返回指定节点之后的下一个兄弟节点,(即:相同节点树层中的下一个节点)。nextSibling属性与nextElementSibling属性的差别: nextSibling属性返回元素节点之后的兄弟节点(包括文本节点、注释节点即回车、换行、空格、文本等等); nextElementSibling属性只返回元素节点之后的兄弟元素节点(不包括文本节点、注释节点);注意:空…

  • TiDB建库建用户及授权

    TiDB建库建用户及授权

  • Arm-A53资料「建议收藏」

    2012年10月Cortex-A53推出了市场,它带来了ARMv8指令集,在高能效比、节省面积基础上还有显著的性能提升。目前Cortex-A53已可以授权,ARM多个合作伙伴会在2014年推出相关芯片。开发者、OEM厂商和SoC设计者需要了解的Cortex-A53的几个重要特点如下:1、ARM低功耗/高效率的传承ARM9是ARM历史上授权最多的处理器,有超过250个授权。它树立了一个非常重要的功率/成本平衡的最有效点。基于ARM926的功能手机(Nokia…

  • Android开源代码_源代码目录结构

    Android开源代码_源代码目录结构Android源码的第一级目录结构Android/abi(abi相关代码。ABI:applicationbinaryinterface,应用程序二进制接口)Android/bionic(bionicC库)Android/bootable(启动引导相关代码)Android/build(存放系统编译规则及generic等基础开发配置包)

    2022年10月16日
  • 当前汇总(最新序列号破解)「建议收藏」

    当前汇总(最新序列号破解),https://javaforall.cn/100143.html。详细ieda激活码不妨到全栈程序员必看教程网一起来了解一下吧!

发表回复

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

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