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

%% Code Cell[1]:

clear; close all
save_figures = true;
format compact % console spacing zapper

%% Markdown Cell:
% Let's begin by defining the system parameters.

%% Code Cell[2]:

Kt_spec = 13.7; % oz-in/A ... torque constant from spec
Kv_spec = 10.2; % V/krpm ... voltage constant from spec
Tmax_spec = 2.82; % N-m ... max (stall) torque from spec
Omax_spec = 628; % rad/s ... max speed (no load) from spec 
N_oz = 0.278013851; % N/oz
m_in = 0.0254; % m/in
Kt_si = Kt_spec*N_oz*m_in; % N-m/A
rads_krpm = 1e3*2*pi/60; % (rad/s)/krpm
Kv_si = Kv_spec/rads_krpm; % V/(rad/s)
d = 2.5*m_in; % m ... flywheel diameter
thick = 1*m_in; % m ... flywheel thickness
vol = pi*(d/2)^2*thick; % flywheel volume
rho = 8000; % kg/m^3 ... flywheel density (304 stainless)
m = rho*vol; % kg ... flywheel mass
Jf = 1/2*m*(d/2)^2; % kg-m^2 ... inertia of flywheel
Jr = 56.5e-6; % kg-m^2 ... inertia of rotor
J = Jf+Jr; % kg-m^2 ... total inertia
Bm = 16.9e-6; % N-m/s^2 ... motor damping coef
Bd = 20e-6; % N-m/s^2 ... bearing damping coef
B = Bm + Bd; % N-m/s^2 ... total damping coef
R = 1.6; % Ohm ... armature resistance
L = 4.1e-3; % H ... armature inductance
TF = Kv_si; % N-m/A ... trans ratio/motor constant

