% Parameters of the nominal closed-loop nonlinear Pendulum-Cart system

%
theta_trim = 0.0;
%
% Parameter values
m = 0.104; % kg
M = ureal('M',0.768,'Percentage',10); % kg
g = 9.81; % m/s^2
l = 0.174; % m
I = ureal('I',2.83*10^(-3),'Percentage',20); % kg m^2
I_t = I + m*l^2; % kg m^2
M_t = M + m; % kg
f_c = ureal('f_c',0.5,'Percentage',20); % N s/m
f_p = ureal('f_p',6.65*10^(-5),'Percentage',20);% N m s/rad
den = M_t*I_t - m^2*l^2*cos(theta_trim);
k_F = 12.86; % N
u_max = 0.5;
%Fd = 1.1975875; % N dry friction

% Generates the uncertain model of the Cart-Pendulum Plant

%
% trim condition
theta_trim = 0.0;
%
% Parameter values
par_pendulum_cart
%
A = [0 0 1 0
0 0 0 1
0 m^2*l^2*g/den -f_c*I_t/den -f_p*l*m/den
0 M_t*m*g*l/den -f_c*m*l/den -f_p*M_t/den];
%
B = [ 0
0
I_t/den
l*m/den]*k_F;
%
C = [1 0 0 0
0 1 0 0];
%
D = [0
0];
%
G_ulin = ss(A,B,C,D);

% Design of LQG regulator for the Cart-Pendulum Control System

%
% Plant model
unc_lin_model
%
% Noise shaping filters
k1 = (0.235/2^12)/sqrt(12);
k2 = (2*pi/2^12)/sqrt(12);
Wn1 = k1*tf([5 50],[0.01 1]); %
Wn2 = k2*tf([0.5 10],[0.01 1]); %
%-------------------------------------
Wn = [Wn1 0
0 Wn2];
%-------------------------------------
figure(1)
bodemag(Wn1,'r-',Wn2,'b--',{10^(-1) 10^4})
grid
title('Sensor noise shaping filters')
legend('W_{n1} (cart position)','W_{n2} (pendulum angle)', ...
'Location','NorthWest')
%
% open-loop connection with the integrator
%
G = G_ulin.Nominal;
Ts = 0.01;
DG = c2d(G,Ts);
A = DG.A;
B = DG.B;
A_opt = [A zeros(4,1) % Add position error integrator
-Ts zeros(1,3) 1.0];
B_opt = [B;0];
%
% Weighting matrices
Q = diag([0.1 1 0.1 1 10])*1000*eye(5);
R = 1000;
%
K_opt = dlqr(A_opt,B_opt,Q,R);
%----------------------------------------------
% Filter design
%
% Open-loop connection with the shaping filters
%
systemnames = ' G Wn ';
inputvar = '[ control; noise{2} ]';
outputvar = '[ G+Wn ]';
input_to_G = '[ control ]';
input_to_Wn = '[ noise ]';
P = sysic;
%
Ts = 0.01;
Pd = c2d(P,Ts);
[A,B,C,D] = ssdata(Pd);
%
Af = A;
Bf = B(:,1);
Cf = C;
Gf = B(:,2:3);
Hf = D(:,2:3);
V = [1 0
0 1];
W = 10^(-3)*[1 0 % W set slightly different
0 1]; % from zero matrix
%
sys = ss(Af,[Bf Gf],Cf,[zeros(2,1) Hf],Ts);
[KF,L,P] = kalman(sys,V,W,zeros(2));
P

% Simulation of the nonlinear closed-loop Pendulum-Cart system

