%% Laboratory notes: Simulation of nonlinear systems in Simulink®
% Riccardo Antonello (riccardo.antonello@unipd.it) and Giulia Michieletto (giulia.michieletto@unipd.it)
% March 10, 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: dynamics of a simple pendulum
% Natural response of the damped nonlinear pendulum
% Natural response of the undamped nonlinear pendulum
% Natural response comparison with the linearized undamped pendulum
%
%% 1.  Introduction: dynamics of a simple pendulum
% Consider the simple pendulum shown in Fig. 1.
%
% Figure 1: simple pendulum.
% The equations of motion can be easily derived from the Newton-Euler 2nd law:
% (1)   J\,\ddot{\theta} = \textstyle{\sum_n} \tau_n
% where J is the moment of inertia of the mass with respect to the pivot O, i.e.
% (2)   J = m l^2
% and \textstyle{\sum_n} \tau_n is the sum of all the torques acting on the system. These are the torque due to gravity:
% (3)   \tau_1 = -mgl\,\sin\theta
% and a possible additional viscous fricion torque which damps the oscillations of the pendulum:
% (4)   \tau_2 = -b\,\dot{\theta}
% By replacing (2)-(4) in (1), the following equation of motion results for the simple pendulum:
% (5)   m l^2\,\ddot{\theta} + b\,\dot{\theta} + m g l \,\sin\theta = 0
% which is a nonlinear ODE, because one of the state variables appears as an argument to a trigonometric function.
%
%
%% 2.  Natural response of the damped nonlinear pendulum
% The nonlinear ODE can be integrated in Simulink after representing it in block diagram form. A possible way to proceed is as follows.
% Given a generic ODE in implicit form of the type:
% (6)   f\left( y(t), \cdots,  y^{(n-1)}(t), y^{(n)}(t),\; u(t) \right) = 0
% suppose that an explicit expression of y^{(n)}(t) can be found:
% (7)   y^{(n)}(t) = g\left( y(t), \cdots,  y^{(n-1)}(t),\; u(t) \right)
% Then, the ODE can be represented in block diagram form as shown in Fig. 2 (Lord Kelvin's scheme).
%
%
% Figure 2: Lord Kelvin's scheme (chain of integrators scheme).
%
% Implementation of the nonlinear ODE (5) with the Lord Kelvin's scheme:

%  conversion gains
rad2deg = 180/pi;
deg2rad = pi/180;

%  model parameters
m = 1;
l = 0.1;
b = 3e-3;
g = 9.81;

%  open simulink model
open_system('nonlin_pend.slx');

%
%
% Simulate the natural response of the nonlinear pendulum with initial conditions (ICs): \theta(0)=60\,\mathrm{deg}, \dot{\theta}(0)=0\,\mathrm{deg/s}.

%  set simulation parameters
set_param('nonlin_pend', ...
    'SolverType', 'Variable-step', ...
    'Solver', 'ode45', ...
    'MaxStep', 'auto', ...
    'MinStep', 'auto', ...
    'AbsTol', 'auto', ...
    'RelTol', '1e-3', ...
    'StopTime', '15');

%  initial conditions
th0 = 60 * deg2rad;
dot_th0 = 0;

%  run simulation
sim('nonlin_pend');

%
% Plot the natural response:

%  extract simulation results
t = simres.time;
th = simres.signals(1).values;
dot_th = simres.signals(2).values;

%  plot results
figure;
subplot(2, 1, 1);
plot(t, th, 'b-', 'LineWidth', 1.5);
ylabel('Position [deg]');
grid on;

subplot(2, 1, 2);
plot(t, dot_th, 'b-', 'LineWidth', 1.5);
ylabel('Seed [deg/s]');
xlabel('Time [s]')
grid on;

%
% The previous response is not sufficiently smooth, due to the limited number of integration instants considered for the simulation.
% A smoother response can be obtained by reducing the max integration step of the simulation.

%  set simulation parameters
set_param('nonlin_pend', ...
    'SolverType', 'Variable-step', ...
    'Solver', 'ode45', ...
    'MaxStep', '0.01', ...
    'MinStep', 'auto', ...
    'AbsTol', 'auto', ...
    'RelTol', '1e-3', ...
    'StopTime', '15');

%  run simulation
sim('nonlin_pend');

%
% Simulation results with reduced integration max step size.

%  extract simulation results
t = simres.time;
th = simres.signals(1).values;
dot_th = simres.signals(2).values;

%  plot results
figure;
subplot(2, 1, 1);
plot(t, th, 'b-', 'LineWidth', 1.5);
ylabel('Position [deg]');
grid on;

subplot(2, 1, 2);
plot(t, dot_th, 'b-', 'LineWidth', 1.5);
ylabel('Seed [deg/s]');
xlabel('Time [s]')
grid on;

%
%% 3.  Natural response of the undamped nonlinear pendulum
% Consider now the case without friction, i.e. b=0 (undamped pendulum).

%  set damping to zero
b = 0;

%  run simulation
sim('nonlin_pend');

%
% Natural response of the undamped nonlinear pendulum:

%  extract simulation results
t = simres.time;
th = simres.signals(1).values;
dot_th = simres.signals(2).values;

%  plot results
figure;
subplot(2, 1, 1);
plot(t, th, 'b-', 'LineWidth', 1.5);
ylabel('Position [deg]');
grid on;

subplot(2, 1, 2);
plot(t, dot_th, 'b-', 'LineWidth', 1.5);
ylabel('Seed [deg/s]');
xlabel('Time [s]')
grid on;

% With no friction, there is no energy dissipation, and therefore the pendulum swings indefinitely.
% The natural response is a persistent oscillation with constant amplitude.
%
% The oscillation of the nonlinear undamped pendulum is not harmonic, i.e. it is not a purely sinusoidal motion. This is more evident for large oscillations.
% Therefore, simulate the natural response with ICs: \theta(0)=175\,\mathrm{deg}, \dot{\theta}(0)=0\,\mathrm{deg/s}

%  initial conditions
th0 = 175 * deg2rad;
dot_th0 = 0;

%  set simulation parameters
set_param('nonlin_pend', ...
    'SolverType', 'Variable-step', ...
    'Solver', 'ode45', ...
    'MaxStep', '0.01', ...
    'MinStep', 'auto', ...
    'AbsTol', 'auto', ...
    'RelTol', '1e-3', ...
    'StopTime', '15');

%  run simulation
sim('nonlin_pend');

%
% Natural response of the nonlinear undamped pendulum for large oscillations:

%  extract simulation results
t = simres.time;
th = simres.signals(1).values;
dot_th = simres.signals(2).values;

%  plot results
figure;
subplot(2, 1, 1);
plot(t, th, 'b-', 'LineWidth', 1.5);
ylabel('Position [deg]');
grid on;

subplot(2, 1, 2);
plot(t, dot_th, 'b-', 'LineWidth', 1.5);
ylabel('Seed [deg/s]');
xlabel('Time [s]')
grid on;

%
% For an undamped nonlinear pendulum with ICs \theta(0)=\theta_0, \dot{\theta}(0)=0 (note: the initial speed is zero), the period od the oscillation is equal to:
% (8)   T = 4 \sqrt{\frac{l}{g}}\,K\left(\sin\frac{\theta_0}{2}\right)
% where
% (9)   K(m) = \displaystyle{\int}_0^{\pi/2}\!\!\frac{d \theta}{\sqrt{1-m^2\sin^2\theta}}
% is the elliptical integral of the first kind. In particular, from (9) it follows that the period of oscillation depends on the initial angle \theta_0.
%
% For the derivation of (8), see:
% https://www.math24.net/nonlinear-pendulum/
% http://www.sciencedirect.com/science/article/pii/S089812211200017X
%
% For the numerical evaluation of the elliptical integral (9), use the routine ellipke.
% Note: the input argument acceted by ellipke is the variable m^2 in (9), namely it evaluates the integral in the following form:
% (10)   K(m') = \displaystyle{\int}_0^{\pi/2}\!\!\frac{d \theta}{\sqrt{1-m'\sin^2\theta}}

