
%% Exported from Jupyter Notebook
% Run each section by placing your cursor in it and pressing Ctrl+Enter

%% Code Cell[1]:

save_figures = 0;

%% Markdown Cell:
% For many nonlinear models, numerical solution of the state equation is required.
% For linear models, we can always solve them analytically using the methods of this chapter.
% However, due to its convenience, we will often want to use numerical techniques even when analytic ones are available.
% 
% Matlab has several built-in and *Control Systems Toolbox* functions for analyzing state-space system models, especially *linear* models.
% We'll explore a few, here.
% 
% Consider, for instance, a linear state model with the following $A$, $B$, $C$, and $D$ matrices:
% \begin{subequations}
% \begin{align}
% A &=\begin{bmatrix}
% -3 & 4 & 5 \\
% 0 & -2 & 3 \\
% 0 & -6 & 1
% \end{bmatrix}
% \quad
% B &=\begin{bmatrix}
% 1 \\ 0 \\ 1
% \end{bmatrix}
% \quad
% C &=\begin{bmatrix}
% 1 & 0 & 0 \\
% 0 & -1 & 0
% \end{bmatrix}
% \quad
% D &=\begin{bmatrix}
% 0 \\ 0
% \end{bmatrix}.
% \end{align}
% \end{subequations}

%% Code Cell[2]:

A = [-3,4,5;0,-2,3;0,-6,1];
B = [1;0;1];
C = [1,0,0;0,-1,0];
D = [0;0];

%% Markdown Cell:
% For a step input $u(t) = 3 u_s(t)$ and initial state $\bm{x}(0) = \begin{bmatrix} 1 & 2 & 3 \end{bmatrix}^\top$, let's compare analytic and numerical solutions for the output response $\bm{y}(t)$.

%% Code Cell[3]:

u = @(t) 3*ones(size(t)); % for t>=0
x_0 = [1; 2; 3];

%% Markdown Cell:
% ## Analytic solution

%% Markdown Cell:
% For an analytic solution, we'll use a rearranged version of \cref{eq:output_response}.\footnote{Although we call this the "analytic" solution, we are not solving for a detailed symbolic expression, although we *could*. In fact, \cref{eq:state_output_solution_remix} *is* the analytic solution and what follows is an attempt to represent it graphically.}
% \begin{subequations}\label{eq:state_output_solution_remix}
% \begin{align}
% 	\bm{y}(t) &= C \Phi(t) \bm{x}(0) + C \Phi(t) \int_0^t \Phi(-\tau) B \bm{u}(\tau) d\tau + D \bm{u}(t).
% \end{align}
% \end{subequations}
% First, we need the state transition matrix $\Phi(t)$, so we consider the eigenproblem.

%% Code Cell[4]:

[M,L] = eig(A)

%% Markdown Cell:
% Note that, when assigning its output to two variables `M` and `L`, the `eig` function returns the modal matrix to `M` and the eigenvalue matrix to `L`.
% The modal matrix of eigenvectors `M` has each column (eigenvector) normalized to unity.
% Also notice that `M` and `L` are *complex*.
% The imaginary parts of two eigenvalues and their corresponding eigenvectors are significant.
% Finally, since the *real* parts of the all eigenvalues are *negative*, the system is stable.

%% Markdown Cell:
% The "diagonal"-basis state transition matrix $\Phi'(t)$ is simply
% \begin{align}
%     \Phi'(t) = e^{\Lambda t}.
% \end{align}
% Let's define this as an "anonymous" function.

%% Code Cell[5]:

Phi_p = @(t) diag(diag(exp(L*t))); % diags to get diagonal mat

%% Markdown Cell:
% The original-basis state transition matrix $\Phi(t)$ is, from \cref{eq:state_transition_pref},
% \begin{align}
%     \Phi(t) = M \Phi'(t) M^{-1}.
% \end{align}

%% Code Cell[6]:

M_inv = M^-1; % compute just once, not on every call
Phi = @(t) M*Phi_p(t)*M_inv;

%% Markdown Cell:
% ### Free response
% 
% The free response is relatively straightforward to compute.

%% Code Cell[7]:

