测试卡尔曼滤波器(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越小越平滑。