DOB.rar_DOB干扰观测器_dob观测器_matlab dob_干扰观测器_观测

close all clc clear %Parameter Initial syms s %有效时间到哪里？ T_ini=0; T_span=0.001; T_end=3; A_r=1; w_r=5*2*pi; ph_r=0; kp=1; kv=0.00; %Plant Jp=1; Bp=101; Kp=950; Jn=1; Bn=101; Kn=950; tor=0.001; Gps=Kp/(Jp*s^2+Bp*s); Gns=Kn/(Jn*s^2+Bn*s); Qs=(3*tor*s+1)/(tor^3*s^3+3*tor^2*s^2+3*tor*s+1); G1s=Qs; G2s=Qs*(1/Gns); [pnGps pdGps]=f_s2p(Gps); %转变为零极型 [pnQs pdQs]=f_s2p(Qs); [pnG1s pdG1s]=f_s2p(G1s); [pnG2s pdG2s]=f_s2p(G2s); %signal nn=(T_end/T_span); T_time=zeros(1,nn); sig_r=zeros(1,nn); sig_y=sig_r; sig_v=sig_y; sig_u=sig_v; sig_u1=sig_u; sig_u2=sig_u; sig_d=sig_u; sig_0=sig_u; sig_f=sig_u; %z trans [nG1z,dG1z]=c2dm(pnG1s,pdG1s,T_span,'tustin') [nG2z,dG2z]=c2dm(pnG2s,pdG2s,T_span,'tustin'); [nGpz,dGpz]=c2dm(pnGps,pdGps,T_span,'tustin'); sig_y0=0; sig_v0=0; sig_d0=0; %控制器设计 for i=1:nn %Input Signal T_time(i)=i*T_span; sig_r(i)=A_r*sin(w_r*T_time(i)+ph_r); % sig_r(i)=1; %PD 控制器 sig_u(i)=kp*(sig_r(i)-sig_y0)+kv*sig_v0; %PD controller output S = 1; %select DOB if 1 == S sig_u1(i)=sig_u(i)-sig_d0; %PD output minus DOB output else sig_u1(i)=sig_u(i); %not use DOB end M = 1; if 1 == M sig_f(i)=f_getf(sig_u1(i),sig_v0); %get Friction else sig_f(i)=f_getf(sig_u1(i),sig_v0)+0.5*sin(2*pi*T_time(i)); %get Friction + disturbe end sig_u2(i)=sig_u1(i)+sig_f(i); %PD+DO output adding friction sig_y(i)=f_zout_s(nGpz,dGpz,sig_u2,sig_y,i); %plant output sig_v(i)=(sig_y(i)-sig_y0)/T_span; %calculating D %干扰观测器 sig_d(i)=f_zout_s(-nG1z,dG1z,sig_u1,sig_d,i); %z域输出 sig_d(i)=sig_d(i)+f_zout_s(nG2z,dG2z,sig_y,sig_0,i); %Parameters Update sig_y0=sig_y(i); %output update sig_v0=sig_v(i); %velicity update sig_d0=sig_d(i); %DOB update end figure(1) hold on plot(T_time,sig_r,'r') plot(T_time,sig_y,'g') title('sig_r and sig_y') figure(3) plot(T_time,sig_u) title('sig_u') figure(4) plot(T_time,sig_v) title('sig_v') figure(5) hold on plot(T_time,sig_f,'r') plot(T_time,sig_d,'g') title('sig_f and sig_d')