📄 zuiyou.cpp
字号:
#include<stdlib.h>
#include<math.h>
#include<stdio.h>
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();
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -