%% Laboratory notes: Introduction to the Control System Toolbox (CST)
% Riccardo Antonello (riccardo.antonello@unipd.it) and Giulia Michieletto (giulia.michieletto@unipd.it)
% March 04, 2024
% Dept. of Management and Engineering, University of Padova, Italy
%
%
% This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International license.
%
%% Contents
% Introduction
% Definition of continuous-time LTI models
% Conversion between LTI model representations
% Definition of discrete-time LTI models
% Conversion between LTI model representations (discrete-time case)
% Connections of LTI models
% Time-domain analysis of continuous-time LTI models
% Frequency-domain analysis of LTI models
% Stability analysis of LTI models
% Model discretization
% Control design by state space methods
%
%% 1.  Introduction
% The Control System Toolbox (CST) is a MATLAB toolbox conceived for the analysis, design and tuning of linear control systems.
% Only linear, time-invariant  (LTI) systems are supported by CST.
% Systems can be either scalar (SISO) or multivariable (MIMO); both continuous and discrete time systems are supported (but not "hybrid time" or "multi-rate" systems).
%
%
%% 2.  Definition of continuous-time LTI models
% The CST uses custom data structures called LTI objects to store model-related data.
% An LTI object is an object according to the commondefinition of the object-oriented programming paradigm, namely a data structure that encapsulates both data and specialized functions operating on such data.
% An LTI object can be defined by starting from different representations of the same dynamical model. Accepted representations are:
% Transfer function numerator / denominator (TF)
% Transfer function zeros, poles and gain (ZPK)
% State space model (SS)
% Frequency response data (FRD)
%
% Consider the continuous-time LTI model:
% G(s) = \frac{5 s + 50}{s^2 + 101s + 100} = 5\,\frac{s+10}{(s+1)(s+100)}
%
% 1) Definition by using a TF object:

%   non-encapsulated TF data
num = 5 * [1, 10];
den = [1, 101, 100];                        %   alternative: den = conv([1,1], [1,100]);

%   TF data encapulated in a TF object
sysG1 = tf(num,den)

%
% Note:  an LTI object encapsulates all the relevant information pertaining the representation (in a certain form) of an LTI model.
% It contains several data fields that can be accessed by using the "dot" notation.

%  show all the LTI object fields
get(sysG1)  

%   access a particular field
sysG1.Numerator{1}          

%
% Alternative: use the LTI object for the Laplace variable "s", and the overloaded operators "+" (addition), "*" (multiplication), "/" (division) for the class of LTI objects.

%   define the Laplace variable "s" (LTI object)
s = tf('s');

%   define the TF object by using the "s" variable and the +, *, / operators
sysG2 = 5*(s+10)/((s+1)*(s+100))

%
% 2) Definition by using a ZPK object:

%   non-encapsulated ZPK data
z = -10;
p = [-1, -100];
k = 5;

%   ZPK data encapulated in a ZPK object  
sysG3 = zpk(z,p,k)

%
% 3) Definition by using a SS object:
% Realization of the LTI model in controllable canonical form:
% \begin{array}{ll}
% A = \left[
% \begin{array}{cc}
% 0 & 1\\
% -100 & -101
% \end{array}
% \right]\,,
% \qquad
% &
% B = \left[
% \begin{array}{c}
% 0 \\ 1
% \end{array}
% \right] \\ \\
% %%%
% C = \left[
% \begin{array}{cc}
% 50 & 5
% \end{array}
% \right]\,,
% &
% D = 0
% \end{array}

%   non-encapsulated SS data
A = [0, 1; -100, -101];
B = [0; 1];
C = [50, 5];
D = 0;

%   SS data encapulated in a SS object
sysG4 = ss(A, B, C, D)

%
%% 3.  Conversion between LTI model representations
%
% 1) Conversion between different representations in encapsulated form (LTI objects):
%
% TF object \rightarrow ZPK / SS object

%   remind: sysG1 is a TF obj
sysG1

%   from TF to ZPK object
sysG1a = zpk(sysG1)  

%   from TF to SS object
sysG1b = ss(sysG1)

%
% ZPK object \rightarrow TF / SS object

%  remind: sysG3 is a ZPK obj 
sysG3 

%  from ZPK to TF object
sysG3a = tf(sysG3)  

%  from ZPK to SS object
sysG3b = ss(sysG3)

%
% SS object \rightarrow TF / ZPK object

% remind: sysG4 is a SS obj
sysG4  

%  from SS to TF object
sysG4a = tf(sysG4)  

%  from SS to ZPK object
sysG4b = zpk(sysG4)

%
% 2)  Conversion between different representations in non-encapsulated form:
%
% TF data \rightarrow ZPK / SS data

%  TF data 
num = 5 * [1, 10];
den = [1, 101, 100];

%  from TF to ZPK data
[z,p,k] = tf2zp(num,den)                 

%  from TF to SS data
[A,B,C,D] = tf2ss(num,den)              

%
% ZPK data \rightarrow TF / SS data

%  ZPK data
z = -10;
p = [-1, -100];
k = 5;

%  from ZPK to TF data
[num,den] = zp2tf(z,p,k)                

%  from ZPK to SS data
[A,B,C,D] = zp2ss(z,p,k)                  

%
% SS data \rightarrow TF / ZPK data

%  SS data
A = [0, 1; -100, -101];
B = [0; 1];
C = [50, 5];
D = 0;

%  from SS to TF data
[num,den] = ss2tf(A,B,C,D)                

%  from SS to ZPK data
[z,p,k] = ss2zp(A,B,C,D)               

%
% Note: tf2ss returns a realization similar to a controllable canonical form (with the characteristic polynomial on the first row of A, instead of last row); zp2ss does not return a controllable canonical form.
%
% 3)  From encapsulated form to non-encapsulated form:
%

%  TF object
sysG1

%  from TF object to TF data
[num,den] = tfdata(sysG1, 'v')          

%

%  ZPK object
sysG2

%  from ZPK object to ZPK data
[z,p,k]   = zpkdata(sysG2, 'v')         

%
% Note: without the 'v' option, the return values of tfdata and zpkdata are cell arrays.
%

%  SS data
sysG3

%  from SS object to SS data
[A,B,C,D] = ssdata(sysG3)               %#ok<ASGLU>

%
% 4)  Conversion between different state-space representations (encapsulated form):
%
% generic state transformation
% Given the state-space model \Sigma = (A,B,C,D) and the state transformation x'=T x, obtain the new state-space realization \Sigma' = (A', B', C', D') = (T A T^{-1}, TB, C T^{-1}, D):
%

%  SS object
sysG4

%  state transformation matrix
T = [1, 1; 1, -1];    

%  convert to new state z=T*x
sysG5 = ss2ss(sysG4, T)     

%
% Conversion to special state-space representations


% convert to Jordan normal form
sysG6 = canon(sysG5, 'modal')   

% similar (but not equal) to observable canonical form
sysG7 = canon(sysG5, 'companion')   

% convert to balanced realization
sysG8 = balreal(sysG5)               

%
%
%% 4.  Definition of discrete-time LTI models
% A discrete-time LTI object can be defined by resorting to the same CST routines used for the continous-time case (e.g. tf, zpk, ss).
% A sampling period Ts > 0 must be usually specified in the definition of a discrete-time LTI object. To define a discrete-time LTI object with an "unspecified" sampling period, use the fictitious sampling period Ts = -1.
% A continuous-time LTI object has by default a sampling period Ts = 0.
%
% Consider the discrete-time LTI model:
% H(z) = 5\,\frac{z-0.3}{(z-0.1)(z+0.2)}
% with sampling period T_s = 1\,s.
%
% 1) Definition by using a TF object:

%  sampling time
Ts = 1;

%  non-encapsulated TF data
num = 5 * [1, -0.3];
den = conv([1, -0.1], [1, 0.2]);

%  TF data encapsulated in a TF object
sysH1 = tf(num, den, Ts) 

% Alternative: use the LTI object for the Z-transform variable "z"

%   define the Z-transform variable "z" (LTI object)
z = tf('z', Ts);

%   define the TF object by using the "z" variable and the +, *, / operators
sysH2 = 5*(z-0.3)/((z-0.1)*(z+0.2))

% Note: in digital signal processing (DSP), it is customary to write transfer functions as rational expressions in the z^{-1} variable, and to order the numerator and denominator in ascending powers of such variable, e.g.
% H(z^{-1}) = \frac{5 z^{-1} - 1.5 z^{-2}}{1 + 0.1 z^{-1} - 0.02 z^{-2}}
% Use one of the following methods to define a discrete-time LTI in the DSP-like tf form:

% by using "filt"
sysH3a = filt([0, 5, -1.5], [1, 0.1, -0.02], Ts)

% by using "tf" and specifying the variable "z^-1" 
sysH3b = tf([0, 5, -1.5], [1, 0.1, -0.02], Ts, 'Variable', 'z^-1')

% by using the variable "z" defined before
sysH3c = (5*z^-1 - 1.5*z^-2)/(1 + 0.1*z^-1 - 0.02*z^-2)

%
% 2) Definition by using a ZPK object:

%  non-encapsulated ZPK data
z = 0.3;
p = [0.1, -0.2];
k = 5;

%  ZPK data encapsulated in a ZPK object
sysH4 = zpk(z, p, k, Ts)

%
% 3) Definition by using a SS object:
% Realization of the LTI model in reachable canonical form:
% \begin{array}{ll}
% A = \left[
% \begin{array}{cc}
% 0 & 1\\
% -0.02 & 0.1
% \end{array}
% \right]\,,
% \qquad
% &
% B = \left[
% \begin{array}{c}
% 0 \\ 1
% \end{array}
% \right] \\ \\
% %%%
% C = \left[
% \begin{array}{cc}
% -1.5 & 5
% \end{array}
% \right]\,,
% &
% D = 0
% \end{array}

%  non-encapsulated SS data
A = [0, 1; -0.02, 0.1];
B = [0; 1];
C = 5 * [-0.3, 1];
D = 0;

%  SS data encapsulated in a SS object
sysH5 = ss(A, B, C, D, Ts)

%
%% 5.  Conversion between LTI model representations (discrete-time case)
% Conversions between discrete-time LTI model representations are performed similarly to the continuous-time case.
% With encapsulated data: use tf, zpk, ss
% With non-encapsulated data: use tf2zp, tf2ss, zp2ss, …
%
%% 6.  Connections of LTI models
% Consider the systems:
% G_1(s) = \frac{1}{s(s+1)}\,,\qquad G_2(s) = \frac{s+1}{s+2}

sysG1 = zpk([], [0, -1], 1)
sysG2 = tf([1,1], [1,2])

