% Chapter 15 Exercise 16.
% The second part of this exercise requires the functions dervabk.m,
% and invp.m, listed below:
%
%
%
% function xd = dervabk(t,x);
% % this function returns the derivatives of the unit feedback system
% % described by the equation:
% % x' = A*x + B*u
% % where u = -K*x
% % The calling function must contain the definition of the
% % matrixes A B K and the statement
% % global A B K;
% % Delete next line, if you are using a MATLAB version prior to 4.0.
% global A B K
% u = -K*x;
% xd = A*x + B*u;
% end; % of function dervabk
% function wd=invp(t,w);
% % inverted pendulum on a cart.
% % This function simulates the dynamics of an inverted pendulum on cart.
% % The states are:
% % w(1) position of the trolley, x
% % w(2) its derivative, x_dot
% % w(3) angle of the pendulum from the vertical th
% % w(4) its derivative th_dot
% % The array K determines the external force u=-K*w applied to the cart
% % and must be defined and declared global in the calling function
% % The physical laws that define the system are:
% %
% % (M+m) x_dd + ml cos(th) th_dd = ml sin(th) (th_d)^2 + F
% % ml cos(th) x_dd + m l^2 th_dd = mgl sin(th)
%
% % --- define parameters of the system
% g = 9.81; % gravity acceleration, m/s^2
% l = 0.5; % length of the pendulum, m
% M = 1.0; % mass of the cart, Kg
% m = 0.1; % mass of the pendulum
% % --- compute external force
% % if you are using a version of MATLAB prior to 4.0
% % please delete next line
%
% global K;
% F =-K*w;
%
% % --- compute second derivatives x_dd and th_dd
% x = w(1);
% x_d = w(2);
% th = w(3);
% th_d = w(4);
% G = [(M+m) m*l*cos(th);
% m*l*cos(th) m*(l^2) ];
% H = [m*l*(sin(th))*(th_d)^2 + F ;
% m*g*l*sin(th) ];
% Drv = G\H; % [x_dd; th_dd]
% [m,n] = size(w); wd=zeros(m,n);
% wd(1) = w(2);
% wd(2) = Drv(1);
% wd(3) = w(4);
% wd(4) = Drv(2);
end; % of function invp.m
%
% --- define parameters of the problem:
g = 9.81; % gravity acceleration in m/s^2
M =1; % mass of the cart, Kg
m = 0.1; % mass of the pendulum, Kg
l = 0.5; % length of the pendulum, m
% ---part a), method 1.
% --- The linearized equations are:
% (M+m) * x_ddot + m*l * th_ddot = u
% m*l * x_ddot + m*l^2 * th_ddot = m*g*l * th
% The above system can be solved in closed form:
% x_ddot = -m*g/M * th + 1/M * u
% th_ddot = (M+m)/(M*l) * g * th - 1/(M*l) * u
% These two equations, together with the relations:
% w(1)' = w(2)
% w(3)' = w(4)
% provide a first solution to the linearization problem,
% the matrixes A1 and B1
% --- solution by method 1.
A1= [0 1 0 0;
0 0 -m*g/M 0;
0 0 0 1;
0 0 (M+m)*g/(M*l) 0];
B1= [0;
1/M;
0;
-1/(M*l)];
% ---part a), method 2.
% Alternatively, we can let MATLAB do the work of solving the linear
% system numerically, providing the solutions A2 and B2:
% --- describe the system in the form:
% E*w'= G*w + H*u
E = [ 0 (M+m) 0 m*l;
0 m*l 0 m*l^2;
1 0 0 0;
0 0 1 0];
G = [ 0 0 0 0;
0 0 m*g*l 0;
0 1 0 0;
0 0 0 1];
H = [ 1;
0;
0;
0];
A2 = E\G;
B2 = E\H;
% Yoy may check that, possibly up to numerical errors of the
% processor A1=A2 and B1=B2. Let us choose one of the solutions
% and display it.
A = A1
B =B1
disp(' To produce K, press any key ... '); pause
% --- part b). Find the gain.
Q = eye(4,4);
R = 0.1;
K = lqr(A,B,Q,R);
% --- display K
K
disp(' To simulate the linearized system, press any key ... '); pause
% --- part c). Simulate the linearized system
global A B K;
t0 = 0; tf=5;
w0 = [0.2;
0.05;
0.1;
0.15];
[t_lin,w_lin] = ode23('dervabk',t0, tf, w0);
x_lin = w_lin(:,1);
th_lin = w_lin(:,3);
subplot(2,1,1);
plot(t_lin, x_lin); grid;
title('Linearized system');
ylabel('x_lin');
subplot(2,1,2);
plot(t_lin, th_lin); grid
ylabel('th_lin');
% --- part d). Simulate the non-linear system
disp(' To simulate the non-linear system, press any key ... '); pause
[t,w] = ode23('invp',t0, tf, w0);
x = w(:,1);
th = w_lin(:,3);
clg
subplot(2,1,1);
plot(t,x); grid;
title('Non-linear system');
ylabel('x');
subplot(2,1,2);
plot(t,th); grid
ylabel('th');
% --- part e). Compare linearized and non-linear systems.
disp(' To compare the two systems, press any key ... '); pause
clg
subplot(2,1,1);
plot(t_lin,w_lin(:,1), t, w(:,1));
ylabel(' Position of the cart'); grid;
title(' Comparison near the linearization point');
subplot(2,1,2);
plot(t_lin,w_lin(:,3), t, w(:,3));
ylabel(' Angle of the pendulum'); grid;
% --- part f). Show an appeciable difference between the two systems.
disp(' To compare the two systems around w=[0 0 0.5 0], press any key ...');
pause;
w0 = [0.0; 0.0; 0.5; 0.0];
[t_lin,w_lin] = ode23('dervabk',t0, tf, w0);
[t,w] = ode23('invp',t0, tf, w0);
clg
subplot(2,1,1);
plot(t_lin,w_lin(:,1), t, w(:,1));
ylabel(' Position of the cart'); grid;
title(' Comparison away from the linearization point');
subplot(2,1,2);
plot(t_lin,w_lin(:,3), t, w(:,3));
ylabel(' Angle of the pendulum'); grid;
