最小二乘状态估计.cpp

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

#include "math.h"
#include "stdio.h"
#include "time.h"

/*#defint CLOCKS_PER_SEC 18.2;*/

int KK1,IT3,N2[7],NA[1000],NB[1000],NOB,NOE,NOM,IMAX,IIO[1000],IOI[1000],IIOE[1000],
    MHT[1000],IHT[1000],IH[1000],MH[1000],IOIE[1000],
    IIH[1000],IHTH[1000],JHTH[1000],NOETS,
    IKE[1000][3],ILE[1000][3],ju[5000],JA[1000],MSYS[1000][3],N[1000],N1[1000],
    IJE[1000],IEKI[1000],IEK[1000],ITKI[1000],NOKT,NOET,NOLT,NOJ,NOMT,NOT,NOBT,NOST;

float V0,E,EPSL,ELE[1000][5],FMDT[1000][5],V[1000],gii[1000],bii[1000],u[5000],
      gij[1000], bij[1000],HTMM[1000],HII[1000],A1[1000],FLE[1000][3],
      SYS[1000][2];
   SENT(int I,int IHT[],int MHT[],float HTMM[]);
   YY(float GII[],float  BII[],float GIJ[],float BIJ[]);
   RY(int i,int K0);//
   RS(float v[],float REST[]);
   RX(float REST[],float x[]);
   ZR(float REST[],float epsl,int mbd[],float bd[]);
   XX(int nox,float x[],int ju[],float u[]);
   MSP();//是否可以进行状态估计计算
   WRI1();
   MSAN();//????
   rest();
   STS();
   H(int IH[],int MH[],int IIH[],float HII[]);
   HT(int ih[],int mh[],float hii[],int iht[],int mht[],float htmm[],int nom);
   HTH(int ih[],int mh[],int iht[],int mht[],int ihth[],int jhth[],int nox);
   GS(int nox,int JU[],float U[],int ku);
   FILE *fp1;
*****************************************************************************************************************
   main()
   {
       int e,j,f,i,k;
       double start1, finish_new,duration;    	// get circle matrix        /*FILE *fp6;*/
       
       FILE *fp;
       if((fp=fopen("4bus","r"))==NULL)
       {
		   printf("cannot open this file\n");
		   return 0;
	   }                      
       for(i=1;i<=7;i++)
	   {
			fscanf(fp,"%1d",&N2[i-1]);//有四种情况输入结束 书本上44页
	   }
       fscanf(fp,"%d %f %f %f",&KK1,&V0,&E,&EPSL);// 2  112.15      .00010      9.30000     

       if(KK1>=1)
       {
			fscanf(fp,"%d %d %d",&NOB,&NOE,&NOM);//NOB网络实际节点数 4   4  NOM是子系统所包括的测点总数 16 
            for(i=0,e=0,f=0;i<NOE+NOM;e++,i++)//一共20行数据
            {
				if(i<NOE) //扫前四行
					for(j=0;j<5;j++)
                    {
						fscanf(fp,"%f",&ELE[e][j]);
					}
                 else//扫前后十六行
				 {
					 for(j=0;j<4;j++)
                     fscanf(fp,"%f",&FMDT[f][j]);
                     f++;
                 }
			}
            fscanf(fp,"%d",&IT3);//结尾数据 10
            if(KK1<3)
            {
				for(i=1;i<=NOM;i++)
                 {
					if(KK1==3) 
						FMDT[i-1][3]=1;
                    FMDT[i-1][4]=0;//加一列数据 0
                 }
			}
			fp1=fopen("out.txt","w");
            start1 = clock();
            STS();
            rest();
	   }
       else
	   {
	   }

  //以下可能为延时程序
       finish_new=clock();
       duration=(finish_new-start1)/CLOCKS_PER_SEC;//CLOCKS_PER_SEC=1000
       fprintf(fp1,"形成回路时间 %1f 秒\n",duration);
       fclose(fp1);
       fclose(fp);
   }