%
%  Connection of LTI models in encapsulated form:
% 1) Parallel connection:
%
% G_p(s) = G_1(s) + G_2(s) = \frac{1}{s(s+1)} +\; \frac{s+1}{s+2} = \frac{s^3 + 2s^2 + 2s + 2}{s(s+1)(s+2)}
% use the operator " + "

sysGp = sysG1 + sysG2   

%  ... or the function parallel of the CST

sysGp = parallel(sysG1, sysG2)

%
% 2) Series connection:
%
% G_s(s) = G_1(s)\,G_2(s) = \frac{s+1}{s(s+1)(s+2)} = \frac{1}{s(s+2)}
% use the operator "*"

sysGs = sysG1 * sysG2

%  ... or the function series of the CST

sysGs = series(sysG1, sysG2)

%
% Note: use minreal (minimal realization of a given model) to simplify common factors between numerator and denominator:

sysGs = minreal(sysGs)

%
% 3) Feedback connection: use the function feedback
%
% G_{f1}(s) = \frac{\frac{1}{s(s+1)}}{1+\frac{1}{s(s+2)}} = \frac{s+2}{(s+1)^3}

% sysG1 in feedforward path
% sysG2 in feedback path 
% (default: negative feedback)
sysGf1 = feedback(sysG1, sysG2)

%
%
% G_{f2}(s) = \frac{\frac{1}{s(s+2)}}{1+\frac{1}{s(s+2)}} = \frac{1}{(s+1)^2}

% sysG1*sysG2 in feedforward path 
% unitary feedback 
% (default: negative feedback)
sysGf2 = feedback(sysG1*sysG2, 1)
sysGf2 = minreal(sysGf2, 1e-5)

%
% Connection of LTI models in non-encapsulated form:
% parallel, series and feedback also accept LTI objects specified in non-encapsulated form:

%  get TF data from TF objects
[numG1, denG1] = tfdata(sysG1, 'v');
[numG2, denG2] = tfdata(sysG2, 'v');

%  evaluate interconnections of LTI models in non-encapsulated form
[numGp, denGp] = parallel(numG1, denG1, numG2, denG2);
[numGs, denGs] = series(numG1, denG1, numG2, denG2);
[numGf1, denGf1] = feedback(numG1, denG1, numG2, denG2);

%  get SS data from SS objects
[A1, B1, C1, D1] = ssdata(sysG1);
[A2, B2, C2, D2] = ssdata(sysG2);

%  evaluate interconnections of LTI models in non-encapsulated form
[Ap, Bp, Cp, Dp] = parallel(A1, B1, C1, D1, A2, B2, C2, D2);
[As, Bs, Cs, Ds] = series(A1, B1, C1, D1, A2, B2, C2, D2);
[Af1, Bf1, Cf1, Df1] = feedback(A1, B1, C1, D1, A2, B2, C2, D2);

%
% Connections of discrete-time LTI objects
% Connections of discrete-time LTI objects can be performed similarly to the continuous-time case.
% All the operations must be performed on LTI objects with the same sampling period (or with an unspecified sampling period).
%
%
%% 7. Time-domain analysis of LTI models
% Consider the LTI model:
% G(s) = \frac{5}{s+2}

sysG = tf(5, [1, 2]);

%
% Impulse response
% For the impulse response, use impulse :

% display the impulse response 
% on an interactive plot
figure;
impulse(sysG);  
grid on;

% returns the impulse response g evaluated at the time instants t
% (automatically determined by impulse; can be specified as 2nd argument)
[g, t] = impulse(sysG);

% compare result with expected analytical response
ga =  5 * exp(-2*t);

figure;
plot(t, g, 'b.-');
hold on;
plot(t, ga, 'r');
grid on;
xlabel('Time [s]');
title('sysG - impulse response');
legend('with impulse', 'analytical response');

%
% Step response
% For the (unit) step response, use step  :

% display the step response 
% on an interactive plot
figure;
step(sysG);
grid on;

% returns the impulse response y evaluated at the time instants t
% (automatically determined by step; can be specified as 2nd argument)
[y, t] = step(sysG);

% compare result with expected analytical response
ya = (5/2)*(1-exp(-2*t));

figure;
plot(t, y, 'b.-');
hold on;
plot(t, ya, 'r');
xlabel('Time [s]');
title('sysG - unit step response');
legend('with step', 'analytical response');
grid on;

%
% With a unit step input, the system output approaches the value 2.5, which corresponds to the system DC gain.
% Use dcgain to get the DC gain of an LTI model:

dcgain(sysG)

%
% Use stepinfo to get other relevant information regarding the step response :

stepinfo(sysG)

%
% Natural response
% For the natural output response from specfied initital conditions, use initial.
% Note: it can be only applied to LTI objects in ss form.

% ss realization
A = -2; B = 5; C = 1; D = 0;
sysG = ss(A,B,C,D);

% display the natural ouput response 
% on an interative plot
x0 = 5;  % initial state
figure;
initial(sysG, x0);
grid on;

% returns the natural response yn evaluated at the time instants t
% (automatically determined by initial; can be specified as 2nd argument)
[yn, t] = initial(sysG, x0);

% compare result with expected analytical response
yna = C*exp(-2*t)*x0;

figure;
plot(t, yn, 'b.-');
hold on;
plot(t, yna, 'r');
xlabel('Time [s]');
title('sysG - natural response');
legend('with initial', 'analytical response');
grid on;

