c语言 卡尔曼滤波

#include <stdlib.h>
#include <stdio.h>
#include <math.h>

typedef struct{
	double prevData;
	double p,q,r,kGain;
}Kalman;

void KalmanInit(Kalman *k){
	k->kGain=0;
	k->p=5;	//p初值可以随便取，但是不能为0（0的话最优滤波器了）
	k->q=0.001;	//q参数调滤波后的曲线平滑程度，q越小越平滑
	k->r=0.5;	//r参数调整滤波后的曲线与实测曲线的相近程度，越小越接近
	k->prevData=0;
	return;
}
double KalmanFilter(Kalman *k,double data){
	k->p=k->p+k->q;
	k->kGain=k->p/(k->p+k->r);
	data=k->prevData+k->kGain*(data-k->prevData);
	k->p=(1-k->kGain*k->p);
	k->prevData=data;
	return data;
}
main(){
	Kalman kalman;
	int i;
	double t[100],tk[100];
	FILE *fp;
	#define NOISE (rand()%2000/1000.-1)
	KalmanInit(&kalman);
	kalman.p=5;
	kalman.q=0.8;
	kalman.r=4;
	for(i=0;i<100;i++){
		t[i]=sin(i/10.)+0.3*NOISE;
		tk[i]=KalmanFilter(&kalman,t[i]);
	}
	fp=fopen("1.csv","w");
	for(i=0;i<100;i++){
		fprintf(fp,"%d,%f,%f\n",i,t[i],tk[i]);
	}
	fclose(fp);
}

注：结构体的目的是可以很方便地使用多个滤波器