www.pudn.com > zuiyou.rar > zuiyou.cpp
#include#include #include FILE *f1,*f2,*f3,*f4,*f5,*f6,*f7,*f8; //2维方阵乘法函数 void MatrM2(double MATR1[2][2],double MATR2[2][2],double MATR0[2][2]) { int i,j,n; for(i=0;i<2;i++) for(j=0;j<2;j++) { MATR0[i][j]=0.0; for(n=0;n<2;n++)MATR0[i][j]=MATR0[i][j]+MATR1[i][n]*MATR2[n][j]; } } //平滑算法子函数 void pinghua(double XGKK_P[2],double XGK1N[2],double PK1_N[2][2],double PK_P[2][2], double XGKN[2],double PK_N[2][2]) { int i,j; double Fia[2][2]={1.0,1.0,0.0,1.0},Fiaz[2][2]={1.0,0.0,1.0,1.0}; double PKIK1[2][2],PKIK[2][2],PKIKT[2][2],PKIKF[2][2],ASK1[2][2],ASK[2][2],ASKT[2][2]; double XGKN1[2],XGKN2[2],XGKN3[2],XGKIN[2]; double Hls; double PK_N1[2][2],PK_N2[2][2],PK_N3[2][2]; MatrM2(Fia,PK_P,PKIK1); MatrM2(PKIK1,Fiaz,PKIK); MatrM2(PK_P,Fiaz,ASK1); //FAN PKIKT[0][0]=PKIK[1][1]; PKIKT[0][1]=-PKIK[0][1]; PKIKT[1][0]=-PKIK[1][0]; PKIKT[1][1]=PKIK[0][0]; Hls=PKIK[0][0]*PKIK[1][1]-PKIK[0][1]*PKIK[1][0]; for(i=0;i<2;i++) for(j=0;j<2;j++) { PKIKF[i][j]=PKIKT[i][j]/Hls; } MatrM2(ASK1,PKIKF,ASK); XGKN1[0]=XGKK_P[0]+XGKK_P[1];//计算X(K/N)的预测估计 XGKN1[1]=XGKK_P[1]; XGKN2[0]=XGKIN[0]-XGKN1[0]; XGKN2[1]=XGKIN[1]-XGKN1[1]; XGKN3[0]=ASK[0][0]*XGKN2[0]+ASK[0][1]*XGKN2[1]; XGKN3[1]=ASK[1][0]*XGKN2[0]+ASK[1][1]*XGKN2[1]; XGKN[0]=XGKK_P[0]+XGKN3[0]; XGKN[1]=XGKK_P[1]+XGKN3[1]; //计算预测的误差方差阵 for(i=0;i<2;i++) for(j=0;j<2;j++) { PK_N1[i][j]=PK1_N[i][j]-PKIK[i][j]; } MatrM2(ASK,PK_N1,PK_N2); ASKT[0][0]=ASK[0][0]; ASKT[0][1]=ASK[1][0]; ASKT[1][0]=ASK[0][1]; ASKT[1][1]=ASK[1][1]; MatrM2(PK_N2,ASKT,PK_N3); for(i=0;i<2;i++) for(j=0;j<2;j++) { PK_N[i][j]=PK_P[i][j]+PK_N3[i][j]; } } void main() { int i,j; double PO[2][2]={10.0,0.0,0.0,10.0},XO[2]={0.0,0.0},RK=0.1,QK=0,XG0[2]={0.0,0.0}; double XGK1[2]={0.0,0.0},XGKK1[2],XGK[2],XGK_1,XGK_2[2],XGK5[2]; double Hk[2]={1.0,0.0},Fia[2][2]={1.0,1.0,0.0,1.0},Fiaz[2][2]={1.0,0.0,1.0,1.0}; double Fia5[2][2]={1.0,5.0,0.0,1.0},Fia5z[2][2]={1.0,0.0,5.0,1.0}; double loop=1.0,Flag=1.0; float Zk,ZK; double Pk[2][2],Pkk1[2][2],Pk1[2][2]={10.0,0.0,0.0,10.0},Pkk11[2][2],Pk_1[2][2]; double I[2][2]={1.0,0.0,0.0,1.0},Kk2,Kk[2]={0.0,0.0}; double PJK[2][2],PJK1[2][2]; double XGN[2]={0,0},XGN1[2]={0,0},XGN2[2]={0,0},XGN3[2]={0,0},XGN4[2]={0,0},XGN5[2]={0,0}; double T1[2],T2[2],T3[2],T4[2]; double PPK1[2][2]={0,0,0,0},PPK2[2][2]={0,0,0,0},PPK3[2][2]={0,0,0,0},PPK4[2][2]={0,0,0,0},PPK5[2][2]={0,0,0,0}; double TP1[2][2]={0,0,0,0}, TP2[2][2]={0,0,0,0}, TP3[2][2]={0,0,0,0}, TP4[2][2]={0,0,0,0}; double XGKN[2]={0,0},XGK_1N[2]={0,0},XGK_2N[2]={0,0},XGK_3N[2]={0,0},XGK_4N[2]={0,0},XGK_5N[2]={0,0}; double PK_N[2][2]={0,0,0,0},PK1_N[2][2]={0,0,0,0},PK2_N[2][2]={0,0,0,0},PK3_N[2][2]={0,0,0,0}, PK4_N[2][2]={0,0,0,0},PK5_N[2][2]={0,0,0,0}; f1=fopen("DATA.DAT","r"); f2=fopen("guji.dat","w+"); f3=fopen("yuce.dat","w+"); f4=fopen("pinghua.dat","w+"); f5=fopen("gujiwucha.dat","w+"); f6=fopen("yucewucha.dat","w+"); f7=fopen("pinghuawucha.dat","w+"); f8=fopen("phcs.dat","w+"); do{ //读数据 fscanf(f1,"%f\n",&ZK); //printf("%f\t%f\n",ZK,loop); Zk=ZK; printf("%f\n",loop); //Kalman滤波方程 MatrM2(Fia,Pk1,Pkk11); MatrM2(Pkk11,Fiaz,Pkk1); Kk2=1/(Pkk1[0][0]+0.1); Kk[0]=Pkk1[0][0]*Kk2; Kk[1]=Pkk1[1][0]*Kk2; Pk_1[0][0]=I[0][0]-Kk[0]; Pk_1[1][0]=I[1][0]-Kk[1]; Pk_1[0][1]=0.0; Pk_1[1][1]=1.0; MatrM2(Pk_1,Pkk1,Pk); XGKK1[0]=Fia[0][0]*XGK1[0]+Fia[0][1]*XGK1[1]; XGKK1[1]=Fia[1][0]*XGK1[0]+Fia[1][1]*XGK1[1]; XGK_1=Zk-(XGK1[0]+XGK1[1]); XGK_2[0]=Kk[0]*XGK_1; XGK_2[1]=Kk[1]*XGK_1; XGK[0]=XGKK1[0]+XGK_2[0]; XGK[1]=XGKK1[1]+XGK_2[1]; //打印滤波估计及误差方差阵 fprintf(f2,"%f\t%f\t%f\n",Zk,XGK[0],XGK[1]); fprintf(f5,"%f\t%f\t%f\t%f\t%f\n",loop,Pk[0][0],Pk[0][1],Pk[1][0],Pk[1][1]); //保存平滑所需数据 T1[0]=XGN1[0]; T1[1]=XGN1[1]; T2[0]=XGN2[0]; T2[1]=XGN2[1]; T3[0]=XGN3[0]; T3[1]=XGN3[1]; T4[0]=XGN4[0]; T4[1]=XGN4[1]; XGN[0]=XGK[0]; XGN[1]=XGK[1]; XGN1[0]=XGK1[0]; XGN1[1]=XGK1[1]; XGN2[0]=T1[0]; XGN2[1]=T1[1]; XGN3[0]=T2[0]; XGN3[1]=T2[1]; XGN4[0]=T3[0]; XGN4[1]=T3[1]; XGN5[0]=T4[0]; XGN5[1]=T4[1]; for(i=0;i<2;i++) for(j=0;j<2;j++) { TP1[i][j]=PPK1[i][j]; TP2[i][j]=PPK2[i][j]; TP3[i][j]=PPK3[i][j]; TP4[i][j]=PPK4[i][j]; } for(i=0;i<2;i++) for(j=0;j<2;j++) { PPK1[i][j]=Pk1[i][j]; PPK2[i][j]=TP1[i][j]; PPK3[i][j]=TP2[i][j]; PPK4[i][j]=TP3[i][j]; PPK5[i][j]=TP4[i][j]; } //第六个观测量时对K-5进行平滑（采用逆向算法） if(Flag>=6) { pinghua(XGN1,XGN,Pk,PPK1,XGKN,PK_N);//对x(k-1/k)平滑 for(i=0;i<2;i++) { XGK_1N[i]=XGKN[i]; } for(i=0;i<2;i++) for(j=0;j<2;j++) { PK1_N[i][j]=PK_N[i][j]; } pinghua(XGN2,XGK_1N,PK1_N,PPK2,XGKN,PK_N);//对x(k-2/k)平滑 for(i=0;i<2;i++) { XGK_2N[i]=XGKN[i]; } for(i=0;i<2;i++) for(j=0;j<2;j++) { PK2_N[i][j]=PK_N[i][j]; } pinghua(XGN3,XGK_2N,PK2_N,PPK2,XGKN,PK_N);//对x(k-3/k)平滑 for(i=0;i<2;i++) { XGK_3N[i]=XGKN[i]; } for(i=0;i<2;i++) for(j=0;j<2;j++) { PK3_N[i][j]=PK_N[i][j]; } pinghua(XGN4,XGK_3N,PK3_N,PPK4,XGKN,PK_N);//对x(k-4/k)平滑 for(i=0;i<2;i++) { XGK_4N[i]=XGKN[i]; } for(i=0;i<2;i++) for(j=0;j<2;j++) { PK4_N[i][j]=PK_N[i][j]; } pinghua(XGN5,XGK_4N,PK4_N,PPK5,XGKN,PK_N);//对x(k-5/k)平滑 for(i=0;i<2;i++) { XGK_5N[i]=XGKN[i]; } for(i=0;i<2;i++) for(j=0;j<2;j++) { PK5_N[i][j]=PK_N[i][j]; } fprintf(f8,"%f\t%f\t%f\t%f\t%f\t%f\n",Flag,PPK1[0][0],PPK2[0][0],PPK3[0][0],PPK4[0][0],PPK5[0][0]); fprintf(f4,"%f\t%f\t%f\t%f\n",Zk,XGK_5N[0],XGK_5N[1]); fprintf(f7,"%f\t%f\t%f\t%f\t%f\t%f\n",Zk,PK5_N[0][0],PK5_N[0][1],PK5_N[1][0],PK5_N[1][1]); } for(i=0;i<2;i++) for(j=0;j<2;j++) { Pk1[i][j]=Pk[i][j]; } XGK1[0]=XGK[0]; XGK1[1]=XGK[1]; //预测 XGK5[0]=XGK[0]+5*XGK[1]; XGK5[1]=XGK[1]; MatrM2(Fia5,Pk,PJK1); MatrM2(PJK1,Fia5z,PJK); //打印预测及误差方差阵 fprintf(f3,"%f\t%f\t%f\n",Zk,XGK5[0],XGK5[1]); fprintf(f6,"%f\t%f\t%f\t%f\t%f\n",loop,PJK[0][0],PJK[0][1],PJK[1][0],PJK[1][1]); //下一个采样周期 loop=loop+1.0;//一秒一次 Flag=Flag+1.0; }while(loop<729002.0); fcloseall(); }