%降維觀測器設計（極點配置法穩定系統）

% 極點配置法穩定系統
Qc=ctrb(A,B);
EA=[-10   0        0        0;...
     0  -10        0        0;...
     0    0  -2-2*sqrt(3)*i 0;...
     0    0        0        -2+2*sqrt(3)*i];
PP=polyvalm(poly(EA),A);
Ks=[0 0 0 1]*inv(Qc)*PP;%狀態反饋矩陣
%降維觀測器設計
q=rank(C);
P=[C;0 1 0 0;0 0 0 1];
Q=inv(P);
Q1=Q(:,1:q);
Q2=Q(:,q+1:rank(Q));

A1=P*A*inv(P);
B1=P*B;
%A1矩陣分塊
A11=A1(1:q,1:q);
A12=A1(1:q,q+1:4);
A21=A1(q+1:4,1:q);
A22=A1(q+1:4,q+1:4);
%B1矩陣分塊
B11=B1(1:q,:);
B12=B1(q+1:4,:);
Pole=[-1 -2];%期望極點
K=place(A22',A12',Pole);%極點配置
L=K';%狀態觀測器增益矩陣