%  period of the nonlinear pendulum
k = sin(th0/2);
K = ellipke(k^2);
T_nlin = 4*sqrt(l/g)*K

%
% A possible way to verify that the oscillation of the nonlinear undamped pendulum is not harmonic consists of evaluating its spectrum, using the FFT.

%  select final time to consider
%  an integer number of periods
t1f = T_nlin * floor(t(end)/T_nlin);

%  resample at fixed rate
Ts = T_nlin/100;
t1 = 0:Ts:t1f;
th1 = interp1(t, th, t1);

%  compute fft
Fs = 1/Ts;
N1 = length(t1);
f1 = linspace(0, Fs, N1);

th1_fft = (1/N1)*fft(th1);

%  plot fft mag
figure;
plot(f1, abs(th1_fft), 'b.-', 'MarkerSize', 10, 'LineWidth', 1.5);
xlabel('Frequency [Hz]');
ylabel('FFT (magnitude)');
xlim([0,5]);
ylim([0,105]);

% The computed spectrum reveals that the oscillation is composed of several harmonics, namely the harmonic at the fundamental frequency \omega=\frac{2\pi}{T} (1st harmonic), and all the harmonics at the odd multiples of the fundamental frequency (i.e. 3rd, 5th, 7th, ... harmonics).
%
%
%% 4.  Natural response comparison with the linearized undamped pendulum
% For small oscillations, the nonlinear ODE (5) can be linearized by using the approximation \sin\theta\approx \theta. The resulting linearized ODE is:
% (11)   m l^2\,\ddot{\theta} + b\,\dot{\theta} + m g l \,\theta = 0
% A possible state-space realization is:
% (12)   \left[
% \begin{array}{c}
% \dot{\theta}\\
% \ddot{\theta}
% \end{array}
% \right]
% =
% \left[
% \begin{array}{cc}
% 0  &  1\\
% -\frac{g}{l} & -\frac{b}{ml^2}
% \end{array}
% \right]
% %%%
% \left[
% \begin{array}{c}
% \theta\\
% \dot{\theta}
% \end{array}
% \right]
%
% Compare the natural response of the nonlinear undamped pendulum described by (5) with that of the linearized one, described by (11)-(12).

