最小二乘状态估计.cpp

博士师兄编写的电力系统状态估计程序

		  i=mh[l-1];
          A[i-1]=A[i-1]+1;
	  }
      B[0]=1;iht[0]=1;
      for(i=1;i<=2*NOB-1;i++)
      {
		  B[i]=B[i-1]+A[i-1];
          iht[i]=B[i];
	  }
      for(l=1;l<=nom;l++)
      { 
		  for(i=ih[l-1];i<=ih[l]-1;i++)
          {
			  m=mh[i-1];
              n=B[m-1];
              mht[n-1]=l;
              htmm[n-1]=hii[i-1];
              B[m-1]=B[m-1]+1;
         }
	  }
  }

 HTH(int ih[],int mh[],int iht[],int mht[],int ihth[],int jhth[],int nox)
 {
	 int jk,i,l,k,j,jj,m,mi,ji,n;
     jk=1;ihth[0]=1;
     for(i=1;i<=nox;i++)
     {
		 k=1;  
		 for(l=1;l<=1000;l++)
			 N[l-1]=0;
		 for(mi=iht[i-1];mi<=iht[i]-1;mi++)
		 {
			 m=mht[mi-1];
			 n=ih[m]-1;
			 for(ji=ih[m-1];ji<=n;ji++)
			 {
				 j=mh[ji-1];
                 if(j<i)   continue;
                 jj=k-1;
 /*  l0:               if(j<N[jj]) 
                         {N[jj+1]=N[jj];
                          jj=jj-1;
                      if(jj!=0) goto l0;}  */
				 for(;j<N[jj];)
				 {
					 N[jj+1]=N[jj];jj=jj-1;
					 if(jj==0)  break;
				 } 
				 if(j==N[jj])
				 {
					 for(m=jj+1;m<=k-1;m++)
						 N[m]=N[m+1];
				 }
				 else
				 {
					 N[jj+1]=j;
                        k=k+1;
				 }
			 }     
		 }
		 for(m=1;m<=k-1;m++)
		 {
			 jhth[jk-1]=N[m];
			 jk=jk+1;
		 }
		 ihth[i]=jk;
	 }  
        /*      for(i=1;i<=2*NOB;i++)
              printf("IHTH[%d]=%d   ",i-1,ihth[i-1]);printf("\n");
              for(i=1;i<=ihth[2*NOB-1]-1;i++)
              printf("JHTH[%d]=%d   ",i-1,jhth[i-1]);
              printf("\n");*/
 }

  float fjx,fjxb;
  int ia,k;
  int IT;
  rest()//残差 
  {
	  int kk,a1,ia1,KU,MBD[1000],i,j,k5,IMAY,IA,I,i0,j0;
      float RST[1000],X[1000],Z,a,aa,BD[1000],angle;
      float jiao1,jiao2,ISHI,Ixu;
      float angl,r,x,M,G,SI1,CO1,SI2,CO2;
      Z=180/3.1415926;k5=N2[4];IT=1;
  	  fprintf(fp1,"\n****进行状态估计REST()\n");
             //IMAY=NOB;//此时IMAY为子系统的最大外部节点号 NOB为子系统的节点数
	  fprintf(fp1,"迭代收敛精度E=%f   \n",E);//E为程序开始时读入数据
      if(NOP!=1)
      {
		  k=500;
          kk=NOM+1;
          NOX=2*NOB-1;//如果有NOB个节点则有2*NOB-1个变量因为每个节点都有电压和相角两个量去处平横节点
         for(i=1;i<=NOB;i++)//进行初始化0246点为初始电压值，1357为初始相角值
		 {
			 V[2*(i-1)]=V0;
			 V[2*i-1]=0;
		 }
         for(;;)
		 {
			 if(IT==IT3) break;//前边读入的数据fscanf(fp,"%d",&IT3);//结尾数据 10
	         H(IH,MH,IIH,HII);
             HT(IH,MH,HII,IHT,MHT,HTMM,NOM);
             HTH(IH,MH,IHT,MHT,IHTH,JHTH,NOX);
	         GS(NOX,ju,u,KU);
             RS(V,RST);
             if(IT>=5)
				 ZR(RST,EPSL,MBD,BD);
             RX(RST,X);
             XX(NOX,X,ju,u); 
             a=0;
             for(i=1;i<=2*NOB-1;i++)
			 {
				 aa=fabs(X[(i-1)]); 
				 if(aa>a)
				 {
					 a=aa;
					 IA=IIO[(int)((i-1)/2)];
				 }
			 }
	        fprintf(fp1,"\n****迭代结果输出\n");
            fprintf(fp1,"不包含不良数据的目标函数值jx=%f  \n包含不良数据的目标函数值jb=%f \n",fjx,fjxb);
            fprintf(fp1,"自由矢量最大的节点AEMAX\n");
	        fprintf(fp1,"外部结点号=%d  自由矢量值a=%f\n",IA,a);
	        fprintf(fp1,"MaxResidual=%9.6f  量测类型=%d  量测地点=%d \n",
		      RST[ia-1],(int)(FMDT[ia-1][0]),(int)(FMDT[ia-1][1]));
	        fprintf(fp1,"\n****各量测量的残差\n");
	        fprintf(fp1,"\n量测量序号 量测量类型 量测地点  残差\n");
            if(a>=E)
			{
				a1=2000;
                for(j=1;j<=2*NOB-1;j++)
				{
					V[j-1]=V[j-1]+X[j-1];
                    if(a1<V[j-1]) continue;
					a1=V[j-1];
					ia1=IIO[(int)((j-1)/2)];
				}
				if(k5!=0)
				{
					for(j=1;j<=NOM;j++)
						fprintf(fp1,"REST %3d  %3d  %3d %9.6f\n",j,(int)(FMDT[j-1][0]),
                             (int)(FMDT[j-1][1]),RST[j-1]);
				}
			}
			else
			{
				for(j=1;j<=NOM;j++)
					fprintf(fp1,"REST %3d  %3d  %3d %9.6f\n",j,(int)(FMDT[j-1][0]),
                             (int)(FMDT[j-1][1]),RST[j-1]);
				break;
			}
			IT=IT+1;
			fprintf(fp1,"迭代次数IT=%d \n  ",IT);
		 }
	  } 
	  fprintf(fp1,"最终迭代次数IT=%d  \n",IT);
	  fprintf(fp1,"\n****状态估计结果输出\n");
	  fprintf(fp1,"\n****结点电压结果输出IVE\n");
	  fprintf(fp1,"外部结点号 内部结点号 电压幅值 电压相角（度）\n");
         	   //  fprintf(fp1,"外部结点号 内部结点号 电压实部 电压虚部\n");
	  for(i=1;i<=NOB;i++)
	  {
		  j=IOI[i-1];//j不等于零时进行程序
          if(j==0) continue;
          angle=Z*V[2*j-1];
          fprintf(fp1,"%d  %d  %9.6f  %9.6f\n",i,j,V[2*(j-1)],angle);
	  }
	  fprintf (fp1,"\n****支路电流结果输出\n");
	  fprintf(fp1,"支路号 首端节点 尾端节点  电流实部  电流虚部\n"	);
	  for(I=1;I<=NOE;I++)
	  {
		i=(int)(ELE[I-1][0]);
		i0=IOI[i-1];//转换成内部节点编号
        j=(int)(ELE[I-1][1]);
        j0=IOI[j-1];//转换成内部节点编号
        r=ELE[I-1][2];
        x=ELE[I-1][3];
        G=r*r+x*x;
		M=sqrt(G);
		angl=atan2 (x,r); 
		jiao1=V[2*i0-1]-angl;
		jiao2=V[2*j0-1]-angl;
		SI1=sin(jiao1);
		CO1=cos(jiao1);
		SI2=sin(jiao2);
		CO2=cos(jiao2);
		ISHI=(V[2*(i0-1)]/M)*CO1-(V[2*(j0-1)]/M)*CO2;
		Ixu=(V[2*(i0-1)]/M)*SI1-(V[2*(j0-1)]/M)*SI2;
		fprintf(fp1,"%d %d %d %9.6f %9.6f\n",I,i,j,ISHI,Ixu);
	  } 
	  WRI1();
 }

 RS(float v[],float REST[])//计算量测量残差程序
 {
	 int m,i,j,j1,l,k;
	 float R,X,a,G,B,r,FK,SI,CO,H;
	 for(m=1;m<=NOM;m++)
		 REST[m-1]=0;
	 a=0;
	 for(m=1;m<=NOM;m++)
	 {
		 if((int)(FMDT[m-1][4])!=0) continue;
		 j=abs((int)(FMDT[m-1][0]));
		 j1=j-3;
		 l=(int)(FMDT[m-1][1]);
		 if(j>=4)
		 {
			 R=ELE[l-1][2];
			 X=ELE[l-1][3];
			 G=R/(R*R+X*X);
			 B=-X/(R*R+X*X);
			 i=IOI[(int)(ELE[l-1][0])-1];
			 j=IOI[(int)(ELE[l-1][1])-1];
			 FK=ELE[l-1][4];
			 r=v[2*i-1]-v[2*j-1];
			 SI=sin(r);CO=cos(r);
			 if(j1<=1||j1>4)
			 {
				 if(FK>0.0) 
					 H=-v[2*(i-1)]*v[2*(j-1)]*B*SI/FK;
				 else
					 H=v[2*(i-1)]*(v[2*(i-1)]*G-v[2*(j-1)]*(G*CO+B*SI));
			 }
			 if(j1==2)
			 {
				 if(FK>0.0) 
					 H=(-v[2*(i-1)]/FK+v[2*(j-1)]*CO)*B*v[2*(i-1)]/FK;
				 else 
					 H=-v[2*(i-1)]*(v[2*(i-1)]*(B-FK)-v[2*(j-1)]*(B*CO-G*SI));
			 }
			 if(j1==3)
			 {
				 if(FK>0.0)
					 H=v[2*(i-1)]*v[2*(j-1)]*B*SI/FK;
				 else 
					 H=v[2*(j-1)]*(v[2*(j-1)]*G-v[2*(i-1)]*(G*CO-B*SI));
			 }
			 if(j1==4)
			 {
				 if(FK>0.0) 
					 H=(v[2*(i-1)]*CO/FK-v[2*(j-1)])*B*v[2*(j-1)];
				 else
					 H=v[2*(j-1)]*(-v[2*(j-1)]*(B-FK)+v[2*(i-1)]*(G*SI+B*CO));
			 }
		 }
		 else 
		 {
			 i=IOI[l-1];
			 if(j>=2)
			 {
				 RY(i,0);
				 H=0.0;
				 k=1;
				 for(;;)
				 {
					 j1=JB[k-1];
					 if(j1==0) break;
					 r=v[2*i-1]-v[2*j1-1];
					 SI=sin(r);CO=cos(r);
					 if(j==2)  
						 H=H+v[2*(i-1)]*v[2*(j1-1)]*(GG[k-1]*CO+BB[k-1]*SI);
					 else
						 H=H+v[2*(i-1)]*v[2*(j1-1)]*(GG[k-1]*SI-BB[k-1]*CO);
					 k=k+1;
				 }
			 }
			 else H=v[2*(i-1)];
		 }
		 H=H-FMDT[m-1][2];REST[m-1]=-H;
		 if(a<fabs(H))
		 {
			 a=fabs(H);
			 ia=m;
		 } 
	 }
 }

 RX(float REST[],float x[])
 {
	 float EI,CI;
	 int m,l,l1;
	 for(l=1;l<=2*NOB-1;l++)
	 {
		 EI=0;
		 for(l1=IHT[l-1];l1<=IHT[l]-1;l1++)
		 {
			 m=MHT[l1-1];
			 CI=1.0/(FMDT[m-1][3]*FMDT[m-1][3]);
			 EI=EI+HTMM[l1-1]*CI*REST[m-1];
		 }
		 x[l-1]=EI;
	 }
 }

 XX(int nox,float x[],int ju[],float u[])//
 {
	 int ku,i,j;
     float D;
	 ku=1;i=0;
	 do
	 {
		 i=i+1;
		 D=u[ku-1];
		 for(;;)
		 {
			 ku=ku+1;
			 if(ju[ku-1]==0) break;
			 j=ju[ku-1];
			 x[j-1]=x[j-1]-u[ku-1]*x[i-1];
		 }
		 x[i-1]=x[i-1]/D;
     }
     while(i!=nox);
	 for(;;)
     {
		 ku=ku-1;
		 if(ju[ku-1]==0)
		 {
			 i=i-1;
			 if(i==0) break;
		 }
		 else
		 {
			 j=ju[ku-1];
			 x[i-1]=x[i-1]-x[j-1]*u[ku-1];
		 }
	 }
 }
  
 ZR(float REST[],float epsl,int mbd[],float bd[])//零残差辨识程序
 {
	 int a1,a2,k1,m,IT,i;
	 float R;
	 fprintf(fp1,"残差的门槛值epsl=%f\n ",epsl);
	 k1=0;k=0;fjxb=0;fjx=0;
	 for(m=1;m<=NOM;m++)
     {
		 R=REST[m-1]/FMDT[m-1][3];
		 a1=1;a2=1;
		 fjxb=fjxb+R*R;
		 if(fabs(R)<epsl)
			 fjx=fjx+R*R;
		 else
		 {
			 k=k+1;
			 mbd[k-1]=m;
			 bd[k-1]=R;
			 REST[m-1]=0;
		 }
	 }
	 IT=IT+1;
     fprintf(fp1,"残差的门槛值epsl=%f  不良数据个数NOBD=%d  \n",epsl,k);
	 for(i=1;i<=k;i++)
	 {
		 a1=mbd[i-1]-1;
		 fprintf(fp1,"BD MBD %f  %f  %f\n",bd[i-1],FMDT[a1][0],FMDT[a1][1]);
	 }
 }

 #include "math.h"
 WRI1()
 {
	 int i,j,l,N,j1,i1;
	 float z,XJ,EI,FI,RESTJ,VI,
           PH,QH,EJ,FJ,DE,DF,R1,
           FII,FIR,PIJ,QIJ,QJI,R,X,
           PJI,DPH,DQH,YK,PP[1000];
	 z=180/3.1415926; 
	 fprintf(fp1,"****子系统结点数NOB=%d \n",NOB);
              //fprintf(fp1,"NOB %d \n",NOB);
	 fprintf(fp1,"\n****结点注入结果输出\n");
	 fprintf(fp1,"外部结点号  内部结点号  结点有功PH  结点无功QH\n");
	 for(j=1;j<=NOB;j++)
     {
		 i=IOI[j-1];
		 if(i==0)
		 {
			 RESTJ=0;
			 XJ=0;
			 continue;
		 }
		 XJ=V[2*(i-1)];
		 EI=XJ*cos(V[2*i-1]);
		 FI=XJ*sin(V[2*i-1]);
		 RESTJ=V[2*i-1]*z;
		 VI=XJ*XJ;
		 PH=0;
		 QH=0;
		 for(l=IYL[i-1];l<=IYL[i]-1;l++)
		 {
			 N=JYLL[l-1][1];
			 j1=JYLL[l-1][0];
			 EJ=V[2*(j1-1)]*cos(V[2*j1-1]);
			 FJ=V[2*(j1-1)]*sin(V[2*j1-1]);
			 PH=PH+gij[N-1]*(EI*EJ+FI*FJ)+bij[N-1]*(EJ*FI-EI*FJ);
			 QH=QH-bij[N-1]*(EI*EJ+FI*FJ)+gij[N-1]*(EJ*FI-EI*FJ);
		 }
		 PH=PH+gii[i-1]*VI;
		 QH=QH-bii[i-1]*VI;
		 for(l=IY[i-1];l<=IY[i]-1;l++)
		 {
			 j1=JYY[l-1];
			 EJ=V[2*(j1-1)]*cos(V[2*j1-1]);
			 FJ=V[2*(j1-1)]*sin(V[2*j1-1]);
			 PH=PH+gij[l-1]*(EI*EJ+FI*FJ)+bij[l-1]*(EJ*FI-EI*FJ);
			 QH=QH-bij[l-1]*(EI*EJ+FI*FJ)+gij[l-1]*(EJ*FI-EI*FJ);
		 }
		 PP[j-1]=PH;
		 fprintf(fp1,"%d  %d  %9.6f  %9.6f\n",j,i,PH,QH);
	 }
	 DPH=0;
	 DQH=0;
	 fprintf(fp1,"\n****支路功率结果输出\n");
	 fprintf(fp1,"支路号 I  J   PIJ   QIJ   PJI   QJI\n");
	 for(l=1;l<=NOE;l++)
	 {
		 i=(int)(ELE[l-1][0]);
		 j=(int)(ELE[l-1][1]);
		 R=ELE[l-1][2];
		 X=ELE[l-1][3];
		 YK=ELE[l-1][4];
		 i1=IOI[i-1];
		 j1=IOI[j-1];
		 EI=V[2*(i1-1)]*cos(V[2*i1-1]);
		 FI=V[2*(i1-1)]*sin(V[2*i1-1]);
		 EJ=V[2*(j1-1)]*cos(V[2*j1-1]);
		 FJ=V[2*(j1-1)]*sin(V[2*j1-1]);
		 if(YK>0)
		 {
			 EI=EI/YK;FI=FI/YK;
			 YK=0;
		 }
		 DE=EI-EJ;
		 DF=FI-FJ;
		 R1=R*R+X*X;
		 FII=(DE*R+DF*X)/R1;
		 FIR=(DF*R-DE*X)/R1;
		 PIJ=FII*EI+FIR*FI;
		 QIJ=FII*FI-FIR*EI+(EI*EI+FI*FI)*YK;
		 PJI=-FII*EJ-FIR*FJ;
		 QJI=-FII*FJ+FIR*EJ+(EJ*EJ+FJ*FJ)*YK;
		 DPH=DPH+PIJ+PJI;
		 DQH=DQH+QIJ+QJI;
		 fprintf(fp1,"%d %d %d %9.6f %9.6f %9.6f %9.6f\n",
			 l,i,j,PIJ,QIJ,PJI,QJI);
	 }
      //fprintf(fp1,"DPH DQH %f %f \n",DPH,DQH);
 }