www.pudn.com > AUV-tarcking.zip > WLM.m, change:2012-04-25,size:1315b


function [sys,x0,str,ts] = s_function(t,x,u,flag) 
 
switch flag, 
  case 0, 
    [sys,x0,str,ts]=mdlInitializeSizes; 
  case 1, 
    sys=mdlDerivatives(t,x,u); 
 
  case 3, 
    sys=mdlOutputs(t,x,u); 
 
  case {2,4,9} 
    sys=[]; 
     
  otherwise 
    error(['Unhandled flag = ',num2str(flag)]); 
 
end 
 
function [sys,x0,str,ts]=mdlInitializeSizes 
 
sizes = simsizes; 
 
sizes.NumContStates  = 3; 
sizes.NumDiscStates  = 0; 
sizes.NumOutputs     = 3; 
sizes.NumInputs      = 3; 
sizes.DirFeedthrough = 0; 
sizes.NumSampleTimes = 1;    
 
sys = simsizes(sizes); 
 
x0  = [0.01;0;0]; 
str = []; 
ts  = [0 0]; 
 
function sys=mdlDerivatives(t,x,u) 
L=1.46; m=45;density=1025.0; 
L2=0.5*density*L*L; 
L3=L2*L; 
L4=L3*L; 
L5=L4*L; 
Xdu=-1.5777e-3*L3;Xuu=-5.9e-3*L2;Xu=0; 
Ydv=-3.0753e-2*L3;Yv=-4.4961e-2*L2;Yvv=-1.6687e-1*L2; 
Iz=8.067;Ndr=-1.6012e-3*L5;Nrr=-1.4432e-3*L5;Nr=-1.1679e-2*L4; 
 
 
 
M=[m-Xdu 0 0 ;0 m-Ydv 0 ;0 0 Iz-Ndr]; 
C=[0 0  -(m-Ydv)*x(2);0 0  -(m-Xdu)*x(1);(m-Ydv)*x(2) -(m-Xdu)*x(1) 0]; 
D=-[Xu+Xuu*norm(x(1)) 0 0 ;0 Yv+Yvv*norm(x(2)) 0 ;0 0  Nr+Nrr*norm(x(3))]; 
g=[0;0;0]; 
 
V=[x(1);x(2);x(3)]; 
T=[u(1);u(2);u(3)]; 
f=[0;0;0]; 
sys=inv(M)*(T-g-C*V-D*V+f); 
function sys=mdlOutputs(t,x,u) 
sys(1)=x(1); 
sys(2)=x(2); 
sys(3)=x(3);