%
% Forced response to generic forcing input
% For the forced response to a generic input, use lsim .
% Note: for LTI objects in ss form, the routine can be used to compute the natural + forced response, by specifying the initial condition as an extra parameter.

t = 0:0.1:15;   % time instants
u = sin(2*t);   % input signal (alt.: use "gensig")
x0 = 0;         % init state

% display the forced response 
% on an interactive plot
figure;
lsim(sysG, u, t, x0);
grid on;

% returns the forced response y 
% evaluated at the time instants t
y = lsim(sysG, u, t, x0);

figure;
plot(t, u, 'k--');
hold on;
plot(t, y, 'b');
xlabel('Time [s]');
title('sysG - forced response');
legend('input', 'output');
grid on;

%
% Time-domain analysis of discrete-time LTI models
% The time-domain analysis of discrete-time LTI models can be carried out with the same routines of the continuous-time case.
% The analysis is performed at the sampling instants, i.e. at the time instants multiple of the sampling period specified in the discrete-time LTI object.
% For evaluating the forced response of a discrete-time system to a generic input, the filter routine can be used instead of lsim.
%
% Consider the discrete-time LTI model (filter) :
% F(z) = \frac{5}{z-0.8}
% with sampling period T_s = 0.1\,s. Determine the forced time-response to a sinusoidal input with unit amplitude and frequency \omega = 2\,\mathrm{rad/s}.

Ts = 0.1;      % sampling period
t = 0:Ts:15;   % sampling instants 
u = sin(2*t);  % input signal

numF = 5;
denF = [1, -0.8];
y = filter(numF, denF, u);

figure;
stairs(t, u, 'k--');
hold on;
stairs(t, y, 'b');
xlabel('Time [s]');
title('sysF - forced response');
legend('input', 'output');
grid on;

%
%% 8.  Frequency-domain analysis of LTI models
% Bode plots
% Use freqresp to get the frequency response on specific points in the frequency domain.

% freq points (uniformly spaced in log scale) 
w = logspace(-1, 1, 100); 

%  evaluate the frequency response (alternative: use evalfr)
frG = freqresp(sysG, w);

%
% frG is a multidimensional array (num of inputs \times num of outputs \times length of array w) containing the (complex) values of the frequency response, evaluated at the frequency points specified in w (in [rad/s]).
% In the SISO case, use squeeze to get the single non-empty dimension of frG.

whos frG
frG = squeeze(frG);
whos frG

%
% The magnitude and phase of frG can be plotted vs frequency w on semi-logarithmic scale, obtaining the Bode plots of sysG:

%  get magnitude and phase
magG = abs(frG);
phaG = angle(frG);

%  convert to dB and deg units
magG_dB = 20*log10(magG);
phaG_deg = phaG * 180/pi;

%  Bode plot
figure;
subplot(2,1,1);
semilogx(w, magG_dB);
ylabel('Magnitude [dB]');
xlabel('Frequency [rad/s]');
grid on;

subplot(2,1,2);
semilogx(w, phaG_deg);
ylabel('Phase [deg]');
xlabel('Frequency [rad/s]');
grid on;

%
% A more convinient method to sketch the Bode plot consists of using  bode :

% display the Bode diagram on an interactive plot
figure;
bode(sysG);

% get the frequency response (mag [absolute value] + phase [deg]) 
% evaluated at the freq points w (in [rad/s])
[magG, phaG_deg] = bode(sysG, w); 

%
% Note: bode plots the "real" Bode plot, NOT the "asymptotic" Bode plot.
% No CST routines are available to sketch the asymptotic Bode plot. However, some public-domain routines for drawing the asymptotic Bode plot are available at the Matlab Central website www.mathworks.com/matlabcentral/.

%   asymptotic Bode plot with "bodeasymp" routine
figure;
bodeasymp(sysG);

%   asymptotic Bode plot with "bode_asymptotic" routine
[numG, denG] = tfdata(sysG, 'v');
figure;
bode_asymptotic(numG, denG);

%
% Nyquist plots
% For the Nyquist plot, use nyquist :

% display the Nyquist diagram on an interactive plot
figure;
nyquist(sysG);

% get the the real/imag part of the frequency response 
% evaluated at the freq points w (in [rad/s])
[ReG, ImG] = nyquist(sysG, w); 

%
% Relevant parameters of the frequency response
% Other relevant details of the frequency response can be obtained by using bandwidth, getPeakGain and getGainCrossover:

% define a 2nd order sys with under-damped (i.e. resonant) poles
wn = 1;     % natural freq
d = 0.1;    % damping factor
sysG2 = tf(wn^2, [1, 2*d*wn, wn^2])

% 3dB-bandwidth (in [rad/s])
bandwidth(sysG2)  

% peak gain of frequency resp (in absolute value)
getPeakGain(sysG2)  

% 0dB-crossover freq (in [rad/s])
getGainCrossover(sysG2, 1)  

%
% Frequency-domain analysis of discrete-time LTI models
% The frequency-domain analysis of discrete-time LTI models can be carried out with the same routines of the continuous-time case.
%
% Interactive model analysis
% ltiview provides an interactive tool to sketch and analyze time/frequency responses of multiple LTI models.
% Use the option File/Import ... from the LTI Viewer window menu to import an LTI model defined in the Matlab workspace.
% By right-clicking over the plot, a pop-up menu opens: from that menu, it is possible to select:
% the kind of analysis to perform (time response analysis, frequency response analysis),
%  the type of excitation signal to consider (impulse, step),
% the response characteristics to show in the plot (overshoot, settling time, rise time, etc.).

