%
% Implementation example, v13.10.2020
%
%% ------
%
% Algorithm implementation in Matlab
% 
% In this exercise you will go through the steps to implement in matlab a difference equation that you derived from a given transfert function H(s),
% steps are:
% 
% 1) Given the transfer function H(s) = 1 / (a*s + 1) 
% 2) Get H(z) using Tustin approximation, by hand
% 3) Derive the difference equation y(k), by hand
% 4) In Matlab, write the code that computes difference equation y(k) function of the current/previous y and u and relative coefficients. 
%    Be careful with your indexes, especially when you access the k-1 sample.
% 5) To compute the coefficients, use sampling period T = 0.04 [s],  a = 0.2
% 6) Define the step input  U = ones(1, nSteps); # nSteps = 50
% 7) Compute the y(k) function of Y and U, for each k going from 1 to nSteps
% 8) Plot the result,  you can use 'stair(X,Y)' to plot your resulting Y.
% 9) Then compared your result with the Y* computed using the 'tf([num],[den])', 'c2d(Hs,T,'tustin')' and 'step(Hz)' built-in function, 
%     they should be identical.
%
% ------  
%  
%% 1)
%                  1
% given H(s) =  ---------
%                a*s + 1
%
%% 2) Get H(z) using Tustin approximation, by hand
% 
%    H(s) = 1 / (a*s + 1) 
%    H(z) = 1 / (a*(2 * (z-1) / T* (z+1)) + 1)   - replace s by Tustin approx.
%    H(z) = (Tz+ T) / ( 2az - 2a + Tz + T) = Y(z)/U(z)
%    Y(z)*(2az - 2a     + Tz + T)      = U(z)*(Tz + T)
%    Y(z)*(2a  - 2az^-1 + T  + Tz^-1)  = U(z)*(T  + Tz^-1)
%
%% 3) Derive the difference equation y(k), by hand
%
%    y(k)*(2a+T) + y(k-1)*(T-2a) = u(k)*T + u(k-1)*T
%               T                T                  2a - T
%    y(k) = ---------  u(k) + ---------  u(k-1) +  ---------  y(k-1)
%             2a + T           2a + T               2a + T
%
% with a=0.2 and T = 0.04[s]
%
%     y(k) = 0.09091 u(k) + 0.09091 u(k-1) + 0.8182 y(k-1) 

%% 4) In Matlab, write the code that computes difference equation y(k)

clear all, close all

% 5) To compute the coefficients, use sampling period T = 0.04 [s],  a = 0.2

T = 0.04
a = 0.2

c1= T/(2*a + T)
c2= T/(2*a + T)
c3= (2*a - T)/(2*a + T)

% 6) Define the step input  U = ones(1, nSteps); # nSteps = 50
nSteps=50;
U = ones(1,nSteps); 
Y = zeros(1,nSteps);

% 7) Compute the y(k) function of Y and U, for each k going from 1 to nSteps
Y(1) = c1*U(1); % since we start at 2 we need to compute Y(1)

for n = 2:nSteps  % we start at 2 since we use n-1 below
   Y(n) = c1*U(n) + c2*U(n-1) + c3*Y(n-1);
end

%% 8) Plot the result
figure
hold on

stairs([0:nSteps-1]*T,Y,'b-');  % our computation

%% 9) Then compared your result with the Y* computed using the built-in functions 

Hc = tf([1],[0.2 1])

step(Hc,'r-')

Hd = c2d(Hc,T,'Tustin')

step(Hd,'g*')