%  initial conditions
th0 = 10 * deg2rad;
dot_th0 = 0;

%  linear model parameters
A = [0, 1; -g/l, -b/m/l^2];
B = zeros(2,1);
C = eye(2);
D = zeros(2,1);
x0 = [th0; dot_th0];

%  open simulink model
open_system('nonlin_vs_lin_pend.slx');

%
%
% Simulate the natural responses with initial conditions (ICs): \theta(0)=10\,\mathrm{deg}, \dot{\theta}(0)=0\,\mathrm{deg/s}.

%  set simulation parameters
set_param('nonlin_vs_lin_pend', ...
    'SolverType', 'Variable-step', ...
    'Solver', 'ode45', ...
    'MaxStep', '0.01', ...
    'MinStep', 'auto', ...
    'AbsTol', 'auto', ...
    'RelTol', '1e-3', ...
    'StopTime', '15');

%  run simulation
sim('nonlin_vs_lin_pend');

%
% Comparison of the natural responses of the two systems (nonlinear vs linear undamped pendulums):

%  extract simulation results
t = simres.time;

th = simres.signals(1).values(:,1);
th_lin = simres.signals(1).values(:,2);

dot_th = simres.signals(2).values(:,1);
dot_th_lin = simres.signals(2).values(:,2);

%  plot results
figure;
subplot(2, 1, 1);
plot(t, th, 'b-', 'LineWidth', 1.5);
hold on;
plot(t, th_lin, 'r-', 'LineWidth', 1.5);
ylabel('Position [deg]');
grid on;
xlim([0, 15]);
legend('Nonlinear', 'Linear');

subplot(2, 1, 2);
plot(t, th-th_lin, 'b-', 'LineWidth', 1.5);
ylabel('Linearization error [deg]');
xlabel('Time [s]')
grid on;
xlim([0, 15]);

