mod_straightline.m

用于无人自动舰艇的直行

% state space model for straight line tracking 
clear all;close all;
% nomorilized velocity 
Vnorm = 1.1;

% rudder -> yaw rate (K-T model)
K = 2;
T = 0.4;

% yaw -> veloctity angle

Tb = 1.2 * Vnorm;

% states
% [ r   :  yaw rate
% [ phi :  yaw angle
% [ beta:  velocity angle
% [ d   :  distance to line

% input
% [ rudder:  rudder deflection ]
% [ refdir:  reference direction of straight line]

% scale factor
Fs = Vnorm*pi/180;  % yawrate , yaw, velocity angle measured in degree
              % the last equation need angle in rad

Am = [ -1/T  0    0    0;
        1    0    0    0;
        0  1/Tb -1/Tb  0;
        0    0    Fs   0
     ]   

Bm = [ K/T  0;
         0  0;
         0  0;
         0 -Fs
     ]
     
Bm1 = Bm(:,1); % rudder control vector
Bm2 = Bm(:,2); % refdir control vector
     
Cm = [1 0 0 0 ; % yaw rate
      0 0 1 0 ; % velocity angle
      0 0 0 1 ] % distance 
  
Dm = zeros(3,2);
Dm1 = zeros(3,1);

% Add measurement delay to [velocity angle and distance]
Delay = 2; % 2 second
[N2,D2] = pade(Delay, 1);
% delay in state space
[ad2,bd2,cd2,dd2] = ssdata(ss(tf(N2,D2))); 

% test controlbility and observability
ccc= ctrb(Am,Bm1);
rank(ccc)

ooo = obsv(Am,Cm);
rank(ooo)

% norminal model
Pn = tf(K,[T 1 0]);
Pn0 = tf(1,[Tb 1]) * Pn;
Pn = tf(N2,D2) * Pn0;

% calculate additional error for delay from +-.5 from norminal value
w = logspace(-2,1,1000); 
% Nominal Responce
Fn = freqresp(Pn,w);
Fn = squeeze(Fn);
Mag = 20*log(abs(Fn));

% figure,
% %     semilogx(w, Mag), grid on
% %     hold on
% 
% % U range
% for U = 1.5:0.1:1.9
%     [tn,td]=pade(U,1);
%     delaySys = tf(tn,td);
%     Pp = delaySys * Pn0;
%     detaSys = (Pp - Pn)/Pn ;
%     
%     Fd = freqresp(detaSys,w);
%     Fd = squeeze(Fd);
%     
%     Mag = 20*log10(abs(Fd));
%     
%     semilogx(w, Mag), grid on
%     hold on
% end     
% 
% for U = 2.1:0.1:2.5
%     [tn,td]=pade(U,1);
%     delaySys = tf(tn,td);
%     Pp = delaySys * Pn0;
%     detaSys = (Pp - Pn)/Pn ;
%     
%     Fd = freqresp(detaSys,w);
%     Fd = squeeze(Fd);
%     
%     Mag = 20*log10(abs(Fd));
%     
%     semilogx(w, Mag), grid on
%     hold on
% end     

% nW1=[ 108.4302    0.2169    0.0001];
% 
% 
% dW1 =  [0.0010    0.5765    1.2557];

nW1 =[   49.9367    0.0499];


dW1 =[    1.0000   89.4966];

% h2 weighting for dir error
nW2 =[ 0    1.0418];
dW2 = [1.0000    0.0010];


wa = tf(nW1,dW1);
    Fd = freqresp(wa,w);
    Fd = squeeze(Fd);
    
    Mag = 20*log10(abs(Fd));
    
%     semilogx(w, Mag), grid on

% design 2 two measurement 
[A,B,C,D] = LINMOD('designModel');
plant = pck(A,B,C,D);
% % [k,g,norms,kfi,gfi,hamx,hamy] = h2syn(plant,1,1)
%  [gopt,Kch] = hinflmi(plant,[3 1])
%  [Ak3,Bk3,Ck3,Dk3] = unpck(Kch)
% 
%  [Ak3d,Bk3d,Ck3d,Dk3d] = c2dm(Ak3,Bk3,Ck3,Dk3,0.1,'tustin');

% mix h2/hinf design
r = [2 3 1];
hinfgamm = 1;
obj = [ hinfgamm 0 0 1]; 
[gopt, h2opt, K,R,S] =hinfmix(plant, r, obj);%,reg);

if isempty(K)
    disp('*********************************');
    disp('------ NO solution! -------------');
    disp('*********************************');
    
else
    [Ak,Bk,Ck,Dk]= unpck(K);
    % combine integral
    [Ack,Bck,Cck,Dck] = linmod('controller_integral');
    
    % Loop transfer 
    [Aop,Bop,Cop,Dop] = linmod('loopTF');
    Cop = -Cop;
    omega=logspace(-2,2,5000);
    figure, bode(ss(Aop,Bop,Cop,Dop),omega);
    
    [Ackd,Bckd,Cckd,Dckd] = c2dm(Ack,Bck,Cck,Dck,0.1,'tustin');

    fname=input('Save As: ');
    save fname Ackd Bckd Cckd Dckd -ASCII -DOUBLE
end