************************************************************************************************************
	  int IMAY;//子系统的最大外部结点号
      ORD()
      {
		  int j,e,f,i,k,m,n,NOB1,IIA[1000];
          fprintf(fp1,"\n结点次序优化ORD()\n");
          NB[0]=1;IMAX=NOB;NOB=0;
          for(i=1;i<=IMAX;i++)
		  {
			  IOI[i-1]=0;
			  IIO[i-1]=0;
			  IIA[i-1]=0;
		  }

		  
              
	      for(i=1;i<=NOE;i++)
		  {
			  m=(int)(ELE[i-1][0]);//每条线路的两个节点 但不包括m=n情况
			  n=(int)(ELE[i-1][1]);
			 // printf("%d\n",m);
              if(m!=n)
			  {
				  if(m!=0) IIA[m-1]++; 
                  if(n!=0) IIA[n-1]++;
			  }			  
		  } //统计各结点的出线数，结果保存在数组IIA[]中	结果为 IIA[] 2 2 3 1
		  
          for(i=1;i<=IMAX;i++)//IIA[i-1]中就不可能有为零的数因为if(m!=0) IIA[m-1]++; 语句
		  {
			  if(IIA[i-1]!=0) 
			  {
				  NOB=NOB+1;
				  IMAY=i;//这里好像没用
			  }
		  }//计算子系统实际结点数
          fprintf (fp1,"****子系统结点数NOB=%d\n****子系统实际出现的最大外部结点号IMAY=%d\n",NOB,IMAY);
        
          for(i=1;i<=NOB;i++) //前面已经清了一次零
                  IIO[i-1]=0;
          NOB1=NOB;//NOB1=4
          for(i=1;i<=NOM;i++)
		  {
			  if((int)(FMDT[i-1][0])==0)//由第一列的类型说明是平衡节点
			  {
				  m=(int)(FMDT[i-1][1]);
                  IIO[NOB-1]=m;//平衡结点为内部最后的结点
                  IIA[m-1]=0;
			  }//平衡结点为内部最后的结点   //发生变化IIA[] 0 2 3 1
			  if((int)(FMDT[i-1][0])<0)//何时才能出现小于零的情况
			  {
				  NOB1=NOB1-1;
                  m=(int) (FMDT[i-1][1]);
                  IIO[NOB1-1]=m;
                  IIA[m-1]=0;
			  }
		  } 

           for(i=1;i<=(IMAY+1);i++)//清零
                 NA[i-1]=0;
		   
           for(i=1;i<=IMAY;i++)//这段做什么 //IIA[] 0 2 3 1
           {
			   m=IIA[i-1];
		       NA[m-1]++;
//                 printf("%d\n",NA[m-1]);
		   }//NA[] 1 1 1 0
 /*
fprintf(fp1,"NA[]");
 for(i=1;i<=IMAY;i++)
 {
     fprintf(fp1,"%d   ",NA[i-1]);
 }
 fprintf(fp1,"\n");
*/ 
           for(i=1;i<NOB;i++)//这里NB做什么用//NA[] 1 1 1 0
              NB[i]=NB[i-1]+NA[i-1];         //NB[] 1 0 0 0    1 2 0  0    1 2 3 0
                                             //NB[] 1 2 3 4	
           for(i=1;i<=IMAY;i++)
           {
			   m=IIA[i-1];k=NB[m-1];//IIA[] 0 2 3 1
			   //printf("%d\n",k);k=0 2 3 1
               if(m!=0) 
               {
				   IIO[k-1]=i;NB[m-1]=NB[m-1]+1;
				                  }
			  // printf("%d\n", IIO[k-1]);0 2 3 4 
			 //  printf("%d\n",NB[m-1]);0 3 4 2
           }//形成程序内部结点号-->外部结点号转换表，保存在数组IIA[]中

           for(i=1;i<=NOB;i++)
           {
			   m=IIO[i-1];IOI[m-1]=i; 
           }//由IIO[]形成外部结点号-->程序内部结点号转换表，保存在数组IOI[]中
		   fprintf(fp1,"****内部结点号-->外部结点号转换表IIO[]\n");
           for(i=1;i<=NOB;i++)
           fprintf(fp1,"IIO[%d]=%d\n",i-1,IIO[i-1]);
		   fprintf(fp1,"\n");

           fprintf(fp1,"****外部结点号-->程序内部结点号转换表IOI[]\n");
           for(i=1;i<=IMAX;i++)
               fprintf(fp1,"IOI[%d]=%d\n",i-1,IOI[i-1]);
		   fprintf(fp1,"\n****计算参考电（外部结点号）=%d \n",IIO[NOB-1]);
           fprintf(fp1,"\n");
      }       



  int A[1000],B[1000],IY[1000],JYY[1000];
  JJY()
  {
	  int l,i,j,mm,k,m,n;
      fprintf(fp1,"\n****形成导纳矩阵上三角非零元素的位置表JJY()\n");
   //fprintf(fp1,"\n使用分配排号法后输出结果\n");
   //统计导纳矩阵每行上三角非零元素个数，按内部结点号排序
      for(i=1;i<=NOB+1;i++)
            A[i-1]=0;
      for(l=1;l<=NOE;l++)
      {
		  i=(int)(ELE[l-1][0]);
		  j=(int)(ELE[l-1][1]);
          i=IOI[i-1];
		  j=IOI[j-1];//有外部结点号找到内部结点号
          if(i==j)
			  continue;
          if(i>j)  i=j;//根据导纳阵的对称性所以记录j节点,后边一同样把其转化过来
		  A[i-1]=A[i-1]+1; 
      }//把结果存到数组A[]中
   //for(i=1;i<=NOE;i++) fprintf(fp1,"A[]%d=%d ",i-1,A[i-1]);
   //fprintf(fp1,"\n");
   //求各行非零互导纳元素起点位置
      B[0]=1;IY[0]=1;
      for(i=1;i<=NOB;i++)
      {
		  B[i]=B[i-1]+A[i-1];
          IY[i]=B[i];//IY相当于C阵 
       }//把结果存到数组IY[]中
	         //for(i=1;i<=NOB+1;i++) fprintf(fp1,"IY[%d]=%d ",i-1,IY[i-1]);
	          //fprintf(fp1,"\n");
	          //求各行非零元素的列号
	// for(i=0;i<=NOB;i++){printf("%d\n",IY[i] );}IY=1 2 4 5 5
	         for(l=1;l<=NOE;l++)
       {
		   i=(int)(ELE[l-1][0]);
		   j=(int)(ELE[l-1][1]);
           i=IOI[i-1];
		   j=IOI[j-1];
           if(i==j)  continue;//此处形成互导纳，所以I==J被去掉
           if(i>j)  
		   {
			   m=i;
			   i=j;
			   j=m;//由于是存储上三角阵，所以大到小转换一下，以为导纳阵具有对称型
		   }
           k=B[i-1];JYY[k-1]=j;B[i-1]=B[i-1]+1;
	   }//把结果存到数组JYY[]中
	               //for(i=0;i<IY[NOB];i++)fprintf(fp1,"JYY[%d]=%d",i,JYY[i]);
	               //fprintf(fp1,"\n");
                   /* for(i=1;i<=NOB;i++)
                      printf("B[%d]=%d\n",i-1,B[i-1]);
                      printf("\n");
	               */

       for(l=1;l<=NOB;l++)//NOB网络实际节点数
       {                  //使用插入排号法，对段内数据进行排序
		   m=IY[l-1];//IY[0]=1 IY[1]=3 IY[2]=5 IY[3]=6 IY[4]=6
		   n=IY[l]-2;//说明中间有两个或两个以上元素，需排序      
           if(n<m)    //同一段内进行从新排序用插入排号法
			   continue;//下面为m>=n
           for(i=m;i<=n;i++)
           {
			   mm=JYY[i];
			   j=i;
               do 
			   {
				   if(mm>=JYY[j-1]) 
				   break;
                   JYY[j]=JYY[j-1];
			       j=j-1;
			   }
              while(j!=m-1);
                  JYY[j]=mm;
		   }       
	   }  
                        	//fprintf(fp1,"\n使用插入排号法后输出结果\n");
                          	//for(i=1;i<=NOB;i++)fprintf(fp1,"IY[%d]=%d",i-1,IY[i-1]);
                         	//fprintf(fp1,"\n");
                        	//for(i=0;i<IY[NOB];i++)fprintf(fp1,"JYY[%d]=%d",i,JYY[i]);
                        	//fprintf(fp1,"\n");
       for(i=1;i<=NOB;i++)  //NOB网络实际节点数
       {
		   for(j=IY[i-1]+1;j<=IY[i]-1;j++)//同一段内如果有并连支路则合为一个
           {
			   if(j>=IY[i]) break;
               if(JYY[j-1]==JYY[j-2])
               {
				   for(m=j;m<=NOE;m++)
                   {
					   JYY[m-1]=JYY[m];
				   }
                   for(m=i+1;m<=NOB+1;m++)//合并完后去处并联支路留下的空位
                   {
					   IY[m-1]=IY[m-1]-1;//合并完后去处并联支路留下的空位即起始位依次减一
				   }
                }
		   }
	   }
	   fprintf(fp1,"****合并并联支路后输出结果\n");
	   fprintf(fp1,"****导纳矩阵上三角各行非零互导纳首址表IY[]\n");
	   for(i=1;i<=NOB+1;i++) 
	      fprintf(fp1,"IY[%d]=%d\n",i-1,IY[i-1]);
	   fprintf(fp1,"\n");
	   fprintf(fp1,"****导纳矩阵上三角各行非零互导纳的列号JYY[]\n");
	   for(i=0;i<IY[NOB];i++)
		  fprintf(fp1,"JYY[%d]=%d\n",i,JYY[i]);
	   fprintf(fp1,"\n");
                /*  for(i=1;i<=NOB+1;i++)
                printf("IY[%d]=%d   ",i-1,IY[i-1]);
                printf("\n");
                for(i=1;i<=NOE;i++)
                printf("JYY[%d]=%d   ",i-1,JYY[i-1]);*/
       fprintf(fp1,"\n");
 }



    int IYL[1000],JYLL[1000][2];
    JYL()//建立虚拟的导纳矩阵下三角非零元素位置表
    {
		int ll,i,j,l,n,m,mm,ml;
        fprintf(fp1,"****建立虚拟的导纳矩阵下三角非零元素位置表JYL()\n");
        for(i=1;i<=NOB;i++)//清零
             A[i-1]=0;
        for(l=1;l<=NOE;l++)
        {
			m=JYY[l-1];//jyy中存储的为列号
			A[m-1]=A[m-1]+1;
		} 
        B[0]=1;IYL[0]=1;
	         //求导纳矩阵各行下三角非零元素首址表
	    fprintf(fp1,"****导纳矩阵各行下三角非零元素首址表IYL[]\n");
        for(l=1;l<=NOB;l++)
        {
			B[l]=B[l-1]+A[l-1];
			IYL[l]=B[l];//类似形成C表 只是由列形成，因为上下三角阵的对称性所以
			             //上三角阵中列的非零元素的个数即为下三角阵中行中非零元素的个数
		}//把结果存到数组IYL[]中
	    for(i=1;i<=NOB+1;i++) 
			fprintf(fp1,"IYL[%d]=%d\n",i-1,IYL[i-1]);
	    fprintf(fp1,"\n");

        for(l=1;l<=NOB;l++)//为导纳矩阵上三角非零元素的行号=下三角元素的列号
        {
			for(ll=IY[l-1];ll<=IY[l]-1;ll++)//通过ll依次扫描上三角元素每行的列号
            {
				m=JYY[ll-1];//m为上三角元素每行的列号=下三角元素的行号
	            n=B[m-1];//b[m-1]为下三角元素每行的列号指针，n为下三角元素每行的列号
			    JYLL[n-1][0]=l;
                JYLL[n-1][1]=ll;//JYLL[][1]保存与之对应的上三角元素在数组JYY[]中的位置指针
                 B[m-1]=B[m-1]+1;//下三角元素每行的列号指针+1
             }
         }
         fprintf(fp1,"****导纳矩阵各行下三角非零元素的列号JYLL[][0]\n");
         fprintf(fp1,"****导纳矩阵各行下三角非零元素的导纳值的存放位置JYLL[][1]\n");
         for(i=1;i<=NOE;i++)
		 {
			 fprintf(fp1,"JYLL[%d][0]=%d    ",i-1,JYLL[i-1][0]);
		     fprintf(fp1,"JYLL[%d][1]=%d    ",i-1,JYLL[i-1][1]);
		     fprintf(fp1,"\n");
		 }
	}