% The two oscillations are not identical; in particular, their periods are slightly different.
%
% The undamped linear pendulum is an harmonic oscillator, i.e. its natural response is a purely sinusoidal oscillation.
% In fact, the linearized ODE (11) can be rewritten as:
% (13)   \ddot{\theta} + \omega_0^2 \,\theta = 0
% from which it can be immediately noticed that the natural response is a sinusoidal oscillation with frequency \omega_0 and period:
% (14)   T_l = \frac{2\pi}{\omega_0} = 2 \pi \sqrt{ \frac{l}{g} }
% Differently from the nonlinear pendulum case, the period and frequency of the harmonic oscillator (13) are obviously independent of the IC.
%
% Plot of the ratio T/T_l vs the initial angle \theta_0:

%  period of the nonlinear pendulum
th0 = linspace(0, pi, 100);
k = sin(th0/2);
K = ellipke(k.^2);
T_nlin = 4*sqrt(l/g)*K;

%  period of the linear pendulum
T_lin = 2*pi*sqrt(l/g);16-18

%  nonlinear vs linear pendulum period
figure;
plot(th0*rad2deg, T_nlin/T_lin, 'b-', 'LineWidth', 1.5);
xlim([0, 180]);
ylim([0, 3.5]);
grid on;
xlabel('Initial angle [deg]');
ylabel('Nonlinear vs linear period ratio (T_nonlin / T_lin)');

% Note that T/T_l\geq 1 for \theta_0 \in [0,180], namely the period of the undamped nonlinear pendulum is always larger than that of the linearized pendulum, approaching it for \theta_0\to 0.
%
% The lineariztion error is of the form:
% (15)    \begin{array}{ll}
% e(t) & = \theta_0 \left( \cos \omega t - \cos \omega_l t \right) + \text{H.O.H.} \\[4pt]
%  & = - 2 \theta_0 \sin\left( \frac{\omega - \omega_l}{2}\,t \right) \sin\left( \frac{\omega + \omega_l}{2}\,t \right) + \text{H.O.H.}
% \end{array}
% where H.O.H. stands for high order harmonics (of the nonlinear pendulum oscillation).
% In particular, the error has a beat with frequency (\omega-\omega_l)/2.
%
% Re-evaluate the linearization error with ICs equal to \theta(0)=60\,\mathrm{deg}, \dot{\theta}(0)=0\,\mathrm{deg/s}:

%  initial conditions
th0 = 60 * deg2rad;
dot_th0 = 0;

%  linear model parameters
A = [0, 1; -g/l, -b/m/l^2];
B = zeros(2,1);
C = eye(2);
D = zeros(2,1);
x0 = [th0; dot_th0];

%  set simulation parameters
set_param('nonlin_vs_lin_pend', ...
    'SolverType', 'Variable-step', ...
    'Solver', 'ode45', ...
    'MaxStep', '0.01', ...
    'MinStep', 'auto', ...
    'AbsTol', 'auto', ...
    'RelTol', '1e-3', ...
    'StopTime', '20');

%  run simulation
sim('nonlin_vs_lin_pend');

%
% Natural responses of the nonlinear vs linear pendulums:

%  extract simulation results
t = simres.time;

th = simres.signals(1).values(:,1);
th_lin = simres.signals(1).values(:,2);

dot_th = simres.signals(2).values(:,1);
dot_th_lin = simres.signals(2).values(:,2);

%  plot results
figure;
subplot(2, 1, 1);
plot(t, th, 'b-', 'LineWidth', 1.5);
hold on;
plot(t, th_lin, 'r-', 'LineWidth', 1.5);
ylabel('Position [deg]');
grid on;
xlim([0, t(end)]);
legend('Nonlinear', 'Linear');

subplot(2, 1, 2);
plot(t, th-th_lin, 'b-', 'LineWidth', 1.5);
ylabel('Linearization error [deg]');
xlabel('Time [s]')
grid on;
xlim([0, t(end)]);