路径跟踪（直线） AUV路径跟踪 matlab

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);