%% Application: State and Output Trajectories of LTI Systems using Laplace Transform - Example 2
%
% Table of Contents
% Introduction
% LTI System of 2nd Order: the Cart-Spring-Damper Example
%     Step 0: Declare the Variables
%         Initial Conditions
%         System Parameters
%     Step 1 & 2: Declare the LTI Equations in the Laplace Transform Domain and Solve them!
%          Solve the Equations Using the Laplace Transform
%     Step 3: Substitute Numerical Values
%     Step 4: Plot the results.
%
%
%% Introduction
% Symbolic workflows keep calculations in their natural symbolic form rather than numerical form. This approach helps to understand the solution's properties and uses exact symbolic values. Numbers are substituted for symbolic variables only when a numerical result is required or cannot be continued in a symbolic manner.
%
% Typically, the steps are:
% Declare the variables and the (state and output) equations.
% Solve the equations using the Laplace transform.
% Substitute numerical values.
% Plot the results.
% Analyse the results.
%
%% LTI System of 2nd Order: the Cart-Spring-Damper Example
%
% We want to determine state and output trajectories for the LTI system:
%
%
% \left\{ \begin{array}{rcl}
% \dot{x_1}(t) &=& x_2(t)\\
% \dot{x_2}(t) &=& -\frac{k}{M} x_1(t) - \frac{h}{M} x_2(t) +\frac{1}{M} u(t) \\
% y(t) &=& x_1(t)
% \end{array} \right.
% with initial condition x_1(0) = x_0\,,\; x_2(0) = v_0 and input u(t) = 1+A \cos(\omega t) \cdot 1(t)\,
%
%% Step 0: Declare the Variables

clear

syms t s Xs Ys  % the variables 
syms x0 [2 1]                                       % in the time and frequency domain
syms k h M Au omega % parameters

assume(t >=0)
assume([k h M Au omega] >0) % a compact way to declare the parameters as positive real
                           % symbolic variables


u(t) = 1+Au * cos(omega * t) % Note: you DO NOT NEED to explicity write the Heaviside function!

%
%% Initial Conditions
%
% Modify the initial position x(0) and the initial speed \dot{x}(0) of the cart and analyse the result


init_pos   = -2; % initial position [m]
init_speed = -5.5; % initial speed [m/s]

x0VAL = [init_pos; init_speed];

%
%
%% System Parameters
%
% Note: the control law parameters \alpha,\,\beta are influencing the controlled system behaviour

k_bar = 10.09; % N/m <-- nominal value  of the spring elastic constant
% Note: we do not use this parameter in the controller <--> compare with the open-loop controller

Mval = 123.52;  % kg [cart mass]
hval = 9.12;   % Ns/m [damping constant]
kval = 10.89; %k_bar;   % N/m [spring elastic coeff.] <-- change the value; use for instance k = 10.89    
% using this set of params --> roots([M h k]) complex conj.
% 

Aval = 2.50;      % magnitude of the external input force u [N]
OMval = 2*pi*0.1; % angular frequency of the input [rad/s]

%
%
%
%
%% Step 1 & 2: Declare the LTI Equations in the Laplace Transform Domain and Solve them!
%
% The most efficient way to determine statte and output trajectories of an LTI system, when the state equations are available, is to start directly with the matricial equations the Laplace transform domain.
%
% First step: declare the matrices in the state equations. Note: they are symbolic matrices.

A = [    0      1;
      -k/M  -h/M];
B = [0; 1/M];
C = [1 0];
D = 0;

% Then, evaluate the symbolic expression of the state and output trajectories as Laplace transforms:

sI_A = (s*eye(size(A))-A)
sI_A_inv = inv(sI_A) % it's a symbolic matrix; do not worry about matrix inversion!

%
% Now we are ready to express the state trajectory and the output trajectory using the Laplace transform
% \left\{ \begin{array}{rcl}
% X(s) &=& \left( sI-A\right)^{-1} x(0) + \left( sI-A\right)^{-1}\,B\,U(s) \\
% Y(s) &=& C\,X(s) +D\,U(s)
% \end{array} \right.
%

Us = laplace(u,t,s)
Xs = sI_A_inv * x0 + sI_A_inv * B * Us;
Ys = C * Xs + D* Us;

% What does the equations look like?

disp(Xs)
disp(Ys)

%
%%  Solve the Equations Using the Laplace Transform
%
% Let's compute the inverse Laplace transform of the state trajectory:

xSOL_t = ilaplace(Xs);
ySOL_t = ilaplace(Ys);

% What does the equation in the Laplace transform domain look like?

disp(xSOL_t)

%
%
%% Step 3: Substitute Numerical Values
% Note: In order to continue and evaluate the state and output trajectories in the time domain, we must replace every symbolic parameter  with the corresponding numerical value

xSol_IC = subs(xSOL_t, x0, x0VAL);
xSol_IC = subs(xSol_IC, [k h M Au omega], [kval hval Mval Aval OMval]);
xSol_IC = simplify(xSol_IC)

ySol_IC = subs(ySOL_t, x0, x0VAL); % output trajectory
ySol_IC = subs(ySol_IC, [k h M Au omega], [kval hval Mval Aval OMval]); % output trajectory
ySol_IC = simplify(ySol_IC)

%
%
%% Step 4: Plot the results.
%

figure('Units','normalized','Position',[0.1,0.1,0.9,0.9])

subplot(2,1,1)
fplot(xSol_IC,[0 15], 'Linewidth', 1.5)                      
title('State trajectory')
ylabel('x(t)', 'FontSize', 16)
xlabel('t', 'FontSize', 16)
grid on
legend('x_1', 'x_2', 'Location', 'best', 'FontSize', 16) 
subplot(2,1,2);
fplot(ySol_IC,[0 15], 'Linewidth', 1.5)                      
title('Output trajectory')
ylabel('y(t)', 'FontSize', 16)
xlabel('t', 'FontSize', 16)
grid on
legend('y', 'Location', 'best', 'FontSize', 16) 

%%
%
%
%
%
%
%
%