LZY\79495
83 lines
2.7 KiB
Matlab
83 lines
2.7 KiB
Matlab
function [Glon,Glat] = linfdm_numericalpert(fcn,op_point)
|
|
options = linearizeOptions('SampleTime',0); % 0 ¡ª Use for continuous-time systems.
|
|
options.LinearizationAlgorithm = 'numericalpert';
|
|
G = linearize(fcn,op_point,options);
|
|
sn = G.StateName;
|
|
n = length(sn);
|
|
st_idx = zeros(8,1); % u w q tht, v p r phi
|
|
for k=1:n
|
|
sn{k}=strrep(sn{k},' ','');
|
|
if strcmp(sn{k}(end-5:end),'psi(1)')
|
|
st_idx(8) = k;
|
|
elseif strcmp(sn{k}(end-5:end),'psi(2)')
|
|
st_idx(4) = k;
|
|
elseif strcmp(sn{k}(end-4:end),'wb(1)')
|
|
st_idx(1) = k;
|
|
elseif strcmp(sn{k}(end-4:end),'wb(2)')
|
|
st_idx(5) = k;
|
|
elseif strcmp(sn{k}(end-4:end),'wb(3)')
|
|
st_idx(2) = k;
|
|
elseif strcmp(sn{k}(end-3:end),'r(1)')
|
|
st_idx(6) = k;
|
|
elseif strcmp(sn{k}(end-3:end),'r(2)')
|
|
st_idx(3) = k;
|
|
elseif strcmp(sn{k}(end-3:end),'r(3)')
|
|
st_idx(7) = k;
|
|
end
|
|
end
|
|
in = G.InputName;
|
|
n = length(in);
|
|
in_idx = zeros(4,1); % de thr da dr
|
|
for k=1:n
|
|
in{k}=strrep(in{k},' ','');
|
|
if strcmp(in{k}(end-1:end),'da')
|
|
in_idx(3) = k;
|
|
elseif strcmp(in{k}(end-1:end),'de')
|
|
in_idx(1) = k;
|
|
elseif strcmp(in{k}(end-1:end),'dr')
|
|
in_idx(4) = k;
|
|
elseif strlength(in{k})>=3 && strcmp(in{k}(1:3),'thr')
|
|
in_idx(2) = k;
|
|
end
|
|
end
|
|
out = G.OutputName;
|
|
n = length(out);
|
|
out_idx = zeros(8,1); % V alpha p tht , beta p r phi
|
|
for k=1:n
|
|
out{k}=strrep(out{k},' ','');
|
|
if strlength(out{k})>=3 && strcmp(out{k}(end-2:end),'TAS')
|
|
out_idx(1) = k;
|
|
elseif strlength(out{k})>=5 && strcmp(out{k}(end-4:end),'alpha')
|
|
out_idx(2) = k;
|
|
elseif strlength(out{k})>=4 && strcmp(out{k}(end-3:end),'beta')
|
|
out_idx(5) = k;
|
|
elseif strlength(out{k})>=6 && strcmp(out{k}(end-5:end),'pqr(1)')
|
|
out_idx(6) = k;
|
|
elseif strlength(out{k})>=6 && strcmp(out{k}(end-5:end),'pqr(2)')
|
|
out_idx(3) = k;
|
|
elseif strlength(out{k})>=6 && strcmp(out{k}(end-5:end),'pqr(3)')
|
|
out_idx(7) = k;
|
|
elseif strlength(out{k})>=11 && strcmp(out{k}(end-10:end),'attitude(1)')
|
|
out_idx(8) = k;
|
|
elseif strlength(out{k})>=11 && strcmp(out{k}(end-10:end),'attitude(2)')
|
|
out_idx(4) = k;
|
|
end
|
|
end
|
|
Alon = G.A(st_idx(1:4),st_idx(1:4));
|
|
Blon = G.B(st_idx(1:4),in_idx(1:2));
|
|
Clon = G.C(out_idx(1:4),st_idx(1:4));
|
|
Dlon = G.D(out_idx(1:4),in_idx(1:2));
|
|
Glon = ss(Alon,Blon,Clon,Dlon);
|
|
Glon.StateName = {'ub','wb','q','tht'};
|
|
Glon.InputName = {'de','thr'};
|
|
Glon.OutputName = {'V','AoA','q','tht'};
|
|
|
|
Alat = G.A(st_idx(5:8),st_idx(5:8));
|
|
Blat = G.B(st_idx(5:8),in_idx(3:4));
|
|
Clat = G.C(out_idx(5:8),st_idx(5:8));
|
|
Dlat = G.D(out_idx(5:8),in_idx(3:4));
|
|
Glat = ss(Alat,Blat,Clat,Dlat);
|
|
Glat.StateName = {'vb','p','r','phi'};
|
|
Glat.InputName = {'da','dr'};
|
|
Glat.OutputName = {'AoS','p','r','phi'};
|
|
end |