kalman_filter-C.rar_c语言 卡尔曼_klman_卡尔曼 C语言_卡尔曼滤波c

#include "stdio.h" #include "math.h" #include "time.h" #include "brmul.c" #include "brinv.c" #define M 40 void random1(int nn,float n[],float mean,float var) { int j; float a,b,tt; srand((unsigned)time(NULL)); for(j=0;j<nn;j++) { tt=rand(); if((tt-0.000001)<0) { j--; continue; } a=sqrt(-2.0*log(tt/32767.0)); b=2*3.14159265*rand()/32767.0; n[j]=var*a*cos(b)+mean; } } main() { int i,j,k; double Xavr[3][1],Xavr0=0,Xavr1=0,Xavr2=0; double S[3][1],S1,S2,S3; double Xgavr[3][1],Xgavr0=0,Xgavr1=0,Xgavr2=0,n[10]; double F[3][3]={{0.5,0.3,0.1},{0,-0.8,0.2},{0,0,0.4}}; double F2[3][3],Ft[3][3]; double C[2][3]={{0.3,1,0.8},{0.5,0.4,0.7}},Ct[3][2]; double X[3][1]={{3},{-1},{2}},Xg[3][1]={{3},{-1},{2}}; double X1[3][1],X2[3][1],X3[3][1]; double Y[2][1],Y1[2][1]; double v1[3][1],v2[2][1]; double Q1[3][3],Q2[2][2]; double G[3][2],G1[3][3],G2[3][2],G3[2][3],G4[2][2]; double alpha[2][1],alpha1[2][1]; double P[3][3],P1[3][2],P2[3][3]; double K[3][3]={{0,0,0},{0,1,0},{0,0,1}}; double K1[3][3],K2[3][3]; for(i=0;i<2;i++) { for(j=0;j<3;j++) Ct[j][i]=C[i][j]; } for(i=0;i<3;i++) { for(j=0;j<3;j++) F2[i][j]=F[i][j]; } brinv(F2,3); for(i=0;i<3;i++) { for(j=0;j<3;j++) Ft[j][i]=F[i][j]; } for(k=1;k<M;k++) { random1(10,n,0,1); v1[0][0]=n[0]*sin(0.1*k); v1[1][0]=n[1]*cos(0.1*k); v1[2][0]=n[2]; v2[0][0]=n[3]; v2[1][0]=n[4]*sin(0.5*k); Q1[0][0]=pow(sin(0.1*k),2);Q1[0][1]=0;Q1[0][2]=0; Q1[1][0]=0;Q1[1][1]=pow(cos(0.1*k),2);Q1[1][2]=0; Q1[2][0]=0;Q1[2][1]=0;Q1[2][2]=1; Q2[0][0]=1;Q2[0][1]=0; Q2[1][0]=0;Q2[1][1]=pow(sin(0.5*k),2); brmul(F,K,3,3,3,G1); brmul(G1,Ct,3,3,2,G2); brmul(C,K,2,3,3,G3); brmul(G3,Ct,2,3,2,G4); for(i=0;i<2;i++) { for(j=0;j<2;j++) G4[i][j]=G4[i][j]+Q2[i][j]; } brinv(G4,2); brmul(G2,G4,3,2,2,G); /*end G*/ brmul(C,X,2,3,1,Y1); Y[0][0]=Y1[0][0]+v2[0][0]; Y[1][0]=Y1[1][0]+v2[1][0]; /*end Y */ brmul(C,Xg,2,3,1,alpha1); alpha[0][0]=Y[0][0]-alpha1[0][0]; alpha[1][0]=Y[1][0]-alpha1[1][0]; /*end alpha*/ brmul(F,Xg,3,3,1,X1); brmul(G,alpha,3,3,1,X2); Xg[0][0]=X1[0][0]+X2[0][0]; Xg[1][0]=X1[1][0]+X2[1][0]; Xg[2][0]=X1[2][0]+X2[2][0]; /* end Xg */ brmul(F,X,3,3,1,X3); X[0][0]=X3[0][0]+v1[0][0]; X[1][0]=X3[1][0]+v1[1][0]; X[2][0]=X3[2][0]+v1[2][0]; /* end X*/ brmul(F2,G,3,3,2,P1); brmul(P1,C,3,2,3,P2); brmul(P2,K,3,3,3,P2); for(i=0;i<3;i++) { for(j=0;j<3;j++) P[i][j]=K[i][j]-P2[i][j]; } /* end P */ brmul(F,P,3,3,3,K1); brmul(K1,Ft,3,3,3,K2); for(i=0;i<3;i++) { for(j=0;j<3;j++) K[i][j]=K2[i][j]+Q1[i][j]; } /* end K */ Xavr0=Xavr0+X[0][k]; Xavr1=Xavr1+X[1][k]; Xavr2=Xavr2+X[2][k]; Xgavr0=Xgavr0+Xg[0][k]; Xgavr1=Xgavr1+Xg[1][k]; Xgavr2=Xgavr2+Xg[2][k]; S1=pow((X[0][k]-Xg[0][k]),2); S2=pow((X[1][k]-Xg[1][k]),2); S3=pow((X[2][k]-Xg[2][k]),2); } Xavr0=Xavr0/M;Xavr1=Xavr1/M;Xavr2=Xavr2/M; Xgavr0=Xgavr0/M;Xgavr1=Xgavr1/M;Xgavr2=Xgavr2/M; Xavr[0][0]=Xavr0;Xavr[1][0]=Xavr1;Xavr[2][0]=Xavr2; Xgavr[0][0]=Xgavr0;Xgavr[1][0]=Xgavr1;Xgavr[2][0]=Xgavr2; S[0][0]=S1/(M-1);S[1][0]=S2/(M-1);S[2][0]=S3/(M-1); printf("-------------------------------"); for(i=0;i<3;i++) printf("\nXavr=%f,Xgavr=%f\n",Xavr[i][0],Xgavr[i][0]); getch(); printf("-------------------------------"); for(i=0;i<3;i++) printf("\nS=%f\n",S[i][0]); getch(); /* run M times */ }