YY(float GII[],float BII[],float GIJ[],float BIJ[])
{ 
	int i,j,m,n,l,II,IJ;
    float  B,R,X,G,FK;
    fprintf(fp1,"\n****形成导纳矩阵YY()\n");
    for(l=1;l<=NOB;l++)//清零
    {
		GII[l-1]=0.0;
		BII[l-1]=0.0;
	}
    for(l=1;l<=NOE;l++)
    {
		GIJ[l-1]=0;
		BIJ[l-1]=0;
	}
    for(l=1;l<=NOE;l++)//每寻找一条线路自导纳增加一部分则
		               //寻完全部线路则自导纳才完全完成
	{
		R=ELE[l-1][2];//z=R+jX,1/z=G+jB,所以1/z=(R-jX)/(R*R+X*X)
		X=ELE[l-1][3];
	    G=R/(R*R+X*X);
		B=-X/(R*R+X*X);
        FK=ELE[l-1][4];//接地电容或为变比
        i=IOI[(int)(ELE[l-1][0])-1];//内外节点转化
        j=IOI[(int)(ELE[l-1][1])-1];
        if(i>j) //保持II<IJ
		{
			II=j;
			IJ=i;
		}
        else
		{
			II=i;
			IJ=j;
		}
        for(m=IY[II-1];m<=IY[II]-1;m++)//IY[II-1]与IY[II]-1之间为一个节点的非零互导纳数
        {
			if(IJ==JYY[m-1])//互导纳的寻找过程为由IY（为每行中互导纳个数）出发，来查找JYY书上219页  
			{
				n=m;
				break;
			}//n为需寻找的上三角非零元素的列号
		}
		if(i==j)
		{
			BII[i-1]=BII[i-1]+B;
			continue;
		}
		else
		{
			if(FK>0.0) //为变压器
            {
				GII[j-1]=GII[j-1]+G;//GII前边已清零，G为输入数据得出（已知）
                BII[j-1]=BII[j-1]+B;
                BII[i-1]=BII[i-1]+B/(FK*FK);
                GII[i-1]=GII[i-1]+G/(FK*FK);
                GIJ[n-1]=GIJ[n-1]-G/FK;
                BIJ[n-1]=BIJ[n-1]-B/FK;
		        continue;
			}
            else //FK<0.0为线路减相当于加
			{
				 GIJ[n-1]=GIJ[n-1]-G;
                 BIJ[n-1]=BIJ[n-1]-B;
                 GII[j-1]=GII[j-1]+G;
                 BII[j-1]=BII[j-1]+B-FK;
			}
		}
        BII[i-1]=BII[i-1]+B-FK;
        GII[i-1]=GII[i-1]+G;
		continue;
	}
	    fprintf(fp1,"****导纳矩阵各结点的自电导GII[]  自电纳BII[]\n");
		for(i=1;i<=NOB;i++)
		{          //fprintf(fp1,"GII[%d]=%f   BII[%d]=%f\n",i-1,GII[i-1],i-1,BII[i-1]);
			fprintf(fp1,"gii[%d]=%f,   bii[%d]=%f\n",i-1,gii[i-1],i-1,bii[i-1]);
		}
        fprintf(fp1,"\n****导纳矩阵各结点的互电导GIJ[]  互电纳BIJ[]\n");
		for(i=1;i<=NOE;i++)
		{             //fprintf(fp1,"GIJ[%d]=%f   BIJ[%d]=%f\n",i-1,GIJ[i-1],i-1,BIJ[i-1]);
			fprintf(fp1,"gij[%d]=%f,   bij[%d]=%f\n",i-1,gij[i-1],i-1,bij[i-1]);
		}
}

    GS(int nox,int JU[],float U[],int ku)
    {