%% 定义船舶主尺度变量
Loa =325.5000;
Lpp =320;
B =58;
T =20.8000;
Cb =0.8100;
Xg =11.1300;
Iz =2.00*10^12;
Vs =7.9732;
m=Lpp*B*T*Cb*1.025*1000*1.006;
u=Vs;
L=Lpp;
q=1025;
%% 给船舶运动状态赋初值
du=0;
dt=0.01;%时间步进为0.01s
v=0;
r=0;
u1=0;
v1=0;
r1=0;
a=0;%定义初值舵角
c=0;%定义初始首向角
C=zeros(1,000000);%定义首向角矩阵
idx=0;
A=zeros(1,000000);%定义舵角矩阵
T=zeros(1,000000);%定义时间矩阵
t=0;%赋予时间初值
i=0;%转舵速度控制参数
j=0;%步进参数
%% 循环迭代计算各个时刻所需数值
while idx<100000
idx=idx+1;
A(idx)=radtodeg(a);%将每次计算的舵角储存进数组
C(idx)=radtodeg(c);%将每次计算的航向角储存进数组
T(idx)=t;%将每次累进的时间储存进数组
x=0.5*q*u^2*L^2;%力的无因次比
y=0.5*q*u^2*L^2;%Y力的无因次比
n=0.5*q*u^2*L^3;%力矩的无因次比
z=u;%速度v的无因次比
s=u/L;%角速度的无因次比
z1=u^2/L;%加速度的无因次比
s1=u^2/L^2;%角加速度的无因次比
%% 水动力导数有因次化
Xa = 0*x;
Xaa = -0.00272*x;
Xaaa = 0*x;
Xu = -0.0022*x/u;
Xuu = 0.0015*x/u/u;
Xuuu = 0*x/u/u/u;
Xu1 =-0.001135*x/z1;
Xv = 0*x/u;
Xvv = 0.00159*x/u/u;
Xvvv =0*x/u/u/u;
Xr = 0*x/s;
Xrr =0.00338*x/s/s;
Xrrr =0*x/s/s/s;
Xva =0.001609*x/u;
Xra =-0.001034*x/s;
Xvr =0.01391*x/u/s;
Ya =0.00408*y;
Yaa =-0.000114*y;
Yaaa =-0.003059*y;
Yv =-0.01902*y/u;
Yvv =0.000639*y/u/u;
Yvvv =-0.1287*y/u/u/u;
Yv1 =-0.014508*y/z1;
Yr =0.005719*y/s;
Yrr =-0.000002*y/s/s;
Yrrr =-0.000048*y/s/s/s;
Yr1 =-0.001209*y/s1;
Yua =-0.00456*y/u;
Yvvr =0.0211*y/u/u/s;
Yvrr =-0.02429*y/u/s/s;
Yvaa =0.00326*y/u;
Yvva =0.003018*y/u/u;
Yraa =-0.002597*y/s;
Yrra =0.000895*y/s/s;
Na =-0.001834*n;
Naa =-0.00056*n;
Naaa =0.001426*n;
Nv =-0.007886*n/u;
Nvv =-0.000308*n/u/u;
Nvvv =0.00175*n/u/u/u;
Nv1 =-0.000588*n/z1;
Nr =-0.003701*n/s;
Nrr =-0.000002*n/s/s;
Nrrr =-0.000707*n/s/s/s;
Nr1 =-0.000567*n/s1;
Nua =0.00232*n/u;
Nvvr =-0.019*n/u/u/s;
Nvrr =0.003726*n/u/s/s;
Nvaa =-0.001504*n/u;
Nvva =-0.001406*n/u/u;
Nraa =0.001191*n/s;
Nrra =-0.000398*n/s/s;
%% 计算当前时刻船舶运动参数
Fx=Xu*du+Xuu*du^2+Xuuu*du^3+Xvv*v^2+(m*Xg+Xrr)*r^2+(m+Xvr)*v*r+Xaa*a^2+Xva*v*a+Xra*r*a;
Fy=-m*u*r+Yv*v+Yvvv*v^3+Yr*r+Yrrr*r^3+Yvrr*v*r^2+Yvvr*v^2*r+Ya*a+Yaaa*a^3+Yua*du*a+Yvaa*v*a^2+Yvva*v^2*a+Yraa*r*a^2+Yrra*r^2*a;
Fn=-m*Xg*u*r+Nv*v+Nvvv*v^3+Nr*r+Nrrr*r^3+Nvrr*v*r^2+Nvvr*v^2*r+Na*a+Naaa*a^3+Nua*du*a+Nvaa*v*a^2+Nvva*v^2*a+Nraa*r*a^2+Nrra*r^2*a;
u2=u+Fx*dt/(m-Xu1);
v2=v+(Fy*(Iz-Nr1)-Fn*(m*Xg-Yr1))*dt/((m-Yv1)*(Iz-Nr1)-(m*Xg-Nv1));
r2=r+(Fn*(m-Yv1)-Fy*(m*Xg-Nv1))*dt/((m-Yv1)*(Iz-Nr1)-(m*Xg-Nv1));
du=u2-Vs;
u=u2;
v=v2;
c1=c+r*dt;
r=r2;
t=t+dt;
a1=a+degtorad(2.34)*dt*(-1)^i;
if abs(a1)>=degtorad(10);%判断舵角是否转到正负10度，当舵角转到正负10度，保持舵角不变
a=degtorad(10)*(-1)^i;
else a=a1;
end
if (abs(c)-degtorad(10))*(abs(c1)-degtorad(10))<=0;%判断航向角是否到达正负10度
j=j+1;
i=ceil(j/2);
end
c=c1;%航向角更新
end
plot(T,C);
hold on;
plot(T,A);
%% 创建输出变量读取船模试验数据
[~, ~, raw] = xlsread('数据.xlsx','Sheet1','H1:K59');
data = reshape([raw{:}],size(raw));
%% 将导入的数组分配给列变量名称
X= data(:,1);
Y= data(:,2);
X1= data(:,3);
Y1= data(:,4);
%% 清除临时变量
clearvars data raw;
plot(X,Y,'--');
plot(X1,Y1,'--');
hold off;
%% 设置图例
xlabel('t [s]');
ylabel('C,A [度]');
legend('Sim','Sim','Exp','Exp')
