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

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

1）滤波前

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

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

。

 

相关C语言代码：

#define LINE 1024



static float prevData=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; 

end



plot(outData);

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

 

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

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