ltiview

%
%
%% 9.  Stability analysis of LTI models
% Poles and zeros
% Use pole and zero to retrieve the poles and zeros of an LTI model specified in encapsulated form (i.e. an LTI object) :

numG = [1, 15, 50];
denG = [1, 10, 47, 142, 250, 200, 0];
sysG = tf(numG, denG);

p1 = pole(sysG)
z1 = zero(sysG)  % also available: [z,k] = zero(sysP)

%
% For an LTI model specified in non-encapsulated form, use roots to evaluate the roots of the tf numerator / denominator (i.e. zeros / poles):

p2 = roots(denG)
z2 = roots(numG)

%
% Use eig to evaluate the eigenvalues of an LTI model specified in non encapsulated ss form:

A = ssdata(sysG);
eig(A)

%
% Use damp to get the natural frequency \omega_n and the damping factor \delta of the LTI model poles / eigenvalues.
% Remark: for a pair of complex-conjugate poles (i.e. resonant poles), it holds that :
% p_{1,2} = -\delta\omega_n \,\pm\, j\,\omega_n\,\sqrt{1-\delta^2}
% so that \omega_n = |p_{1,2}| and \delta = -\Re(p_{1,2})/\omega_n (with \Re(\cdot) denoting the real part of the complex number).

% natural freq and damping factor of 
% system poles using damp
% note: poles are sorted by increasing freq
[wn1, d1] = damp(sysG)

% compare with expected results
wn2 = abs(p1);                  % natural freqs  
d2 = -real(p1)./wn2;            % damping factors
[~, ks] = sort(wn2,'ascend');   % sort wrt increasing freq
wn2 = wn2(ks)                   % sorted natural freqs
d2 = d2(ks)                     % sorted damping factors  

%
% Use esort to sort the continuous-time poles/eigenvalues according to their real parts; use dsort to sort the discrete-time poles according to their magnitude:

esort(p1)

%
% Use pzmap to plot the poles ( \times ) and zeros ( \bigcirc ) of an LTI model on the complex plane.
% Use sgrid (zgrid in the discrete-time case) to superimpose a grid showing the constant damping ratio and natural frequency loci.

pzmap(sysG);
axis equal;
sgrid;

%
% Root locus
% Use rlocus(sys) to sketch the root locus of the negative feedback loop consitsing of the model sys and a proportional controller with gain K ranging from 0 to +\infty ("positive" root locus).
% Remark: if L(s)=B(s)/A(s) denotes the loop transfer function of the (negative) feedback loop, the root locus shows how the roots of the characteristic polynomial (i.e. poles of the closed-loop system):
% p(s) = A(s) + K\,B(s)
% moves on the complex plane as K varies from 0 to +\infty.

% show the root locus 
% on the complex plane
figure;
rlocus(sysG);

% return the root locus points at different gain values
K = [0.1, 1, 10];
R = rlocus(sysG, K)  % out: one column for each gain value

%
% Stability margins
% Use margin(sys) to get the stability margins (gain/phase) of the model sys.

% show the stability margins on the Bode plot
figure;
margin(sysG);

% return the stability margins
[gm, phim, wgc, wcp] = margin(sysG)

%
%
%% 10. Model discretization
% Use c2d to discretize a given continuous-time model with one of the following methods :
% zero-order hold (zoh) equivalent: get exact discretization in time domain for staircase inputs.
% first-order hold (foh) equivalent: get exact discretization in time domain for piecewise linear inputs.
% impulse-invariant (impulse) equivalent: get exact discretization for impulse train inputs
% Tustin (or bilinear) approximation (tustin): yields the best frequency-domain match between the continuous-time and discretized systems.
% zero-pole matching equivalent: yields a discrete-time equivalent with same DC gain and poles/zeros related to their continuous-time counterparts by the transformation z = e^{s T}

% continuous-time model
sysG = tf([-1 1], [1, 2*0.3, 1]);

% discretizations
Ts = 0.4;
sysGd1 = c2d(sysG, Ts, 'zoh')
sysGd2 = c2d(sysG, Ts, 'foh')
sysGd3 = c2d(sysG, Ts, 'impulse')
sysGd4 = c2d(sysG, Ts, 'tustin')
sysGd5 = c2d(sysG, Ts, 'matched')

%
% Note: the c2d routine does not implement the Forward and Backward Euler methods.
% A convenient way to compute the discrete equivalents with the Forward or Backward Euler discretized models is as follows:

% define z-domain variable
z = tf('z', Ts);

% define Forward Euler (FE) transformation
s = (z-1)/Ts;

% obtain FE discrete equivalent
sysGd6 = (-s+1)/(s^2 + 2*0.3*s + 1);
sysGd6 = minreal(sysGd6)
% define Backward Euler (BE) transformation
s = (1-z^-1)/Ts;

% obtain BE discrete equivalent
sysGd7 = (-s+1)/(s^2 + 2*0.3*s + 1);
sysGd7 = minreal(sysGd7)

%

% compare frequency responses
figure;
subplot(2,3,1);
bode(sysG, sysGd1, sysGd2);
grid on;
legend('CT', 'zoh', 'foh');

