MATLAB/Simulink在控制系统仿真与CAD应用(一)

MATLAB编程部分

将一个离散系统传递函数输入计算机

如果T=0.1s
$$H(z)=\frac{z^2+0.568}{(z-1)(z^2-0.2z+0.99)}$$

z = tf('z',0.1);
H = (z^2+0.568)/((z-1)*(z^2-0.2*z+0.99));

由微分方程得传递函数和零极点模型

$$y^{(3)}(t)+13y^{(2)}(t)+4y^{\prime }(t)+5y(t)=2u(t)$$

% 第一种状态变量选取方法（能观标准型）
A = [0,0,-5;1,0,-4;0,1,-13];
B = [2,0,0]';
C = [0,0,1];
D = 0;
 
G1 = ss(A,B,C,D)
G2 = tf(G1)
G3 = zpk(G1)
 
% 第二种状态变量选取方法（能控标准型）
a=[0,1,0;0,0,1;-5,-4,-13];
b=[0,0,1]';
c=[2,0,0];
d=0;
 
g1 = ss(a,b,c,d)
g2 = tf(g1)
g3 = zpk(g1)
 
% 直接根据微分方程写出系统传递函数
num = [2];
den = [1,13,4,5];
G = tf(num,den)

• Tips:得到系统零极点（pole和zero函数）

s = tf('s');
G = 1/(s*(s+5));
pole(G)
zero(G)

如果是多变量系统，系统的零点定义和单变量不同，因此需要使用tzero函数来求取。

输入差分方程到MATLAB工作空间

差分方程一般很容易转换为脉冲传递函数形式。
$$y(k+2)+y(k+1)+0.16y(k)=u(k+1)+2u(k)$$

取 $Z$ 变换：
$$H(z)=\frac{z+2}{z^2+z+0.16}$$
我们依旧用 $tf()$ 函数，只不过我们需要指明采样周期 $T$。在这里，我们的 $Z$ 变换结果是基于 $T=1s$ 计算的。

num = [1,2];
den = [1,1,0.16];
H = tf(num,den,'Ts',1)

$T$ 写的不同不会影响传递函数形式，但是我们需要知道的是，它会影响经过离散系统后信号之间的时间间隔，也就是 $Z$ 变换就是对一组离散的数进行某种运算产生新的一组离散的数，无论你 $T$ 取多少，原始数据不变Z变换后数就不变，变的只是时间间隔。

SISO线性系统推导闭环传递函数

feedback函数不支持符号运算，只能是针对确定参数模型，因此我们有必要编写一个小程序段方便化简。

function U = feedbacksym_1(G1,G2,key)
if nargin==2;key = -1;end
U = 1/(sym(1)-key*G1*G2);
end

nargin是输入函数变量个数。
下面是一个例子：
$$G(s)=\frac{s+1}{J{s}^2+2s+5}$$
$$G_c(s)=\frac{K_{p}s+K_i}{s}$$

syms s J Kp Ki;
G = (s+1)/(J*s^2+2*s+5);
Gc = (Kp*s+Ki)/s;
simplify(feedbacksym_1(G*Gc,1))

结果为：
$$\frac{\left({\mathrm{K}}_i+\mathrm{K}\mathrm{p}s\right)\left(s+1\right)}{{\mathrm{K}}_i+5s+{\mathrm{K}}_{i}s+\mathrm{K}\mathrm{p}s+J{s}^3+\mathrm{K}\mathrm{p}{s}^2+2s^2}$$

传递函数转状态空间以及状态空间状态轨迹绘制

• 传递函数转状态空间

其实就是将建立好的 $tf$ 模型使用 $ss(G)$ 的方法就能转换成状态空间，只不过需要明确的是：传递函数到状态空间的表示不唯一，而状态空间到传递函数得表示唯一，这也应证了状态变量选择的多样性。

• 状态空间状态轨迹绘制
  step函数可以返回时间，输出，状态变量值，因此可以绘制系统内部状态轨迹。

A = [-0.2 0.5 0 0 0;0 -0.5 1.6 0 0;0 0 -14.3 85.8 0;0 0 0 -33.3 100;0 0 0 0 -10]
B = [0 ;0 ;0; 0 ;30];
C = [1 0 0 0 0];
D = 0;
G = ss(A,B,C,D);
[y,t,x] = step(G)
plot(t,y)             %输出响应
plot(t,x)             %状态轨迹，随t变化。

