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

大侠好，欢迎来到FPGA技术江湖，江湖偌大，相见即是缘分。大侠可以关注FPGA技术江湖，在“闯荡江湖”、"行侠仗义"栏里获取其他感兴趣的资源，或者一起煮酒言欢。

今天给大侠带来测试卡尔曼滤波器(Kalman Filter)，话不多说，上货。

本文由“壹伴编辑器”提供技术支持

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

1）滤波前

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

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

相关C语言代码：

代码语言：javascript复制

#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, "%fn",outData[i]); 
    } 
    fclose(fp2);

}

matlab代码：

代码语言：javascript复制

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越小越平滑。

END

后续会持续更新，带来Vivado、 ISE、Quartus II 、candence等安装相关设计教程，学习资源、项目资源、好文推荐等，希望大侠持续关注。

大侠们，江湖偌大，继续闯荡，愿一切安好，有缘再见！

fpga

0 人点赞