扩张状态观测器程序

clear;
a=0; b=0.2; 
h=0.001;
step=0.001;
%%% the parameters for TD
R1=17000;
R2=20000000;
h0=step;
d01=R1*h0;
d02=h0*d01;
d03=R2*h0;%*0.05;
d04=h0*d03;
d05=10;
d0=0.001;

%%%%%the parameters for ESO and controller
a0=1.2;a1=0.75;a2=0.9;
d1=0.001;
d2=0.001;
d3=0.001;
PI=3.14;
bt01=10;bt02=80;bt03=800;bt04=1000;
bt1=100;bt2=1300;bt3=4;


y1（1）=0;y2（1）=0;y3（1）=0;y4（1）=0;y6（1）=0;vT（1）=1;
z1（1）=0;z2（1）=0;z3（1）=0;z4（1）=0;v1（1）=0;v2（1）=0;v3（1）=0;s1（1）=0;s2（1）=0;u=0;u0=0;
I=（b-a）/h;
i=1;
t=0;
T0=10;
La1=1;Ra1=1;Kt1=1;Jm1=0.0008;Kb1=1;stiff1=50;Bm1=1.2;b1=Kt1/（La1*Jm1）;
%%%%%%%%%%%%%%%paramter of motor%%%%%%%%%%%%%%
%Jm=0.002;
La=1;Ra=1;Kt=1;Bm=1.2;Jm=0.0008;Kb=1;n=1;stiff=50;%Bm=1.2;
r=10/360*2*PI;ww=2*PI*100;b0=Kt/（La*Jm）;

%%%%%%%%%%%%%%%%%%%%
while i%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%This is the disturbance;   
wT（i）=0.5*r*sign（cos（0.125/2*ww*t））;
w=wT（i）;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%simulation plan numbers counter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K11y=y2（i）;
K12y=y3（i）;
K13y=-（La*Bm+Ra*Jm）/（La*Jm）*y3（i）-（La*stiff/（n*n）+Ra*Bm+Kt*Kb）*y2（i）-Ra*stiff/（n^2*La*Jm）*y1（i）+b0*u-w;

K21y=y2（i）+step*K12y/2;
K22y=y3（i）+step*K13y/2;
K23y=-（La*Bm+Ra*Jm）/（La*Jm）*（y3（i）+step*K13y/2）-（La*stiff/（n*n）+Ra*Bm+Kt*Kb）*（y2（i）+step*K12y/2）-Ra*stiff/（n^2*La*Jm）*（y1（i）+step*K11y/2）+b0*u-w;

K31y=y2（i）+step*K22y/2;
K32y=y3（i）+step*K23y/2;
K33y=-（La*Bm+Ra*Jm）/（La*Jm）*（y3（i）+step*K23y/2）-（La*stiff/（n*n）+Ra*Bm+Kt*Kb）*（y2（i）+step*K22y/2）-Ra*stiff/（n^2*La*Jm）*（y1（i）+step*K21y/2）+b0*u-w;

K41y=y2（i）+step*K32y;
K42y=y3（i）+step*K33y;
K43y=-（La*Bm+Ra*Jm）/（La*Jm）*（y3（i）+step*K33y）-（La*stiff/（n*n）+Ra*Bm+Kt*Kb）*（y2（i）+step*K32y）-Ra*stiff/（n^2*La*Jm）*（y1（i）+step*K31y）+b0*u-w;

y1（i+1）=y1（i）+step*（K11y+2*K21y+2*K31y+K41y）/6;
y2（i+1）=y2（i）+step*（K12y+2*K22y+2*K32y+K42y）/6;
y3（i+1）=y3（i）+step*（K13y+2*K23y+2*K33y+K43y）/6;

%%%%%%%%%
%%% plan desired output and various states%%
%%%%%%%%%%%%%%%
yd1（i）=r*sign（sin（0.125/2*ww*t））;
%yd1（i）=r*sin（ww*t）;
%yd2（i）=r*ww*cos（ww*t）;
%yd3（i）=-r*ww*ww*sin（ww*t）;

vt1=yd1（i）;
v=v1（i）-vt1+step*v2（i）;%?????
g0=（d0^2+8*R1*abs（v））^0.5;
if （abs（v））<=d02;
    g=v2（i）+v/step;%???????
else
    if （v>0）
        g=v2（i）+（g0-d01）/2;