kalman2.cpp

我自己写的利用kalman进行目标运动分析的程序

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


int qsd=0;
float VarErrMat[18][4*4];/*={100000.0,0,0,0,0,100000.0,0,0,0,0,100000.0,0,0,0,0,100000.0};*/
float XVxYVy[18][4];/*={0,0,0,0};*/

void damul(float a[16],float b[16],int m,int n,int k,float c[16]);
void dcinv(float a[4]);
void kalmanfilt(float d,float betaAB,float alphaA,float alphaB,float AngA,float AngB,int pihao);
void main()
{
	FILE *fid1,*fid2,*fid3,*fid4;
	float d;
	float betaAB;
	float alphaA;
	float alphaB;
	float AngA;
	float AngB;
	int   PosTim;
	int over;
	int pihao;
	int i,datain;
	
	d=11000.0;
	betaAB=90*3.1416/180;    /* 90 : 1.57*/
	alphaA=90*3.1416/180;
	alphaB=90*3.1416/180;
	PosTim=2667;
	for(i=0;i<18;i++)
	{
		VarErrMat[i][0]=100000.0;
		VarErrMat[i][1]=0;
		VarErrMat[i][2]=0;
		VarErrMat[i][3]=0;
		VarErrMat[i][4]=0;
		VarErrMat[i][5]=100000.0;
		VarErrMat[i][6]=0;
		VarErrMat[i][7]=0;
		VarErrMat[i][8]=0;
		VarErrMat[i][9]=0;
		VarErrMat[i][10]=100000.0;
		VarErrMat[i][11]=0;
		VarErrMat[i][12]=0;
		VarErrMat[i][13]=0;
		VarErrMat[i][14]=0;
		VarErrMat[i][15]=100000.0;
		XVxYVy[i][0]=0;
		XVxYVy[i][1]=0;
		XVxYVy[i][2]=0;
		XVxYVy[i][3]=0;
	}
	pihao=0;

	fid1=fopen("e:\\fusion\\data\\AngA1.txt","r");
	fid2=fopen("e:\\fusion\\data\\AngB1.txt","r");
	fid3=fopen("e:\\fusion\\data\\Xout.txt","w");
	fid4=fopen("e:\\fusion\\data\\Yout.txt","w");
	for(datain=0;datain<PosTim;datain++)
	{
		fscanf(fid1,"%f",&AngA);
		fscanf(fid2,"%f",&AngB);

		kalmanfilt(d,betaAB,alphaA,alphaB,AngA,AngB,pihao);

		fprintf(fid3,"%f ",XVxYVy[pihao][0]);
		fprintf(fid4,"%f ",XVxYVy[pihao][2]);
	}
	fclose(fid1);
	fclose(fid2);
	fclose(fid3);
	fclose(fid4);
	over=0;
}

void damul(float a[16],float b[16],int m,int n,int k,float c[16])
/* damul() : 计算矩阵a(m*n)与矩阵b(n*k)的乘积c(m*k)*/
{
	int i,j,l,u;
	for (i=0;i<=m-1;i++)
	for (j=0;j<=k-1;j++)
	{
		u=i*k+j;
		c[u]=0.0;
		for (l=0;l<=n-1;l++)
			c[u]=c[u]+a[i*n+l]*b[l*k+j];
	}
}
void dcinv(float a[4])
/* dcinv(): 计算矩阵a(2*2)的逆矩阵a(2*2)*/
{
	float b[4];
	float temp;
	int k;
	temp=a[0]*a[3]-a[1]*a[2];
	b[0]=a[3]/temp;
	b[1]=-a[1]/temp;
	b[2]=-a[2]/temp;
	b[3]=a[0]/temp;
	for(k=0;k<4;k++)
	{
		a[k]=b[k];
	}
}

