mainfun.m

利用LQR方法计算反馈增益

%%计算闭环系统矩阵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