首页 > 代码库 > 测试卡尔曼滤波器(Kalman Filter)

测试卡尔曼滤波器(Kalman Filter)

2024-07-09 22:31:27   221人阅读

真实的温度测试数据，通过加热棒加热一盆水测得的真实数据，X轴是时间秒，Y轴是温度：

1）滤波前

kalman_before

2）滤波后（p=10, q=0.0001, r=0.05, kGain=0;）

kalman_after

2）滤波后（p=10, q=0.00001, r=1, kGain=0;），Y轴放大10倍并取整

kalman3。

 

相关C语言代码：

#define LINE 1024static float prevData=http://www.mamicode.com/0; static float p=10, q=0.0001, r=0.05, kGain=0;float kalmanFilter(float inData) {    p = p+q;     kGain = p/(p+r);    inData = prevData+(kGain*(inData-prevData));     p = (1-kGain)*p;    prevData = inData;    return inData; }char *ReadData(FILE *fp, char *buf) {     return fgets(buf, LINE, fp); }int main() {     FILE *fp, *fp2;     char *p, *buf;     size_t len = 0;     ssize_t read;     float inData[1000];     float outData[1000];     uint32_t i,cnt=0;    fp = fopen("d2.txt", "r");     if (fp==NULL)         exit(1);     buf = (char*)malloc(LINE*sizeof(char));     p=ReadData(fp, buf);    while(p) {         inData[cnt]=atof(p);         cnt++;         p=ReadData(fp,buf);     }    fclose(fp);    for(i=0;i<cnt;i++)        outData[i]=kalmanFilter(inData[i]);     }    fp2 = fopen("d3.txt", "w");     for(i=0;i<cnt;i++)        fprintf(fp2, "%f\n",outData[i]);     }     fclose(fp2);}

matlab代码：

d2 = load(‘d2.txt‘); plot(d2); prevData=0.0; p=10; q=0.0001; r=0.05; kGain=0; outData=[];for i=1:length(d2) p=p+q; kGain=p/(p+r); temp=d2(i); temp=prevData+(kGain*(temp-prevData)); p=(1-kGain)*p; prevData=temp; outData(i)=temp; endplot(outData);

说明：d2.txt存放的是输入的数据，每行一个。d3是输出的数据。

 

r参数调整滤波后的曲线与实测曲线的相近程度，r越小越接近。

q参数调滤波后的曲线平滑程度，q越小越平滑。


联系
我们