卡尔曼(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)


相关推荐

  • navicat激活2022【中文破解版】[通俗易懂]

    (navicat激活2022)这是一篇idea技术相关文章,由全栈君为大家提供,主要知识点是关于2021JetBrains全家桶永久激活码的内容https://javaforall.cn/100143.htmlIntelliJ2021最新激活注册码,破解教程可免费永久激活,亲测有效,上面是详细链接哦~1TCF2R91JZ-eyJsaWNlb…

  • 如何写 Cover Letter 论文投稿「建议收藏」

    如何写 Cover Letter 论文投稿「建议收藏」论文投稿时一般要求附带一篇“CoverLetter”,好的“CoverLetter”不仅可以使文章锦上添花,更有可能直接决定论文是否被录用。本人切身体会,以已发表论文为例,现将其写法经验归纳如下:Dear Editor,We would like to submit the enclosed manuscript entitled "论文题目", which we wish to be con…

  • 在 pycharm中安装pytorch

    在 pycharm中安装pytorch参考文章:在pycharm中安装pytorch:https://blog.csdn.net/weixin_43183872/article/details/83473009torch包在pycharm里面的导入问题:https://blog.csdn.net/qq_31187803/article/details/79601643…

  • java.lang.OutOfMemoryError:GC overhead limit exceeded

    java.lang.OutOfMemoryError:GC overhead limit exceeded简单来说,java.lang.OutOfMemoryError:GCoverheadlimitexceeded发生的原因是,当前已经没有可用内存,经过多次GC之后仍然没能有效释放内存。

  • 猴子摘香蕉问题python_硬币找零&&爬楼梯&&猴子摘香蕉「建议收藏」

    猴子摘香蕉问题python_硬币找零&&爬楼梯&&猴子摘香蕉「建议收藏」硬币找零&&爬楼梯&&猴子摘香蕉假设有几种硬币,如1、3、5,并且数量无限。请找出能够组成某个数目的找零所使用最少的硬币数。#include”CoinProblem.h”#includeintcountNum=MAX;voidCoinProblem(int*coin,intLength,intValue,intcount){if(Value==0){if(count…

  • 一些安全扫描工具_web弱口令扫描工具

    一些安全扫描工具_web弱口令扫描工具术语表术语定义敏感数据敏感数据的具体范围取决于产品具体的应用场景,产品应根据风险进行分析和判断。典型的敏感数据包括口令、银行帐号、大批量个人数据、用户通信内容和密钥等。一类如果丢失或者泄漏,会对数据的所有者造成负面影响的数据。本基线定义的“敏感数据”包括但不限于:口令、通行码、密钥、证书、License、隐私数据(如短消息的内容)、授权凭…

发表回复

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

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