%6機系統k1-k6模型特征值計算（模型選擇見smanyx.m)
%包括潮流計算、QR法、模糊聚類、等效二階SMA
%數據文件
%zhilu1.mat %支路數據文件                   
%fuhe1.mat %負荷數據文件                   
%fadian1.mat%發電機數據文件
%gpm1.mat%發電機參數文件
%調入數據文件(電科院6機系統)
clear
load zhilu1.mat -ascii;   %支路數據文件                   
load fuhe1.mat -ascii;   %負荷數據文件                   
load fadian1.mat -ascii;  %發電機數據文件
load gpm1.mat -ascii;  %發電機參數文件
%load fhtx.mat -ascii; %負荷靜態特性數組文件                   
%-----------------------------------------
%發電機的類型參數
%mt1=[5 5 5 5 5 5];
mt1=[4 4 4 4 4 4];
%mt1=[1 1 1 1 1 1];
%-----------------------------------------
nm=max(zhilu1(:,2:3)); nn=max(nm');%網絡節點總數
bnn=size(zhilu1);      bn=bnn(1,1);%支路數
lnn=size(fuhe1);      ln=lnn(1,1);%負荷節點數
gnn=size(fadian1);     gn=gnn(1,1);%發電機節點數
%==================BX型PQ分解法=====================
%形成bp,不考慮充電電容及變壓器非標準變比對導納矩陣的影響
%但要考慮支路電阻的影響
ynb=zeros(nn,nn);
for l=1:bn;
   n=abs(zhilu1(l,1));
   m=abs(zhilu1(l,2));
   if n==m
      y1=0;
   else
    y1=1./(zhilu1(l,3)+j*zhilu1(l,4));
    ynb(n,n)=ynb(n,n)+y1;
    ynb(m,m)=ynb(m,m)+y1;
    ynb(n,m)=-y1;
    ynb(m,n)=-y1;
   end
end,
bp=imag(ynb);
%-----------------------------------------
%形成bpp,不考慮支路電阻的影響,
%但要考慮支路充電電容及變壓器的非標準變比的影響
ynp=zeros(nn,nn);
for l=1:bn;
   n=abs(zhilu1(l,1));
   m=abs(zhilu1(l,2));
   if n==m
      ynp(n,n)=ynp(n,n)+1./(j*zhilu1(l,5));
   else
      y1=1./(j*zhilu1(l,4));%不考慮支路電阻
      if (zhilu1(l,1)>0&zhilu1(l,2)>0)       %不是變壓器支路,是輸電線支路 
         ynp(n,n)=ynp(n,n)+y1+j*zhilu1(l,5);
         ynp(m,m)=ynp(m,m)+y1+j*zhilu1(l,5);
         ynp(n,m)=-y1;
         ynp(m,n)=-y1;
      elseif zhilu1(l,1)<0   %非標準變比側          
         kt=zhilu1(l,5);
         ynp(n,n)=ynp(n,n)+y1/(kt^2);
         ynp(m,m)=ynp(m,m)+y1;
         ynp(n,m)=-(ynp(n,m)+y1/kt);
         ynp(m,n)=ynp(n,m);
      elseif zhilu1(l,2)<0   %非標準變比側
         kt=zhilu1(l,5);
         ynp(n,n)=ynp(n,n)+y1;
         ynp(m,m)=ynp(m,m)+y1/(kt^2);
         ynp(n,m)=-(ynp(n,m)+y1/kt);
         ynp(m,n)=ynp(n,m);
      end
   end
end,
bpp=imag(ynp);
%---------------------------------------
%求取導納矩陣
yy=zeros(nn,nn);
for l=1:bn;
   n=abs(zhilu1(l,1));
   m=abs(zhilu1(l,2));
   if n==m
      yy(n,n)=yy(n,n)+1./(j*zhilu1(l,5));
   else
   y1=1./(zhilu1(l,3)+j*zhilu1(l,4));
   if (zhilu1(l,1)>0&zhilu1(l,2)>0)       %不是變壓器支路,是輸電線支路 
         yy(n,n)=yy(n,n)+y1+j*zhilu1(l,5);
         yy(m,m)=yy(m,m)+y1+j*zhilu1(l,5);
         yy(n,m)=-y1;
         yy(m,n)=-y1;
   elseif zhilu1(l,1)<0   %非標準變比側          
         kt=zhilu1(l,5);
         yy(n,n)=yy(n,n)+y1/(kt^2);
         yy(m,m)=yy(m,m)+y1;
         yy(n,m)=-(yy(n,m)+y1/kt);
         yy(m,n)=yy(n,m);
    elseif zhilu1(l,2)<0   %非標準變比側
         kt=zhilu1(l,5);
         yy(n,n)=yy(n,n)+y1;
         yy(m,m)=yy(m,m)+y1/(kt^2);
         yy(n,m)=-(yy(n,m)+y1/kt);
         yy(m,n)=yy(n,m);
      end
   end
end,
%-----------------------------------
%定義變量名
pv=0;          %PV節點的個數
v=ones(nn,1);  %電壓
pg=zeros(nn,1);%發電機有功
qg=zeros(nn,1);%發電機無功
pl=zeros(nn,1);%負荷有功
ql=zeros(nn,1);%負荷無功
o=zeros(nn,1); %電壓相角
%----------------------------------
%根據節點類型對bp，bpp進行修正原則是:將bp，bpp通取為n階
%PO迭代:bp中對平衡節點所在的行和列,除對角線元素取作1外,其他元素均取為0.
%其相應的右端元素opv(i)也取為0(見后面計算);
%QV迭代:bpp中對平衡節點和PV節點所在的行和列,除對角線元素取作1外,其他元素均取為0.
%其相應的右端元素oqv(i)也取為0(見后面計算).
%取PQ節點的有功和無功功率和PV節點的有功,電壓
for l=1:gnn;%發電機
   if fadian1(l,2)==0&fadian1(l,3)==0 %平衡節點
      f=fadian1(l,1);
      lxl=f;
      v(f)=1.00;%平衡節點電壓
      bp(f,:)=0;bp(:,f)=0;bp(f,f)=1;
      bpp(f,:)=0;bpp(:,f)=0;bpp(f,f)=1;
   elseif abs(fadian1(l,1))>fadian1(l,1)%PV節點
      f=abs(fadian1(l,1));
      pg(f)=fadian1(l,2);
      v(f)=fadian1(l,3);
      bpp(f,:)=0;bpp(:,f)=0;bpp(f,f)=1;
      pv=pv+1;
      pvj(pv)=f;%記錄PV節點的節點號
   else
      f=fadian1(l,1);
      pg(f)=fadian1(l,2);
      qg(f)=fadian1(l,3);
   end
end
for l=1:lnn %負荷
   f=fuhe1(l,1);
   pl(f)=fuhe1(l,2);
   ql(f)=fuhe1(l,3);
end;
%===============================================================================
%功率誤差
oo=ones(nn,1);
ov=ones(nn,1);
opv=ones(nn,1);
oqv=ones(nn,1);
%--------------------------------------
while(max(abs(opv))>0.000001|max(abs(oqv))>0.000001)
   %---------PO迭代----------------
   if max(abs(opv))>0.000001
     av=0;%平均電壓
     for i=1:nn
       su=0;
       for l=1:nn
         if yy(i,l)~=0
           do=o(i)-o(l); 
           su=su+v(l)*(real(yy(i,l))*cos(do)+imag(yy(i,l))*sin(do));
         end
       end
       opv(i)=(pg(i)-pl(i)-v(i)*su)/v(i);%計算常數項
       av=av+v(i);
     end
     opv(lxl)=0;%位于后面計算方便,令其維數為n
     av=(av+1.05)/nn;%平均電壓
     oo=inv(bp)*opv/av;%計算誤差量oo,
     o=o-oo;%修正相角
  end 
  %----------QV迭代---------------
  if max(abs(oqv))>0.000001
     for i=1:nn
       su=0;
       for l=1:nn
          if yy(i,l)~=0
             do=o(i)-o(l);
            su=su+v(l)*(real(yy(i,l))*sin(do)-imag(yy(i,l))*cos(do));
          end
       end
      oqv(i)=(qg(i)-ql(i)-v(i)*su)/v(i);%計算常數項
    end
    for i=1:pv
     l=pvj(i);
     oqv(l)=0;%對PV節點令其右端項為0
    end
   oqv(lxl)=0;
   ov=inv(bpp)*oqv;
   v=v-ov;%修正幅值
 end 
 %----------下一次迭代-------- 
end
%=================潮流已經收斂======================2002.3.28
%計算平衡節點的有功,無功
for i=1:nn
   if yy(lxl,i)~=0
      do=o(lxl)-o(i);
      pg(lxl)=pg(lxl)+v(lxl)*v(i)*(real(yy(lxl,i))*cos(do)+imag(yy(lxl,i))*sin(do));
      qg(lxl)=qg(lxl)+v(lxl)*v(i)*(real(yy(lxl,i))*sin(do)-imag(yy(lxl,i))*cos(do));
   end
end
%計算PV節點的無功功率
for i=1:pv
   k=pvj(i);
   for l=1:nn
      if yy(k,l)~=0
         do=o(k)-o(l);
         qg(k)=qg(k)+v(k)*v(l)*(real(yy(k,l))*sin(do)-imag(yy(k,l))*cos(do));
      end
   end
end
%================潮流計算完畢====================================
%與特征值計算程序銜接2002.3.28
%支路數據文件的銜接
bzm=zeros(bn,8);
for i=1:bn
   bzm(i,1)=1;bzm(i,2)=zhilu1(i,1);bzm(i,3)=zhilu1(i,2);
   bzm(i,4)=0; bzm(i,5)=zhilu1(i,3);bzm(i,6)=zhilu1(i,4);
   if zhilu1(i,1)<0|zhilu1(i,2)<0
      bzm(i,2)=abs(zhilu1(i,1));bzm(i,3)=abs(zhilu1(i,2));
      bzm(i,8)=zhilu1(i,5);
   else
      bzm(i,7)=zhilu1(i,5);
   end
end
%負荷數據的銜接
lpqv=zeros(ln,10);
for i=1:ln
   lpqv(i,1)=fuhe1(i,1);lpqv(i,2)=1;lpqv(i,5)=fuhe1(i,2);lpqv(i,6)=fuhe1(i,3);
   lpqv(i,7)=v(fuhe1(i,1));lpqv(i,8)=o(fuhe1(i,1))*180/pi;
   lpqv(i,9)=lpqv(i,7)*cos(o(fuhe1(i,1)));lpqv(i,10)=lpqv(i,7)*cos(o(fuhe1(i,1)));
end
%發電機節點數據的銜接
gpqv=zeros(gn,10);
for i=1:gn
   gpqv(i,1)=abs(fadian1(i,1));gpqv(i,2)=1;
   gpqv(i,3)=pg(gpqv(i,1));gpqv(i,4)=qg(gpqv(i,1));
   gpqv(i,7)=v(gpqv(i,1));gpqv(i,8)=o(gpqv(i,1))*180/pi;
   gpqv(i,9)=gpqv(i,7)*cos(o(gpqv(i,1)));gpqv(i,10)=gpqv(i,7)*sin(o(gpqv(i,1)));
   if i==lxl %平衡節點
      gpqv(i,2)=1;
   end
   for l=1:pv
      if pvj(l)==abs(fadian1(i,1));%PV節點
         gpqv(i,2)=-1;
      end
   end
end
%=========================銜接部分完成===========================

%原始網絡導納矩陣yy
%================================
%恒定阻抗負荷模型下的網絡導納矩陣ynl
yl=zeros(nn);
for l=1:ln,
    n=lpqv(l,1);
    yl(n,n)=(lpqv(l,5)-j*lpqv(l,6))./(lpqv(l,7)^2);
end,yl;
ynl=yy+yl;
%--------------------------------
%消去中間節點后的發電機節點間輸電導納矩陣yt
yff=ynl(1:gn,1:gn);
yfl=ynl(1:gn,gn+1:nn);
ylf=ynl(gn+1:nn,1:gn);
yll=ynl(gn+1:nn,gn+1:nn);
yt1=yff-yfl*inv(yll)*ylf; %發電機節點（含無窮大母線）間的導納矩陣
%--------------------------------
if mt1(1,1)==0            %有無窮大母線時
   yt=yt1(2:gn,2:gn);     %發電機節點間導納矩陣
   yt0=yt1(2:gn,1);       %無窮大母線與發電機節點間的互導納向量
   u0=gpqv(1,7);          %無窮大母線電壓幅值
   gpqv(1,:)=[];
   mt=mt1(1,2:gn);
   gn=gn-1;               %實際發電機節點數
 else                     %無無窮大母線時
   yt=yt1;                %發電機節點間導納矩陣
   yt0=zeros(gn,1);       %無窮大母線與發電機節點間的互導納置0。
   u0=0;
   mt=mt1;
end
%====================================
%發電機參數對角矩陣
xd=diag(gpm1(:,2));    xdp=diag(gpm1(:,3));    xdpp=diag(gpm1(:,4));
xq=diag(gpm1(:,5));    xqpp=diag(gpm1(:,6));   ra=diag(gpm1(:,7));
tdp=diag(gpm1(:,8));   tdpp=diag(gpm1(:,9));   tqpp=diag(gpm1(:,10));
tj=diag(gpm1(:,11));   ke=diag(gpm1(:,12));    te=diag(gpm1(:,13));
D=diag(gpm1(:,14));
%采用六階模型,加入
xqp=diag(gpm1(:,15));  tqp=diag(gpm1(:,16));
%-------------------------------
%根據發電機模型類型修正發電機參數
xq1=xq;
xqq=zeros(gn,gn);   xqqp=zeros(gn,gn);
for n1=1:gn,
   if mt(1,n1)==1,         %五階模型考慮阻尼繞組D+Q
      D(n1,n1)=0;
   end
   if mt(1,n1)==2,         %只考慮交軸阻尼繞組Q 
      xdpp(n1,n1)=xdp(n1,n1);
      D(n1,n1)=0;
   elseif mt(1,n1)==3,     %只考慮直軸d阻尼繞組D
      xqpp(n1,n1)=xq(n1,n1);
      D(n1,n1)=0;
   elseif mt(1,n1)==4,     %三階模型考慮綜合阻尼繞組
      xdpp(n1,n1)=xdp(n1,n1);
      xqpp(n1,n1)=xq(n1,n1);
   elseif mt(1,n1)==5,     %六階模型考慮阻尼繞組D+g+Q
      D(n1,n1)=0;
      xqq(n1,n1)=xq(n1,n1)-xqp(n1,n1);
      xqqp(n1,n1)=xqp(n1,n1)-xqpp(n1,n1);
      xq(n1,n1)=xqpp(n1,n1);
   end
end
%--------------------------------
xdd=xd-xdp;           xddp=xdp-xdpp;         xqqpp=xq-xqpp+xqqp;xq=xq1;
xqd=xqpp-xdpp;        z=ra+j*xdpp;           
%====================================
%D-Q坐標系與d-q坐標系間的變換矩陣T 
PQ=gpqv(:,3)+j*gpqv(:,4); %發電機復功率
U=gpqv(:,9)+j*gpqv(:,10); %發電機機端電壓 
I=conj(PQ./U);                      %發電機定子電流
EQ=U+(ra+j*xq)*I;                   %虛構電勢
d0=angle(EQ)-pi/2;                  %d-q坐標系超前D-Q坐標系的角度
T=diag(exp(j*d0));                  % D-Q坐標系與d-q坐標系間的變換矩陣T 
%====================================
%變量初值計算
i=inv(T)*I;            id=diag(real(i));     iq=diag(imag(i));
u=inv(T)*U;            ud=diag(real(u));     uq=diag(imag(u)); 
ut=diag(abs(u));       epp=u+z*i-xqd*imag(i);
edp=diag(real(epp));   eqp=diag(imag(epp));
%====================================
%系數矩陣k1-k15 （gn*gn階）
y=inv(T)*inv(inv(yt)+z)*T;             yr=real(y);   yi=imag(y);
y0=diag(inv((inv(yt)+z))*inv(yt)*yt0); y0r=real(y0);y0i=imag(y0);
md=-yi*edp-yr*eqp-yi*xqd*iq;           mq=yr*edp-yi*eqp+yr*xqd*iq;
pd=md-diag(sum(md'))+y0i*u0;           pq=mq-diag(sum(mq'))-y0r*u0;
yq=inv(eye(gn)-yi*xqd)*yr;             yd=-yi+yr*xqd*yq;
fq=inv(eye(gn)-yi*xqd)*pq;             fd=pd+yr*xqd*fq;
kq=inv(eye(gn)-yi*xqd)*yi;             kd=yr+yr*xqd*kq;
bd=edp+xqd*iq;                         bq=eqp+xqd*id;
cd=ud*inv(ut)*ra+uq*inv(ut)*xdpp;      cq=ud*inv(ut)*xqpp-uq*inv(ut)*ra;
%----------------------------------
k1=bd*fd+bq*fq;  k2=bd*yd+bq*yq+iq;    k3=xdd*yd;         k4=xdd*fd;
k5=cq*fq-cd*fd;  k6=cq*yq-cd*yd+uq;    k7=cq*kd-cd*kq+ud; k8=xddp*fd;
k9=xddp*yd;      k12=bd*kd+bq*kq+id;
k13=xdd*kd;      k14=xddp*kd;         
%六階模型需計算---------------------
k10=-xqqpp*fq;    k11=-xqqpp*kq;         k15=-xqqpp*yq;       
k18=-xqq*fq;      k17=-xqq*kq;           k16=-xqq*yq;
%====================================
%六階模型需要加入狀態變量edp
%狀態向量x=[δ,ω,eqp,eqpp,edp,edpp,efd]'7*gn維
a=zeros(7*gn);

a(1:gn,gn+1:2*gn)=100*pi*eye(gn);

a(gn+1:2*gn,1:gn)=-inv(tj)*k1;
a(gn+1:2*gn,gn+1:2*gn)=-inv(tj)*D;
a(gn+1:2*gn,3*gn+1:4*gn)=-inv(tj)*k2;
a(gn+1:2*gn,5*gn+1:6*gn)=-inv(tj)*k12;

a(2*gn+1:3*gn,1:gn)=-inv(tdp)*k4;
a(2*gn+1:3*gn,2*gn+1:3*gn)=-inv(tdp);
a(2*gn+1:3*gn,3*gn+1:4*gn)=-inv(tdp)*k3;
a(2*gn+1:3*gn,5*gn+1:6*gn)=-inv(tdp)*k13;
a(2*gn+1:3*gn,6*gn+1:7*gn)=inv(tdp);

a(3*gn+1:4*gn,1:gn)=-inv(tdpp)*k8-inv(tdp)*k4;
a(3*gn+1:4*gn,2*gn+1:3*gn)=inv(tdpp)-inv(tdp);
a(3*gn+1:4*gn,3*gn+1:4*gn)=-inv(tdpp)-inv(tdpp)*k9-inv(tdp)*k3;
a(3*gn+1:4*gn,5*gn+1:6*gn)=-inv(tdpp)*k14-inv(tdp)*k13;
a(3*gn+1:4*gn,6*gn+1:7*gn)=inv(tdp);
%處理tqp的不可逆的情況                                         
for n1=1:gn,
   if mt(1,n1)<5,
      tqp(n1,n1)=1;
   end
end   
vtqp=inv(tqp);
for n1=1:gn,
   if mt(1,n1)<5,
      vtqp(n1,n1)=0;
   end
end   
a(4*gn+1:5*gn,1:gn)=-vtqp*k18;
a(4*gn+1:5*gn,3*gn+1:4*gn)=-vtqp*k16;
a(4*gn+1:5*gn,4*gn+1:5*gn)=-vtqp;
a(4*gn+1:5*gn,5*gn+1:6*gn)=-vtqp*k17;


a(5*gn+1:6*gn,1:gn)=-inv(tqpp)*k10-vtqp*k18;
a(5*gn+1:6*gn,3*gn+1:4*gn)=-inv(tqpp)*k15-vtqp*k16;
a(5*gn+1:6*gn,4*gn+1:5*gn)=inv(tqpp)-vtqp;
a(5*gn+1:6*gn,5*gn+1:6*gn)=-inv(tqpp)-inv(tqpp)*k11-vtqp*k17;


a(6*gn+1:7*gn,1:gn)=-inv(te)*ke*k5;