Kalman Filter)

The actual temperature test data is the real data measured by heating a basin of water through a heating rod. The X axis is time second, and the Y axis is temperature:

1) Before Filtering

2) After filtering (P = 10, q = 0.0001, r = 0.05, kgain = 0 ;)

2) After filtering (P = 10, q = 0.00001, r = 1, kgain = 0;), the Y axis is enlarged by 10 times and rounded up.

.

 

C language code:

#define LINE 1024static 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 code:

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);

Note: d2.txt stores the input data, with one row. D3 is the output data.

 

The R parameter adjusts the similarity between the filtered curve and the measured curve. The smaller the R parameter, the closer it is.

The smoothing degree of the curve after the Q parameter is adjusted and filtered. The smaller the Q parameter, the smoother it is.