% with LQ regulator and Kalman filter
%
par_pendulum_cart
%
Ts = 0.01; % s
val_all = [];
sim('clp_LQG',50)
%
figure(1)
plot(position.time,position.signals.values(:,1),'b.', ...
position.time,position.signals.values(:,2),'k--', ...
position.time,position.signals.values(:,3),'r-')
grid on
xlabel('Time (s)')
ylabel('p (m)')
title('Cart position')
legend('Exact position','Position measurement','Position estimate',
...
'Location','NorthEast')
%
figure(2)
plot(theta.time,theta.signals.values(:,1),'b.', ...
theta.time,theta.signals.values(:,2),'k--', ...
theta.time,theta.signals.values(:,3),'r-')
grid on
xlabel('Time (s)')
ylabel('\theta (deg)')
title('Pendulum angle')
legend('\Theta','\Theta - measurement','\Theta - estimate', ...
'Location','SouthEast')
%
figure(3)
plot(control.time,control.signals.values,'r-')
xlabel('Time (s)')
ylabel('u')
title('Control action')
axis([0 50 -0.15 0.15])
grid on
%
[nf,nc] = size(theta.time)
err_position_meas = norm(position.signals.values(:,1) - ...
position.signals.values(:,2))/sqrt(nf)
max_err_position_meas = norm(position.signals.values(:,1) - ...
position.signals.values(:,2),Inf)
err_position_est = norm(position.signals.values(:,1) - ...
position.signals.values(:,3))/sqrt(nf)
max_err_position_est = norm(position.signals.values(:,1) - ...
position.signals.values(:,3),Inf)
%
err_theta_meas = norm(theta.signals.values(:,1) - ...
theta.signals.values(:,2))/sqrt(nf)
max_err_theta_meas = norm(theta.signals.values(:,1) - ...
theta.signals.values(:,2),Inf)
err_theta_est = norm(theta.signals.values(:,1) - ...
theta.signals.values(:,3))/sqrt(nf)
max_err_theta_est = norm(theta.signals.values(:,1) - ...
theta.signals.values(:,3),Inf)

% Robust stability analysis of the Pendulum-Cart Control System

%
LQG_design
%
s = tf('s');
Intg = 1/s;
Ts = 0.01; % s
Dintg = c2d(Intg,Ts);
%
% Discretization of uncertain plant model
[M,Delta] = lftdata(G_ulin);
M_d = c2d(M,Ts);
DG_ulin = lft(Delta,M_d);
%
% Discretization of noise shaping filters
DWn = c2d(Wn,Ts);
z = tf('z',Ts);
delay = 1/z;
%
% Closed-loop connection for the discrete-time system
systemnames = ' DG_ulin KF DWn K_opt Dintg delay ';
inputvar = '[ ref; noise{2} ]';
outputvar = '[ DG_ulin ]';
input_to_DG_ulin = '[ -K_opt ]';
input_to_delay = '[ -K_opt ]';
input_to_KF = '[ delay; DG_ulin+DWn ]';
input_to_K_opt = '[ KF(3:6); Dintg ]';
input_to_DWn = '[ noise ]';
input_to_Dintg = '[ ref-KF(3) ]';
dsys_ic = sysic;
%
% Frequency response
omega = logspace(-3,log10(pi/Ts),200);
dclp_g = ufrd(dsys_ic,omega);
%
% Robust stability analysis
opt = robopt('Display','on');
[stabmarg,destabu,report,info_stab] = robuststab(dclp_g,opt);
report
figure(1)
semilogx(info_stab.MussvBnds(1,1),'r-',info_stab.MussvBnds(1,2),'b--')
grid
title('Robust stability')
xlabel('Frequency (rad/sec)')
ylabel('\mu')
legend('\mu-upper bound','\mu-lower bound','Location','NorthWest')
%
% Open-loop connection
systemnames = ' DG_ulin KF DWn K_opt Dintg delay ';
inputvar = '[ err; noise{2} ]';
outputvar = '[ KF(3) ]';
input_to_DG_ulin = '[ -K_opt ]';
input_to_delay = '[ -K_opt ]';
input_to_KF = '[ delay; DG_ulin+DWn ]';
input_to_K_opt = '[ KF(3:6); Dintg ]';
input_to_DWn = '[ noise ]';
input_to_Dintg = '[ err ]';
olp = sysic;
%
% Determine classical and disk stability margins
L = olp(1,1);
[cm,dm] = loopmargin(L)

