卡尔曼滤波算法及C语言实现_源代码
卡尔曼滤波理论很容易就可以在MATLAB软件环境下实现,但是,实际的硬件板子上还是需要C语言,当然可以自动代码生成,还有一种就是直接手动编写C语言。
1.前言
在google上搜索卡尔曼滤波,很容易找到以下这个帖子:http://blog.csdn.net/lanbing510/article/details/8828109
帖子最后用matlab实现了kalman,然后博主的前面一些帖子也有详细转载相关贴子,自己也给出了一些源代码,例如转载的这篇卡尔曼滤波器通俗介绍:https://blog.csdn.net/weixin_38451800/article/details/85462129
2.卡尔曼滤波的C语言
网上很多的是关于多维kalman实现。参照网上的一些代码,本文实现了一个一维地滤波,对于有C语言基础的同学来讲,理解起来应该很容易了。
百度百科里面有这个帖子:http://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html
讲解的是一维kalman滤波器,但是最后printf出来的都是分立的值,看不出来什么。参考那段代码,改写成了下面这段代码,在labwindows里面绘制了一段曲线,效果就很直观了:
/*-------------------------------------------------------------------------------------------------------------*/
void KalmanFilter(unsigned int ResrcDataCnt,const double *ResrcData,double *FilterOutput,double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
{unsigned int i;double R = MeasureNoise_R;double Q = ProcessNiose_Q;double x_last = *ResrcData;double x_mid = x_last;double x_now;double p_last = InitialPrediction;double p_mid ;double p_now;double kg;for(i=0;i<ResrcDataCnt;i++){x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声kg=p_mid/(p_mid+R); //kg为kalman filter,R为噪声x_now=x_mid+kg*(*(ResrcData+i)-x_mid);//估计出的最优值p_now=(1-kg)*p_mid;//最优值对应的covariance*(FilterOutput + i) = x_now;p_last = p_now; //更新covariance值x_last = x_now; //更新系统状态值}}
/*-------------------------------------------------------------------------------------------------------------*/
参考上面的代码,优化了一下之后(运行在STM32上):
/*-------------------------------------------------------------------------------------------------------------*/
/* Q:过程噪声,Q增大,动态响应变快,收敛稳定性变坏R:测量噪声,R增大,动态响应变慢,收敛稳定性变好
*/double KalmanFilter(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
{double R = MeasureNoise_R;double Q = ProcessNiose_Q;static double x_last;double x_mid = x_last;double x_now;static double p_last;double p_mid ;double p_now;double kg; x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声kg=p_mid/(p_mid+R); //kg为kalman filter,R为噪声x_now=x_mid+kg*(ResrcData-x_mid);//估计出的最优值p_now=(1-kg)*p_mid;//最优值对应的covariance p_last = p_now; //更新covariance值x_last = x_now; //更新系统状态值return x_now;
}/*-------------------------------------------------------------------------------------------------------------*/
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3.接下来,是另外一个版本的单维卡尔曼滤波的C语言源代码:
#include "stdio.h"
#include "stdlib.h"
#include "math.h"
double frand()
{ return 2*((rand()/(double)RAND_MAX) - 0.5); // 随机噪声}void main()
{ float x_last=0;
float p_last=0.02;
float Q=0.018;
float R=0.542;
float kg;
float x_mid;
float x_now;
float p_mid;
float p_now;
float z_real=0.56;//0.56
float z_measure;
float sumerror_kalman=0;
float sumerror_measure=0;
int i;
x_last=z_real+frand()*0.03;
x_mid=x_last;
for(i=0;i<20;i++)
{ x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q= 噪声
kg=p_mid/(p_mid+R); //kg 为kalman filter ,R为噪声
z_measure=z_real+frand()*0.03; // 测量值
x_now=x_mid+kg*(z_measure-x_mid); // 估计出的最优值
p_now=(1-kg)*p_mid; // 最优值对应的covariance
printf("Real position: %6.3f \n",z_real); // 显示真值
printf("Mesaured position: %6.3f [diff:%.3f]\n",z_measure,fabs(z_real-z_measure));// 显示测量值以及真值与测量值之间的误差
printf("Kalman position: %6.3f [diff:%.3f]\n",x_now,fabs(z_real - x_now)); // 显示kalman 估计值以及真值和卡尔曼估计值的误差
sumerror_kalman += fabs(z_real - x_now); //kalman 估计值的累积误差
sumerror_measure += fabs(z_real-z_measure); // 真值与测量值的累积误差
p_last = p_now; // 更新covariance 值
x_last = x_now; // 更新系统状态值
}
printf(" 总体测量误差 : %f\n",sumerror_measure); // 输出测量累积误差
printf(" 总体卡尔曼滤波误差: %f\n",sumerror_kalman); // 输出kalman 累积误差
printf(" 卡尔曼误差所占比例: %d%% \n",100-(int)((sumerror_kalman/sumerror_measure)*100));
——————————————————————————————————————————
部分内容转载自:https://www.amobbs.com/thread-5571611-1-1.html 和https://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html
卡尔曼滤波算法及C语言实现_源代码相关推荐
- 卡尔曼滤波算法c语言stm32,卡尔曼滤波算法及C语言实现_源代码
a往南向北 2019-01-16 20:39:20 11340 收藏 111 分类专栏: C语言嵌入式 文章标签: 卡尔曼滤波 C代码 卡尔曼滤波理论很容易就可以在MATLAB软件环境下实现,但是 ...
- 卡尔曼滤波算法及c语言代码,卡尔曼滤波算法及C语言代码
卡尔曼滤波算法及C语言代码 卡尔曼滤波简介及其算法实现代码 卡尔曼滤波算法实现代码(C,C++分别实现) 卡尔曼滤波器简介 近来发现有些问题很多人都很感兴趣.所以在这里希望能尽自己能力跟大家讨论一些力 ...
- 四阶龙格库塔法c语言程序,四阶龙格_库塔算法的C语言实现_毋玉芝
2001年3月焦作大学学报 1 第1期JOURNALOFJIAOZUOUNIVERSITYMar.2001 四阶龙格 库塔算法的C语言实现 毋玉芝 (焦作财会学校) 摘 要 本文叙述了四阶龙格 库塔算 ...
- 卡尔曼滤波算法及C语言实现(转载)
版权声明:本文为博主原创文章,未经博主允许不得转载. https://blog.csdn.net/sinat_20265495/article/details/51006311 kalman filt ...
- 数据结构与算法python语言实现答案_数据结构与算法:Python语言实现 源代码 PPT 练习答案 源码.zip...
1 60660-数据结构与算法:Python语言实现[练习答案]Solutions Manual.rar 943.25 KB 2018/11/1 12:03:34 2 __MACOSX 0 Bytes ...
- c代码实现 ifft运算_fft算法c语言_matlab fft算法_ifft c语言
FFT快速算法C程序_工学_高等教育_教育专区.电子信息工程综合课程设计报告书 DSP 课程设计 报告 题学 目: 院: FFT 快速算法 C 程序 计算机与信息工程学院 09 ... fft算法代码 ...
- iar环境下c语言编程,c语言_源代码-iar环境配置.pdf
c语言_源代码-iar环境配置 欢迎光临我的博客:/mikehendry 其实,IAR 编译环境的配置是相当重要的,没配置正确或者不符合自己的习惯的话,使用起来就会很麻烦.下面 我根据网上的经验和资料 ...
- matlab温度数据怎么滤波_卡尔曼滤波算法思想理解 Kalman filter 第一篇
卡尔曼滤波算法思想理解 Kalman filter 第一篇 最近在初步的理解目标跟踪的领域, 其中一个非常经典的算法卡尔曼滤波Kalman filter是需要有很好的理解才行, 由于已经脱离了学校,懂 ...
- python中栈的描述是_数据结构与算法:Python语言描述 栈和队列.ppt
数据结构与算法:Python语言描述 栈和队列 迷宫问题 迷宫问题的特点: 存在一集可能位置,一些位置相互连通,一步可达 一个位置可能连通若干位置,出现向前探查的多种可能(有分支) 目标是找到一条路径 ...
最新文章
- python打砖块游戏算法设计分析_python实现打砖块游戏
- 最大同性恋交友网站被微软收购,我不服!
- 视觉SLAM笔记(1) 初识SLAM
- ipfs c++client
- 物流系统开发中,Dev倥件的使用
- redis(版本redis-5.0.2)的安装步骤
- 【BERT】小学生级上手教程,从原理到上手全有图示,还能直接在线运行
- 软件测试(四):软件测试用例设计
- 与时俱进的迅捷多功能转换器
- mysql字段动态扩展_数据库动态扩展字段
- 32768HZ晶振不起振
- 关于纯流量卡-物联网卡的一点个人看法
- 联想潮5000 BIOS 解锁 CFG Lock
- 树莓派linux led字符设备驱动(信号量)
- 提取动漫视频里的BGM
- Google如何在新标签打开页面打开链接?
- svn提交报错,Error running context: 远程主机强迫关闭了一个现有的连接
- 产品经理(PM)一秒理解MRD、PRD、BRD
- 从业务需求出发,开启IDC高效运维之路
- 中兴ZXHN F412/F460/F660等破解超级密码、破解用户限制、关闭远程控制、恢复路由器拨号
热门文章
- 使用DCDC升压电路——MT3608芯片
- vasp能装在window系统里吗_Window安装系统详解
- fastqc检验时不能执行java_安装Fastqc软件遇到的坑
- Word下划线怎么打?速速get这5个实用方法!
- 基于双目视觉的非标机械臂的空间定位流程(未完待续)
- Windows自带的“QQ”
- 代码主题darcula_仿IntelliJ Darcula的Swing主题FlatLaf使用方法
- 會懷孕的木乃伊~恐怖!! 使木乃伊懷孕的人~更加恐怖!
- windows 剪贴板监控
- dw cs6 html建站点,dreamweaver cs6网页中制作锚点链接的教程方法