% %RungeKuttta4.m
%
% function dy=MMG1(t,y)
% u=y(1);v=y(2);r=y(3);xg=y(4);yg=y(5);course=y(6);xB=0;% xB 是漂心
% % Cr=(0.0067*u^4-0.166*u^3+1.553*u^2-6.3699*u+10.02)*10^(-3);
%
%
% %假设
% % u=1;v=0.01;r=0.0001;
% xC=0;% xf 是浮心
%
% u0=13.5*1852/3600;%=0.6945
% p=1.025;M=14278.12*p;Lw=124;B=20.8;dm=8;Cb=0.681;Cp=0.692;panbi=0.67;%盘面比
% Vpai=14278.12;%排水体积
% Dp=4.6;P=3.66;L=126;n=154/60;
% zhanbi=1.72;%展铉比
% Ar=18.8;Hr=6.1;%舵高
% mper=M/(0.5*p*L^2*dm);%mxper=0.0133;myper=0.2022;Jzzper=0.046;Izzper=0.0113;
% % =0.2248
% V=(u^2+v^2)^0.5;
% Izzper=(1+Cb^4.5)*p*M/24*(L^2+B^2)/(0.5*p*L^4*dm); % p145 杨论文
% % =0.0116
% % mxper=mper/100*(0.398+11.97*Cb*(1+3.73*dm/B)-2.89*Cb*L/B*(1+1.113*dm/B)+0.175*Cb*(L/B)^2*(1+0.541*dm/B)-1.107*L/B*dm/B);
% % =0.0133
% mxper=0.0112;
%
% myper=1.129*mper*(0.882-0.54*Cb*(1-1.6*dm/B)-0.156*(1-0.673*Cb)*L/B+0.826*dm/B*L/B*(1-0.678*dm/B)-0.638*Cb*dm/B*L/B*(1-0.669*dm/B));
% % =0.2283
% Jzzper=0.01*(33-76.85*Cb*(1-0.784*Cb)+3.43*L/B*(1-0.63*Cb))*mper/2.6;%0.0177
% % Jzzper=mper*(Jzz/L)^2;% =0.0094
%
% %计算Xuu
% %计算Ct
% %计算水湿面积
% % k=5.5;
% k=2.77;
%
% S=k*(Vpai*Lw)^0.5;%桑地公式p149杨
% %计算Cf p151杨
% vv=1.18831*10^(-6);
% Rn=u*L/vv;%=7*10^8
% Cf1=0.066/(log10(Rn)-2.03)^2;% Rn雷诺数
% Cf2=0.455/(log10(Rn))^2.58;
% Cf3=0.075/(log10(Rn)-2.03)^2;%=0.0016
% Cf4=0.455/(log10(Rn))^2.58-1700/Rn;
% Car=0.000362;
% Cr=(0.0067*V^4-0.166*V^3+1.553*V^2-6.3699*V+10.02)*10^(-3)*0.85/0.95;%=6*10^(-4)
% % Cr=(0.0067*V^4-0.166*u^3+1.553*u^2-6.3699*u+10.02)*10^(-3)
% Ct4=Cf3+Cr+Car;%=0.0032
% Ct=Ct4*1.001;
% Xuu=-S/(L*dm)*Ct;%=-0.0089
% Xvv=0.4*B/L-0.006*L/dm;%=-0.0285
% Xrr=0.0005*L/dm;%=0.0079
% Xvr=(1.11*Cb-0.07)*myper;%=0.1387
% XH=Xuu*(u/u0)^2+Xvv*(v/u0)^2+Xvr*(v/u0)*(r*L/u0)+Xrr*(r*L/u0)^2;% 0--0.0123
%
%
% %计算Xp=(1-tp)*T
% %计算 tp 旋转中的推力减额系数
% tp0=0.5*Cp-0.12;
% tp0per=0.04+(tp0-0.04)*u/u0;%=0.08--0.1
% lR=-0.95;
% pjiao=atan(-v/u);
% pjiaoR=pjiao-lR*r*L/u0;%pjiaoP=atan(-(v+lp*r)/u) p109 杨
% % pjiaoR=atan(-(v+lR*r)/u);
%
% % 求 tp p111 杨
% lcb=xC/L*100;
% rA=(B/dm)*(1.3*(1-Cb)-3.1*lcb);
% kt=0.00023*(rA*L/Dp)-0.028;%=-0.0212
% f=kt*pjiaoR;
% tp=tp0per-f; %=-0.0173
% %计算T=p*n^2*Dp^4*Kt
% % Kt Kq的计算
% % pjiaoP=pjiao+0.5*L*r/V;
% lp=-0.5*L;
% % pjiaoP=atan(-(v+lp*r)/u);%p109 杨
% pjiaoP=pjiao+0.5*L/u0*r;%p104 人民交通出版社 操纵与耐波性 吴秀恒
% wp0=0.5*Cb-0.05 ; % =0.2905 p105 杨
% wp=wp0*exp(-4*(pjiaoP)^2);%p104 人民交通出版社 操纵与耐波性 吴秀恒 =0.295
% J=(1-wp)*u/(n*Dp);%= 0.3818p148 杨 论文
% Kt=0.0821*J^3-0.2308*J^2-0.2768*J+0.3499;%=0.2918
% Kq=0.0085*J^3-0.027*J^2-0.0252*J+0.0416;%=0.0361
% XP=(1-tp)*p*n^2*Dp^4*Kt/(p*L*dm*u0^2);%=0.0376--0.016
% %计算 Xr=(1-tR)*Fn*sin(rudder)
% %计算 Fn=-0.5*p*Ar*fa*Vr^2*sin(aR);
% zx=-22.2*(Cb*B/L)^2+0.02*(Cb*B/L)+0.68;% zx=0.4017 是整流系数 p135 杨
% up0=u*(1-wp0);
% s0=1-up0/(n*P);
% rudder0=-2*s0*pi/180;
% rudder=-2*s0*pi/180;
% % rudder=35*pi/180;
% aR=rudder-rudder0-zx*pjiaoR;% 0.34--0.06
% %计算Ur p149 杨 论文
% C=1.065 ;% 向右转时 C=0.935; 向左转时
% wR0=0.36;% p123 杨
% wR=wR0*wp/wp0;k=0.6*(1-wp)/(1-wR);
% yxbi=Dp/Hr;% yxi p149 杨 论文
% s=1-(1-wp)*u/(n*P);
% Gs=yxbi*k*(2-(2-k)*s)*s/(1-s)^2;
% Ur=V*(1-wR)*(1+C*Gs)^0.5;
% fa=6.13*zhanbi/(2.25+zhanbi);% p118 杨 fa是升力系数
% % Fn=-0.5*p*Ar*fa*Vr^2*sin(aR)/(0.5*p*L^2*dm*V);
% Fn=-0.5*p*Ar*fa*Ur^2*sin(aR)/(p*L*dm*u0^2);
% % Fn=0.5;
% tR=0.29;
% XR=(1-tR)*Fn*sin(rudder); % XR= -0.0037;
% y11=((mper+myper)*v/u0*r*L/u0+XH+XP+XR)/(mper+mxper) ;%0.1561--0.0153
% % Yp=0.025*T;%正车时 p112 杨
% % y22=(-(mper+mxper)*u*r+Yh+Yr)/(mper+myper)
% % y33=(Nh+Nr-xG*Yh)/(Izzper+Jzzper)
% % Yv=-pi*dm/L*(1.0+0.4*Cb*B/dm); % p148 杨=-0.3407
% Yv=-0.3569;
% % Yr=-pi*dm/L*(-0.5+2.2*B/L+0.08*B/dm);%=-0.0142
% Yr=0.0997;
% % Nv=-pi*dm/L*(0.5+0.24*dm/L);%=-0.1028
% Nv=-0.127;
% % Nr=-pi*dm/L*(0.25-0.56*B/L+0.039*B/dm);%=-0.0517
% Nr=-0.0525;
%
% Yvv=0.048265-6.293*(1-Cb)*dm/B; % p161 杨 =-0.7238
% Yrr=0.0045-0.445*(1-Cb)*dm/B; %=-0.0501
% Yvr=-0.3791+1.28*(1-Cb)*dm/B;%=-0.2221
% Nrr=-0.0805+8.6092*(Cb*B/L)^2-36.9816*(Cb*B/L)^3;%=-0.0242
% Nvvr=-6.0856+137.4735*(Cb*B/L)-1029.514*(Cb*B/L)^2+2480.6082*(Cb*B/L)^3;%=-0.1177
% Nvrr=-0.0635+0.04414*(Cb*dm/B);%=-0.0519
% YH=Yv*v/u0+Yr*r*L/u0+Yvv*v/u0*abs(v/u0)+Yvr*abs(v/u0)*r*L/u0+Yrr*r*L/u0*abs(r*L/u0);% p145 杨论文
% NH=Nv*v/u0+Nr*r*L/u0+Nvvr*(v/u0)^2*r*L/u0+Nvrr*v/u0*(r*L/u0)^2+Nrr*r*L/u0*abs(r*L/u0);
% aH0=0.6784-1.3374*Cb+1.8891*Cb^2 ;% p136 杨
% aH=aH0;% p137杨
% xH=-(0.4+0.1*Cb);% p136杨
% YR=(1+aH)*Fn*cos(rudder); % p138 杨
% NR=(-0.5+aH*xH)*Fn*cos(rudder);
% xG=0;
% y22=(-(mper+mxper)*u/u0*r*L/u0+YH+YR)/(mper+myper);
%RungeKuttta4.m
function dy=MMG1(t,y)
u=y(1);v=y(2);r=y(3);xg=y(4);yg=y(5);course=y(6);xB=0;% xB 是漂心
% Cr=(0.0067*u^4-0.166*u^3+1.553*u^2-6.3699*u+10.02)*10^(-3);
dy(8)=r;
%假设
% u=1;v=0.01;r=0.0001;
xC=0;% xf 是浮心
u0=13.5*1852/3600;%=0.6945
p=1.025;M=6859.07*p;Lw=119.7;B=20.5;dm=4.4;Cb=0.653;Cp=0.692;panbi=0.71;%盘面比
Vpai=6859.07;%排水体积
Dp=4.5;P=3.99;L=119.7;n=126/60;
zhanbi=1.68;%展铉比
Ar=17.57;Hr=5.49;%舵高
mper=M/(0.5*p*L^2*dm);%mxper=0.0133;myper=0.2022;Jzzper=0.046;Izzper=0.0113;
% =0.2248
V=(u^2+v^2)^0.5;
Izzper=(1+Cb^4.5)*p*M/24*(L^2+B^2)/(0.5*p*L^4*dm); % p145 杨论文
% =0.0116
% mxper=mper/100*(0.398+11.97*Cb*(1+3.73*dm/B)-2.89*Cb*L/B*(1+1.113*dm/B)+0.175*Cb*(L/B)^2*(1+0.541*dm/B)-1.107*L/B*dm/B);
% =0.0133
mxper=0.0112;
myper=1.129*mper*(0.882-0.54*Cb*(1-1.6*dm/B)-0.156*(1-0.673*Cb)*L/B+0.826*dm/B*L/B*(1-0.678*dm/B)-0.638*Cb*dm/B*L/B*(1-0.669*dm/B));
% =0.2283
Jzzper=0.01*(33-76.85*Cb*(1-0.784*Cb)+3.43*L/B*(1-0.63*Cb))*mper/2.6;%0.0177
% Jzzper=mper*(Jzz/L)^2;% =0.0094
%计算Xuu
%计算Ct
%计算水湿面积
% k=5.5;
k=2.77;
S=k*(Vpai*Lw)^0.5;%桑地公式p149杨
%计算Cf p151杨
vv=1.18831*10^(-6);
Rn=V*L/vv;%=7*10^8
Cf1=0.066/(log10(Rn)-2.03)^2;% Rn雷诺数
Cf2=0.455/(log10(Rn))^2.58;
Cf3=0.075/(log10(Rn)-2.03)^2;%=0.0016
Cf4=0.455/(log10(Rn))^2.58-1700/Rn;
Car=0.000362;
Cr=(0.0067*V^4-0.166*V^3+1.553*V^2-6.3699*V+10.02)*10^(-3)*0.85/0.95;%=6*10^(-4)
% Cr=(0.0067*V^4-0.166*u^3+1.553*u^2-6.3699*u+10.02)*10^(-3)
Ct4=Cf3+Cr+Car;%=0.0032
Ct=Ct4*1.001;
Xuu=-S/(L*dm)*Ct;%=-0.0089
Xvv=0.4*B/L-0.006*L/dm;%=-0.0285
Xrr=0.0003*L/dm;%=0.0079
Xvr=(1.11*Cb-0.07)*myper;%=0.1387
XH=Xuu*(u/u0)^2+Xvv*(v/u0)^2+Xvr*(v/u0)*(r*L/u0)+Xrr*(r*L/u0)^2;% 0--0.0123
%计算Xp=(1-tp)*T
%计算 tp 旋转中的推力减额系数
tp0=0.5*Cp-0.12;
% tp0per=0.04+(tp0-0.04)*u/u0;%=0.08--0.1
tp0per=tp0;
lR=-0.95;
pjiao=atan(-v/u);
pjiaoR=pjiao-lR*r*L/V;%pjiaoP=atan(-(v+lp*r)/u) p109 杨
% pjiaoR=atan(-(v+lR*r)/u);
% 求 tp p111 杨
lcb=xC/L*100;
rA=(B/dm)*(1.3*(1-Cb)-3.1*lcb);
kt=0.00023*(rA*L/Dp)-0.028;%=-0.0212
f=kt*pjiaoR;
% tp=tp0per-f; %=-0.0173
%计算T=p*n^2*Dp^4*Kt
% Kt Kq的计算
% pjiaoP=pjiao+0.5*L*r/V;
lp=-0.5*L;
% pjiaoP=atan(-(v+lp*r)/u);%p109 杨
pjiaoP=pjiao+0.5*L/u0*r;%p104 人民交通出版社 操纵与耐波性 吴秀恒
wp0=0.5*Cb-0.05 ; % =0.2905 p105 杨
wp=wp0*exp(-4*(pjiaoP)^2);%p104 人民交通出版社 操纵与耐波性 吴秀恒 =0.295
tp=tp0per*exp(-4*pjiaoP^2);
J=(1-wp)*u/(n*Dp);%= 0.3818p148 杨 论文
Kt=0.0821*J^3-0.2308*J^2-0.2768*J+0.3499;%=0.29