连续传递函数采样至离散传递函数

$$G(s)=\frac{(s+1{)}^2(s^2+2s+400)}{(s+5{)}^2(s^2+3s+100)(s^2+3s+2500)}$$

这里千万不要把G化简然后用 $tf[num,den{,}^{\prime }T{s}^{\prime },0.1]$ 这种做法，那是针对的已知离散传递函数模型的搭建，这里已知的是连续传递函数模型，我们不能说对它采样后离散传递函数就是把s换成z，因此需要使用 c2d函数。

s = tf('s');
G = (s+1)^2*(s^2+2*s+400)/(s+5)^2/(s^2+3*s+100)/(s^2+3*s+2500);
H1 = c2d(G,0.01);
H2 = c2d(G,0.1);
H3 = c2d(G,1);
subplot(2,2,1);step(G);title('原系统的阶跃响应')
subplot(2,2,2);step(H1);title('离散系统 T=0.01时')
subplot(2,2,3);step(H2);title('离散系统 T=0.1时')
subplot(2,2,4);step(H3);title('离散系统 T=1时')

这个结果应该是自带了 $zero-order-holder$ 的结果。事实上，在Simulink仿真离散系统的时候，往往采样开关都可以省略，系统自己会带上保持器。

判定系统稳定

有如下闭环传递函数：
$$W_B(s)=\frac{1}{s^3+2s^2+s+2}$$
我们有三种判断稳定性方法：

1. 零极点图
2. 特征根
3. isstable函数

num = [1];
den = [1,2,1,2];
G1  = tf(num,den);
pzmap(G1);title('G1');pz1 = eig(G1)
GG1 = isstable(G1)

可以看到，系统不稳定（临界震荡）。
离散系统和状态方程模型也可以使用 $isstable$ 函数。

在这里说明一个很有趣的问题，有人要判断如下系统稳定性：
$$H(z)=\frac{-3z+2}{z^3-0.2z^2-0.25z+0.05}$$
也许会说“离散系统稳定性和采样周期有关，所以无法直接判断其稳定性”。这句话其实没错，但是我们根据最基本的知识也知道上面这个离散系统是可以判断稳定性的，也就是 $z$ 域单位圆比较的方法。但是上面那种说法到底是为什么呢？因为它是基于从连续系统到离散系统这一过程来说的，在这其中就包含了采样周期，但是如果我的系统从一开始设计就是在离散域设计的就根本不存在采样周期影响稳定性的情况，只有可能是系统本身参数影响。

自治系统状态方程解析解

自治系统有如下形式：
$$\dot{x}(t)=Ax(t)$$
$$x(0)=x_0$$
则系统状态变量的解为：
$$x(t)=e^{At}{x}_0$$

syms t;
A = [-5,2,0,0;0,-4,0,0;-3,2,-4,-1;-3,2,0,-4];
x0= [1,2,0,1]';
y = expm(A*t)*x0
 
%解析图画图
subplot(2,1,1)
ezplot(y(1),[0,10]);hold on;
ezplot(y(2),[0,10]);hold on;
ezplot(y(3),[0,10]);hold on;
ezplot(y(4),[0,10]);
set(gca,'Ylim',[-0.5,2],'Xlim',[0,10])
title('解析解曲线')

$gca$：获取当前坐标轴的句柄。
$ezplot$：可以直接传入符号函数表达式，例如
syms t;
ezplot(t);

传递函数解析解求法

• 零初值
  零初值可直接调用laplace和ilaplace即可。
• 非零初值
  非零初值可以手工将传递函数转化为微分方程形式，然后使用dsolve函数求解。
  $$G(s)=\frac{s^3+7s^2+3s+4}{s^4+7s^3+17s^2+17s+6}$$
  $$y(0)=1,y^{\prime }(0)=0.5,y^{\prime \prime }(0)=y^{\prime \prime \prime }(0)=0$$
  $$u(t)=sint$$
  则微分方程为：
  $$y^{\prime \prime \prime \prime }+7y^{\prime \prime \prime }+17y^{\prime \prime }+17y^{\prime }+6y=u^{\prime \prime \prime }+7u^{\prime \prime }+3u^{\prime }+4u$$