void kalmanfilt(float d,float betaAB,float alphaA,float alphaB,float AngA,float AngB,int pihao)
/* kalmanfilt(): 两个阵检测动目标轨迹卡尔曼滤波
   d：两阵间距　betaAB: 两阵连线与正北夹角
   alphaA、alphaB: 阵A和B的阵艏与正北的夹角
   AngA、AngB: 阵A和B分别测得的目标方位角（相对于正北）*/
{
	float Fai[4*4]={1.0,1.0,0,0,0,1.0,0,0,0,0,1.0,1.0,0,0,0,1.0};
	float FaiT[4*4]={1.0,0,0,0,1.0,1.0,0,0,0,0,1.0,0,0,0,1.0,1.0};
	float H[2*4]={1.0,0,0,0,0,0,1.0,0};
	float HT[4*2]={1.0,0,0,0,0,1.0,0,0};
	float I[4*4]={1.0,0,0,0,0,1.0,0,0,0,0,1.0,0,0,0,0,1.0};
	float Q[4*4]={0.000001,0,0,0,0,0.000001,0,0,0,0,0.000001,0,0,0,0,0.000001};
	float R[2*2];/*={1000000.0,1000,1000,1000000.0};*/
	float dA=2*3.1416/180;    /* 阵的测向误差*/
	float dB=2*3.1416/180;
	float dt=1.0;
	float TarX,TarY;
	float gamaA,gamaB;		
	int k;
	float Xj[4*1];
	float temp1[4*4];
	float Pj[4*4];
	float temp2[2*4];/* or temp2[4*2]*/
	float S[2*2];
	float K[4*2];
	float Zj[2*1];
	float P[4*4];
	float X[4*1];
		
	if(fabs(sin(AngA-AngB))<0.000001)   /* 判断用于坐标计算的分母是否为零 */
	{
		TarX=XVxYVy[pihao][0];
		TarY=XVxYVy[pihao][2];
	}
	else
	{
		TarX=-d*sin(AngA)*sin(AngB-betaAB)/sin(AngA-AngB);
		TarY=-d*cos(AngA)*sin(AngB-betaAB)/sin(AngA-AngB);
	}
	if(qsd<2)        /* 前两点用于确定目标坐标和速度的初始值 */
	{
		qsd=qsd+1;
		XVxYVy[pihao][1]=(TarX-XVxYVy[pihao][0])/dt;
		XVxYVy[pihao][3]=(TarY-XVxYVy[pihao][2])/dt;
		XVxYVy[pihao][0]=TarX;
		XVxYVy[pihao][2]=TarY;
	}
	else
	{
		gamaA=AngA-alphaA+1.57;  /* pi/2=1.57 */
		if(gamaA<=0)
		{
			gamaA=gamaA+6.2832;  /* 2*pi=6.2832	*/		
		}
		else if(gamaA>6.2832)
		{
			gamaA=gamaA-6.2832;
		}
		gamaB=AngB-alphaB+1.57;
		if(gamaB<=0)
		{
			gamaB=gamaB+6.2832;
		}
		else if(gamaB>6.2832)
		{
			gamaB=gamaB-6.2832;
		}
		/* 30 :0.5236 20: 0.34907 15 : 0.2618  10 : 0.1745 8 :0.1396 */
		if((fabs(gamaA-1.5708)<0.2618)||(fabs(gamaA-4.7124)<0.2618)||(fabs(gamaB-1.5708)<0.2618)||(fabs(gamaB-4.7124)<0.2618))
		{/* 判断目标是否处于阵的低精度区，低精度区时设阵的测向误差为10度，否则为2度 */
			/* XVxYVy[pihao][0]=XVxYVy[pihao][0]+XVxYVy[pihao][1]*dt; 
			   XVxYVy[pihao][2]=XVxYVy[pihao][2]+XVxYVy[pihao][3]*dt; */
			dA=10*3.1416/180;
			dB=10*3.1416/180;
		}
		/* else */
		{				
			R[0]=d*d*(pow(sin(AngA)*cos(AngA)*dB,2)+pow(sin(AngB)*cos(AngB)*dA,2))/pow(sin(AngA-AngB),4);
			R[1]=d*d*(pow(sin(AngA)*cos(AngA),3)*pow(dB,2)+pow(sin(AngB)*cos(AngB),3)*pow(dA,2))/pow(sin(AngA-AngB),4);
			R[2]=R[1];
			R[3]=d*d*(pow(cos(AngA),4)*pow(dB,2)+pow(cos(AngB),4)*pow(dA,2))/pow(sin(AngA-AngB),4);
			/* Kalman filter */
			/* X=fai*X; */
			for(k=0;k<4;k++)
				X[k]=XVxYVy[pihao][k];
			damul(Fai,X,4,4,1,Xj);
			/* P=fai*P*fai'+Q; */
			for(k=0;k<16;k++)
				P[k]=VarErrMat[pihao][k];
			damul(Fai,P,4,4,4,temp1);
			damul(temp1,FaiT,4,4,4,Pj);
			for(k=0;k<=15;k++)
				Pj[k]=Pj[k]+Q[k];
			/* S=H*P*H'+R; */
			damul(H,Pj,2,4,4,temp2);
			damul(temp2,HT,2,4,2,S);
			for(k=0;k<=3;k++)
				S[k]=S[k]+R[k];
			/* K=P*H'*inv(S); */
			dcinv(S);
			damul(Pj,HT,4,4,2,temp2);
			damul(temp2,S,4,2,2,K);
			/* P=[I-K*H]*P; */
			damul(K,H,4,2,4,temp1);
			for(k=0;k<=15;k++)
				temp1[k]=I[k]-temp1[k];
			damul(temp1,Pj,4,4,4,P);
			for(k=0;k<16;k++)
				VarErrMat[pihao][k]=P[k];
			/* X=X+K*[Z-H*X]; */
			damul(H,Xj,2,4,1,Zj);
			Zj[0]=TarX-Zj[0];
			Zj[1]=TarY-Zj[1];
			damul(K,Zj,4,2,1,X);
			for(k=0;k<4;k++)
				XVxYVy[pihao][k]=Xj[k]+X[k];
		}
	}
}