% Frequency responses of the Pendulum-Cart Control System

% with LQR and Kalman filter
%
% Design of LQ regulator and Kalman filter
LQR_Kalman_design
%
s = tf('s');
Intg = 1/s;
Ts = 0.01; % s
Dintg = c2d(Intg,Ts);
%
[M,Delta] = lftdata(G_ulin);
M_d = c2d(M,Ts);
DG_ulin = lft(Delta,M_d);
%
DWn = c2d(Wn,Ts);
z = tf('z',Ts);
delay = 1/z;
%
% Closed-loop connection
systemnames = ' DG_ulin KF DWn K_opt Dintg delay ';
inputvar = '[ ref; noise{2} ]';
outputvar = '[ DG_ulin; KF; ref-DG_ulin(1); Dintg; -K_opt ]';
input_to_DG_ulin = '[ -K_opt ]';
input_to_delay = '[ -K_opt ]';
input_to_KF = '[ delay; DG_ulin+DWn ]';
input_to_K_opt = '[ KF(3:6); Dintg ]';
input_to_DWn = '[ noise ]';
input_to_Dintg = '[ ref-KF(3) ]';
clp = sysic;
%
% closed-loop frequency response
ref_loop = clp(1,1);
omega = logspace(-2,log10(pi/Ts),200);
figure(1)
bode(ref_loop,'b-',omega), grid
title('Bode plot of the closed-loop system p(z)/r(z)')
%
% magnitude plot of the output sensitivity function
sen_loop = clp(11,1);
omega = logspace(-4,log10(pi/Ts),200);
figure(2)
bodemag(sen_loop,'b-',omega), grid
title('Magnitude plot of the output sensitivity e_p(z)/r(z)')
%
% magnitude plot of the integral component
sen_loop = clp(12,1);
omega = logspace(-4,log10(pi/Ts),200);
figure(3)
bodemag(sen_loop,'b-',omega), grid
title('Magnitude plot of the integral error \int(e_p)(z)/r(z)')
%
% sensitivity of control action to reference and noises
cont_loop1 = clp(13,[1]);
cont_loop2 = clp(13,[2]);
cont_loop3 = clp(13,[3]);
omega = logspace(-4,log10(pi/Ts),500);
figure(4)
bodemag(cont_loop1,'m-.',cont_loop2,'b-',cont_loop3,'c.', ...
omega), grid
axis([10^(-4) pi/Ts -300 100])
title('Sensitivity of control to reference and noises')
legend('Control action due to reference', ...
'Control action due to noise in p', ...
'Control action due to noise in \theta', ...
'Location','SouthEast')
% Design of LQ regulator and Kalman filter for the
% Cart-Pendulum Control System
%
% Plant model
unc_lin_model
%
% Noise shaping filters
k1 = (0.235/2^12)/sqrt(12);
k2 = (2*pi/2^12)/sqrt(12);
Wn1 = k1*tf([5 50],[0.01 1]); %
Wn2 = k2*tf([0.5 10],[0.01 1]); %
%-------------------------------------
Wn = [Wn1 0
0 Wn2];
%-------------------------------------
%
% open-loop connection with the integrator
%
G = G_ulin.Nominal;
Ts = 0.01;
DG = c2d(G,Ts);
A = DG.A;
B = DG.B;
A_opt = [A zeros(4,1) % Add position error integrator
-Ts zeros(1,3) 1.0];
B_opt = [B;0];
%
% Weighting matrices
Q = diag([0.1 1 0.1 1 10])*1000*eye(5);
R = 1000;
%
K_opt = dlqr(A_opt,B_opt,Q,R)
%----------------------------------------------
% Filter design
%
% Open-loop connection with the shaping filters
systemnames = ' G Wn ';
inputvar = '[ control; noise{2} ]';
outputvar = '[ G+Wn ]';
input_to_G = '[ control ]';
input_to_Wn = '[ noise ]';
P = sysic;
%
Ts = 0.01;
Pd = c2d(P,Ts);
[A,B,C,D] = ssdata(Pd);
%
Af = [A [1.0; zeros(5,1)] % Add bias equation
zeros(1,6) 1.0];
Bf = [B(:,1)
0 ];
Cf = [C [0;0]];
Gf = [B(:,2:3) zeros(6,1)
 zeros(1,2) 0.01];