syms t;u = sin(t);
uu = diff(u,3)+7*diff(u,2)+3*diff(u)+7*u;
y = dsolve(['D4y+7*D3y+17*D2y+17*Dy+6*y=',char(uu)],...
    'y(0)=1','Dy(0)=0.5','D2y(0)=0','D3y(0)=0')
fplot(y,[0,100])

根轨迹

$$G(s)=\frac{K(s+6)(s-6)}{s(s+3)(s+4-4j)(s+4-4j)}$$
$rlocus()$：绘制根轨迹。

z = [6,-6];
p = [0,-3,-4+4i,-4-4i];
G = zpk(z,p,1)
figure(1);
rlocus(G);

系统辨识

T = arx([y,u],[n,m,d])

y为输出 的列向量，u为输入的列向量，n为分布多项式阶次，m-1为分子多项式阶次，d为纯滞后。我们做系统辨识都是基于一些采样点而言的，因此系统辨识实际上是辨识离散系统，对于连续系统，我们可以将辨识出来的离散系统使用d2c恢复成连续系统。（辨识出来的G(z)的默认采样时间是1s，需要改成你实际的采样时间。）

num = [6 2 8 10];
den = [2 6 4 8];
G0 = tf(num,den)
G1 = c2d(G0,3)
G2 = d2c(G1)

step(G0,G1,G2)

如果辨识的采样周期大了，恢复回连续系统的准确度就会下降。

可以看出恢复的系统和原系统存在差别。

• 实例

T = arx([y,u],[1,4,0])
T.Ts = 0.02;
G1 = d2c(T,'tustin') 
G2 = tf(G1)
[y1,~,~] = lsim(G1,u,0:0.02:0.02*2000);
plot(0:0.02:2000*0.02,y1)
hold on
plot(0:0.02:2000*0.02,y)

y，u已经输入工作空间。

蓝线为辨识模型，红线为系统响应。

模型降阶

Pade近似

对延时项近似

[a,b]=padecoef(2,2);
G = tf(a,b)

第一个参数是延时时间，第二个是用几阶系统近似。

pade(exp(-2*s),2)
[n,m] = pade(2,2)          %分子多项式、分母多项式

下面给出近似结果;

因为阶次比较低，近似效果不是很好。
另外，pade还可以对带延时的LTI模型直接近似：

s = tf('s');
G = 25/(s^2*(s+50))；
G.iodelay = 2；
G = pade(G,1);

近似过程是把LTI的延时项近似再乘上去。
我们发现，pade和padecoef都是用分子分母次数相同的模型近似，下面给出可调阶次的延时项近似函数：

paderm实现对延时项独立分子分母阶次的将阶处理
function [n,d] = paderm(tau,r,k)
c(1) = 1;
for i = 2:r+k+1
    c(i) = -c(i-1)*tau/(i-1);
end
Gr = pade_app(c,r,k);
n = Gr.num{1}(k-r+1:end);
d = Gr.den{1};
end

