%  Program name:  zerodynamics_tracking_comparison_Ex6_6_5_main
%  The purpose of this example is to compare the tracking performance of a
%  fully and partially linearizable system resulting from different choices
%  of the output equations.
clc, clear all, close all, format short g
global g K KK ro rro ncon
%%  Normal-form system matrices in controllable canonical form
Ac=[0 1; 0  0];						%  Controllable Ac in z-domain
Bc=[0;  1];						%  Controllable Bc in z-domain
Cc=[1  0];						%  Output matrix
D=0;							%   Throughput matrix
g=[0; 1];						%  g(x) matrix in xdot=f (x) +g (x) u
%%  Pole placement to design for normal form
zeta=0.7;  wn=6; wd=wn*sqrt (1-zeta^2)
p=[-zeta*wn+j*wd   -zeta*wn-j*wd]     					%  Chosen complex poles
K=place(Ac, Bc, p)					%  Poles placement gain matrix K
Acl=Ac-Bc*K;					%  CL system matrix in z-domain
%  Step response of y(t)=x1 (t) in the z-coordinates with r(t)=ro  (rad)
ro=8;
t=0: 0.01:2;
step (Acl,Bc*K(1)*ro,Cc,D,1,t) ;grid			%  Get y(t)=x1 (t) step response statistics
title ('Step response of fully linearizable system:  \rho=2=n:  y(t)=x_1(t)', 'fontsize',12)
xlabel ('t', 'fontsize',12), ylabel ('y(t)=x_1(t) ', 'fontsize',12)
%%  Case 1:  Step responses of fully linearizable system
ro=8;							%  Step input r(t)=ro
x0=[0; 0.2];						%  Initial condition of x
t=0:0.01:2;
ncon=1;						%  Case 1
[t,x]=ode23 ('zerodynamics_tracking_comparison_Ex_6_6_5_fn', t, x0);
figure,plot(t,x(:,1),t,x(:,2), '--', 'linewidth',1.5),grid
xlabel('t (sec) ', 'Fontsize',12)
title('Step response of fully linearizable system:\rho=2:x_1(t)==>ro=8,x_2(t)','fontsize',12)
legend('x_1(t)', 'x_2(t) ', 'location', 'best')
%%  Case 2:  Step responses of normal form – note that this system is not robust
rro=2;
KK=4.2;						%  Control gain = closed-loop pole
t=0:0.01:6;
x0=[0; 0.2];						%  Initial condition of x
ncon=2;						%  Case 2
[t,x]=ode23('zerodynamics_tracking_comparison_Ex_6_6_5_fn',t,x0);
figure,plot(t,x(:,1),t,x(:,2), '—','linewidth',1.5)grid
xlabel('t(sec)', 'Fontsize',12)
title('Step responses of normal form:  \rho=1: x_1(t), x_2(t) ==>r(t)=2',
'fontsize',12)
legend('x_1(t) '',x_2(t) ', 'location','best')
n=2*[4.2]; d = [1 4.2];	%  Get the settling time of Case 2
figure,step(n,d),grid


%  Function name:  zerodynamics_tracking_comparison_Ex6_6_5_fn
%  function xdot=zerodynamics_tracking_comparison_Ex_6_6_5_fn
%  The purpose of this example is to compare the tracking performance of a
%  fully and partially linearizable system resulting from different choices
%  of the output equations.

function xdot=zerodynamics_tracking_comparison_Ex_6_6_5_fn(t,x)

global g K KK ro rro  ncon   

f=[-x(1)+x(2)^3;-x(2)];
alpha=x(1)-4*x(2)^3; 
Dx=3*x(2)^2;					%  Decoupling matrix

%%  Case 1:  Step input for fully linearizable system
if ncon==1						%  Case 1

	z=[x(1);-x(1)+x(2)^3];			%  Transformation z=T(x)
	v=-K(1)*z(1)+K(2)*z(2)+K(1)*ro;	%  z-domain linear tracking control law
	u=inv(Dx)*(-alpha+v);	%  Linearizing feedback tracking control law
else							%  Case 2

%%  Case 2:  Step input for normal form
             alpha=-x(2);
             Dx=1;
             v=-KK*(x(2)-rro);
             u=inv(Dx)*(-alpha+v); %  Linearizing feedback tracking control law

end

      xdot=f+g*u;