% Section4.m
%
% Generate figures for Section 4 of paper
% "The Effect of Rotor Tilt on the Stability and Gust Rejection Properties
%  of VTOL Multirotor Aircraft"
% by James Whidborne Arthur Mendez and Alastair Cooke

% (c) James Whidborne
% Cranfield University, 20 November 2020 

disp(' ')
disp('4 Linear Analysis')
disp('-----------------')
disp(' ')

%% Analyze effect of rotor tilt angle,  Upsilon$  linearized body axes
% model of the dynamics in still air ignoring the aerodynamics of the rotor
% state: \x=[z,w,x,u,theta,q]
% control: \u=[Tl+Tr-mg, Tl-Tr]
% output: \y=[z,x,theta]
syms s That Upsilon m MoI ell g uT1 uT2 % define sybolic variables
% s - Laplace variavble
% thetahat - trim roll angle (rad)
% That - trim thrust (N)
% Upsilon - tilt angle (rad)
% m - mass (kg)
% MoI - moment of inertia (kgm^2)
% ell - arm length (m)
% g  - gravitational constant (m/s^2)

%
disp('4.1 Ignoring the Rotor Aerodynamics')
disp('    -------------------------------')
disp(' ')
disp('Trim: T=That and theta=thetahat')
disp('Trim: That=mg/(2 cos Upsilon)  and   thetahat=0 ')
disp('Linearized state matrix eq (4.4)')
AT = [0 1 0 0 0 0;
    0 0 0 0 0 0;
    0 0 0 1 0 0;
    0 0 0 0 -g 0;
    0 0 0 0 0 1;
    0 0 0 0 0 0]
disp(' ')
disp('Linearized input matrix eq (4.5)')
BT = [0,                   0;
    -cos(Upsilon)/m,       0;
    0,                     0;
    0,       -sin(Upsilon)/m;
    0,                     0;
    0, -ell*cos(Upsilon)/MoI]
disp(' ')
disp('Output matrix')
CT = [1 0 0 0 0 0;
    0 0 1 0 0 0;
    0 0 0 0 1 0]
disp(' ')
disp('Non-zero thetahat trim transfer function matrix')
G = CT*inv(s*eye(6)-AT)*BT


%% with aerodynamics
disp(' ')
disp('4.2 Including Rotor Aerodynamics')
disp('    ----------------------------')
disp(' ')

% x=[z,w,x,u,theta,q]
% u=[Omegal+Omegar-\hat{Omegal}-\hat{Omegar}, Omegal-Omegar-\hat{Omegal} +\hat{Omegar}  ]

syms Zw Xu Xq Mq Mu Zu1 Xu2 Mu2 % aerodynamic derivatives

disp(' ')
disp('Linearized state matrix eq (4.9)')
A = [0  1  0  0  0  0;
    0 Zw  0  0  0  0;
    0  0  0  1  0  0;
    0  0  0 Xu -g Xq;
    0  0  0  0  0  1;
    0  0  0 Mu  0 Mq]
A22 = [Xu -g Xq;
    0  0  1;
    Mu  0 Mq];
A11 = [ 0  1  0;
    0 Zw  0;
    0  0  0] ;
B = [  0   0;
    Zu1   0;
    0   0;
    0 Xu2;
    0   0;
    0 Mu2];
B2 = [Xu2;
    0;
    Mu2];
C = [ 1 0 0 0 0 0;
    0 0 1 0 0 0;
    0 0 0 0 1 0];
C2 = [1 0 0;
    0 1 0];
disp(' ')
disp('Characteristic equation (4.10)')
q = charpoly(A,s)
Q = charpoly(A22,s)*charpoly(A11,s)

disp(' ')
disp('System transfer function')
G = C*inv(s*eye(6)-A)*B;
simplify(G)

clear s That Upsilon m MoI ell g uT1 uT2 Zw Xu Xq Mq Mu Zu1 Xu2 Mu2
clear AT BT CT DT A A11 A22 B B2 C C2 q Q G
disp('Press any key to continue ...')
pause


disp(' ')
disp(' ')
%%
echo on
% trim and linearize birotor PVTOL  in constant  horizontal wind for a range
% of Upsilon \in[Upsilon_min,Upsilon_max] and plot aerodynamic derivatives and
% eigenvalues
%
% NOTE that the eigenvalue locations are very sensitive to Upsilon when Upsilon
% is near zero. Hence we cannot rely on a fixed Upsilon step size to plot
% the eigenvalue variation with Upsilon accurately. Instead, a variable stepsize
% is calculated, which ensures the stepsize is small when the eigenvalue
% sensitivity is high.
echo off
disp(' ')

