% L4_1_statefbk.m - MATLAB source code for Lecture 4.1
% RMM, 26 Oct 03 (updated 22 Oct 06; 20 Oct 2012)
%
% Required files: predprey.m, predprey_rh.m

%%
%% Cart Pendulum system
%%
%% This is the cart pendulum system that we have used in lectures and 
%% homework.  Some of the parameters have been changed and I have derived
%% the equations a bit differently, but the dynamics are basically the
%% same.
%%

M = 5;					% mass of cart, kg
m = 0.5;				% mass of pendulum, kg
b = 0;					% damping of cart, N/m/sec
l = 0.3;				% length of pendulum, m
J = 0.006;				% inertia of pendulum, kg m^2
g = 9.8;				% gravitational constant, kg m/sec^2

% Compute the inertia matrix for the system
InertiaMatrix = [M + m, -m*l; -m*l, J+m*l^2];

% Build the state space representation
Acp = [[zeros(2,2), eye(2,2)]; ...
     [InertiaMatrix \ [0, 0; 0, m*g*l], ...
      InertiaMatrix \ [-b, 0; 0, 0] ]];
Bcp = [[0; 0]; InertiaMatrix \ [1; 0]];
Ccp =[1 -l 0 0];
Dcp = 0;
cartpend=ss(Acp,Bcp,Ccp,Dcp);

% Build up controllability matrix
Wr = [Bcp Acp*Bcp Acp*Acp*Bcp (Acp^3 * Bcp)];

%%
%% Predator Prey example
%%

% Define global functions for use in the feedback
global predprey_K predprey_xd predprey_ud;

% Set paramater values for the simulation
alpha = 12.5;		% multiplier for hare population
beta = 25;		% multipler for lynx population
gamma = 0.8;		% multiplier for time scale

r = 2 * gamma;			% birth rate of hares
d = 0.7 * gamma;		% death rate for lynxes
b = 0.3 * beta / alpha;		% birth coefficient for lynxes
k = 10 * alpha;			% carrying capacity for hares
a = 8 * gamma * alpha / beta;	% interaction coefficient
c = 4 * alpha;			% saturation coefficient

% Compute the equilibrium point (Note: parameters must be set in predprey.m)
f = inline('predprey(0, x)', 'x');
xeq = fsolve(f, [20, 30])'; He = xeq(1); Le = xeq(2);

% Generate the linearization around the equilibrium point
% App, Bpp computed from symbolic expressions (using Mathematica)
App = [
  -((a*c*k*Le + (c + He)^2*(2*He - k)*r)/((c + He)^2*k)), -((a*He)/(c + He));
  (a*b*c*Le)/(c + He)^2, -d + (a*b*He)/(c + He)
];
Bpp = [He*(1 - He/k); 0];
Cpp = [0 1];

% Assign the desired eigenvalues for the system
Kpp = place(App, Bpp, [-0.1; -0.2]);

% Compute the reference input and gain
yd = 30; ref = 0;
krpp = -1 / (Cpp * ((App - Bpp * Kpp) \ Bpp));

% Now set the parameters for our feedback function
predprey_K = Kpp;
predprey_xd = xeq;
predprey_ud = krpp * ref;

% Initial condition and simulation time
x0 = [15, 20];
Tmax = 100;

% Generate a simulation of the system
figure(1); clf; subplot(221);
[t, x] = ode45('predprey', [0 Tmax], x0, [], -r, d, b, k, a, c);
plot(t, x(:,1), '-', t, x(:,2), '--');
xlabel('Time (years)');
ylabel( 'Population', 'Rotation', 90);

lgh = legend('Hare', 'Lynx');
legend(lgh, 'boxoff');
print -dpng 'predprey-sim.png'

% Save the final point we reached
xfin = x(length(t), :);

% Generate the phase portrait
figure(2); clf; subplot(221); hold on;
phaseplot('predprey', [0 50 5], [0 50 6], 0.2, [], ...
 0, -r, d, b, k, a, c); 
phaseplot('predprey', [0 50 5], [60 100 4], 0.13, [], ...
 0, -r, d, b, k, a, c);
phaseplot('predprey', [60 100 4], [0 50 6], 0.15, [], ...
 0, -r, d, b, k, a, c);
phaseplot('predprey', [60 100 4], [60 100 4], 0.1, [], ...
 0, -r, d, b, k, a, c);
ah = phaseplot('predprey', ...
  [0 100 10], [0 100 10], 0, ...
  [ 0.5 10; 100 45; 100 70 ], ...
  Tmax/2, -r, d, b, k, a, c);
set(ah, 'LineWidth', 2);
xlabel('Hares'); ylabel('Lynxes', 'Rotation', 90);
axis([0 100 0 100]); set(gca, 'DataAspectRatio', [1, 1, 1]);

hold on; plot(xfin(1), xfin(2), '.');
print -dpng predprey-pp.png;

% Check the linearization to make sure that computations are OK
eqchk = xeq - (App - Bpp * Kpp) \ (Bpp * krpp * r)