t_a = 0:.05:5; % simulation time
y_fr = NaN*ones(size(C,1),length(t_a)); % initialize
for i = 1:length(t_a)
    y_fr(:,i) = C*Phi(t_a(i))*x_0;
end
y_fr(:,1:3) % first three columns

%% Markdown Cell:
% A time array `t_a` was defined such that `Phi` could be evaluated.
% The first three columns of $\bm{y}_\text{fr}$ are printed for the first three moments in time.
% Note how there's a "hanging chad" of imaginary components.
% Before we `real`ize them, let's make sure they're negligibly tiny.

%% Code Cell[8]:

max(max(abs(imag(y_fr))))
y_fr = real(y_fr);

%% Code Cell[9]:

h = figure;
p = plot(t_a,y_fr,'linewidth',1.5);
xlabel('time (s)',...
    'interpreter','latex');
ylabel('free response $\bm{y}_\text{fr}(t)$',...
    'interpreter','latex');
grid on
l = legend('$y_1$','$y_2$');
set(l,'interpreter','latex');
hgsave(h,'figures/temp');

%% Code Cell[10]:

if save_figures
    fn = 'simulating_state_space_response_01.tex';
    h = hgload('figures/temp');
    cleanfigure;
    matlab2tikz(['figures/',fn],...
        'figurehandle',h,...
        'width','.9\linewidth',...
        'height','.33\linewidth',...
        'extraAxisOptions',...
        [...
            'scaled ticks=false,',... 
            'tick label style={/pgf/number format/fixed}'...
        ],...
        'showInfo', false...
    );
end

%% Markdown Cell:
% The results are plotted in \cref{fig:simulating_state_space_response_01}.
% As we might expect from the eigenvalues, the free responses of both outputs oscillate and decay.

%% Markdown Cell:
% ### Forced response

%% Markdown Cell:
% Now, there is the matter of integration in \cref{eq:state_output_solution_remix}. 
% Since Matlab does not excel in symbolic manipulation, we have chosen to avoid attempting to write the solution, symbolically.\footnote{Mathematica or SageMath would be preferrable for this.}
% For this reason, we choose a simple numerical (trapezoidal) approximation of the integral using the `trapz` function.

%% Markdown Cell:
% First, the integrand can be evaluated over the simulation interval.

%% Code Cell[11]:

integrand_a = NaN*ones(size(C,2),length(t_a)); % initialize
for i = 1:length(t_a)
    tau = t_a(i);
    integrand_a(:,i) = Phi(-tau)*B*u(tau);
end

%% Markdown Cell:
% Now, numerically integrate.

%% Code Cell[12]:

integral_a = zeros(size(integrand_a));
for i = 2:length(t_a)
    i_up = i; % upper limit of integration
    integral_a(:,i) = ... % transposes for trapz
        trapz(t_a(1:i_up)',integrand_a(:,1:i_up)')'; 
end

%% Markdown Cell:
% Now, evaluate the forced response at each time.

%% Code Cell[13]:

y_fo = NaN*ones(size(C,1),length(t_a)); % initialize
for i = 1:length(t_a)
    y_fo(:,i) = C*Phi(t_a(i))*integral_a(:,i);
end
y_fo(:,1:3) % first three columns

%% Code Cell[14]:

max(max(abs(imag(y_fo))))
y_fo = real(y_fo);

%% Code Cell[15]:

h = figure;
p = plot(t_a,y_fo,'linewidth',1.5);
xlabel('time (s)',...
    'interpreter','latex');
ylabel('forced response $\bm{y}_\text{fo}(t)$',...
    'interpreter','latex');
grid on
l = legend('$y_1$','$y_2$');
set(l,'interpreter','latex');
hgsave(h,'figures/temp');

%% Code Cell[16]:

if save_figures
    fn = 'simulating_state_space_response_02.tex';
    h = hgload('figures/temp');
    cleanfigure;
    matlab2tikz(['figures/',fn],...
        'figurehandle',h,...
        'width','.9\linewidth',...
        'height','.33\linewidth',...
        'extraAxisOptions',...
        [...
            'scaled ticks=false,',... 
            'tick label style={/pgf/number format/fixed}'...
        ],...
        'showInfo', false...
    );