function Gr = pade_app(c,r,k)
w = -c(r+2:r+k+1)';
vv = [c(r+1:-1:1)';zeros(k-1-r,1)];
W = rot90(hankel(c(r+k:-1:r+1),vv));
V = rot90(hankel(c(r:-1:1)));
x = [1 (W\w)'];
dred = x(k+1:-1:1)/x(k+1);
y = [c(1) x(2:r+1)*V'+c(2:r+1)];
nred = y(r+1:-1:1)/x(k+1);
Gr = tf(nred,dred);
end

对系统近似

给出下面函数pademod代码以供参考：

pade近似(不能对延时项近似)
function G_r = pademod(G_Sys,r,k)
c = timmomt(G_Sys,r+k+1);
G_r = pade_app(c,r,k);
end


function Gr = pade_app(c,r,k)
w = -c(r+2:r+k+1)';
vv = [c(r+1:-1:1)';zeros(k-1-r,1)];
W = rot90(hankel(c(r+k:-1:r+1),vv));
V = rot90(hankel(c(r:-1:1)));
x = [1 (W\w)'];
dred = x(k+1:-1:1)/x(k+1);
y = [c(1) x(2:r+1)*V'+c(2:r+1)];
nred = y(r+1:-1:1)/x(k+1);
Gr = tf(nred,dred);
end

function M = timmomt(G,k)
G = ss(G);
C = G.c;
B = G.b;
iA = inv(G.a);
iA1 = iA;
M = zeros(1,k);
for i = 1:k
    M(i) = -C*iA1*B;
    iA1 = iA*iA1;
end
end

s = tf('s');
K = s/(s^3+2*s^2+s+1)
F = pademod(K,1,2)
step(K,'-',F,'--')

这里面用到了一个hankel函数，在这里说明一下hankel矩阵：汉克尔矩阵 (Hankel Matrix) 是指每一条逆对角线上的元素都相等的矩阵，在数字信号处理、数值计算、系统控制等领域均有广泛的应用。 第一个传入参数是hankel矩阵第一列，第二个传入参数是hankel矩阵最后一行，之后就会返回自动填充的hankel矩阵。
r，k为分子分母次数。
结果：

近似效果一般。

Routh降阶

Routh降阶方法能保证系统稳定性，而Pade近似方法可能降阶后系统不稳定。

Schur降阶

Schur降阶适用于状态方程降阶，这里给出调用格式：

Gr = schmr(G,1,k)

第二个参数填1不用动，k代表你想消去的状态变量个数。

最优Hankel范数降阶

同样这种方法适用于状态方程的将阶，函数入口与schmr一致。

Gr = ohklmr(G,1,k)

次最优降阶

利用最优化的思想，我们可以使用次最优降阶的方法得到逼近效果最好的降阶模型。

%次最优降阶
function Gr=opt_app(G,r,k,key,GO)
GS=tf(G);
num=GS.num{1};
den=GS.den{1};
Td = totaldelay(GS);
GS.ioDelay=0;
GS.Inputdelay=0;
GS.Outputdelay=0;
s = tf('s');
if nargin<5 
    GO=(s+1)^r/(s+1)^k;
end
beta=GO.num{1}(k+1-r:k+1);
alph=GO.den{1};
Tau=1.5*Td;
x=[beta(1:r) , alph(2:k+1) ] ;
if abs(Tau) <1e-5 
    Tau=0.5;
end
dc=dcgain(GS) ;
if key==1 
    x=[x,Tau] ;
end
y=opt_fun(x,GS, key, r, k, dc) ;
x=fminsearch(@opt_fun,x,[], GS, key, r, k,dc) ;
alph=[1,x(r+1:r+k)] ; beta=x(1:r+1) ;
if key==0
    Td=0;
end
beta(r+1) =alph(end) *dc;
if key==1
    Tau=x(end) +Td;
else
    Tau=0;
end
Gr=tf(beta,alph,'ioDelay',Tau) ;
end


function y=opt_fun(x, G, key, r, k, dc)
ff0=1e10;a=[1,x(r+1:r+k)];b=x(1:r+1);b(end)=a(end)*dc;
if key==1, tau=x(end) ;
    if tau<=0
        tau=eps;
    end
    [n,d]=pade(tau,3);
    gP=tf(n,d);
else 
    gP=1;
end
G_e=G-tf(b,a) *gP; 
G_e.num{1}=[0,G_e.num{1}(1:end-1)] ;
[y, ierr] =geth2(G_e) ;
if ierr==1 
    y=10*ff0;
else 
    ff0=y;
end
end

%子函数get h 2
function [v, ierr] =geth2(G)
G=tf(G) ; num=G.num{1}; den=G.den{1};ierr=0;v=0;n=length(den);
if abs(num(1) ) >eps
    disp('System not strictly proper') ;
    ierr=1; return
else 
    a1=den; 
    b1=num(2:length(num)) ;
end
    for k=1:n-1
        if(a1(k+1) <=eps) 
            ierr=1;
            return
        else
            aa=a1(k)/a1(k+1);
            bb=b1(k)/a1(k+1);
            v=v+bb*bb/aa;
            k1=k+2;
            for i=k1:2:n-1
                a1(i) =a1(i) -aa*a1(i+1) ;
                b1(i) =b1(i) -bb*a1(i+1) ;
            end
        end
    end
    v=sqrt(0.5*v);
end

s = tf('s');
G = (3*s+1)/(s+1)^3;
Gr = opt_app(G,1,2,0)
step(G,'-',Gr,'--')

结果：

可以看出，这种降阶方法效果最好。

Bode图、Nyquist图、Nichols图

$margin(G)$：求出G的幅值裕度，相位裕度及其各自对应的 $w$。

s = tf('s');
G = 8*(s+1)/s^2/(s+15)/(s^2+6*s+10);
[gm,pm,wg,wp] = margin(G)
figure(1)
subplot(2,2,1);bode(G);
subplot(2,2,2);nyquist(G);
subplot(2,2,3);nichols(G);
subplot(2,2,4);step(feedback(G,1));

nyquist和nichols绘图后加grid命令可以添加网格，对于nichols图这一点很关键，因为可以通过网格看出谐振峰值等信息。

超前滞后校正

超前滞后校正器的传递函数模型为如下形式：
$$G_c(s)=K\frac{(\alpha T_{1}s+1)(T_{2}s+1)}{(T_{1}s+1)(\beta T_{2}s+1)}$$
我们编写了如下函数求取给定剪切频率，相位裕度以及速度误差系数下的校正器模型：

function Gc=leadlagc(G,Wc,Gam_c,Kv,key)
G=tf(G); [Gai,Pha]=bode(G,Wc);
Phi_c=sin((Gam_c-Pha-180)*pi/180);
den=G.den{1}; a=den(length(den):-1:1);
ii=find(abs(a)<=0); num=G.num{1}; G_n=num(end);
if ~isempty(ii), a=a(ii(1)+1); else, a=a(1); end
alpha=sqrt((1-Phi_c)/(1+Phi_c)); Zc=alpha*Wc;
Pc=Wc/alpha; Kc=sqrt((Wc*Wc+Pc*Pc)/(Wc*Wc+Zc*Zc))/Gai;
K1=G_n*Kc*alpha/a;
if nargin==4, key=1;
    if Phi_c<0, key=2; else
        if K1<Kv, key=3; end, end
end
switch key
    case 1, Gc=tf([1 Zc]*Kc,[1 Pc]);
    case 2
        Kc=1/Gai; K1=G_n*Kc/a;
        Gc=tf([1 0.1*Wc],[1 K1*Gcn(2)/Kv]);
    case 3
        Zc2=Wc*0.1; Pc2=K1*Zc2/Kv;
        Gcn=Kc*conv([1 Zc],[1,Zc2]);
        Gcd=conv([1 Pc],[1,Pc2]); Gc=tf(Gcn,Gcd);
end
end

关于它的算法和自控书上有区别，在这里不一一赘述。下面我们以一个例子说明使用方法：
已知校正前开环传递函数:
$$G(s)=\frac{210(s+1.5)}{(s+1.75)(s+16)(s+1.5+3j)(s+1.5-3j)}$$
给定$w_c=2rad/s$,$\varphi =\pi /2$,$K_v=10$。求校正器和校正后系统。

z = [-1.5];
p = [-1.75;-16;-1.5+3i;-1.5-3i];
G = zpk(z,p,210);
figure(1)
bode(G)
Gc = leadlagc(G,2,pi/2,10,3);
figure(2)
bode(Gc)
figure(3)
bode(Gc*G)
[a,phi]=bode(Gc*G,2)

• 结果显示
  校正前bode图，相位裕度大概在60°左右。
  
  校正器bode图
  
  校正后bode图，相位裕度大概在90°，满足设计要求。
  

状态反馈实现动态解耦

首先我们需要明确解耦针对的是相同输入输出维数的系统，由于状态变量互相对输出都有影响，但我们想将其解耦，也就是实现一个输入控制一个输出。下面我们将给出两种解耦方法。

解耦成对角积分器模型

篇幅所限，这里不给出算法公式，只给出实例代码。

状态反馈解耦
function [K,Gam,G] = decouple(G)
G = ss(G);
A = G.a; B = G.b; C = G.c;
[n,m] = size(B);
F = [];
H = [];
d = [];
for i = 1:m
    for j = 0:n-1
        if(norm(C(i,:)*A^j*B)>eps)
            d(i) = j;
            break;
        end
    end
    F = [F;C(i,:)*A^d(i)*B];
    H = [H;C(i,:)*A^(d(i)+1)];
end
Gam = inv(F);
K = Gam * H;
G = minreal(tf(ss(A-B*K,B,C,G.d))*Gam);
end

A = [2.25 -5 -1.25 -0.5;2.25 -4.25 -1.25 -0.25;0.25 -0.5 -1.25 -1;1.25 -1.75 -0.25 -0.75];
B =[4 6;2 4;2 2;0 2];
C = [0 0 0 1;0 2 0 2];
D = 0;
G1 = ss(A,B,C,D);
[K,Gam,G] = decouple(G1);
figure(1)
step(G1)
figure(2)
step(G)

左图为解耦前，右图为解耦后，可以看出解耦后输入向量的第一个维度对输出向量的第二个维度没有贡献，输入向量的第二个维度对输出向量的第一个维度没有贡献。因此解耦成功。

极点配置状态反馈解耦

如果只是将状态解耦成积分型，由于积分的作用输出会一直增大，因此这种解耦在实际应用中存在缺陷，我们设想如果能将状态解耦成标准传递函数形式，则对于每个输出的控制将会变得更加容易。

• 例子
  解耦
  
  脚本部分：

A = [0 0 0;0 0 1;-1 -2 -3];
B = [1 0;0 0;0 1];
C = [1 1 0;0 0 1];
D = 0;
G = ss(A,B,C,D);
[G1,K,d,Gam] = decouple_pp(G,5);
step(G)
step(G1)

函数部分：

%标准传递函数
function G = std_tf(wn,n)
M=[1 1 0 0 0 0 0 0;1 1.41 1 0 0 0 0 0;
    1 1.75 2.15 1 0 0 0 0;1 2.1 3.4 2.7 1 0 0 0;
    1 2.8 5 5.5 3.4 1 0 0;1 3.25 6.6 8.6 7.45 3.95 1 0;
    1 4.475 10.42 15.08 15.54 10.64 4.58 1];
G = tf(wn^n,M(n,1:n+1).*(wn.^[0:n]));
end

%极点配置状态反馈解耦
function [G1,K,d,Gam] = decouple_pp(G,wn)
G = ss(G);
A = G.a; B = G.b; C = G.c;
[n,m] = size(B);
F = [];
E = [];
for i = 1:m
    for j = 0:n-1
        if(norm(C(i,:)*A^j*B)>eps)
            d(i) = j;
            break;
        end
    end
    g1 = std_tf(wn,d(i)+1);
    [n1,d1]=tfdata(g1,'v');
    F = [F;C(i,:)*polyvalm(d1,A)];
    E = [E;C(i,:)*A^d(i)*B];
end
Gam = inv(E);
K = Gam * F;
G1 = minreal(tf(ss(A-B*K,B,C,G.d))*Gam);
end

• 结果：
  
  可以看出，非对角响应为0，主对角呈标准传函响应，解耦成功。

基于状态空间模型的控制器设计方法

• 状态反馈控制
  
  如果把 $u(t)=v(t)-Kx(t)$ 代入开环系统状态方程模型，就能得到闭环状态方程模型：
  $$\begin{gathered} \dot{x}(t)=(A-BK)x(t)+Bv(t) \\ y(t)=(C-DK)x(t)+Dv(t) \end{gathered}$$
  如果系统 $(A,B)$ 完全可控，那么就存在 $K$ 矩阵能使系统特征根即 $A-BK$ 的特征值配置到任意地方。

LQR方法

A=[2 0 4 1 2;1 -2 -4 0 1;1 4 3 0 2;2 -2 2 3 3;1 4 6 2 1]
B=[1 2;0 1;0 0;0 0;0 0]
Q=diag([1000 0 1000 500 500]);
R=eye(2);
[K,S]=lqr(A,B,Q,R)

其中 $K$ 为状态反馈矩阵。$S$ 为 $Riccati$ 方程的解矩阵。把 $K$ 代回公式就是优化后的控制器。

G = ss(A-B*K,B,C-D*K,D);
fprintf('极点结果:\n ')
eig(G)
figure;pzmap(G);
figure;step(G)

极点配置法

Ackermann算法

K=acker(A,B,p)

其中 $p$ 为要配置到的极点向量。但是这只适用于单变量系统。

鲁棒极点配置算法

K=place(A,B,p)

$place()不适用于重根极点的情况。$

• 例子
  

function [Gc,H]=h_2_8_1a(G,K,L)
H=ss(G.a-L*G.c,L,K,0); Gc=ss(G.a-G.b*K-L*G.c+L*G.d*K,G.b,-K,1);
end
运行代码：
A = [-0.2,0.5,0,0,0;0,-0.5,1.6,0,0;0,0,-14.3,85.8, 0;0,0,0,-33.3,100;0,0,0,0 ,-10];
B = [0,0,0,0,30]';
C = [1,0,0,0,0];
D = [0];
G = ss(A,B,C,D);
fprintf('原系统零点:\n ')
z = zero(G)
fprintf('原系统极点:\n ')
p = pole(G)
P = [-1,-2,-3,-4,-5];
fprintf('状态反馈矩阵:\n ')
K = place(A,B,P)
fprintf('闭环极点配置验证:\n ')
eig(A-B*K)
fprintf('观测器:\n ')
L = acker(A',C',P)'
[Gc,H]=h_2_8_1a(G,K,L); 
fprintf('控制器:\n ')
tf(Gc)

标准法极点配置

这个算法是基于标准型给出的，根据现代控制理论书上对算法的描述编写了如下脚本：

clc;clear;close all;

%-------------标准法极点配置-------------%
A = [0 0 0;0 0 1;-1 -2 -3];
B = [1 ;0 ;1 ];
C = [1 1 0];
D = 0;
[A1,B1,C1,D1,P] = Control_standard(A,B,C,D);
syms s;
J = (s-5)*(s-4)*(s-3);  %配置极点位置
collect(J);
H1 = sym2poly(J);
H1 = H1(end:-1:2);
H2 = -A1(end,:);
disp("反馈矩阵：");
K = (H1 - H2)*P
disp("原始极点：");
eig(A)
G1 = A-B*K;
disp("配置后极点：")
eig(G1)

状态观测器设计方法

先给出状态观测器的方框图：

这里有几个注意问题：

1. 为什么有L？我们知道如果不用L搭建一个与被控对象相同的模型并且A的特征根实部小于0即可做到状态观测，但是这样有几个问题，第一是A可能不满足实部小于0；第二是我们无法主观能动地设计状态观测器的性能，即极点配置。
2. 下文是基于D = 0的。
   观测器表达：
   $$\begin{gathered} {\hat{x}}^{\prime }(t)=(A-LC)\hat{x}(t)+Bu(t)+Ly(t) \\ \hat{y}=\hat{x}(t) \end{gathered}$$
   问题的关键是找到L使A-LC的特征根实部全部小于0。 于是问题转化为$(A^T,C^T)$能控性问题，也是$(A,C)$能观性问题。我们利用place极点配置的方法即可找到这个L。
   找到L后我们需要进行仿真，我们需要将系统的输入分成两部分，即y和u，利用叠加原理编写程序，这里会使用一个lsim函数来仿真状态空间，当然这个函数不只可以仿真状态空间。

clc;clear;close all;
%--------------状态观测器---------------%
A = [0 2 0 0;0 -0.1 8 0;0 0 -10 16;0 0 0 -20];
B = [0;0;0;0.3953];
C = [0.09882 0.1976 0 0];
D = 0;
disp("原系统极点：")
eig(A)
disp("L矩阵求解等同于A转置C转置能控问题")
P = [-4 -3 -2 -1];    %配置观测器极点
L = place(A',C',P')'   %place不能处理重极点
disp("A-LC即观测器的极点为：")
eig(A-L*C)

G = ss(A,B,C,D);
[y,t,x] = step(G);
G1 = ss(A-L*C,B,eye(4),0);
G2 = ss(A-L*C,L,eye(4),0);
[y1,~,xh1] = lsim(G1,ones(size(t)),t);
[y2,~,xh2] = lsim(G2,y,t);
xh = xh1 + xh2;
figure(1)
plot(t,xh,'-',t,x,'--')
xlim([0,4])

注意看G1、G2的构造。lsim第二个参数是输入，第三个参数是仿真时间。G2的输入来自step原始系统。最后xh = xh1 + xh2（叠加原理）。

观测器效果如下：

其中虚线是观测器观测状态变量，实线是系统状态变量。