DOB设计:
%% Controller & DoB design
% Setting feedback control (PD) gains
Kp=500; Kd=4;
C=tf([Kd Kp],[1]);
Cz=c2d(C,T,'matched');
Gcz=feedback(Gz*Cz,1);
% Design of a Q filter (Notice that the relative order of Gdnew(z) is 2)
w = 50*2*pi; Q1 = tf([w],[1 w]);
syms z; z =tf('z',T); alpha = exp(-w*T); QQ = ((1-alpha)/2*(z+1)/(z-alpha))^2;
Q = Q1*Q1;
Qz = c2d(Q,T,'matched')
Qz = Qz / dcgain(Qz); % To make the Q(exp(j0T)) = 1
[aQ,bQ] = tfdata(Qz,'v'); % Model parameters of Q filter
% Design of DOB
GDOB = minreal(Qz*Gz^-1);
[aD,bD] = tfdata(GDOB,'v'); % Model parameters of Q*G^-1
%% Disturbance
v = zeros(1,N);
for k=2:N, v(k) = (yd(k)-yd(k-1))/T; end
for k=1:N
g = 9.81;
gear_ratio = 1; l = 5; m = 20;
d1(k) = -m*g*l*gear_ratio*sin(yd(k)*10*pi/180); % Gravity
d2(k) = 800*(sin(