end

%% Markdown Cell:
% The forced response is shown in \cref{fig:simulating_state_space_response_02}, which shows damped oscillations.

%% Markdown Cell:
% ### Total response

%% Markdown Cell:
% The total response is found from the sum of the free and forced responses: $\bm{y}(t) = \bm{y}_\text{fr} + \bm{y}_\text{fo}$.
% We can simply sum the arrays.

%% Code Cell[18]:

y_t = y_fr + y_fo;

%% Markdown Cell:
% The result is plotted in \cref{fig:simulating_state_space_response_03}.

%% Code Cell[19]:

h = figure;
p = plot(t_a,y_t,'linewidth',1.5);
xlabel('time (s)',...
    'interpreter','latex');
ylabel('total response $\bm{y}(t)$',...
    'interpreter','latex');
grid on
l = legend('$y_1$','$y_2$');
set(l,'interpreter','latex');
hgsave(h,'figures/temp');

%% Code Cell[20]:

if save_figures
    fn = 'simulating_state_space_response_03.tex';
    h = hgload('figures/temp');
    cleanfigure;
    matlab2tikz(['figures/',fn],...
        'figurehandle',h,...
        'width','.9\linewidth',...
        'height','.33\linewidth',...
        'extraAxisOptions',...
        [...
            'scaled ticks=false,',... 
            'tick label style={/pgf/number format/fixed}'...
        ],...
        'showInfo', false...
    );
end

%% Markdown Cell:
% ### Numerical solution

%% Markdown Cell:
% The numerical solution of the state equations is rather simple using Matlab's `ss` and `step` or `lsim` commands, as we show, here.
% First, we define an `ss` model object&mdash;a special kind of object that encodes a state-space model.

%% Code Cell[21]:

sys = ss(A,B,C,D);

%% Markdown Cell:
% At this point, using the `step` function would be the easiest way to solve for the step response.
% However, we choose the more-general `lsim` for demonstration purposes.

%% Code Cell[22]:

y_t_num = lsim(sys,u(t_a),t_a,x_0);

%% Markdown Cell:
% This total solution is shown in \cref{fig:simulating_state_space_response_04}.

%% Code Cell[23]:

h = figure;
p = plot(t_a,y_t_num,'linewidth',1.5);
xlabel('time (s)',...
    'interpreter','latex');
ylabel('total response (numerical) $\bm{y}(t)$',...
    'interpreter','latex');
grid on
l = legend('$y_1$','$y_2$');
set(l,'interpreter','latex');
hgsave(h,'figures/temp');

%% Code Cell[24]:

if save_figures
    fn = 'simulating_state_space_response_04.tex';
    h = hgload('figures/temp');
    cleanfigure;
    matlab2tikz(['figures/',fn],...
        'figurehandle',h,...
        'width','.9\linewidth',...
        'height','.33\linewidth',...
        'extraAxisOptions',...
        [...
            'scaled ticks=false,',... 
            'tick label style={/pgf/number format/fixed}'...
        ],...
        'showInfo', false...
    );
end

%% Code Cell[25]:

d_y = y_t-y_t_num';

%% Code Cell[26]:

h = figure;
p = plot(t_a,d_y,'linewidth',1.5);
xlabel('time (s)',...
    'interpreter','latex');
ylabel('total response error',...
    'interpreter','latex');
grid on
l = legend('$y_1$','$y_2$');
set(l,'interpreter','latex');
hgsave(h,'figures/temp');

%% Code Cell[27]:

if save_figures
    fn = 'simulating_state_space_response_05.tex';
    h = hgload('figures/temp');
    cleanfigure;
    matlab2tikz(['figures/',fn],...
        'figurehandle',h,...
        'width','.9\linewidth',...
        'height','.33\linewidth',...
        'showInfo', false...
    );
end

%% Markdown Cell:
% \cref{fig:simulating_state_space_response_05} shows a plot of the differences between the analytic total solution `y_t` and the numerical `y_t_num` for each output.
% Note that calling this "error" is a bit presumptuous, given that we used numerical integration in the analytic solution.
% If a more accurate method is desired, working out the solution, symbolically, is the best.