%% parameters

[~, pvtol, ~, ~] = Coefficients;
g=9.81;     % gravitational constant (m/s^2)

Vw = 0;     %  constant  horizontal wind airspeed (m/s)
Upsilon_max = pi*15/180; %  define range Upsilon  \in[Upsilon_min,Upsilon_max]=[-15,15]
Upsilon_min = -Upsilon_max;

% step size parameters -  proportional to eigenvalues sensitivity
Upsilonstep_min=1e-5;  % minimum step size
Upsilonstep_scale=50; % larger Upsilonstep_scale => smaller step - typically \in [5,100]

% initial trim guess values
Omegal0 = 150; Omegar0 = 150; theta0 = 0;

% initialize storage vectors
Upsilon = []; thetatrim = []; Omegaltrim = []; Omegartrim = [];
e_val = []; % eigenvalues
Zw = [];    % dwdot/dw
Xu = [];    % dudot/du
Xq = [];    % dudott/dq
Mu = [];    % dudot/du
Mq = [];    % dqdot/dq
Zu1 = [];   % dwdot/du1
Xu2 = [];   % dudot/du2
Mu2 = [];   % dqdot/du2
ZVw = [];   % dwdot/dWv
XVw = [];   % dudot/dWv
MVw = [];   % dqdot/dWv

% options for trim residual minimization
trimoptions = optimset('fminsearch'); trimoptions = optimset('TolFun',1e-7,'TolX',1e-7);

% set initial trim points guess
xtrim0 = [0;0;0;0;theta0;0]; utrim0 = [Omegal0+Omegar0;Omegal0-Omegar0;Vw];
Upsiloni = Upsilon_min;  % first Upsilon value-Upsilon_max;
i=1; % loop counter
fprintf(['\ni=%4i  Upsilon=%8.5f'  ], i,Upsiloni); % display iteration counter
C = [1 0 0 0 0 0;% outputs
    0 0 1 0 0 0;% outputs
    0 0 0 0 1 0];% outputs
%% loop though over range of Upsilon
while Upsiloni<=Upsilon_max
    
    Upsilon(i) = Upsiloni; % record Upsilon
    
    % trim & lin
    [ A,B,xtrim,utrim ] = PvtolTrimLin( pvtol,Vw,Upsiloni,xtrim0,utrim0,trimoptions );
    % Record trim
    thetatrim(i) = xtrim(5);
    Omegaltrim(i) = (utrim(1)+utrim(2))/2;
    Omegartrim(i) = (utrim(1)-utrim(2))/2;
    % Record aero derivatives
    Zw(i)=A(2,2); Xu(i)=A(4,4); Xq(i)=A(4,6);
    Mu(i)=A(6,4); Mq(i)=A(6,6);
    % record control derivatives
    Zu1(i)=B(2,1);  % Zu2(i)=B(2,2);
    Xu2(i)=B(4,2);  % Xu1(i)=B(4,1);
    Mu2(i)=B(6,2);  % Mu1(i)=B(6,1);
    % record disturbance derivatives
    ZVw(i)=B(2,3); XVw(i)=B(4,3); MVw(i)=B(6,3);
    
    % get eigenvalues
    eigA=eig(A);
    if(all(isreal(eigA))) % sort eigenvalues
        [eA99,eAi] = sort(abs(eigA));    % if real sort on distance from origin
    else
        [eA99,eAi] = sort(eigA);         % default is distance from origen
    end
    e_val(:,i) = eigA(eAi); % record eigenvalues
    zxpoly = [Xu2(i), Mu2(i)*Xq(i)-Mq(i)*Xu2(i), - Mu2(i)*g];
    %zthetapoly=[Mu2(i),  Mu(i)*Xu2(i) - Mu2(i)*Xu(i)];
    zx(:,i) = roots(zxpoly);
    ztheta(i)=(Mu2(i)*Xu(i)-Mu(i)*Xu2(i))/Mu2(i);
    % Upsilon increment - stepsize is smaller if eigenvalue sensitivity is larger
    Upsilonstep = max(1/(Upsilonstep_scale*max(condeig(A))),Upsilonstep_min);
    fprintf([repmat(8,1,24) 'i=%4i  Upsilon=%8.5f' ], i,Upsilon(i)); % display iteration counter (8 is ASCII backspace code)
    if (Upsiloni<Upsilon_max && Upsiloni+Upsilonstep>Upsilon_max) % stepped over end of range?
        Upsiloni=Upsilon_max; % then last step
    else
        Upsiloni=Upsiloni+Upsilonstep; % Upsilon increment
    end
    i = i+1; % counter increment
    xtrim0=xtrim; utrim0=utrim; % reset trim guess point
    
