DOB实现例子

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(