matlab积分饱和消除一例

clc;clear all;close all
%convert transfer function to state spaces
num=conv([1 -3],[1 5]);
den_temp=conv([1 0],[1 1]);
den=conv(den_temp,[1 3]);
[Ap,Bp,Cp,Dp]=tf2ss(num,den);
%calculate gain of observer
po_temp=conv([1 10],[1 10]);
po=conv(po_temp,[1 10]);
Kob=T2place(Ap',Cp',po)';
%calculate coficient matrix
O1=zeros(3,1);O2=zeros(1,1);O3=zeros(1,1);
A=[Ap O1;Cp O2];
B=[Bp;O3];
%calculate gain of expended state spaces
p_temp1=conv([1 2],[1 2]);
p_temp2=conv(p_temp1,[1 2]);
p=conv(p_temp2,[1 2]);
K=T2place(A,B,p);
%initional
Kp=K(1,1:3);
Ky=K(1,4);%
delta_t=0.001;
t=0:delta_t:20;

xp_hat_dot=zeros(3,1);
xp=zeros(3,1);
y_plot=zeros(1,length(t));
u_plot=zeros(1,length(t));
y=0;
y_pre=0;
y_plot(1)=y;

u=0;
u_pre=0;
u_plot(1)=u;

r=ones(1,length(t));
disturbance=[zeros(1,(length(t)-1)/2) ones(1,(length(t)-1)/2+1)];
u_max=1.5;
u_min=-1.5;

for k=2:length(t)
    %
    u_dot=-K*[xp_hat_dot;y-r(k)];
    u=u_pre+u_dot*delta_t;
    u_pre=u;
    u=u+disturbance(k);
    if u>u_max
        u=u_max;
    elseif u