gls.cpp

递推广义最小二乘法,主要应用于系统辨识,按照给定的递推方程辨识参数

 #include <iostream.h>
 #include <fstream.h>
 #include <stdlib.h>
 #include <math.h>
 #define N 600                          
 #define na 2                          
 #define nb 1
 #define l 1

 int brmul(double a[],double b[],int m,int n,int k,double c[]);  //矩阵相乘

 void main()
 {
	double u[2*N],e[2*N],z[2*N],sita[na+nb],s[l],p[(na+nb)*(na+nb)],kesai[l],w[na+nb];
	double zs[2*N],us[2*N],ws[na+nb],k[na+nb],f1[na+nb],f2[na+nb],f3[na-1];
    double f4[na-1],f5[(na+nb)*(na+nb)],f6[(na+nb)*(na+nb)],f7[na-1];
	double v[2*N],vl[l],M[l],sita1[na+nb],f8[na-1];
    double y[600*3];
	double ea[na+nb]={-0.5,0.5,1},er[na+nb];
	double c1=0.85,ec;
	double a=pow(10,6);
	int i,j,m;
	ifstream fip1("PRBS.txt");
	ifstream fip2("Gauss.txt");
	ofstream fop1("mdata.txt");
	ofstream fop2("mdata1.txt");
	ofstream fop3("mdata2.txt");
	ofstream fop4("mdata3.txt");
	ofstream fop5("mdata4.txt");
	//读入数据
	for(i=0;i<2*N;i++)
	{
		fip1>>u[i];
		fip2>>e[i];
	}
	fip1.close();
	fip2.close();
	z[0]=u[0]+e[0];
	z[1]=-0.35*z[0]+u[1]+0.85*u[0]+e[1];
	z[2]=-0.35*z[1]-0.5*0.15*z[0]+u[2]+0.85*u[2]+e[2];
	for(i=3;i<2*N;i++)
		z[i]=-0.35*z[i-1]-0.075*z[i-2]-0.425*z[i-3]+u[i]+0.85*u[i-1]+e[i];

    
	//置初值
	for(i=0;i<na+nb;i++)
		sita[i]=0;
	for(i=0;i<l;i++)
		s[i]=0;
	for(i=0;i<na+nb;i++)
		for(j=0;j<na+nb;j++)
		{
			if(i==j)
				p[i*(na+nb)+j]=a;
			else
				p[i*(na+nb)+j]=0;
		}
	kesai[0]=a;
    //递推过程
	zs[0]=0;zs[-1]=0;
	us[0]=0;us[-1]=0;
    for(i=0;i<N;i++)
	{   //计算zs(n+1),us(n+1)
		zs[i+1]=z[i+1]+s[0]*z[i];
		us[i+1]=u[i+1]+s[0]*u[i];
		//计算ws(n+1)
		{
			ws[0]=-zs[i];
			ws[1]=-zs[i-1];
			ws[2]=us[i+1];
		}
		//计算sita(n+1)
		brmul(p,ws,na+nb,na+nb,1,f1);
		brmul(ws,p,1,na+nb,na+nb,f2);
		brmul(f2,ws,1,na+nb,1,f3);
		for(j=0;j<na+nb;j++)
			k[j]=f1[j]/(1+f3[0]);
        brmul(ws,sita,1,na+nb,1,f4);
		for(j=0;j<na+nb;j++)
			sita1[j]=sita[j];
		for(j=0;j<na+nb;j++)
		{
			sita[j]=sita[j]+k[j]*(zs[i+1]-f4[0]);
			er[j]=fabs(sita[j]-ea[j])/ea[j];
			cout<<sita[j]<<"  ";
			fop1<<sita[j]<<" \%n； ";
			y[i,j]=sita[j];                              //pan
			fop2<<sita[j]<<"  ";
			fop3<<sita[j]<<"  ";
			fop4<<sita[j]<<"  ";
			fop5<<sita[j]<<"  ";
		}
		//计算p(n+1)
		brmul(k,ws,na+nb,1,na+nb,f5);
		brmul(f5,p,na+nb,na+nb,na+nb,f6);
		for(j=0;j<na+nb;j++)
			for(m=0;m<na+nb;m++)
				p[j*(na+nb)+m]=p[j*(na+nb)+m]-f6[j*(na+nb)+m];
		//计算vln以及v(n),v(n+1)
        {
			w[0]=-z[i];
			w[1]=-z[i-1];
			w[2]=u[i+5];
		}
		brmul(w,sita1,1,na+nb,1,f7);
		v[i]=z[i]-f7[0];
		vl[0]=v[i]/3.5;
		M[0]=kesai[0]*vl[0]/(1+vl[0]*kesai[0]*vl[0]);
		{
			w[0]=-z[i+1];
			w[1]=-z[i+1-1];
			w[2]=u[i+6];
		}
		brmul(w,sita,1,na+nb,1,f8);
        v[i+1]=z[i+1]-f8[0];
		//计算s
		s[0]=s[0]+M[0]*(v[i+1]-vl[0]*s[0]);
        ec=fabs(s[0]-c1)/c1;
		cout<<s[0]<<endl;
		fop1<<s[0]<<endl;
		y[i,3]=s[0];                                           //pan
		fop2<<s[0]<<endl;                                       //pan
		//fop3<<s[0]<<endl;
		//fop4<<s[0]<<endl;
		//fop5<<s[0]<<endl;
		 kesai[0]=kesai[0]-M[0]*vl[0]*kesai[0];
		if(er[0]<0.0001&&er[1]<0.0001&&er[2]<0.0001&&ec<0.0001)
			break;
	}
	cout<<i<<endl;
	//fop1<<i<<endl;
	fop1.close();
	fop2.close();
	fop3.close();
	fop4.close();
	fop5.close();
 }

  int brmul(double a[],double b[],int m,int n,int k,double c[])     //矩阵相乘
 { 
	int i,j,l1,u;
    for (i=0; i<=m-1; i++)
    for (j=0; j<=k-1; j++)
      { 
		u=i*k+j; c[u]=0.0;
        for (l1=0; l1<=n-1; l1++)
          c[u]=c[u]+a[i*n+l1]*b[l1*k+j];
      }
    return 0;
  }