% Angular Rates Controller Synthesis 
% Rotation Dynamics = Angular Rates 
% Carrilho - 21 Jan 2015

% clear all  
clear, clc, close all 
format shorte

% Load model_parameters 
load model_parameters 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Matrices for angular speeds submodel
% Linearized Rotation Dynamics - p 
A = 0;
B = 1/Jx;
C = 1;

% Indexes 
n_A = size(A,1);
n_B = size(B,2);
n_C = size(C,1);

% Augmented matrices for robust tracking 
A_aug = [zeros(n_C,n_C) C; zeros(n_A,n_C) A];
B_aug = [zeros(n_C,n_B); B];

% LQR control design
% Q_aug = [...
%      1     0;
%      0     1];
% R_aug = [1e8];    
% K_aug = lqr(A_aug,B_aug,Q_aug,R_aug)

% Pole placement
% Controller Frequency -> 100hHz
% Inner loop controller frequency -> 20Hz 
pmf = [-cos(pi/4)+j*sin(pi/4) -cos(pi/4)-j*sin(pi/4)]*20;
K_aug=place(A_aug,B_aug,pmf);

% unassemble de controller 
K_I_p = K_aug(:,1:n_C)
K_f_p = K_aug(:,n_C+1:n_A+n_C)
A_aug_mf = A_aug-B_aug*K_aug;
[V, D] = eig(A_aug_mf);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Matrices for angular speeds submodel
% Linearized Rotation Dynamics -> q 
A = 0;
B = 1/Jy;
C = 1;

% Indexes 
n_A = size(A,1);
n_B = size(B,2);
n_C = size(C,1);

% Augmented matrices for robust tracking 
A_aug = [zeros(n_C,n_C) C; zeros(n_A,n_C) A];
B_aug = [zeros(n_C,n_B); B];

% LQR control design
% Q_aug = [...
%      1     0;
%      0     1];
% R_aug = [1e8];    
% K_aug = lqr(A_aug,B_aug,Q_aug,R_aug)

% Pole placement
% Controller Frequency -> 100hHz
% Inner loop controller frequency -> 20Hz 
pmf = [-cos(pi/4)+j*sin(pi/4) -cos(pi/4)-j*sin(pi/4)]*20;
K_aug=place(A_aug,B_aug,pmf);

% unassemble de controller 
K_I_q = K_aug(:,1:n_C)
K_f_q = K_aug(:,n_C+1:n_A+n_C)
A_aug_mf = A_aug-B_aug*K_aug;
[V, D] = eig(A_aug_mf);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% matrices for angular speeds submodel
% Linearized Rotation Dynamics -r 
A = 0;
B = 1/Jz;
C = 1;

% Indexes 
n_A = size(A,1);
n_B = size(B,2);
n_C = size(C,1);

% Augmented matrices for robust tracking 
A_aug = [zeros(n_C,n_C) C; zeros(n_A,n_C) A];
B_aug = [zeros(n_C,n_B); B];

% LQR control design
% Q_aug = [...
%      1     0;
%      0     1];
% R_aug = [1e8];    
% K_aug = lqr(A_aug,B_aug,Q_aug,R_aug)

% Pole placement
% Controller Frequency -> 100hHz
% Inner loop controller frequency -> 20Hz 
pmf = [-cos(pi/4)+j*sin(pi/4) -cos(pi/4)-j*sin(pi/4)]*20;
K_aug=place(A_aug,B_aug,pmf);

% unassemble de controller 
K_I_r = K_aug(:,1:n_C)
K_f_r = K_aug(:,n_C+1:n_A+n_C)
A_aug_mf = A_aug-B_aug*K_aug;
[V, D] = eig(A_aug_mf);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% K_I_p =
% 
%    3.6144e-03
% 
% 
% K_f_p =
% 
%    2.5557e-04
% 
% 
% K_I_q =
% 
%    3.6507e-03
% 
% 
% K_f_q =
% 
%    2.5814e-04
% 
% 
% K_I_r =
% 
%    7.7472e-03
% 
% 
% K_f_r =
% 
%    5.4781e-04
   
   
% Replicates the controllers 
K_I_RD = [K_I_p 0 0; 0 K_I_q 0; 0 0 K_I_r];
K_f_RD = [K_f_p 0 0; 0 K_f_q 0; 0 0 K_f_r];

% Saves controller data 
save controller_data K_I_RD K_f_RD  

% break 

