Plots

% Chapter 15 Exercise 16.
% The second part of this exercise requires the functions dervabk.m,
% and invp.m
%
%
%
%   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;