卡尔曼滤波C程序

#define ADDataCnt 10

double ADData[ADDataCnt];

void InitADData(double RealValue, unsigned int Cnt) {

unsigned int i;

for( i =0; i < Cnt; i++) {

ADData[i] = RealValue + 0.2*rand()/(double)RAND_MAX - 0.1; } }

double KalmanFilter(unsigned int DataCnt,double *Data,double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction) {

unsigned int i;

double R = MeasureNoise_R; double Q = ProcessNiose_Q; double x_last = *Data; double x_mid = x_last; double x_now;

double p_last = InitialPrediction; double p_mid ; double p_now; double kg;

for(i=0;ix_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)

p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声

kg=p_mid/(p_mid+R); //kg为kalman filter，R为噪声 //z_measure=z_real+frand()*0.03;//测量值

x_now=x_mid+kg*(*(Data+i)-x_mid); //估计出的最优值

p_now=(1-kg)*p_mid; //最优值对应的covariance

p_last = p_now; //更新covariance值

x_last = x_now; //更新系统状态值 }

return x_now; }

int main() {

unsigned int i;

double KalmanFilterReturn; InitADData(2.1,ADDataCnt);

for(i = 0;i < ADDataCnt;i++)

printf(\"ADData[%d] Is : %lf\\n\

KalmanFilterReturn = KalmanFilter(ADDataCnt,ADData,0,0.005,10);

printf(\"\\n Kalman Output : %lf\\n\ while(1); }