%%計算閉環系統矩陣A，B,最后給出反饋增益

clc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%地球參數
R0 = 6378e3;  %米制
omega = 7.292e-5;
g0 = 9.81;
dtr = pi/180;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%dimensionless velocity
Vc = sqrt(R0*g0);   %米/秒
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% atmosphere model
rhos = 1.752;
beta = 1/6700;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % CAV 質量
m = 816.48;
% CAV 參考面積
S = 0.32258;
%%對CAV_L風洞試驗數據進行最小二乘擬和,CL = cl1+cl2*alpha+cl3*alpha.^2;CD = cd1+cd2.*alpha+cd3.*alpha.^2;
%計算升力系數所必需的常系數
cl1 = -0.029625;
cl2 = 0.03077;
cl3 = 8.5e-005;
%計算阻力系數所必需的常系數
cd1 = 0.01565;
cd2 = 0.005185;
cd3 = 0.000562;

%將QEGC得到的解析軌跡作為得到反饋增益的參考軌跡
r = evalin('base','r_all');
V = evalin('base','V_all');
gamma = evalin('base','gamma_all');
gamma = gamma.*dtr;
sigma = evalin('base','sigma_all');
sigma = sigma.*dtr;
alpha = evalin('base','alpha_all');

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

% the atmosphere density 
rho = rhos.*exp(-(r.*R0-R0).*beta);

%升阻系數
CL = cl1+cl2.*alpha+cl3.*alpha.^2;
CD = cd1+cd2.*alpha+cd3.*alpha.^2;

%升力和阻力
L = rho.*(Vc.*V).^2.*S.*CL./(2*m*g0);
D = rho.*(Vc.*V).^2.*S.*CD./(2*m*g0);

%升力和阻力分別對r求偏導
Lr = rho.*(-R0*beta).*(Vc.*V).^2.*S.*CL./(2*m*g0);
Dr = rho.*(-R0*beta).*(Vc.*V).^2.*S.*CD./(2*m*g0);

%
Lv = 2.*rho.*(Vc^2.*V).*S.*CL./(2*m*g0);
Dv = 2.*rho.*(Vc^2.*V).*S.*CD./(2*m*g0);
La = rho.*(Vc.*V).^2.*S./(2*m*g0).*(cl2+2.*cl3.*alpha);
Da = rho.*(Vc.*V).^2.*S./(2*m*g0).*(cd2+2.*cd3.*alpha);

%矩陣A的元素
a11 = tan(gamma);
a12(1:length(a11)) = 0;
a12 = a12';
a13 = r./cos(gamma).^2;
a21 = 1./V./cos(gamma).*(-r.*Dr-D+sin(gamma)./r.^2);
a22 = r./V.^2./cos(gamma).*(-V.*Dv+D+sin(gamma)./r.^2);
a23 = -1./V./cos(gamma).^2.*(1./r+r.*D.*sin(gamma));
a31 = 1./V.^2./cos(gamma).*((r.*Lr+L).*cos(sigma)+cos(gamma)./r.^2);
a32 = r./V.^3./cos(gamma).*((V.*Lv-2.*L).*cos(sigma)+2.*cos(gamma)./r.^2);
a33 = r.*L.*cos(sigma).*sin(gamma)./V.^2./cos(gamma).^2;
for mm = 1:length(a11)
    
A(:,:,mm)  = -[a11(mm),a12(mm),a13(mm);a21(mm),a22(mm),a23(mm);a31(mm),a32(mm),a33(mm)];
end


%矩陣B的元素

b22 = -r.*Da./V./cos(gamma);
b31 = -r.*L.*sin(sigma)./V.^2./cos(gamma);
b32 = r.*La.*cos(sigma)./V.^2./cos(gamma);
b11(1:length(b22)) = 0;
b12(1:length(b22)) = 0;
b21(1:length(b22)) = 0;
b11 = b11';
b12 = b12';
b21 = b21';
for p = 1:length(b22)
    
B(:,:,p) = -[b11(p), b12(p);b21(p), b22(p);b31(p) b32(p)];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%發現反饋增益，通過LQR設計
%狀態加權矩陣
Q = [1,0,0;0,0,0;0,0,0];
%控制加權矩陣
%定義r的偏差為1km，傾斜角sigma的最大允許偏差為20度，攻角最大允許偏差為5度
R = [(1000/R0)^2/(5)^2,0;0,(1000/R0)^2/9];

A = evalin('base','A');
B = evalin('base','B');

%%通過malab 的lqr函數求解反饋增益，KK即為所求
for i = 1:length(A)
    a = A(:,:,i);
    b = B(:,:,i);
    [K,S,e] = lqr(a,b,Q,R);
    KK(:,:,i)=K; 
    e = e;
end