kaerman.c

基于DSP的卡尔曼滤波器的设计

#include "kker.h"
#define NUMBER 5
#define PI 3.1415926

int kaerman();
float InputWave();
float fX[NUMBER]={ 0.0 };
float fInput;
float f1,f2, fS1,fS2;
float ff;
int k, nIn;
float fIn[50];
float ss[1000]={0.0}, W,R;
float a, S,A,e[1000];
float b[10];
main()
{
  a=sqrt(5);
    nIn=0;
	ff=2*PI;
	f1=0;
	f2=PI*1;
//	fS1=2*PI/30;
//	fS2=2*PI*1.4;
	fS1=2*PI/30;
	fS2=2*PI/2.5;
	while( 1 )
	{
		fInput=InputWave();
		fIn[nIn]=fInput;
		nIn++; nIn%=256;
		}
	}
float InputWave()
{
	for (k=NUMBER-1;k>0;k--)
	fX[k]=fX[k-1];
 	fX[0]=sin((float)f1)+cos((float)f2)/6;
	f1+=fS1; 
	if(f1>=ff) f1-=ff;
	f2+=fS2;
	if(f2>=ff) f2-=ff;
	return(fX[0]);
}
int kaerman(float ss[1000], float *e)
{  int k;
   float b[10]={0.0}; 
  for (k=2;k<15;k++)
  { S=W*W/(1-0.5);
    A=W*W/R*R;
    b[1]=S/(S+R*R);
    ss[0]=0;
    ss[1]=a*ss[0]+b[1]*(fX[0]-a*ss[0]); 
   b[k]=(A+5*(b[k-1]))/(1+A+5*(b[k-1]));
   ss[k]=sqrt(5)*ss[k-1]+b[k]*(fX[0]-sqrt(5)*ss[k-1]);
   e[k]=b[k]*R*R;
   }
 	return(ss[k]);
       
  }