end
fprintf( '\n'); % newline

%% plots
figure(11), %Variation of aerodynamic derivatives with Upsilon
% plot marker on points at 15 deg intervals for Mq
Upsilon15=[-15:3:15]*pi/180;
for ii=1:length(Upsilon15)
    idxi=find(abs(Upsilon-Upsilon15(ii))<0.01);
    Mq15(ii)=Mq(min(idxi));
end
plot(Upsilon*180/pi,Zw,':',...
    Upsilon*180/pi,Xu,'--',Upsilon*180/pi,Xq,'-.',...
    Upsilon*180/pi,Mu,'-',Upsilon15*180/pi,Mq15,'-x',...
    'linewidth',lw)
h99=gca; h99.XMinorTick='on'; h99.YMinorTick='on';
grid on
xlabel('$\Upsilon$ (deg)','Interpreter' ,'Latex','fontsize',fs)
ylabel('Derivatives $\{Z_w,X_u,X_q,M_u,M_q\}$','Interpreter' ,'Latex','fontsize',fs)
g99=legend('$Z_w$','$X_u$','$X_q$','$M_u$','$M_q$');
set(g99,'Interpreter' ,'Latex','fontsize',11)
% save
if(printflag) save_pdf(gcf,'DerivsUpsilon0'); end