Hf = D(:,2:3);
V = [1 0 0
0 1 0
0 0 1];
W = 10^(-3)*[1 0
0 1];
%
sys = ss(Af,[Bf Gf],Cf,[zeros(2,1) Hf zeros(2,1)],Ts);
[KF,L,P] = kalman(sys,V,W,zeros(3,2))
P

% Simulation of the nonlinear closed-loop Pendulum-Cart system

% with LQ regulator and Kalman filter
%
par_pendulum_cart
%
Ts = 0.01; % s
val_all = [];
sim('clp_LQG_bias',50)
%
figure(1)
plot(position.time,position.signals.values(:,1),'b.', ...
position.time,position.signals.values(:,2),'k--', ...
position.time,position.signals.values(:,3),'r-')
grid on
xlabel('Time (s)')
ylabel('p (m)')
title('Cart position')
legend('Position','Position measurement','Position estimate', ...
'Location','NorthEast')
%
figure(2)
plot(theta.time,theta.signals.values(:,1),'b.', ...
theta.time,theta.signals.values(:,2),'k--', ...
theta.time,theta.signals.values(:,3),'r-')
grid on
xlabel('Time (s)')
ylabel('\theta (deg)')
title('Pendulum angle')
legend('\Theta','\Theta - measurement','\Theta - estimate', ...
'Location','SouthEast')
%
figure(3)
plot(theta.time,theta.signals.values(:,1),'b.', ...
theta.time,theta.signals.values(:,2),'k--', ...
theta.time,theta.signals.values(:,3),'r-')
axis([6 8 -0.2 0.15])
xlabel('Time [s]');
ylabel('\theta [deg]');
title('Pendulum angle')
legend('\Theta','\Theta - measurement','\Theta - estimate', ...
'Location','SouthEast')
grid on
%
figure(4)
plot(control.time,control.signals.values,'r-')
xlabel('Time (s)')
ylabel('u')
title('Control action')
axis([0 50 -0.15 0.15])
grid on
%
[nf,nc] = size(theta.time)
err_position_meas = norm(position.signals.values(:,1) - ...
position.signals.values(:,2))/sqrt(nf)
max_err_position_meas = norm(position.signals.values(:,1) - ...
position.signals.values(:,2),Inf)
err_position_est = norm(position.signals.values(:,1) - ...
position.signals.values(:,3))/sqrt(nf)
max_err_position_est = norm(position.signals.values(:,1) - ...
position.signals.values(:,3),Inf)
%
err_theta_meas = norm(theta.signals.values(:,1) - ...
theta.signals.values(:,2))/sqrt(nf)
max_err_theta_meas = norm(theta.signals.values(:,1) - ...
theta.signals.values(:,2),Inf)
err_theta_est = norm(theta.signals.values(:,1) - ...
theta.signals.values(:,3))/sqrt(nf)
max_err_theta_est = norm(theta.signals.values(:,1) - ...
theta.signals.values(:,3),Inf)

% Robust stability analysis of the Pendulum-Cart Control System