%% Markdown Cell:
% The state-space model was derived in \autoref{lec:modeling_real_em_system}. First, we construct the $A$, $B$, $C$, and $D$ matrices (`a`, `b`, `c`, and `d`).
% Then we define a *MATLAB* [LTI system model](https://www.mathworks.com/discovery/state-space.html) using the `ss` [command](https://www.mathworks.com/help/control/ref/ss.html).

%% Code Cell[3]:

a = [-B/J,TF/J;-TF/L,-R/L];
b = [0;1/L];
c = [1,0;-B,TF;-TF,-R;0,1;1,0;B,0;...
    0,R;0,1;TF,0;0,1;1,0;0,-TF;0,0;0,1];
d = [0; 0; 1; 0; 0; 0; 0; 0; 0; 0; 0; 0; 1; 0];
sys = ss(a,b,c,d);

%% Markdown Cell:
% ## Simulating the step response

%% Markdown Cell:
% The step input is widely used to characterize the transient response of a system. *MATLAB*'s `step` [function](https://www.mathworks.com/help/control/ref/step.html) conveniently simulates the step response of an LTI system model.

%% Code Cell[4]:

[ys_a,t_a] = step(sys);
disp([t_a(1:6),ys_a(1:6,1:4)]) % print a little

%% Markdown Cell:
% The vector `t_a` contains values of time and array `ys_a` contains a vector of time-series values for each output. If one would like the output for a step input $k u_s(t)$ (scaled *unit* step $u_s(t)$), by the principle of superposition for linear systems, one can scale the output by $k$.
% The outputs are plotted in \autoref{fig:motor_across_through}.

%% Code Cell[5]:

if save_figures
h = figure;
labels = {...
    '$\Omega_J$','$T_J$',...
    '$v_L$','$i_L$',...
    '$\Omega_B$','$T_B$',...
    '$v_R$','$i_R$',...
    '$v_1$','$i_1$',...
    '$\Omega_2$','$T_2$',...
    '$V_s$','$I_s$'...
};
for i = 1:2:size(ys_a,2)
    subaxis(size(ys_a,2)/2,1,(i+1)/2,...
    'Spacing',0.06,'PaddingBottom',0.005,...
    'Margin',0,'SpacingVert',0.03);
    [ax,l1,l2] = plotyy(t_a,ys_a(:,i),t_a,ys_a(:,i+1));
    set(l1,'linewidth',1.5); set(l2,'linewidth',1.5);
    ylabel(ax(1),labels{i},'interpreter','latex')
    ylabel(ax(2),labels{i+1},'interpreter','latex')
    xlim(ax(1),[0,t_a(end)]); xlim(ax(2),[0,t_a(end)]);
    ticks = get(ax,'xtick');
    set(ax,'xtick',[]);
end
set(ax,'xtick',ticks{1});
xlabel('time (s)','interpreter','latex')
hgsave(h,'figures/temp');
end

%% Code Cell[6]:

if save_figures
    fn = 'motor_across_through.tex';
    h = hgload('figures/temp');
    cleanfigure;
    matlab2tikz(['figures/',fn],...
        'figurehandle',h,...
        'width','.8\linewidth',...
        'height','.8\textheight',...
        'extraAxisOptions',...
        [...
            'tick label style={/pgf/number format/fixed},',...
            'try min ticks=3,',...
            'max space between ticks=50pt,',...
            'xticklabel shift={.1cm},',...
            'clip bounding box=upper bound,'...
        ],...
        'showInfo', false...
    );
end

%% Markdown Cell:
% It is also interesting to inspect the power flow and energy associated with each element. Since we have simulated both the across and the through variable for each element, we can compute the instantaneous power by simply taking the product of them at each time step. Moreover, we can cumulatively compute the energy contribution of that power for each element. For energy storage elements, this is the change in energy stored or supplied; for energy dissipative elements, this is the change in energy dissipated; for source elements, this is the energy supplied or absorbed. The results are plotted in \autoref{fig:motor_power_energy}.

%% Code Cell[7]:

P = NaN*ones(size(ys_a,1),size(ys_a,2)/2);
E = NaN*ones(size(P));
j = 0;
for i = 1:2:size(ys_a,2)
    j = j+1;
    P(:,j) = ys_a(:,i).*ys_a(:,i+1);
    E(:,j) = cumtrapz(t_a,P(:,j));
end
disp('power:');
disp(P(1:6,1:4)) % print a little
disp('energy change:')
disp(E(1:6,1:4)) % print a little

%% Code Cell[8]:

if save_figures
h = figure;
labels = {'J','L','B','R','1','2','s'};
for i = 1:2:size(ys_a,2)
    subaxis(size(ys_a,2)/2,1,(i+1)/2,...
    'Spacing',0.06,'PaddingBottom',0.005,...
    'Margin',0,'SpacingVert',0.03);
    P = ys_a(:,i).*ys_a(:,i+1);
    [ax,l1,l2] = plotyy(t_a,P,t_a,cumtrapz(t_a,P));
    set(l1,'linewidth',1.5);
    set(l2,'linewidth',1.5);
    ylabel(ax(1),['$\mathcal{P}_',labels{(i+1)/2},'$'],'interpreter','latex')
    ylabel(ax(2),['$\mathcal{E}_',labels{(i+1)/2},'$'],'interpreter','latex')
    xlim(ax(1),[0,t_a(end)]);
    xlim(ax(2),[0,t_a(end)]);
    ticks = get(ax,'xtick');
    set(ax,'xtick',[]);
end
set(ax,'xtick',ticks{1});
xlabel('time (s)','interpreter','latex')
hgsave(h,'figures/temp');
end

%% Code Cell[9]:

if save_figures
    fn = 'motor_power_energy.tex';
    h = hgload('figures/temp');
    cleanfigure;
    matlab2tikz(['figures/',fn],...
        'figurehandle',h,...
        'width','.8\linewidth',...
        'height','.8\textheight',...
        'extraAxisOptions',...
        [...
            'tick label style={/pgf/number format/fixed},',...
            'try min ticks=3,',...
            'max space between ticks=50pt,',...
            'xticklabel shift={.1cm},',...
            'clip bounding box=upper bound,'...
        ],...
        'showInfo', false...
    );
end

%% Markdown Cell:
% ## Estimating parameters from the step response

%% Markdown Cell:
% Often, our model has a couple parameters we don't know well from the specifications, but must attempt to measure. For the system under consideration, perhaps the two parameters most interesting to measure are the dominant time constant and the transformer ratio $TF$ (most important). In this section, we explore how one might estimate them from a measured step response. Other parameters in the system could be similarly estimated. 
% 
% By way of the transfer function, the state-space model can be transformed into input-output differential equations.

%% Code Cell[10]:

syms B_ J_ TF_ L_ R_ Vs_ s % using underscore for syms

a_ = [-B_/J_,TF_/J_;-TF_/L_,-R_/L_];
b_ = [0;1/L_];

(s*eye(2)-a_)^-1*b_

%% Markdown Cell:
% The differential equation for $\Omega_J$ is
% \begin{align}
%     \frac{d^2 \Omega_J}{d t^2} + \left(\frac{R}{L} + \frac{B}{J}\right) \frac{d \Omega_J}{d t} + \frac{TF^2 + B R}{J L} \Omega_J &= \frac{TF}{J L} V_s.
% \end{align}
% The corresponding characteristic equation is
% \begin{align}
%     \lambda^2 + 
%     \left(\frac{R}{L} + \frac{B}{J}\right) \lambda + 
%     \frac{TF^2 + B R}{J L} = 0
% \end{align}
% which has solution
% \begin{align}
%     \lambda_{1,2} = -\frac{1}{2}\left(\frac{R}{L} + \frac{B}{J}\right)
%     \pm
%     \frac{1}{2} \sqrt{\left(\frac{R}{L} + \frac{B}{J}\right)^2 - 
%     4\frac{TF^2 + B R}{J L}}.
% \end{align}
% For a step input $V_s(t) = k$, $\Omega_J(0) = d\Omega_J(0)/dt = 0$, and distinct roots $\lambda_1$ and $\lambda_2$, the solution is
% \begin{align}\label{eq:forced2}
% 	\Omega_J(t) = k\frac{J L}{TF^2 + B R} 
% 	\left(
% 		1 -
% 		\frac{1}{\lambda_2-\lambda_1} 
% 		\left(
% 			\lambda_2 e^{\lambda_1 t} -
% 			\lambda_1 e^{\lambda_2 t}
% 		\right)
% 	\right)
% \end{align}

%% Markdown Cell:
% Let's compute $\lambda_1$ and $\lambda_2$.

%% Code Cell[11]:

lambda12 = -1/2*(R/L+B/J) + ...
    [1,-1]*1/2*sqrt((R/L+B/J)^2 - 4*(TF^2+B*R)/(J*L))

%% Markdown Cell:
% Both values are *real*, so we expect not an oscillation, but a decay to a final value. However, that decay occurs with two different time constants: $\tau_1 = -1/\lambda_1$ and $\tau_2 = -1/\lambda_2$.

%% Code Cell[12]:

tau12 = -1./lambda12
disp(['ratio: ',num2str(tau12(1)/tau12(2))])

%% Markdown Cell:
% So second decays much faster than the first. That's good news for our estimation project because we can easily ignore the step response's first $5\tau_2 \approx 0.0134$ s and assume the rest is decaying at $\tau_1$, which we call the *dominant time constant* and which we would like to estimate.
% 
% Let's generate some fake response data to get the idea. We'll layer on some Gaussian noise with `randn` to be more realistic. The data is plotted in \autoref{fig:motor_transient_step_data}.

%% Code Cell[13]:

t_data = linspace(0,-6/lambda12(1),200);

O_fun = @(t) TF/(TF^2+B*R)*...
    (1-1/(lambda12(2)-lambda12(1))*...
    (lambda12(2)*exp(lambda12(1)*t)-...
    lambda12(1)*exp(lambda12(2)*t)));
rng(2);
O_data = O_fun(t_data) + .5*randn(size(t_data));

%% Code Cell[14]:

if save_figures
    h = figure;
    p = plot(t_data,O_data,'.','markersize',5);
    xlabel('time (s)')
    ylabel('$\Omega_J$ (rad/s)','interpreter','latex')
    grid on;
    hgsave(h,'figures/temp');
end

%% Code Cell[15]:

if save_figures
    fn = 'motor_transient_step_data.tex';
    h = hgload('figures/temp');
    cleanfigure;
    matlab2tikz(['figures/',fn],...
        'figurehandle',h,...
        'width','.8\linewidth',...
        'height','.5\linewidth',...
        'extraAxisOptions',...
        [...
            'scaled ticks=false,'
        ],...
        'showInfo', false...
    );
end

%% Markdown Cell:
% Let's trim the data to eliminate the time interval corresponding to the first five of the "fast" time constant $\tau_2$.

%% Code Cell[16]:

[t_5,i_5] = min(abs(t_data-(-5/lambda12(2)))); % delete
t_data_trunc = t_data((i_5+1):end);
O_data_trunc = O_data((i_5+1):end);

%% Markdown Cell:
% We need want to take the natural logarithm of the data so we can perform a linear regression to estimate the "experimental" slow time constant $\tilde{\tau}_1$. We must first estimate the steady-state value $\Omega_{J\infty}$ (which we'll also need). We don't want to just take the last value in the array due to its noisiness. The data goes for six slow time constants, so averaging the data for the last time constant is a good estimate.

%% Code Cell[17]:

[t_ss,i_ss] = ...
    min(abs(t_data_trunc-(-5/lambda12(1)))); % start here
O_data_ss = O_data_trunc((i_ss+1):end);
mu_O_ss = mean(O_data_ss)
S_mu_O_ss = std(O_data_ss)/sqrt(length(O_data_ss))

%% Markdown Cell:
% Let's use this result to transform the data into its linear form.

%% Code Cell[18]:

O_lin = log(-(O_data_trunc-mu_O_ss));
O_lin_complex = find(imag(O_lin)>0);
disp(['number of complex values: ',...
    num2str(length(O_lin_complex))])

%% Markdown Cell:
% Now we have encountered a problem. The noisiness of the data makes some of our points wander into negative-land. Logarithms of negative numbers are complex. Naive approaches like just taking real parts, excluding complex values, or coercing complex values to $-\infty$ all have the issue of biasing the data.
% 
% There are a lot of approaches we could take. The best approaches include nonlinear regression and discrete filtering to smooth the data (e.g. `filtfilt`).
% 
% We opt for an easier approach: we find the index at which the time series first transgresses the boundary and exclude the data beyond the previous index.

%% Code Cell[19]:

i_bad = O_lin_complex(1);
t_lin_trunc = t_data_trunc(1:i_bad-1);
O_lin_trunc = O_lin(1:i_bad-1);

%% Markdown Cell:
% This is plotted in \autoref{fig:motor_transient_linear_fit} along with the linear regression least-squares fit, computed below.

%% Code Cell[20]:

pf = polyfit(t_lin_trunc,O_lin_trunc,1);
O_lin_fit = polyval(pf,t_lin_trunc);
tau_1_est = -1/pf(1)

%% Code Cell[21]:

if save_figures
    h = figure;
    p = plot(...
        t_lin_trunc,O_lin_trunc,'b.',...
        t_lin_trunc,O_lin_fit,'r-',...
        'markersize',5);
    xlabel('time (s)')
    ylabel(...
        '$\ln(\Omega_{J\infty}-\Omega_J(t))$',...
        'interpreter','latex'...
    )
    grid on;
    legend('transformed data','linear fit')
    hgsave(h,'figures/temp');
end

%% Code Cell[22]:

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

%% Markdown Cell:
% So our estimate for $\tau_1$ is $\tilde{\tau}_1 = 60.3$ ms. Recall that our analytic expression for $\tau_1$ is known in terms of other parameters. Similarly, the steady-state value of $\Omega_J$, which has already been estimated to be $\Omega_{J\infty} = 10.18$ (i.e. `mu_O_ss`). This occurs when the time-derivatives of $\Omega_J$ are zero. From the solution for $\Omega_J$ (or its differential equation), for constant $V_s(t) = \overline{V_s}$, this occurs when 
% \begin{align}
% \label{eq:omega_J_ss}
%     \Omega_{J\infty} &= \frac{TF}{TF^2+B R} \overline{V_s}.
% \end{align}
% An analytic expression for $TF$ can be found by solving \autoref{eq:omega_J_ss}, which yields
% \begin{align}
%     TF = \overline{V_s} \pm \frac{1}{2\tilde{\Omega}_{J\infty}}\sqrt{V_s^2-4 B R \tilde{\Omega}_{J\infty}^2}
% \end{align}

%% Markdown Cell:
% We choose the solution closer to the *a priori* (spec) value of $0.0974$.

%% Code Cell[23]:

TF_est = (1 + (- 4*B*R*mu_O_ss^2 + 1^2)^(1/2))/(2*mu_O_ss)

%% Markdown Cell:
% This estimate $\tilde{TF} = 0.0976$ is very close to the value given in the specification sheet *because we constructed it to be so*. Real measurements would probably yield an estimate further from the specification, which is why we would estimate it.

%% Markdown Cell:
% We *could* solve the analytic expression for $\lambda_1$ for the parameter $B$ we might like to estimate as $\tilde{B}$ in terms of our estimate $\tilde{\tau}_1$:
% \begin{align}
%     \tilde{B} = \frac{\tilde{TF}^2 \tilde{\tau}_1^2 - J R \tilde{\tau}_1 + J L}{\tilde{\tau}_1(L - R \tilde{\tau}_1)}.
% \end{align}

%% Code Cell[24]:

B_est = (TF_est^2*tau_1_est^2-J*R*tau_1_est+J*L)/(tau_1_est*(L-R*tau_1_est))

%% Markdown Cell:
% However, this is almost three times the "true" value. A little investigation reveals this to be due to a relatively high sensitivity of the expression for $\tidle{B}$ to $\tilde{\tau}_1$ and especially $\tilde{TF}$.
