完善资料让更多小伙伴认识你,还能领取20积分哦, 立即完善>
卡尔曼滤波理论很容易就可以在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 { 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(" 总体测量误差 : %fn",sumerror_measure); // 输出测量累积误差 printf(" 总体卡尔曼滤波误差: %fn",sumerror_kalman); // 输出kalman 累积误差 printf(" 卡尔曼误差所占比例: %d%% n",100-(int)((sumerror_kalman/sumerror_measure)*100)); |
|
|
|
只有小组成员才能发言,加入小组>>
调试STM32H750的FMC总线读写PSRAM遇到的问题求解?
419 浏览 0 评论
X-NUCLEO-IHM08M1板文档中输出电流为15Arms,15Arms是怎么得出来的呢?
448 浏览 1 评论
290 浏览 2 评论
STM32F030F4 HSI时钟温度测试过不去是怎么回事?
257 浏览 2 评论
ST25R3916能否对ISO15693的标签芯片进行分区域写密码?
623 浏览 2 评论
1388浏览 9评论
STM32F0_TIM2输出pwm2后OLED变暗或者系统重启是怎么回事?
85浏览 3评论
45浏览 3评论
STM32CUBEMX4.22.1在main函数里面添加一行语句就死机的原因?
58浏览 3评论
50浏览 3评论
小黑屋| 手机版| Archiver| 电子发烧友 ( 湘ICP备2023018690号 )
GMT+8, 2024-5-5 03:52 , Processed in 0.710157 second(s), Total 75, Slave 59 queries .
Powered by 电子发烧友网
© 2015 bbs.elecfans.com
关注我们的微信
下载发烧友APP
电子发烧友观察
版权所有 © 湖南华秋数字科技有限公司
电子发烧友 (电路图) 湘公网安备 43011202000918 号 电信与信息服务业务经营许可证:合字B2-20210191 工商网监 湘ICP备2023018690号