subplot(2,3,2);
bode(sysG, sysGd3);
grid on;
legend('CT', 'impulse');

subplot(2,3,3);
bode(sysG, sysGd4);
grid on;
legend('CT', 'tustin');

subplot(2,3,4);
bode(sysG, sysGd5);
grid on;
legend('CT', 'matched');

subplot(2,3,5);
bode(sysG, sysGd6);
grid on;
legend('CT', 'FE');

subplot(2,3,6);
bode(sysG, sysGd7);
grid on;
legend('CT', 'BE');

%

% compare step responses
figure;
subplot(2,3,1);
step(sysG, sysGd1, sysGd2);
grid on
legend('CT', 'zoh', 'foh')

subplot(2,3,2);
step(sysG, sysGd3);
grid on;
legend('CT', 'impulse');

subplot(2,3,3);
step(sysG, sysGd4);
grid on;
legend('CT', 'tustin');

subplot(2,3,4);
step(sysG, sysGd5);
grid on;
legend('CT', 'matched');

subplot(2,3,5);
step(sysG, sysGd6);
grid on;
legend('CT', 'FE');

subplot(2,3,6);
step(sysG, sysGd7);
grid on;
legend('CT', 'BE');

%

% compare impulse responses
figure;
subplot(2,2,1);
impulse(sysG, sysGd1, sysGd2);
grid on
legend('CT', 'zoh', 'foh');

subplot(2,2,2);
impulse(sysG, sysGd3);
grid on;
legend('CT', 'impulse');

subplot(2,2,3);
impulse(sysG, sysGd4);
grid on;
legend('CT', 'tustin');

subplot(2,2,4);
impulse(sysG, sysGd5);
grid on;
legend('CT', 'matched');

subplot(2,3,5);
impulse(sysG, sysGd6);
grid on;
legend('CT', 'FE');

subplot(2,3,6);
impulse(sysG, sysGd7);
grid on;
legend('CT', 'BE');

%
% Note: to compensate for the frequency warping of the Tustin transformation (frequency prewarping), use c2dOptions to specify the prewarp frequency (PrewarpFrequency).

Ts = 1.2;
sysGd6 = c2d(sysG, Ts, 'tustin');

opts = c2dOptions('Method', 'tustin', 'PrewarpFrequency', 1);
sysGd6_prw = c2d(sysG, Ts, opts);

figure;
bode(sysG, sysGd6, sysGd6_prw);
legend('CT', 'tustin', 'tustin + prewarp')

%
%% 11. Control design by state space methods
% Controllability (reachability) and observability analysis
% ctrb and obsv return the controllability (reachability) and observability matrices.
% ctrbf returns the decomposition into controllable/uncontrollable subspaces; obsvf returns the decomposition into observable/unobservable subspaces.
%
% Consider the two-mass system with infrastructural actuator shown in Fig.1 below.
%
% Figure 1. Two-mass system with infrastructural actuator.
%
% The state-space model with state vector \mathbf{x} = [x_1,\,\dot{x}_1,\,x_2,\,\dot{x}_2]^T is:
% \mathbf{A}_1 =
% \left[
% \begin{array}{cccc}
% 0 & 1 & 0 & 0 \\
% -k/m_1 & 0 & k/m_1 & 0 \\
%  0 & 0 & 0 & 1 \\
%  k/m_2 & 0 & -k/m_2 & 0
% \end{array}
% \right],      \mathbf{B}_1 = \left[
% \begin{array}{c}
% 0  \\  1/m_1  \\  0  \\ 1/m_2
% \end{array}
% \right]

%  state-space model (state = [x1; dotx1; x2; dotx2])
m1 = 1; m2 = 1; k = 2;
A1 = [0, 1, 0, 0; ...
    -k/m1, 0, k/m1, 0; ... 
    0, 0, 0, 1;
    k/m2, 0, -k/m2, 0];

B1 = [0; 1/m1; 0; -1/m2];

%
% Compute the controllability (reachability) matrix:
% \mathbf{\mathcal{R}}_1 = \left[
% \mathbf{B}_1,\;\;
% \mathbf{A}_1 \mathbf{B}_1,\;\;
% \mathbf{A}_1^2\,\mathbf{B}_1,\;\;
% \mathbf{A}_1^3 \mathbf{B}_1\right]

%  get controllability matrix [B, A*B, A^2*B, A^3*B]
Mc1 = ctrb(A1,B1)

%
% Check the rank of controllability (reachability) matrix:

%  rank of controllability matrix
rank(Mc1)

%  number of uncontrollable states 
Nc1 = size(Mc1,1) - rank(Mc1)

% Note: with the infrastructural actuator, it is impossible to control the "common-mode" position and velocity; only the relative displacement and velocity of the two carts can be controlled.
%
% Consider now the two-mass system with actuator fixed to ground, as shown in Fig.2 below.
%
% Figure 2. Two-mass system with actuator fixed to ground.
%
% The state-space model with state vector \mathbf{x} = [x_1,\,\dot{x}_1,\,x_2,\,\dot{x}_2]^T is:
% \mathbf{A}_2 = \mathbf{A}_1 =
% \left[
% \begin{array}{cccc}
% 0 & 1 & 0 & 0 \\
% -k/m_1 & 0 & k/m_1 & 0 \\
%  0 & 0 & 0 & 1 \\
%  k/m_2 & 0 & -k/m_2 & 0
% \end{array}
% \right],      \mathbf{B}_2 = \left[
% \begin{array}{c}
% 0  \\  1/m_1  \\  0  \\ 0
% \end{array}
% \right]
%