%
LQG_bias_design
%
s = tf('s');
Intg = 1/s;
Ts = 0.01; % s
Dintg = c2d(Intg,Ts);
%
[M,Delta] = lftdata(G_ulin);
M_d = c2d(M,Ts);
DG_ulin = lft(Delta,M_d);
%
DWn = c2d(Wn,Ts);
z = tf('z',Ts);
delay = 1/z;
%
% Closed-loop connection for the discrete-time system
systemnames = ' DG_ulin KF DWn K_opt Dintg delay ';
inputvar = '[ ref; noise{2} ]';
outputvar = '[ DG_ulin ]';
input_to_DG_ulin = '[ -K_opt ]';
input_to_delay = '[ -K_opt ]';
input_to_KF = '[ delay; DG_ulin+DWn ]';
input_to_K_opt = '[ KF(3); KF(4:6); Dintg ]';
input_to_DWn = '[ noise ]';
input_to_Dintg = '[ ref-KF(3) ]';
dsys_ic = sysic;
%
% Frequency response
omega = logspace(-3,log10(pi/Ts),200);
dclp_g = ufrd(dsys_ic,omega);
%
% Robust stability analysis
opt = robopt('Display','on');
[stabmarg,destabu,report,info_stab] = robuststab(dclp_g,opt);
report
%
figure(1)
semilogx(info_stab.MussvBnds(1,1),'r-',info_stab.MussvBnds(1,2),'b--')
grid on
title('Robust stability')
xlabel('Frequency (rad/sec)')
ylabel('\mu')
legend('\mu-upper bound','\mu-lower bound','Location','NorthWest')

% Frequency responses of the Pendulum-Cart Control System

% with LQ regulator and Kalman filter
%
% Design of LQ regulator and Kalman filter
LQG_bias_design
%
s = tf('s');
Intg = 1/s;
Ts = 0.01; % s
Dintg = c2d(Intg,Ts);
%
[M,Delta] = lftdata(G_ulin);
M_d = c2d(M,Ts);
DG_ulin = lft(Delta,M_d);
%
Dwn = c2d(Wn,Ts);
z = tf('z',Ts);
delay = 1/z;
%
% Closed-loop connection
systemnames = ' DG_ulin KF Dwn K_opt Dintg delay ';
inputvar = '[ ref; noise{2} ]';
outputvar = '[ DG_ulin; KF; ref-DG_ulin(1); Dintg; -K_opt ]';
input_to_DG_ulin = '[ -K_opt ]';
input_to_delay = '[ -K_opt ]';
input_to_KF = '[ delay; DG_ulin+Dwn ]';
input_to_K_opt = '[ KF(3:6); Dintg ]';
input_to_Dwn = '[ noise ]';
input_to_Dintg = '[ ref-KF(3) ]';
dclp = sysic;
%
% closed-loop frequency response
ref_loop = dclp(1,1);
omega = logspace(-2,log10(pi/Ts),200);
figure(1)
bode(ref_loop,'b-',omega), grid
title('Bode plot of the closed-loop system p(z)/r(z)')
%
% magnitude plot of the output sensitivity function
sen_loop = dclp(12,1);
omega = logspace(-4,log10(pi/Ts),200);
%
figure(2)
bodemag(sen_loop,'b-',omega), grid
title('Magnitude plot of the output sensitivity e_p(z)/r(z)')
%
% magnitude plot of the integral component
sen_loop = dclp(13,1);
omega = logspace(-4,log10(pi/Ts),200);
figure(3)
bodemag(sen_loop,'b-',omega), grid
title('Magnitude plot of the integral error \int(e_p)(z)/r(z)')
%
% sensitivity of control action to reference and noises
cont_loop1 = dclp(14,[1]);
cont_loop2 = dclp(14,[2]);
cont_loop3 = dclp(14,[3]);
omega = logspace(-4,log10(pi/Ts),100);
figure(4)
bodemag(cont_loop1,'m-.',cont_loop2,'b-',cont_loop3,'c.', ...
omega)
grid on
axis([10^(-4) pi/Ts -250 50])
title('Sensitivity of control to reference and noises')
legend('Control action due to reference', ...
'Control action due to noise in p', ...
'Control action due to noise in \theta', ...
'Location','SouthEast')