卡尔曼滤波理论很容易就可以在MATLAB软件环境下实现，但是，实际的硬件板子上还是需要C语言，当然可以自动代码生成，还有一种就是直接手动编写C语言。
1.前言
在google上搜索卡尔曼滤波，很容易找到以下这个帖子：/lanbing510/article/details/8828109
帖子最后用matlab实现了kalman，然后博主的前面一些帖子也有详细转载相关贴子，自己也给出了一些源代码，例如转载的这篇卡尔曼滤波器通俗介绍：/weixin_38451800/article/details/85462129
2.卡尔曼滤波的C语言
网上很多的是关于多维kalman实现。参照网上的一些代码，本文实现了一个一维地滤波，对于有C语言基础的同学来讲，理解起来应该很容易了。
百度百科里面有这个帖子：/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.接下来，是另外一个版本的单维卡尔曼滤波的Ｃ语言源代码：
#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.56float 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; // 最优值对应的covarianceprintf("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));
——————————————————————————————————————————
部分内容转载自：/thread-5571611-1-1.html 和/view/8523cb6eaf1ffc4ffe47ac24.html