% sort eigenvalues
if max(max(abs(diff(e_val'))))>1 % if discontinuities
    idcs=min(find(max(diff(imag(e_val')),[],2)>1));
    e_val(:,idcs:idcs+1);
    idx=[1,2,5,3,4,6];
    e_val=[e_val(:,1:idcs),e_val(idx,1+idcs:end)];
    
    
    idcs=min(find(max(diff(real(e_val')),[],2)>1));
    e_val(:,idcs:idcs+1);
    idx=[1,2,3,5,4,6];
    e_val=[e_val(:,1:idcs),e_val(idx,1+idcs:end)];
end

idcs=min(find(Upsilon>5*pi/180));
e_val(:,idcs:idcs+1);
idx=[1,2,5,4,3,6];
e_val=[e_val(:,1:idcs),e_val(idx,1+idcs:end)];



%lw=2; % linewidth
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(12) %Variation in real parts of eigenvalues  of small perturbation model for V_w=0
plot(Upsilon*180/pi,real(e_val)','-','linewidth',lw)
h99=gca; h99.XMinorTick='on'; h99.YMinorTick='on';
grid
xlabel('$\Upsilon$ (deg)','Interpreter' ,'Latex','fontsize',fs)
ylabel('$\mathrm{Re}(\lambda)$  ','Interpreter' ,'Latex','fontsize',fs)
% save
if(printflag) save_pdf(gcf,'ReEigUpsilon0'); end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(13)% Variation in imaginary parts of eigenvalues  of small perturbation model for V_w=0
plot(Upsilon*180/pi,imag(e_val)','linewidth',lw)
h99=gca; h99.XMinorTick='on'; h99.YMinorTick='on';
grid on

xlabel('$\Upsilon$ (deg)','Interpreter' ,'Latex','fontsize',fs)
ylabel('$\mathrm{Im}(\lambda)$  ','Interpreter' ,'Latex','fontsize',fs)
% save
if(printflag) save_pdf(gcf,'ImEigUpsilon0'); end

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(14)% Variation in control derivatives with Upsilon
yyaxis left
plot(Upsilon*180/pi,Mu2,'linewidth',lw)
h99=gca; ylim(h99,[-0.57 -0.55]); % change axes limits
h99.YMinorTick='on';
xlabel('$\Upsilon$ (deg)','Interpreter' ,'Latex','fontsize',fs)
ylabel('$M_{u2}$ (rad/ms)','Interpreter' ,'Latex','fontsize',fs)

yyaxis right
plot(Upsilon*180/pi,Xu2,'--',Upsilon*180/pi,Zu1,'-.', 'linewidth',lw)
h99=gca; ylim(h99,[-0.08 0.02]); % change axes limits
h99.XMinorTick='on';
h99.YMinorTick='on';
grid on
xlabel('$\Upsilon$ (deg)','Interpreter' ,'Latex','fontsize',fs)
ylabel('$\{X_{u2}, Z_{u1}\}$ (/s)','Interpreter' ,'Latex','fontsize',fs)
g99=legend('$M_{u2}$ ','$X_{u2}$','$Z_{u1}$');
set(g99,'Interpreter' ,'Latex','fontsize',11,'Position',[0.72  0.3  0.13  0.13 ])
% save
if(printflag) save_pdf(gcf,'BuUpsilon0'); end

%% Variation in disturbance derivatives with Upsilon
figure(15)%
yyaxis left
plot(Upsilon*180/pi,MVw,'linewidth',lw)
h99=gca; ylim(h99,[-4 4]); % change axes limits
h99.YMinorTick='on';
xlabel('$\Upsilon$ (deg)','Interpreter' ,'Latex','fontsize',fs)
ylabel('$M_{V_W}$ (rad/ms)','Interpreter' ,'Latex','fontsize',fs)

yyaxis right
plot(Upsilon*180/pi,XVw,'--',Upsilon*180/pi,ZVw,'-.','linewidth',lw)
grid on
h99=gca; h99.XMinorTick='on'; h99.YMinorTick='on';
xlabel('$\Upsilon$ (deg)','Interpreter' ,'Latex','fontsize',fs)
ylabel('$\{X_{V_W}, Z_{V_W}\}$ (/s)','Interpreter' ,'Latex','fontsize',fs)
g99=legend('$M_{V_W}$ ','$X_{V_W}$','$Z_{V_W}$');
set(g99,'Interpreter' ,'Latex','fontsize',11,'Location','east')

% save
if(printflag) save_pdf(gcf,'BWUpsilon0'); end

%% %%%%%%%%%%%%%%%% OTHER PLOTS %%%%%%%%%%%%%%%%%%%%%%%%%
if(0)
    %% Variation in zeros in x channel with Upsilon
    
    iUpsilon0=max(find(Upsilon<0)); % where Upsilon=0
    figure(115)
    plot(Upsilon(1:iUpsilon0)*180/pi,real(zx(:,1:iUpsilon0)),'-','linewidth',lw)
    grid on
    xlabel('$\Upsilon$ (deg)','Interpreter' ,'Latex','fontsize',fs)
    ylabel('Zeros of $G_{22}(s)$ --- real part','Interpreter' ,'Latex','fontsize',fs)
    h99=gca; h99.XMinorTick='on'; h99.YMinorTick='on';
    
    figure(116)
    plot(Upsilon(iUpsilon0+1:end)*180/pi,real(zx(:,iUpsilon0+1:end)),'-','linewidth',lw)
    grid on
    xlabel('$\Upsilon$ (deg)','Interpreter' ,'Latex','fontsize',fs)
    ylabel('Zeros of $G_{22}(s)$ --- real part','Interpreter' ,'Latex','fontsize',fs)
    h99=gca; h99.XMinorTick='on'; h99.YMinorTick='on';
    ylim(h99,[-80 80]); % change axes limits
    
    disp( ['At Upsilon = ' num2str(Upsilon_max),'deg the zero pair for the x channel is at '])
    zx(:,end)
    % save
    % if(printflag) save_pdf(gcf,'xzeroUpsilon0plus'); end
    
    %% Variation in zero in theta channel with Upsilon
    figure(117)% V
    plot(Upsilon*180/pi,ztheta,'linewidth',lw)
    h99=gca;
    h99.XMinorTick='on';
    h99.YMinorTick='on';
    %ylim(h99,[-4 3]); % change axes limits
    xlabel('$\Upsilon$ (deg)','Interpreter' ,'Latex','fontsize',fs)
    ylabel('Zero of $G_{32}(s)$','Interpreter' ,'Latex','fontsize',fs)
    grid
    
    % save
    %if(printflag) save_pdf(gcf,'thetazeroUpsilon0'); end
    
    
    %% Routh Hurwitz stability check
    figure(198)
    plot(Upsilon*180/pi,Mu,Upsilon*180/pi,(Mq+Xu).*Mq.*Xu./(Xq.*(Mq+Xu)-g),Upsilon*180/pi,-(Mq+Xu).*(Mq.*Xu-Mu.*Xq)-g*Mu)
    grid on
    xlabel('$\Upsilon$ (deg)','Interpreter' ,'Latex','fontsize',fs)
    ylabel(' ','Interpreter' ,'Latex','fontsize',fs)
    title('Routh Hurwitz test')
    
    %% check plot/eigenvalues sensitivity
    figure(199)
    plot(Upsilon(1:end-1)*180/pi,abs(diff(e_val')))
    grid on
    xlabel('$\Upsilon$ (deg)','Interpreter' ,'Latex','fontsize',fs)
    ylabel('diff','Interpreter' ,'Latex','fontsize',fs)
    title('Eigenvalues sensitivity')
    %maxdiff=max(max(abs(diff(e_val')))) % max eignevalue sensitivity(unnormalized!!)
end%(0)