📄 最小二乘状态估计.cpp
字号:
int I1,i,I,ka,I9,I3;
float H,D;
JU[0]=0;
for(I=1;I<=nox;I++)
{
SENT(I,IHT,MHT,HTMM);
I1=0;ku=1;
l5: do
{I1=I1+1;
if(I1!=I)
{D=U[ku-1];
do
{ if(JU[ku-1]==I)
{H=D*U[ku-1];
ka=1;
do {A1[ka-1]=A1[ka-1]-H*U[ku-1];
for(; ;)
{ku=ku+1;ka=ka+1;
if(JU[ku-1]==0)
goto l5;
l6: if(JA[ka-1]!=0) break;
JA[ka]=0;
l7: JA[ka-1]=JU[ku-1];
A1[ka-1]=-H*U[ku-1];
}
}while(JA[ka-1]==JU[ku-1]);
if(JA[ka-1]>JU[ku-1])
{for(I9=ka+1;;I9++)
if(JA[I9-1]==0) break;
for(I3=I9;I3>=ka+1;I3--)
{A1[I3-1]=A1[I3-2];
JA[I3-1]=JA[I3-2];
}
JA[I9]=0; goto l7;
}
else {ka=ka+1;goto l6;}
}
else ku=ku+1;
} while(JU[ku-1]!=0);
}
else{ ka=1;
U[ku-1]=A1[ka-1];
for(; ;)
{ku=ku+1;
ka=ka+1;
if(JA[ka-1]==0)
break;
JU[ku-1]=JA[ka-1];
U[ku-1]=A1[ka-1]/A1[0];
}
JU[ku-1]=0;
break;
}
} while(JU[ku-1]==0);
}
}
SENT(int i,int iht[],int mht[],float htmm[])
{int k,j,KJ,k4,KI,l,m;
float E,EI;
k=1;k4=N2[3];
for(l=IHTH[i-1];l<=IHTH[i]-1;l++)
{JA[k-1]=JHTH[l-1];k=k+1;}
JA[k-1]=0;
k=1;
for(;;k++)
{j=JA[k-1];
if(j==0) break;
E=0;
KI=iht[i-1];
KJ=iht[j-1];
do
{m=mht[KI-1]-mht[KJ-1];
if(m<0)
KI=KI+1;
else if(m==0)
{EI=FMDT[mht[KI-1]-1][3];
EI=1.0/(EI*EI);
E=E+htmm[KI-1]*htmm[KJ-1]*EI;
KI=KI+1;
KJ=KJ+1;
}
else{KJ=KJ+1;}
}
while((KI!=iht[i])&&(KJ!=iht[j]));
A1[k-1]=E;
}
}
int NOX;
STS()
{
NOX=2*NOB-1;
fprintf(fp1,"NOX=%d\n",NOX);
ORD();
JJY();
JYL();
MSP();
YY(gii,bii,gij,bij);
}
int NOP;
MSP()
{int NPA[1000],NQV[1000],N1[1000],N[1000],
N2[1000],NV,NP,NQ,m,l,i,j,NY,k;
fprintf(fp1,"\n****状态估计可计算性检查MSP()\n");
for(i=1;i<=NOB;i++)
{
NPA[i-1]=0;//有功量测对各结点电压相角覆盖数的统计数组
NQV[i-1]=0;//无功或电压量测对各结点电压幅值覆盖数的统计数组
}
NV=0;//电压量侧计数器
NP=0;//有功量测计数器
NQ=0;//无功量测计数器
//结点型量测覆盖与该结点相关的各结点
//支路型量侧只覆盖支路首尾两个结点
for(m=1;m<=NOM;m++)
{
if((int)(FMDT[m-1][4])!=0)//FMDT[m-1][4]此列为程序加入，并非原数据读入 ，此程序中全为零
continue; //量测设备没有使用
if((int)(FMDT[m-1][0])<4)//<4 结点型量测数据 ，>=4为支路量测数据
{
i=(int)(FMDT[m-1][1]);
i=IOI[i-1];//转化到内部结点号
//寻找与结点i相关的各结点，即与i有支路直接相连的各结点号
NY=1;
for(l=IYL[i-1];l<=IYL[i]-1;l++)
{
N[NY-1]=JYLL[l-1][0];
NY=NY+1;
}
N[NY-1]=i;
for(l=IY[i-1];l<=IY[i]-1;l++)
{
NY=NY+1;N[NY-1]=JYY[l-1];
}
} //结果存到数组N[]中
else //支路型量测
{NY=2;l=(int)(FMDT[m-1][1]);
j=(int)(ELE[l-1][0]);
N[0]=IOI[j-1];
j=(int)(ELE[l-1][1]);
N[1]=IOI[j-1];
}//相关结点只有两个，存到数组N[0],N[1]中
switch((int)(FMDT[m-1][0]))
{case 0:
case 1:NQV[i-1]=NQV[i-1]+1;NV=NV+1;//NQV表示有电压或无功测量
break;//结点电压量测
case 2: //结点有功注入量测
case 4: //支路首段有功量测
case 6:for(k=1;k<=NY;k++)//NPA表示有功测量//支路末端有功量测
{
j=N[k-1];NPA[j-1]=NPA[j-1]+1;
}
NP=NP+1;
break;
case 3: //结点无功量测
case 5: //支路首端无功量测
case 7:for(k=1;k<=NY;k++) //支路末端无功量测
{
j=N[k-1];
NQV[j-1]=NQV[j-1]+1;
}
NQ=NQ+1;
break;
}
}
for(i=1;i<=NOB;i++)
{
j=IOI[i-1];
N1[i-1]=NPA[j-1];
N2[i-1]=NQV[j-1];
}
fprintf(fp1,"****有功量测对各结点电压，相角覆盖次数的统计数组NPA[]\n");
fprintf(fp1,"****无功或电压量测对各结点电压幅值覆盖次数的统计数组NQV[]\n");
for(i=1;i<=NOB;i++)
{
fprintf(fp1,"NPA[%d]=%d ",i-1,NPA[i-1]);
fprintf(fp1,"NQV[%d]=%d ",i-1,NQV[i-1]);
fprintf(fp1,"IIO[%d]=%d\n ",i-1,IIO[i-1]);
}
//NOP=0;可进行状态估计计算 NOP=1 不能进行状态估计计算 ？？？？
NOP=0;
for(i=1;i<=NOB;i++)
{
if(NPA[i-1]==0||NQV[i-1]==0)//如果有一个点测量都为零则不能进行状态估计
NOP=1; // NQV表示有电压或无功测量，NPA表示有功测量
}
fprintf(fp1,"电压量测计数器NV=%d \n有功量测计数器NP=%d \n无功量测计数器NQ=%d\n",NV,NP,NQ);
fprintf(fp1,"能否进行状态估计标志NOP=%d \n",NOP);
}
int JB[1000];
float BB[1000],GG[1000];
RY(int i,int K0)//按I取一行矩阵元素的子程序
{
int k,n,l;
k=1;
for(l=IYL[i-1];l<=IYL[i]-1;l++)
{
JB[k-1]=JYLL[l-1][0];
n=JYLL[l-1][1];
GG[k-1]=gij[n-1];
BB[k-1]=bij[n-1];
k=k+1;
}
JB[k-1]=i;
GG[k-1]=gii[i-1];
BB[k-1]=bii[i-1];
k=k+1;
for(l=IY[i-1];l<=IY[i]-1;l++)
{
JB[k-1]=JYY[l-1];
GG[k-1]=gij[l-1];
BB[k-1]=bij[l-1];
k=k+1;
}
JB[k-1]=0;
}
H(int IH[],int MH[],int IIH[],float HII[])
{
int i,j,l,ms,k,j1,km,M,j11;
float B,R,X,Y,G,vi,vj,ri,rj,pi,qi;
fprintf(fp1,"\n****形成量测量的稀疏存放的雅可比矩阵H() \n");
fprintf(fp1,"\n");
fprintf(fp1,"****子系统量测数NOM=%d\n",NOM);
k=1;km=1;IH[0]=1;j1=0,l=0;
for(M=1;M<=NOM;M++) //NOM是子系统所包括的测点总数 16
{
if((int)(FMDT[M-1][4])!=0) continue;//说明测点不可用，同前MSP（）函数
//j11=(int)((fabs)(FMDT[M-1][0]));
j1=(int)((fabs)(FMDT[M-1][0]))+1;//加1 是为了输入为0到7程序选择从1到8
//printf("%d %d\n" ,j11,j1 );
l=(int)(FMDT[M-1][1]);
if(j1==5||j1==7)//支路起点侧或终点侧无功Qij或Qji
{
X=ELE[l-1][3];
R=ELE[l-1][2];
B=-X/(R*R+X*X);
G=R/(R*R+X*X);
i=(int)(ELE[l-1][0]);//内外节点编号转化
i=IOI[i-1];
j=(int)(ELE[l-1][1]);
j=IOI[j-1];
Y=ELE[l-1][4];//接地电容或变压器变比
vi=V[2*(i-1)]; vj=V[2*(j-1)];//电压值
ri=V[2*i-1]; rj=V[2*j-1];//相角值
if(Y<=0.0)//<=0.0说明为普通支路，>0为变压器之路
{
if(j1==5)//H()程序开始处定义了K=1；
{
HII[k-1]=2*vi*G-vj*G*cos(ri-rj)-vj*B*sin(ri-rj);//Qij对Vi的偏导
HII[k]=vi*vj*(G*sin(ri-rj)-B*cos(ri-rj));//Qij对相角i的偏导
HII[k+1]=-vi*(G*cos(ri-rj)+B*sin(ri-rj)); //Qij对Vj的偏导
HII[k+2]=-vi*vj*(G*sin(ri-rj)-B*cos(ri-rj));//Qij对相角j的偏导
}
if(j1==7)
{
HII[k-1]=vj*(-G*cos(ri-rj)+B*sin(ri-rj));//此处同上作用
HII[k]=vi*vj*(G*sin(ri-rj)+B*cos(ri-rj));
HII[k+1]=2*vj*G+vi*(-G*cos(ri-rj)+B*sin(ri-rj));
HII[k+2]=-vi*vj*(G*sin(ri-rj)+B*cos(ri-rj));
}
}
else//变压器支路求法
{
if(j1==5)
{
HII[k-1]=-vj*B*sin(ri-rj)/Y;
HII[k]=-vi*vj*B*cos(ri-rj)/Y;
HII[k+1]=-vi*B*sin(ri-rj)/Y;
HII[k+2]=vi*vj*B*cos(ri-rj)/Y;
}
if(j1==7)
{
HII[k-1]=vj*B*sin(ri-rj)/Y;
HII[k]=vi*vj*B*cos(ri-rj)/Y;
HII[k+1]=vi*B*sin(ri-rj)/Y;
HII[k+2]=-vi*vj*B*cos(ri-rj)/Y;
}
}
if(i<j)//指示各量测量的起始位置
{
MH[k-1]=2*i-1;
MH[k]=2*i;
MH[k+1]=2*j-1;
MH[k+2]=2*j;
}
if(i>j)//采用从小到大顺序如果不是则交换
{
MH[k-1]= 2*j-1;
MH[k]= 2*j;
MH[k+1]= 2*i-1;
MH[k+2]= 2*i;
X=HII[k-1];
HII[k-1]=HII[k+1];
HII[k+1]=X;
X=HII[k];
HII[k]=HII[k+2];
HII[k+2]=X;
}
k=k+4;
}
if(j1==6||j1==8)
{
X=ELE[l-1][3];
R=ELE[l-1][2];
B=-X/(R*R+X*X);
G=R/(R*R+X*X);
i=(int)(ELE[l-1][0]);
i=IOI[i-1];
j=(int)(ELE[l-1][1]);
j=IOI[j-1];
Y=ELE[l-1][4];
vi=V[2*(i-1)]; vj=V[2*(j-1)];
ri=V[2*i-1]; rj=V[2*j-1];
if(Y<=0.0)
{
if(j1==6)
{
HII[k-1]=-2*vi*(B-Y)-vj*(G*sin(ri-rj)-B*cos(ri-rj));
HII[k]=-vi*vj*(G*cos(ri-rj)+B*sin(ri-rj));
HII[k+1]=-vi*(G*sin(ri-rj)-B*cos(ri-rj));
HII[k+2]=vi*vj*(G*cos(ri-rj)+B*sin(ri-rj));
}
else
{
HII[k-1]=vj*(G*sin(ri-rj)+B*cos(ri-rj));
HII[k]=vi*vj*(G*cos(ri-rj)-B*sin(ri-rj));
HII[k+1]=-2*vj*(B-Y)+vi*(G*sin(ri-rj)+B*cos(ri-rj));
HII[k+2]=-vi*vj*(G*cos(ri-rj)-B*sin(ri-rj));
}
}
else
{
if(j1==6)
{
HII[k-1]=-2.0*vi*B/(Y*Y)+vj*B*cos(ri-rj)/Y;
HII[k]=-vi*vj*B*sin(ri-rj)/Y;
HII[k+1]=vi*B*cos(ri-rj)/Y;
HII[k+2]=vi*vj*B*sin(ri-rj)/Y;
}
else
{
HII[k-1]=vj*B*cos(ri-rj)/Y;
HII[k]=-vi*vj*B*sin(ri-rj)/Y;
HII[k+1]=-2*vj*B+vi*B*cos(ri-rj)/Y;
HII[k+2]=vi*vj*B*sin(ri-rj)/Y;
}
}
if(i<j)
{
MH[k-1]=2*i-1;
MH[k]=2*i;
MH[k+1]=2*j-1;
MH[k+2]=2*j;
}
else //i>j时则交换
{
MH[k-1]= 2*j-1;
MH[k]= 2*j;
MH[k+1]= 2*i-1;
MH[k+2]= 2*i;
X=HII[k-1];
HII[k-1]=HII[k+1];
HII[k+1]=X;
X=HII[k];
HII[k]=HII[k+2];
HII[k+2]=X;
}
k=k+4;
}
if(j1==1||j1==2)
{
i=IOI[l-1];//相对自身时导数得1
MH[k-1]=2*i-1;
HII[k-1]=1.0;
IIH[k-1]=1;
k=k+1;
}
if(j1==3||j1==4)
{
pi=0.0;qi=0.0;
i=IOI[l-1];
RY(i,1);
ms=1;
for(;;)
{
if(JB[ms-1]==0) break;
vi=V[2*(i-1)];vj=V[2*(JB[ms-1]-1)];
ri=V[2*i-1];rj=V[2*JB[ms-1]-1];
pi=pi+vi*vj*(GG[ms-1]*cos(ri-rj)+BB[ms-1]*sin(ri-rj));
qi=qi+vi*vj*(GG[ms-1]*sin(ri-rj)-BB[ms-1]*cos(ri-rj));
ms=ms+1;
}
ms=1;
for(;;)
{
if(JB[ms-1]==0) break;
MH[k-1]=2*JB[ms-1]-1;MH[k]=2*JB[ms-1];
vi=V[2*(i-1)];vj=V[2*(JB[ms-1]-1)];
ri=V[2*i-1];rj=V[2*JB[ms-1]-1];
if(j1==3)
{
if(i==JB[ms-1])
{
HII[k-1]=vi*GG[ms-1]+pi/vi;
HII[k]=-vi*vi*BB[ms-1]-qi;
}
else
{
HII[k-1]=vi*(GG[ms-1]*cos(ri-rj)+BB[ms-1]*sin(ri-rj));
HII[k]=vi*vj*(GG[ms-1]*sin(ri-rj)-BB[ms-1]*cos(ri-rj));
}
ms=ms+1;
k=k+2;
}
if(j1==4)
{
if(i==JB[ms-1])
{
HII[k-1]=-vi*BB[ms-1]+qi/vi;
HII[k]=-vi*vi*GG[ms-1]+pi;
}
else
{
HII[k-1]=vi*(GG[ms-1]*sin(ri-rj)-BB[ms-1]*cos(ri-rj));
HII[k]=-vi*vj*(GG[ms-1]*cos(ri-rj)+BB[ms-1]*sin(ri-rj));
}
ms=ms+1;
k=k+2;
}
}
}
if(MH[k-2]==2*NOB)
k=k-1;
km=km+1;
IH[km-1]=k;
}
}
HT(int ih[],int mh[],float hii[],int iht[],int mht[],float htmm[],int nom)
{
int i,l,m,n,A[1000],B[1000];
fprintf(fp1,"\n****形成雅克比矩阵的转置矩阵HT() \n");
for(i=1;i<=2*NOB;i++)
A[i-1]=0;
for(l=1;l<=ih[nom]-1;l++)
{
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -