社区导航

 

搜索
查看: 219|回复: 1

[分享] 测试卡尔曼滤波器(Kalman Filter)

[复制链接]

1444

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

发表于 2020-1-14 07:52 | 显示全部楼层 |阅读模式

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

1)滤波前

image.png

2)滤波后(p=10, q=0.0001, r=0.05, kGain=0;)

image.png

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

image.png

相关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=kalmanFilter(inData); 
    }

    fp2 = fopen("d3.txt", "w"); 
    for(i=0;i<cnt;i++)
        fprintf(fp2, "%f\n",outData); 
    } 
    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越小越平滑。


回复

使用道具 举报

124

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

发表于 2020-1-14 09:12 | 显示全部楼层

学习了,多谢


回复

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

关闭

站长推荐上一条 /6 下一条

  • 论坛活动 E手掌握

    扫码关注
    EEWORLD 官方微信

  • EE福利  唾手可得

    扫码关注
    EE福利 唾手可得

Archiver|手机版|小黑屋|电子工程世界 ( 京ICP证 060456 )

GMT+8, 2020-1-30 04:54 , Processed in 0.094053 second(s), 17 queries , Gzip On, MemCache On.

快速回复 返回顶部 返回列表