clc
  clear
%潮流計(jì)算
p=0.8;Q=0.47;u=1;
xd=1.226;xpd=0.190;
xq=0.830;xe=0.500;
Td0=8.940;D=1;
Tj=5;ke=1;Te=0.05;
WN=314;km=1;Tm=0.5;
s=p+j*Q;I=conj(s/u);q=angle(I);
ut=u+I*(j*xe);qt=angle(ut);
Eqd=ut+j*xq*I;o=angle(Eqd);
id=abs(I)*sin(o-q);iq=abs(I)*cos(o-q);
ud=abs(u)*sin(o);uq=abs(u)*cos(o);
utd=abs(ut)*sin(o-qt);utq=abs(ut)*cos(o-qt);
epq=utq+xd'*id;
xpdm=xpd+xe;xqm=xq+xe;
%求線性化系數(shù)k1--k6
disp('線性化系數(shù)k1--k6')
k1=(epq*uq)/xpdm+((uq*uq-ud*ud)*(xd'-xq))/(xpdm*xqm)
k2=ud/xpdm
k3=(xd-xpd)/xpdm
k4=(ud*(xd-xpd))/xpdm
k5=(xq*utd*uq)/(abs(ut)*xqm)-(xpd*utq*ud)/(abs(ut)*xpdm)
k6=(utq*xe)/(abs(ut)*xpdm)
%求原系統(tǒng)矩陣A,B,C,D
disp('原系統(tǒng)各矩陣:')
A=[-(D/Tj),-(k1/Tj),1/Tj,0,-(k2/Tj);WN,0,0,0,0;0,0,-(1/Tm),0,0;0,0,0,-(1/Te),0;0,-(k4/Td0),0,1/Td0,-(1/(Td0*k3))]
B=[0,0;0,0;0,km/Tm;ke/Te,0;0,0]
C=[0,k5,0,0,k6;0,k1,0,0,k2]
D=0;
%求解原系統(tǒng)傳遞函數(shù)
sys=ss(A,B,C,D);
disp('原系統(tǒng)傳遞函數(shù)為：')
H=tf(sys)
%基于階躍型輸入無靜差,構(gòu)造增廣系統(tǒng)（求系統(tǒng)矩陣）
     %確定內(nèi)摸矩陣（Ac,Bc,Cc)
     disp('系統(tǒng)伺服系統(tǒng)傳遞函數(shù)矩陣為：')
      m=zeros(2);n=zeros(5,2);
      Ac=m
      Bc=[1,0;0,1]
      Cc=eye(2)
   %求增廣系統(tǒng)Am,Bm，Mm(參考輸入矩陣）
 disp('增廣系統(tǒng)傳遞函數(shù)矩陣為：') 
 Am=[A,B;-Bc*C,Ac]
 Bm=[n;Bc]
 Cm=[C m]
%判斷增廣系統(tǒng)的穩(wěn)定性
[b,c]=eig(Am);
disp('增廣系統(tǒng)各極點(diǎn)')
r=real(diag(c))
re=(r>=0);
if re==0
    disp('由以上極點(diǎn)可知增廣系統(tǒng)穩(wěn)定')
else
    ree=r<=0;
    if ree==ones(size(ree))
        disp('由以上極點(diǎn)可知增廣系統(tǒng)臨界穩(wěn)定')
    else
        disp('由以上極點(diǎn)可知增廣系統(tǒng)不穩(wěn)定')
    end
end   
%判斷增廣系統(tǒng)的可控性與可觀性
Qc=cat(2,Bm,Am*Bm,Am^2*Bm,Am^3*Bm,Am^4*Bm,Am^5*Bm,Am^6*Bm);
Qo=cat(1,Cm,Cm*Am,Cm*Am^2,Cm*Am^3,Cm*Am^4,Cm*Am^5,Cm*Am^6);
rc=rank(Qc)
if rc==7
    disp('增廣系統(tǒng)可控')
else
    disp('增廣系統(tǒng)不可控')
end
ro=rank(Qo)
if ro==7
    disp('增廣系統(tǒng)可觀')
else
    disp('增廣系統(tǒng)不可觀')
end
%增廣二次型最優(yōu)控制
disp('增廣二次型最優(yōu)控制')
Qy=input('請(qǐng)?jiān)O(shè)定輸出加權(quán)對(duì)稱常數(shù)矩陣Qy：');
Qy=diag(Qy)
Qu=input('請(qǐng)?jiān)O(shè)定控制加權(quán)對(duì)稱常數(shù)矩陣Qu：');
Qu=diag(Qu)
[K,P,E]=lqr(Am,Bm,Cm'*Qy*Cm,Qu); 
disp('黎卡提方程中矩陣P：')
P
disp('最優(yōu)定常狀態(tài)反饋矩陣K：')
K
disp('最優(yōu)輸出調(diào)節(jié)器的最優(yōu)控制為：u*(t)=kx(t)')
disp('閉環(huán)系統(tǒng)的特征根：')
E
%狀態(tài)觀測(cè)器設(shè)計(jì)
length_of_Am=length(Am);
Om=obsv(Am,Cm);
rank_of_Om=rank(Om);
   if rank_of_Om==length(Am)
    %設(shè)置所需要的極點(diǎn)
   desired_poles=input('請(qǐng)?jiān)O(shè)定期望極點(diǎn)：')
    %desired_poles=[-40 -41+0.32i -41-0.32i -42+1.25i -42-1.25i -43+13.48i -43-13.48i];
    %計(jì)算狀態(tài)觀測(cè)器增益矩陣L
    disp('狀態(tài)觀測(cè)器增益矩陣L:')
    L=place(Am',Cm',desired_poles)'
    disp('狀態(tài)觀測(cè)器系統(tǒng)狀態(tài)空間模式表達(dá)式')
    Ao=Am-L*Cm
    Bo=[Bm L]
    Co=Cm
    Do=0
    else
      disp('不能設(shè)置狀態(tài)觀測(cè)器')
   end
disp('總的閉環(huán)系統(tǒng)的性質(zhì):')
   %帶狀態(tài)觀測(cè)器的狀態(tài)反饋系統(tǒng)
   R=zeros(2,7);
   %An=[Am,Bm*K;L*Cm,(Am-L*Cm+Bm*K)];
   An=[Am-Bm*K,Bm*K;zeros(7,7),(Am-L*Cm)];
   Bn=[Bm;Bm];
   Cn=[Cm R];
   Dn=0;
Qnc=cat(2,Bn,An*Bn,An^2*Bn,An^3*Bn,An^4*Bn,An^5*Bn,An^6*Bn,An^7*Bn,An^8*Bn,An^9*Bn,An^10*Bn,An^11*Bn,An^12*Bn,An^13*Bn);
Qno=cat(1,Cn,Cn*An,Cn*An^2,Cn*An^3,Cn*An^4,Cn*An^5,Cn*An^6,Cn*An^7,Cn*An^8,Cn*An^9,Cn*An^10,Cn*An^11,Cn*An^12,Cn*An^13);
rnc=rank(Qnc)
rno=rank(Qno)
if rnc==14
    disp('總的閉環(huán)系統(tǒng)可控')
else
    disp('總的閉環(huán)系統(tǒng)不可控')
end
if rno==14
    disp('總的閉環(huán)系統(tǒng)可觀')
else
    disp('總的閉環(huán)系統(tǒng)不可觀')
end
disp('  閉環(huán)傳遞函數(shù)： ')
sysk=ss(An,Bn,Cn,Dn);
Hk=tf(sysk)
%仿真結(jié)果
sim('f',60);%仿真模型名為f