%% Isidori I., Example 4.1.4. Feedback linearization
% <html><p class="lead">Coordinate transformation, feedback linearization and zero dynamics.</p></html>
%
%  File: ccs_nonlin_Isidori_Ex414_feedbacklin.m
%  Directory: 4_gyujtemegy/11_CCS/_2_nonlin-pannon/2018
%  Author: Peter Polcz (ppolcz@gmail.com)
%
%  Created on 2018. July 29.
%

%% Prerequisites (1st Matlab practice)
% # Basic symbolic operations
% # Lie derivative and Lie bracket
% # Nonlinear state transformation
% # Pole placement and LQR controller
% # Dynamical systems' simulation with Simulink
% # Solving general transport PDE with Mathematica

%% Addition Mathematica calculations
% 
% Published Mathematica notebook: <file Integral_PDE_Public.pdf>.
% 
% You can download the source notebook: <file Integral_PDE_Public.nb>

%% Model description

syms t u v real
syms x1 x2 x3 real
syms z1 z2 z3 real

x = [x1 ; x2 ; x3];
z = [z1 ; z2 ; z3];

d = @(g) jacobian(g,x);
Lie = @(f,g) d(g)*f;
br = @(f,g) d(g)*f - d(f)*g;

f = [ -x1 ; x1*x2 ; x2 ];
g = [ exp(x2) ; 1 ; 0 ];
h = x3;

phi3 = 1 + x1 - exp(x2);

Phi = [
    h
    Lie(f,h)
    phi3
    ];
Phi_fh = matlabFunction(Phi, 'vars', {x});

% Compute equilibrium point
sol = struct2cell(solve(f,x)); 
xeq = [ sol{:} ]';
zeq = Phi_fh(xeq);

% Quick check: should be zero
% Lie(g,phi3)

%%
pcz_align('%s = %s', '\Phi(x)', latex(Phi));

%%
% Quick check
pcz_align(['L_g \phi_3(x) = ' latex(Lie(g,phi3))])

%%
% $\phi_3(x)$ is given by Isidori, but anyone can check that this is indeed
% a solution for the PDE. This solution can also be computed by
% Mathematica.

Phi_inv = struct2cell(solve(z-Phi, x));
Phi_inv = [ Phi_inv{:} ]';
Phi_inv_fh = matlabFunction(Phi_inv, 'vars', {z});

%%%
% Check if it is indeed the good inverse: $\Phi\big(Phi^{-1}(z)\big) = z$ and
% $\Phi^{-1}\big(\Phi(x)\big) = x$
simplify(Phi_fh(Phi_inv))
simplify(Phi_inv_fh(Phi))

%%
pcz_align('%s = %s', '\Phi^{-1}(z)', latex(Phi_inv));

%%
% Coordinate transformation

b = Lie(f, Lie(f,h));
a = Lie(g, Lie(f,h));
q3 = Lie(f, Phi(3));

%%
pcz_align([ 'b(x) = ' latex(b) ',~ a(x) = ' latex(a) ',~ q_3(x) = ' latex(q3) ])

%%

b = subs(b, x, Phi_inv);
a = subs(a, x, Phi_inv);
q3 = subs(q3, x, Phi_inv);

%%
pcz_align([ 'b(z) = ' latex(b) ',~ a(z) = ' latex(a) ',~ q_3(z) = ' latex(q3) ])

%%
dz = simplify([ z2 ; collect(b+a*u,u) ; q3 ]);

%%
% System dynamics in the new coordinates
pcz_align(['\dot z = ' latex(dz)])

%%
% Linearizing feedback design

u_fdb = a \ (-b + v);

%%
pcz_align(['u = ' latex(u_fdb)]);

%%
% Closed loop
dz_fdb = simplify(subs(dz, u, u_fdb));

%%
pcz_align(['\dot z = ' latex(dz_fdb)]);

%%
% Zero dynamics

Zero_dynamics = simplify(subs(dz_fdb(3), [z ; v], [zeq ; 0]));

%%
% The zero dynamics is stable in the origin.
pcz_align(['\dot z_3 = ' latex(Zero_dynamics)])

%% Linearized dynamics
% Overall dynamics without the zero dynamics.

A = [
    0 1
    0 0
    ];

B = [
    0
    1
    ];

C = [ 1 0 ];

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

s = tf('s');

H = tf(sys)

eps1 = 0.01;
eps2 = 0.01;

H_control = s^2/(eps1*s^2 + eps2*s + 1);

He = minreal(feedback(H*H_control,1))

[POLES, ZEROS] = pzmap(He)

%% Frequency domain controller for the integrator chain
% Transfer function of the feedback controller
% $H(s) = \frac{s + 5}{\alpha s + 1}$.
%
% Download model here: <sim Isidori_Example414_PID.slx>
%
% <img fig/Isidori_Example414_PID.png>

%% <EXTRACT-IO>
pcz_print_model('Isidori_Example414_PID', 'simulate', true)

%% Pole placement controller
% Download model here: 
% 
% # <sim Isidori_Example414_state_feedback_R2015a.slx>
% # <sim Isidori_Example414_state_feedback_R2015b.slx>
% # <sim Isidori_Example414_state_feedback_R2016a.slx>
% # <sim Isidori_Example414_state_feedback_R2016b.slx>
% # <sim Isidori_Example414_state_feedback_R2017a.slx>
% # <sim Isidori_Example414_state_feedback.slx> (R2017b)
%
% <img fig/Isidori_Example414_state_feedback.png>

% State feedback gain designed by LQR
K = lqr(A,B,0.1*eye(2),1);

%% <EXTRACT-IO>
pcz_print_model('Isidori_Example414_state_feedback', 'simulate', true)

Simulink.exportToVersion('Isidori_Example414_state_feedback',...
    'Isidori_Example414_state_feedback_R2015a.slx','R2015a');

Simulink.exportToVersion('Isidori_Example414_state_feedback',...
    'Isidori_Example414_state_feedback_R2015b.slx','R2015b');

Simulink.exportToVersion('Isidori_Example414_state_feedback',...
    'Isidori_Example414_state_feedback_R2016a.slx','R2016a');

Simulink.exportToVersion('Isidori_Example414_state_feedback',...
    'Isidori_Example414_state_feedback_R2016b.slx','R2016b');

Simulink.exportToVersion('Isidori_Example414_state_feedback',...
    'Isidori_Example414_state_feedback_R2017a.slx','R2017a');

%% Pole placement controller with observer (just for fun)
% Note that when a linearizing feedback is applied, we need to know the
% full state vector. But we can play a little bit, what if we forget the
% values of the state variables.
%
% Download model here: <sim Isidori_Example414_obs_state_feedback.slx>
%
% <img fig/Isidori_Example414_obs_state_feedback.png>

% Observer for integrator chain
L = place(A',C',[-2 -3])';

% State feedback gain designed by LQR
K = lqr(A,B,0.1*eye(2),1);

%% <EXTRACT-IO>
pcz_print_model('Isidori_Example414_obs_state_feedback', 'simulate', true, 'density', 140)