% state-space model
A2 = A1;
B2 = [0; 1/m1; 0; 0];

%
% Compute the controllability (reachability) matrix:
% \mathbf{\mathcal{R}}_2 = \left[
% \mathbf{B}_2,\;\;
% \mathbf{A}_2 \mathbf{B}_2,\;\;
% \mathbf{A}_2^2\,\mathbf{B}_2,\;\;
% \mathbf{A}_2^3 \mathbf{B}_2\right]

% get controllability matrix [B, A*B, A^2*B, A^3*B]
Mc2 = ctrb(A2, B2)

%
% Check the rank of controllability (reachability) matrix:

%  rank of controllability matrix
rank(Mc2)

% number of uncontrollable states 
Nc2 = size(Mc2,1) - rank(Mc2)

% Note: with the actuator fixed to ground, the system becomes controllable.
%
% Consider the two-mass system with infrastructural position sensor (i.e. sensor measuring the relative distance of the two carts) shown in Fig.3 below.
%
% Figure 3. Two-mass system with infrstructural position sensor.
%
% The state-space model with state vector \mathbf{x} = [x_1,\,\dot{x}_1,\,x_2,\,\dot{x}_2]^T is:
% \mathbf{A}_3 = \mathbf{A}_1 =
% \left[
% \begin{array}{cccc}
% 0 & 1 & 0 & 0 \\
% -k/m_1 & 0 & k/m_1 & 0 \\
%  0 & 0 & 0 & 1 \\
%  k/m_2 & 0 & -k/m_2 & 0
% \end{array}
% \right],      \mathbf{C}_3 = \left[
% \begin{array}{cccc}
% -1  &  0  &  1  & 0
% \end{array}
% \right]

% state-space model
A3 = A1;
C3 = [-1, 0, 1, 0];

%
% Compute the observability matrix:
% \mathbf{\mathcal{O}}_3 \,=\,
% \left[
% \begin{array}{c}
% \mathbf{C}_3 \\
% \mathbf{C}_3\,\mathbf{A}_3\\
% \mathbf{C}_3\,\mathbf{A}_3^2\\
% \mathbf{C}_3\,\mathbf{A}_3^3
% \end{array}
% \right]

% get observability matrix [C; C*A, C*A^2; C*A^3]
Mo3 = obsv(A3, C3)

%
% Check the rank of the observability matrix:

%  rank of controllability matrix
rank(Mo3)

% number of unobservable states 
No3 = size(Mo3,2) - rank(Mo3)

% Note: with the infrastructural sensor, it is impossible to observe the "common-mode" position and velocity; only the relative displacement and velocity of the two carts can be observed.
%
% Consider now the two-mass system with absolute position sensor attached to one cart, as shown in Fig.4 below.
%
% Figure 4. Two-mass system with absolute position sensor fixed to cart 1.
%
% The state-space model with state vector \mathbf{x} = [x_1,\,\dot{x}_1,\,x_2,\,\dot{x}_2]^T is:
% \mathbf{A}_4 = \mathbf{A}_1 =
% \left[
% \begin{array}{cccc}
% 0 & 1 & 0 & 0 \\
% -k/m_1 & 0 & k/m_1 & 0 \\
%  0 & 0 & 0 & 1 \\
%  k/m_2 & 0 & -k/m_2 & 0
% \end{array}
% \right],      \mathbf{C}_4 = \left[
% \begin{array}{cccc}
% 1  &  0  &  0  & 0
% \end{array}
% \right]

% state space model
A4 = A1;
C4 = [1, 0, 0, 0];

%
% Compute the observability matrix:
% \mathbf{\mathcal{O}}_4 \,=\,
% \left[
% \begin{array}{c}
% \mathbf{C}_4 \\
% \mathbf{C}_4\,\mathbf{A}_4\\
% \mathbf{C}_4\,\mathbf{A}_4^2\\
% \mathbf{C}_4\,\mathbf{A}_4^3
% \end{array}
% \right]

% get observability matrix [C; C*A, C*A^2; C*A^3]
Mo4 = obsv(A4, C4)

%
% Check the rank of the observability matrix:

%  rank of controllability matrix
rank(Mo4)

% number of unobservable states 
No4 = size(Mo4,2) - rank(Mo4)

% Note: by measuring the absolute position of the first cart, the system becomes observable.
%
% Pole placement design
% Use place to design the state-feedback gain \mathbf{K} that places the closed-loop eigenvalues of the controllable pair (\mathbf{A},\,\mathbf{B}), i.e. the eigenvalues of \mathbf{A}-\mathbf{B}\,\mathbf{K}, to the desired locations on complex plane.
% The routine works for both SISO and MIMO systems, but requires that the multiplicity of the closed-loop eigenvalues is less than \mathrm{rank}\,\mathbf{B}. For SISO models, this implies that it is impossible to allocate eigenvalues with multiplicity > 1.
% acker is an alternative routine, and allows to allocate eigenvalues with multiplicity > 1. However, it works only for SISO models, and is less numerically reliable.
%
% Consider the two-mass system with actuator fixed to ground introduced above (see Fig.2). The open-loop eigenvalues are:

%  open-loop eigenvals
lam2 = eig(A2);

%  poles sorted in descending order of real part
lam2 = esort(lam2)

%
% The system is controllable. Therfeore, the closed-loop eigenvalues can be arbitrarily placed on the complex plane.
% It is desired to place them at the following locations:
% \lambda_{d,1}\,=\,e^{j(-\pi +\pi/3)}\,=\,-\frac{1}{2}+\,j\,\frac{\sqrt{3}}{2}   ,       \lambda_{d,4}\,=\,\lambda_{d,1}^*\,=\,-\frac{1}{2}-\,j\,\frac{\sqrt{3}}{2}
% \lambda_{d,2}\,=\,e^{j(-\pi +\pi/6)}\,=\,-\frac{\sqrt{3}}{2}\,+\,j\,\frac{1}{2}   ,       \lambda_{d,3}\,=\,\lambda_{d,2}^*\,=\,-\frac{\sqrt{3}}{2}\,-\,j\,\frac{1}{2}

% desired closed-loop eigenvals
lamd1 = 1.0 * exp(1i*(-pi + pi/3));
lamd2 = 1.0 * exp(1i*(-pi + pi/6));
lamd3 = conj(lamd2);
lamd4 = conj(lamd1);
lamd = [lamd1, lamd2, lamd3, lamd4];

%
% The state-feedback gain \mathbf{K} that place the closed-loop eignevalues at the desired locations can be obtained with the place routine (in fact, all the eigenvalues have multiplicity = 1):

% state feedback control design 
K = place(A2, B2, lamd)

%
% Design check:

%  desired closed-loop eigenvals
%  (sorted in descending order of real parts)
lamd = esort(lamd)

%  closed-loop eigenvals placed by "place"
A2c = A2 - B2*K;
lamc = eig(A2c)  

% plot open vs closed loop eigenvals
figure;
plot(lam2, 'bx', 'MarkerSize', 10, 'LineWidth', 2);
hold on
plot(lamc, 'rx', 'MarkerSize', 10, 'LineWidth', 2);
axis equal;
sgrid;
legend('open-loop eigenvalues', 'closed-loop eigenvalues');

%
% Note:  by exploiting the duality theorem, the place and acker routines can be also used to design the state estimator gain \mathbf{L} that places the estimator eigenvalues, i.e. the eigenvalues of \mathbf{A}-\mathbf{L}\,\mathbf{C}, to the desired locations on complex plane.
%
% Consider the two-mass system with absolute position sensor fixed to cart 1 introduced above (see Fig.4). The open-loop eigenvalues are:

%  open-loop eigenvals
lam4 = eig(A4);

%  poles sorted in descending order of real part
lam4 = esort(lam4)

%
% The system is controllable. Therfeore, the closed-loop eigenvalues can be arbitrarily placed on the complex plane.
% It is desired to place them at the following locations:
% \lambda_{d,1}\,=\,\lambda_{d,2}\,=\,\lambda_{d,3}\,=\,\lambda_{d,4}\,=\,-5

% desired estimator eigenvals
lamd = -5 * ones(4,1);

%
% The state estimator gain \mathbf{L} that places the estimator eigenvalues at the desired locations can only be obtained with the acker routine (in fact, there is only one desired eigenvalue with multiplicity = 4):

% state estimator design 
L = acker(A4.', C4.', lamd).'

%
% Design check:

%  desired estimator eigenvals
lamd

%  estimator eigenvals placed by "acker"
A4e = A4 - L*C4;
lame = eig(A4e)

%
% Linear Quadratic Regulator (LQR) design
% Use lqr or lqry to design the LQR with state or output weighting.
% Use lqi to design the LQR with integral action.
% lqr, lqry, lqi work with both continuous and discrete time models, provided that they are represented in encapsulated form, i.e. with LTI objects.
% lqr, lqry, lqi also accept models in non-encapsulated form: in that case, the models are considered in continuous-time domain. Use dlqr or dlqry when dealing with discrete-time models in non-encapsulated form.
%
% Consider again the two-mass system with actuator fixed to ground (see Fig.2). It is desired to design a LQR that minimizes the cost function
% J = \int_0^{+\infty}\,\mathbf{x}^T(t)\,\mathbf{Q}\mathbf{x}(t) \,+\, \mathbf{u}^T(t)\,\mathbf{R}\,\mathbf{u}(t)\,dt
% with cost matrices
% \mathbf{Q}\,=\,\mathrm{diag}\{ 1,\,10,\,1,\,10\} ,     \mathbf{R}\,=\,10

% cost function weights
Q = diag([1, 10, 1, 10]);   % state weighting matrix
R = 10;                     % input weight

% LQR design
K = lqr(A2, B2, Q, R)

%
% Design check:

% open-loop eigenvals
lam2 = eig(A2);
lam2 = esort(lam2)

% closed-loop eigenvals
A2c = A2 - B2*K;
lamc = eig(A2c)  

% plot open vs closed loop eigenvals
figure;
plot(lam2, 'bx', 'MarkerSize', 10, 'LineWidth', 2);
hold on
plot(lamc, 'rx', 'MarkerSize', 10, 'LineWidth', 2);
axis equal;
sgrid;
legend('open-loop eigenvalues', 'closed-loop eigenvalues (LQR)');

%
% Interactive control design
% sisotool is a convenient interactive tool for the analysis and design of SISO control systems.

sisotool;