diff --git a/Figure/big_figure.m b/Figure/big_figure.m deleted file mode 100644 index ae60098..0000000 --- a/Figure/big_figure.m +++ /dev/null @@ -1,86 +0,0 @@ -clc,clear,close all - -%% Importing Data -load HSVACPMCKVLCC2Z1005 HSVACPMCKVLCC2Z1005 -load HSVACPMCKVLCC2Z1001 HSVACPMCKVLCC2Z1001 -load HSVACPMCKVLCC2Z1505 HSVACPMCKVLCC2Z1505 -load HSVACPMCKVLCC2Z2005 HSVACPMCKVLCC2Z2005 -load HSVACPMCKVLCC2Z2505 HSVACPMCKVLCC2Z2505 -load HSVACPMCKVLCC2Z3005 HSVACPMCKVLCC2Z3005 -load HSVACPMCKVLCC2Z3505 HSVACPMCKVLCC2Z3505 -load HSVACPMCKVLCC2Z1010P HSVACPMCKVLCC2Z1010P -load HSVACPMCKVLCC2Z2010P HSVACPMCKVLCC2Z2010P -load HSVACPMCKVLCC2Z1001 HSVACPMCKVLCC2Z1001 -% t = data(:,1); -% x = data(:,2); -% y = data(:,3); -% psi = data(:,4); -% u = data(:,5); -% v = data(:,6); -% r = data(:,7); -% phi = data(:,8); -% d = data(:,9); - -x = [HSVACPMCKVLCC2Z1005(1:3100,:);HSVACPMCKVLCC2Z2005(1:3100,:);HSVACPMCKVLCC2Z3005(1:3100,:)]; -u = x(:,9)*pi/180; -va = x(:,5)-1.179*ones(size(x(:,5))); -x = [va x(:,6) x(:,7)*pi/180]; -xaug = [x(3:end-3,:) u(3:end-3,:)]; - -%% Plotting raw data -figure -plot(xaug(:,1),'linewidth',6,'color',[0,0.45,0.74]) %velocity of u -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('velocity of u','-r600','-dpng'); -figure -plot(xaug(:,2),'linewidth',6,'color',[0.93,0.69,0.13]) %velocity of v -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('velocity of v','-r600','-dpng'); -figure -plot(xaug(:,3),'linewidth',6,'color',[0.47,0.67,0.19]) %velocity of r -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('velocity of r','-r600','-dpng'); -figure -plot(xaug(:,4),'linewidth',6,'color',[0,0,0]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('excitation signal','-r600','-dpng'); - -%% Plot the derivative of the data -dt=0.05; %In order to keep the prediction process consistent with the time in the actual data -dx = zeros(length(x)-5,3); -for i=3:length(x)-3 - for k=1:size(x,2) - dx(i-2,k) = (1/(12*dt))*(-x(i+2,k)+8*x(i+1,k)-8*x(i-1,k)+x(i-2,k)); - end - end -figure -plot(dx(:,1),'linewidth',1.5,'color',[0,0.45,0.74]) %velocity of u -box off -axis([0 9500 -0.025 0.025]) -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Acceleration of u','-r600','-dpng'); - -figure -plot(dx(:,2),'linewidth',1.5,'color',[0.93,0.69,0.13]) %velocity of v -box off -axis([0 9500 -0.1 0.1]) -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Acceleration of v','-r600','-dpng'); - -figure -plot(dx(:,3),'linewidth',1.5,'color',[0.47,0.67,0.19]) %velocity of r -box off -axis([0 9500 -0.015 0.015]) -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Acceleration of r','-r600','-dpng'); \ No newline at end of file diff --git a/Figure/dynamic_picture_exp.m b/Figure/dynamic_picture_exp.m deleted file mode 100644 index bb6d890..0000000 --- a/Figure/dynamic_picture_exp.m +++ /dev/null @@ -1,156 +0,0 @@ -clc,clear,close all - -%% Importing Data -load HSVACPMCKVLCC2Z1005 HSVACPMCKVLCC2Z1005 %Data->Variable Name -load HSVACPMCKVLCC2Z1001 HSVACPMCKVLCC2Z1001 -load HSVACPMCKVLCC2Z1505 HSVACPMCKVLCC2Z1505 -load HSVACPMCKVLCC2Z2005 HSVACPMCKVLCC2Z2005 -load HSVACPMCKVLCC2Z2505 HSVACPMCKVLCC2Z2505 -load HSVACPMCKVLCC2Z3005 HSVACPMCKVLCC2Z3005 -load HSVACPMCKVLCC2Z3505 HSVACPMCKVLCC2Z3505 -load HSVACPMCKVLCC2Z1010P HSVACPMCKVLCC2Z1010P -load HSVACPMCKVLCC2Z2010P HSVACPMCKVLCC2Z2010P -load HSVACPMCKVLCC2Z1001 HSVACPMCKVLCC2Z1001 -% %The meaning of each column in the data -% t = data(:,1); -% x = data(:,2); -% y = data(:,3); -% psi = data(:,4); %Bow angle -% u = data(:,5); -% v = data(:,6); -% r = data(:,7); -% phi = data(:,8); %Transverse roll angle -% d = data(:,9); %Rudder angle - -x = [HSVACPMCKVLCC2Z1005(1:3100,:);HSVACPMCKVLCC2Z2005(1:3100,:);HSVACPMCKVLCC2Z3005(1:3100,:)]; -u = x(:,9)*pi/180; -va = x(:,5)-1.179*ones(size(x(:,5))); -x = [va x(:,6) x(:,7)*pi/180]; -xaug = [x(3:end-3,:) u(3:end-3,:)]; - -%% Plotting raw data -figure -plot(xaug(:,1),'linewidth',6,'color',[0,0.45,0.74]) %velocity of u -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp1u','-r600','-dpng'); %print figures - -figure -plot(xaug(:,2),'linewidth',6,'color',[0.93,0.69,0.13]) %velocity of v -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp2v','-r600','-dpng'); - -figure -plot(xaug(:,2),'linewidth',6,'color',[0.47,0.67,0.19]) %velocity of v -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp22v','-r600','-dpng'); - -figure -plot(xaug(:,3),'linewidth',6,'color',[0.47,0.67,0.19]) %velocity of r -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp3r','-r600','-dpng'); - -figure -plot(xaug(:,3),'linewidth',6,'color',[0.93,0.69,0.13]) %velocity of r -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp33r','-r600','-dpng'); - -figure -plot(xaug(:,4),'linewidth',6,'color',[0,0,0]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp4d','-r600','-dpng'); - -figure -plot(xaug(:,4),'linewidth',6,'color',[1.00,0.41,0.16]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp44d','-r600','-dpng'); - -figure -plot(xaug(:,1).*xaug(:,1),'linewidth',6,'color',[0,0.45,0.74]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp5uu','-r600','-dpng'); - -figure -plot(xaug(:,1).*xaug(:,3),'linewidth',6,'color',[1.00,0.41,0.16]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp6ur','-r600','-dpng'); - -figure -plot(xaug(:,1).*xaug(:,4),'linewidth',6,'color',[0.47,0.67,0.19]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp7ud','-r600','-dpng'); - -figure -plot(xaug(:,2).*xaug(:,2),'linewidth',6,'color',[0.84,0.84,0.84]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp8vv','-r600','-dpng'); - -figure -plot(xaug(:,2).*xaug(:,3),'linewidth',6,'color',[0,0.45,0.74]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp9vr','-r600','-dpng'); - -figure -plot(xaug(:,3).*xaug(:,3),'linewidth',6,'color',[0,0.45,0.74]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp10rr','-r600','-dpng'); - -figure -plot(xaug(:,4).*xaug(:,4),'linewidth',6,'color',[0,0.45,0.74]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp11dd','-r600','-dpng'); - -figure -plot(xaug(:,2).*xaug(:,2).*xaug(:,2),'linewidth',6,'color',[0.84,0.84,0.84]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp12vvv','-r600','-dpng'); - -figure -plot(xaug(:,3).*xaug(:,3).*xaug(:,3),'linewidth',6,'color',[0.47,0.67,0.19]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp13rrr','-r600','-dpng'); - -figure -plot(xaug(:,2).*xaug(:,2).*xaug(:,3),'linewidth',6,'color',[0.84,0.84,0.84]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp14vvr','-r600','-dpng'); - -figure -plot(xaug(:,2).*xaug(:,3).*xaug(:,3),'linewidth',6,'color',[0.47,0.67,0.19]) %excitation signal -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Exp15vrr','-r600','-dpng'); \ No newline at end of file diff --git a/Figure/dynamic_picture_sim.m b/Figure/dynamic_picture_sim.m deleted file mode 100644 index 51196d9..0000000 --- a/Figure/dynamic_picture_sim.m +++ /dev/null @@ -1,125 +0,0 @@ -clc,clear,close all - -%% Importing data -load npsAUV_zigzag_1010_005 npsAUV_zigzag_1010_005 -load npsAUV_zigzag_2020_005 npsAUV_zigzag_2020_005 -load npsAUV_zigzag_2505_005 npsAUV_zigzag_2505_005 -load npsAUV_zigzag_2510_005 npsAUV_zigzag_2510_005 - -xinput = [npsAUV_zigzag_1010_005,npsAUV_zigzag_2020_005]; -x_1_self = xinput(1:3,:); -u = xinput(4,:)*pi/180; - - -%% Plotting raw data -figure -plot(xinput(1,:),'linewidth',6,'color',[0,0.45,0.74]) %u -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim1u','-r600','-dpng'); - -figure -plot(xinput(2,:),'linewidth',6,'color',[0.93,0.69,0.13]) %v -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim2v','-r600','-dpng'); %print figures - -figure -plot(xinput(3,:),'linewidth',6,'color',[0.47,0.67,0.19]) %r -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim3r','-r600','-dpng'); - -figure -plot(xinput(4,:),'linewidth',6,'color',[0,0,0]) %d -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim4d','-r600','-dpng'); - -figure -plot(xinput(1,:).*abs(xinput(1,:)),'linewidth',6,'color',[0,0.45,0.74]) %u|u| -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim5uabsu','-r600','-dpng'); - -figure -plot(xinput(1,:).*xinput(2,:),'linewidth',6,'color',[1.00,0.41,0.16]) %uv -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim6uv','-r600','-dpng'); - -figure -plot(xinput(1,:).*xinput(3,:),'linewidth',6,'color',[1.00,0.41,0.16]) %uv -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim7ur','-r600','-dpng'); - -figure -plot(xinput(2,:).*xinput(2,:),'linewidth',6,'color',[84/255,130/255,53/255]) %uv -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim8vv','-r600','-dpng'); - -figure -plot(xinput(2,:).*xinput(3,:),'linewidth',6,'color',[84/255,130/255,53/255]) %uv -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim9vr','-r600','-dpng'); - -figure -plot(xinput(2,:).*abs(xinput(2,:)),'linewidth',6,'color',[1.00,0.41,0.16]) %uv -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim10vabsv','-r600','-dpng'); - -figure -plot(xinput(3,:).*xinput(3,:),'linewidth',6,'color',[84/255,130/255,53/255]) %uv -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim11rr','-r600','-dpng'); - -figure -plot(xinput(3,:).*abs(xinput(3,:)),'linewidth',6,'color',[1.00,0.41,0.16]) %uv -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim12rabsr','-r600','-dpng'); - -figure -plot(xinput(1,:).*xinput(1,:).*xinput(4,:),'linewidth',6,'color',[1.00,0.41,0.16]) %uv -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim13uud','-r600','-dpng'); - -figure -plot(xinput(3,:).*xinput(3,:).*xinput(3,:),'linewidth',6,'color',[1.00,0.41,0.16]) %uv -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim14rrr','-r600','-dpng'); - -figure -plot(xinput(1,:).*xinput(1,:).*xinput(4,:).*xinput(4,:),'linewidth',6,'color',[84/255,130/255,53/255]) %uv -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim15uudd','-r600','-dpng'); - -figure -plot(1./xinput(1,:),'linewidth',6,'color',[0,0.45,0.74]) %uv -box off -axis off -set(gcf,'unit','normalized','position',[0.2,0.2,1,0.12]); -print('Sim161u','-r600','-dpng'); \ No newline at end of file diff --git a/Surface Vehicle/HVSA_z35_05_MTGP.mat b/Surface Vehicle/HVSA_z35_05_MTGP.mat deleted file mode 100644 index 5de50ce..0000000 Binary files a/Surface Vehicle/HVSA_z35_05_MTGP.mat and /dev/null differ diff --git a/Surface Vehicle/Step1_dataconstruct3_new.m b/Surface Vehicle/Step1_dataconstruct3_new.m deleted file mode 100644 index d7e7324..0000000 --- a/Surface Vehicle/Step1_dataconstruct3_new.m +++ /dev/null @@ -1,83 +0,0 @@ -set(0,'defaultfigurecolor','w') -%% Step1:load data - -load HSVACPMCKVLCC2Z1005 HSVACPMCKVLCC2Z1005 -load HSVACPMCKVLCC2Z1505 HSVACPMCKVLCC2Z1505 -load HSVACPMCKVLCC2Z2005 HSVACPMCKVLCC2Z2005 -load HSVACPMCKVLCC2Z3005 HSVACPMCKVLCC2Z3005 -load HSVACPMCKVLCC2Z3505 HSVACPMCKVLCC2Z3505 -load HSVACPMCKVLCC2Z1010P HSVACPMCKVLCC2Z1010P -load HSVACPMCKVLCC2Z2010P HSVACPMCKVLCC2Z2010P -load HSVACPMCKVLCC2Z1001 HSVACPMCKVLCC2Z1001 - -h= 0.05; sample= 2; %Sampling step -%h is the set time interval - -data_raw = [HSVACPMCKVLCC2Z1005(1:3100,:);HSVACPMCKVLCC2Z2005(1:3100,:);HSVACPMCKVLCC2Z3005(1:3100,:)]; - -data = data_raw; -num_tr = size(data,1); -t=linspace(0,num_tr,num_tr+1).*h; -data(:,1)=t(1:end-1); - - -data_train = data; -t =data_train(:,1); -psi = data_train(:,4)*pi/180; %Bow angle -u = data_train(:,5); -v = data_train(:,6); -r = data_train(:,7)*pi/180; -phi = data_train(:,8)*pi/180; %Transverse roll angle -d = data_train(:,9)*pi/180; %Rudder angle - -%Data Mapping -figure(1) -subplot(414),plot(t,[psi*180/pi,-d*180/pi],'linewidth',2);hold on -xlabel('time (s)'),ylabel('\psi & \delta (deg)'),grid on;hold on -legend('\psi','\delta') -subplot(411),plot(t,u,'linewidth',2),xlabel('time (s)'),ylabel('u (m/s)');grid on;hold on -subplot(412),plot(t,v,'linewidth',2),xlabel('time (s)'),ylabel('v (m/s)');grid on;hold on -subplot(413),plot(t,r*180/pi,'linewidth',2),xlabel('time (s)'),ylabel('r (deg/s)');grid on;hold on -%% Step2 Construct data -tic -u_x = u(1:end-1); u_y = u(2:end); -v_x = v(1:end-1); v_y = v(2:end); -r_x = r(1:end-1); r_y = r(2:end); -d_x = d(1:end-1); -Xm = [u_x,v_x,r_x,d_x]; -Ym = [u_y,v_y,r_y]; -% noise test -Xm2=[u_x,v_x,r_x]; -Am = (Ym-Xm2)/h; -Am_d = wdenoise(Am); -t_t =t(1:sample:end-1); -Xm_t= Xm(1:sample:num_tr,:); -Am_t = Am(1:sample:num_tr,:); -Am_d_t = Am_d(1:sample:num_tr,:); - -Len1= Am_d_t(1:end-1,1); -Len2= Am_d_t(1:end-1,2); -Len3=Am_d_t(1:end-1,3); - -u_x=u_x(1:2:end); -v_x=v_x(1:2:end); -r_x=r_x(1:2:end); -d_x=d_x(1:2:end); - -figure(2) -sam=10; -scatter(t_t(1:sam:end-1),Am_t(1:sam:end,2));axis([0 320 -0.05 0.05]);hold on; -scatter(t_t(1:sam:end-1),Am_d_t(1:sam:end,2));axis([0 320 -0.05 0.05]);grid on;hold on; - -for i=1:num_tr/2-1 - ua(i)=u_x(i)-1.179; - [X1(i,:),X2(i,:),X3(i,:)]=Construct3(ua,v_x,r_x,d_x,i); -end - -theta1 = (X1'*X1)\X1'*Len1; %Coefficients of the model -theta2 = (X2'*X2)\X2'*Len2; -theta3 = (X3'*X3)\X3'*Len3; - -save theta1 theta1 %Variables->Data files -save theta2 theta2 -save theta3 theta3 \ No newline at end of file diff --git a/Surface Vehicle/Step2_main_new.m b/Surface Vehicle/Step2_main_new.m deleted file mode 100644 index b1da808..0000000 --- a/Surface Vehicle/Step2_main_new.m +++ /dev/null @@ -1,69 +0,0 @@ -load HSVACPMCKVLCC2Z1505 HSVACPMCKVLCC2Z1505 -load HSVACPMCKVLCC2Z2505 HSVACPMCKVLCC2Z2505 -load HSVACPMCKVLCC2Z3005 HSVACPMCKVLCC2Z3005 -load HSVACPMCKVLCC2Z3505 HSVACPMCKVLCC2Z3505 -load HSVACPMCKVLCC2Z1010P HSVACPMCKVLCC2Z1010P -load HSVACPMCKVLCC2Z1001 HSVACPMCKVLCC2Z1001 -load HSVACPMCKVLCC2Z1501 HSVACPMCKVLCC2Z1501 -pre_data= HSVACPMCKVLCC2Z3505; -order=pre_data(:,9)*pi/180; -tic -%deg angle; rad radian -R2D=180/pi; -D2R=pi/180; -h=0.05; %Step length: seconds -m= 180/0.05; %Rhythm - -x = zeros(6,1); %Temporary status variables -TEMP_a = zeros(6,1); -T = zeros(m,1); %Time -Y = zeros(m,14); %Status Variables -%Variable initialization -u0 = 1.179; v0 = 0; r0 = 0; x0 = 0; y0 = 0; psi0 = 0;d0 = 0; -Initial = [u0; v0; r0; x0; y0; psi0]; - -x = Initial; -X1 = zeros(m,2);X2 = zeros(m,7);X3 = zeros(m,7); - -for i=1:1:m - t = h*i; - T(i,1)=t; - d_vali=order(i); - [TEMP_a]=Infante_3(x,theta1,theta2,theta3,d_vali) ; - x= x + h.*TEMP_a; - Y(i,1) = x(1);%u - Y(i,2) = x(2);%v - Y(i,3) = x(3);%r - Y(i,4) = x(4);%x - Y(i,5) = x(5);%y - Y(i,6) = x(6);%psi-yaw -end - -U_pre = Y(:,1); -V_pre = Y(:,2); -R_pre = Y(:,3)*180/pi; -Xp = Y(:,4); -Yp = Y(:,5); -psi = Y(:,6); -duo = Y(:,7); - -duo = duo*180/pi; -psi = psi *180/pi; - -t2 =toc; -figure(3) -subplot(414),plot(T,psi,'linewidth',1.5);hold on -xlabel('time (s)'),ylabel('psi (deg)'),grid on;hold on -subplot(411),plot(T,U_pre,'linewidth',1.5),xlabel('time (s)'),ylabel('u (m/s)');grid on;hold on -subplot(412),plot(T,V_pre,'linewidth',1.5),xlabel('time (s)'),ylabel('v (m/s)');grid on;hold on -subplot(413),plot(T,R_pre,'linewidth',1.5),xlabel('time (s)'),ylabel('r (deg/s)');grid on;hold on - -%For drawing comparison charts -U_pre_25 = U_pre; -V_pre_25 = V_pre; -R_pre_25 = R_pre; - -save U_pre_25 U_pre_25 -save V_pre_25 V_pre_25 -save R_pre_25 R_pre_25 - diff --git a/Surface Vehicle/Step3_plot_cp.m b/Surface Vehicle/Step3_plot_cp.m deleted file mode 100644 index 0dcef83..0000000 --- a/Surface Vehicle/Step3_plot_cp.m +++ /dev/null @@ -1,31 +0,0 @@ -%% Step 1: Load data -load HSVACPMCKVLCC2Z3005 HSVACPMCKVLCC2Z3005 -load HSVACPMCKVLCC2Z2505 HSVACPMCKVLCC2Z2505 -load HSVACPMCKVLCC2Z1505 HSVACPMCKVLCC2Z1505 -load HSVACPMCKVLCC2Z3505 HSVACPMCKVLCC2Z3505 -load HSVACPMCKVLCC2Z1010P HSVACPMCKVLCC2Z1010P -load HSVACPMCKVLCC2Z1001 HSVACPMCKVLCC2Z1001 -data = HSVACPMCKVLCC2Z3505; -set(0,'defaultfigurecolor','w') -t = data(:,1); -x = data(:,2); -y = data(:,3); -psi = data(:,4); -u = data(:,5); -v = data(:,6); -r = data(:,7); -phi = data(:,8); -d = data(:,9); -%Data Mapping -figure(3) -figure_FontSize=20; -set(findobj('FontSize',30),'FontSize',figure_FontSize);set(gcf,'Position',[100,100,400,480]); -subplot(414),plot(t,psi,'linewidth',2);hold on -subplot(411),plot(t,u,'linewidth',2),xlabel('time (s)'),grid on;hold on -subplot(412),plot(t,v,'linewidth',2),xlabel('time (s)'),grid on;hold on -subplot(413),plot(t,r,'linewidth',2),xlabel('time (s)'),grid on;hold on - -%Calculation errors -rmse_u = sqrt(mean((U_pre-data(1:size(U_pre,1),5)).^2)) -rmse_v = sqrt(mean((V_pre-data(1:size(V_pre,1),6)).^2)) -rmse_r = sqrt(mean((R_pre-data(1:size(R_pre,1),7)).^2)) \ No newline at end of file diff --git a/Surface Vehicle/self_library.m b/Surface Vehicle/self_library.m index 7426cc7..6b6060e 100644 --- a/Surface Vehicle/self_library.m +++ b/Surface Vehicle/self_library.m @@ -1,3 +1,4 @@ +clc,clear,close all %% Import data load HSVACPMCKVLCC2Z1005 HSVACPMCKVLCC2Z1005 load HSVACPMCKVLCC2Z1001 HSVACPMCKVLCC2Z1001 @@ -69,7 +70,7 @@ n=4; Nvar = 3; tspan=[0]; -data_pre = HSVACPMCKVLCC2Z3505; +data_pre = HSVACPMCKVLCC2Z2505; xv = [data_pre(1,5)-1.179,data_pre(1,6),data_pre(1,7)*pi/180]; x_p(1,:)=xv; u_p = data_pre(:,9)*pi/180; @@ -99,12 +100,12 @@ % Step1_dataconstruct3_new % Step2_main_new -load U_pre_35 U_pre_35 -load V_pre_35 V_pre_35 -load R_pre_35 R_pre_35 -U_pre_semi = U_pre_35; -V_pre_semi = V_pre_35; -R_pre_semi = R_pre_35; +load U_pre_25 U_pre_25 +load V_pre_25 V_pre_25 +load R_pre_25 R_pre_25 +U_pre_semi = U_pre_25; +V_pre_semi = V_pre_25; +R_pre_semi = R_pre_25; @@ -119,7 +120,7 @@ h2 = plot(tspan(1:3600),x_p(1:3600,1),'--','linewidth',1.5,'color',[0.93,0.69,0.13]); hold on h3 = plot(tspan(1:3600),U_pre_semi,'-.','linewidth',1.5,'color',[0.47,0.67,0.19]); -legend([h1,h2,h3,h4],'Gaussian process','Proposed method','Semi-Abkowitz','Experiment') +legend([h1,h2,h3,h4],'Gaussian process','Proposed HLAR method','Semi-Abkowitz','Experiment') axis([0 180 -inf inf]) diff --git a/Training data/npsAUV_zigzag_1010_005.mat b/Training data/npsAUV_zigzag_1010_005.mat new file mode 100644 index 0000000..e0ddf33 Binary files /dev/null and b/Training data/npsAUV_zigzag_1010_005.mat differ diff --git a/Underwater Vehicle/npsAUV_zigzag_1010_005p.mat b/Training data/npsAUV_zigzag_1010_005p.mat similarity index 100% rename from Underwater Vehicle/npsAUV_zigzag_1010_005p.mat rename to Training data/npsAUV_zigzag_1010_005p.mat diff --git a/Training data/npsAUV_zigzag_2020_005.mat b/Training data/npsAUV_zigzag_2020_005.mat new file mode 100644 index 0000000..430daa8 Binary files /dev/null and b/Training data/npsAUV_zigzag_2020_005.mat differ diff --git a/Underwater Vehicle/npsAUV_zigzag_2020_005p.mat b/Training data/npsAUV_zigzag_2020_005p.mat similarity index 100% rename from Underwater Vehicle/npsAUV_zigzag_2020_005p.mat rename to Training data/npsAUV_zigzag_2020_005p.mat diff --git a/Training data/npsAUV_zigzag_2505_005.mat b/Training data/npsAUV_zigzag_2505_005.mat new file mode 100644 index 0000000..fb9f1ed Binary files /dev/null and b/Training data/npsAUV_zigzag_2505_005.mat differ diff --git a/Training data/npsAUV_zigzag_2510_005.mat b/Training data/npsAUV_zigzag_2510_005.mat new file mode 100644 index 0000000..f052c96 Binary files /dev/null and b/Training data/npsAUV_zigzag_2510_005.mat differ diff --git a/Training data/npsAUV_zigzag_test.mat b/Training data/npsAUV_zigzag_test.mat new file mode 100644 index 0000000..edd0da4 Binary files /dev/null and b/Training data/npsAUV_zigzag_test.mat differ diff --git a/Underwater Vehicle/GetModelZigzag.m b/Underwater Vehicle/GetModel.m similarity index 100% rename from Underwater Vehicle/GetModelZigzag.m rename to Underwater Vehicle/GetModel.m diff --git a/Underwater Vehicle/GetModelZigzag_validation.m b/Underwater Vehicle/GetModel_validation.m similarity index 100% rename from Underwater Vehicle/GetModelZigzag_validation.m rename to Underwater Vehicle/GetModel_validation.m diff --git a/Underwater Vehicle/R_pre_doc.mat b/Underwater Vehicle/R_pre_doc.mat new file mode 100644 index 0000000..b2fc6ee Binary files /dev/null and b/Underwater Vehicle/R_pre_doc.mat differ diff --git a/Underwater Vehicle/R_pre_submarine.mat b/Underwater Vehicle/R_pre_submarine.mat new file mode 100644 index 0000000..99b180b Binary files /dev/null and b/Underwater Vehicle/R_pre_submarine.mat differ diff --git a/Underwater Vehicle/SINDY_npsAUV_optim.m b/Underwater Vehicle/SINDY_npsAUV_optim.m index 4ccb9b0..04fd436 100644 --- a/Underwater Vehicle/SINDY_npsAUV_optim.m +++ b/Underwater Vehicle/SINDY_npsAUV_optim.m @@ -1,4 +1,4 @@ -clc,close all,clear +clc,close all, clear %For each of the three methods, it is necessary to confirm that each method % becomes the correct set of predictions when changing the prediction set load npsAUV_zigzag_1010_005p npsAUV_zigzag_1010_005p @@ -6,14 +6,9 @@ load npsAUV_zigzag_2505_005 npsAUV_zigzag_2505_005 load npsAUV_zigzag_2510_005 npsAUV_zigzag_2510_005 %% Get test data (ZigZag movement) -time = 300; %Simulation time -rudder_1 = 5; %Rudder angle -d_psi_1 = 25; %Expectation Angle -[u_1,x_1] = zigzag(rudder_1,d_psi_1,time); -x_1=[x_1(1,:);x_1(2,:);x_1(6,:)]; %Select u,v,r as state variables - +load npsAUV_zigzag_test npsAUV_zigzag_test +x_1 = npsAUV_zigzag_test; %% Calculating the SINDY model - % GetModelZigzag %Get the best lamda xinput = [npsAUV_zigzag_1010_005p,npsAUV_zigzag_2020_005p]; x_1_self = xinput(1:3,:); @@ -62,26 +57,25 @@ Xi_sphs = Xi; %% Forecasting with Eulerian dispersion - SINDY -GetModelZigzag_validation +GetModel_validation %% Using the semi-conjugate plus PhD thesis model -step1_doc -step2_doc -rmse_u_semi = sqrt(mean((x_1(1,:)-U_pre_doc').^2)) -rmse_v_semi = sqrt(mean((x_1(2,:)-V_pre_doc').^2)) -rmse_r_semi = sqrt(mean((x_1(3,:)-R_pre_doc').^2)) +load U_pre_doc U_pre_doc +load V_pre_doc V_pre_doc +load R_pre_doc R_pre_doc +rmse_u_doc = sqrt(mean((x_1(1,:)-U_pre_doc').^2)) +rmse_v_doc = sqrt(mean((x_1(2,:)-V_pre_doc').^2)) +rmse_r_doc = sqrt(mean((x_1(3,:)-R_pre_doc').^2)) -%% Using the semi-conjugate Gashisenda model -% step1_submarine -% step2_submarine +%% Using the semi-conjugate Gashisenda model load U_pre_submarine U_pre_submarine load V_pre_submarine V_pre_submarine load R_pre_submarine R_pre_submarine -rmse_u_semi = sqrt(mean((x_1(1,:)-U_pre_submarine').^2)) -rmse_v_semi = sqrt(mean((x_1(2,:)-V_pre_submarine').^2)) -rmse_r_semi = sqrt(mean((x_1(3,:)-R_pre_submarine').^2)) +rmse_u_submarine = sqrt(mean((x_1(1,:)-U_pre_submarine').^2)) +rmse_v_submarine = sqrt(mean((x_1(2,:)-V_pre_submarine').^2)) +rmse_r_submarine = sqrt(mean((x_1(3,:)-R_pre_submarine').^2)) %% Final results show @@ -94,7 +88,7 @@ hold on h2 = plot(tspan,U_pre_submarine,'-.','linewidth',1.5,'color',[0.47,0.67,0.19]); h4 = plot(tspan,U_pre_doc,'linewidth',1.5,'color',[0,0.45,0.74]); -legend([h1,h2,h3,h4],{'Proposed Method','Semi-Silvestre','Experiment','Semi-Gertler'}) +legend([h1,h2,h3,h4],{'Proposed HLAR Method','Semi-Silvestre','Experiment','Semi-Gertler'}) axis([0 inf 1 1.6]) grid on box off diff --git a/Underwater Vehicle/Sindy3.m b/Underwater Vehicle/Sindy3.m new file mode 100644 index 0000000..7581275 --- /dev/null +++ b/Underwater Vehicle/Sindy3.m @@ -0,0 +1,56 @@ +function [fx,Xi] = Sindy3(xaug_train,xaug_val,dx_train,lamda,LibraryType) +temp=table2array(lamda); +lamda1=temp(1); +lamda2=temp(2); +lamda3=temp(3); + + +n=4; +Theta = selfpooldata(xaug_train,LibraryType); +Theta_norm = zeros(size(Theta,2),1); +for i = 1:size(Theta,2) + Theta_norm(i) = norm(Theta(:,i)); + Theta(:,i) = Theta(:,i)./Theta_norm(i); +end +m = size(Theta,2); + +lambda_vec = [lamda1,lamda2,lamda3]; + + +if exist('lambda_vec') == 1 + Xi = sparsifyDynamicsIndependent(Theta,dx_train,lambda_vec,n-1); +else + Xi = sparsifyDynamics(Theta,dx_train,lambda,n-1); +end + + +for i = 1:size(Theta,2) + Xi(i,:) = Xi(i,:)./Theta_norm(i); +end + + +Nvar = 3; +dxPool = []; + +x_p=[1,0,0]; +xp=x_p; +dt = 0.05; +u_1 =xaug_val(:,4)*pi/180; +for k=1:length(xaug_val) %Calculate the predicted acceleration magnitude + y=[xp(k,:) u_1(k)]; + xPool = selfpooldata(y,LibraryType); + dxPool = xPool*Xi(:,1:Nvar); + st = xp(k,:); dif=dxPool; + st_next = st+ (dt*dif); + xp(k+1,:) = st_next; +end + + +Rsquare_u = corrcoef(xp(2:end,1),xaug_val(:,1)); +Rsquare_v = corrcoef(xp(2:end,2),xaug_val(:,2)); +Rsquare_r = corrcoef(xp(2:end,3),xaug_val(:,3)); + +fx = -(Rsquare_u(1,2)+Rsquare_v(1,2)+Rsquare_r(1,2)); +%The solver finds the minimum value and converts the maximum value into a minimum value problem. + +end \ No newline at end of file diff --git a/Underwater Vehicle/U_pre_doc.mat b/Underwater Vehicle/U_pre_doc.mat new file mode 100644 index 0000000..87208d2 Binary files /dev/null and b/Underwater Vehicle/U_pre_doc.mat differ diff --git a/Underwater Vehicle/U_pre_submarine.mat b/Underwater Vehicle/U_pre_submarine.mat new file mode 100644 index 0000000..f3d5a4c Binary files /dev/null and b/Underwater Vehicle/U_pre_submarine.mat differ diff --git a/Underwater Vehicle/V_pre_doc.mat b/Underwater Vehicle/V_pre_doc.mat new file mode 100644 index 0000000..280f034 Binary files /dev/null and b/Underwater Vehicle/V_pre_doc.mat differ diff --git a/Underwater Vehicle/V_pre_submarine.mat b/Underwater Vehicle/V_pre_submarine.mat new file mode 100644 index 0000000..06cbea8 Binary files /dev/null and b/Underwater Vehicle/V_pre_submarine.mat differ diff --git a/Underwater Vehicle/selfpooldata.m b/Underwater Vehicle/selfpooldata.m new file mode 100644 index 0000000..93b450a --- /dev/null +++ b/Underwater Vehicle/selfpooldata.m @@ -0,0 +1,17 @@ +function yout = selfpooldata(xaug,LibraryType) +switch LibraryType + case 4 + if size(xaug,1)==4 + xaug = xaug'; + end + u = xaug(:,1); + v = xaug(:,2); + r = xaug(:,3); + d = xaug(:,4); + %Silvestre model + yout = [u.*abs(u),u.*v,u.*r,v.*v,v.*r,v.*abs(v),r.*r,r.*abs(r),u.*u.*d,r.*r.*r,u.*u.*d.*d,1./u]; + +end + +end + diff --git a/Underwater Vehicle/selfpooldatalist.m b/Underwater Vehicle/selfpooldatalist.m new file mode 100644 index 0000000..e0b2703 --- /dev/null +++ b/Underwater Vehicle/selfpooldatalist.m @@ -0,0 +1,30 @@ +function yout = selfpooldatalist(ahat,LibraryType) + +switch LibraryType + + case 4 + newout(1) = {''}; + newout{1,2} = ['udot']; + newout{1,3} = ['vdot']; + newout{1,4} = ['rdot']; + for k=1:size(ahat,1) + for j=1:size(ahat,2) + newout{k+1,1+j} = ahat(k,j); + end + end + newout{2,1} = ['u|u|']; + newout{3,1} = ['uv']; + newout{4,1} = ['ur']; + newout{5,1} = ['vv']; + newout{6,1} = ['vr']; + newout{7,1} = ['v|v|']; + newout{8,1} = ['rr']; + newout{9,1} = ['r|r|']; + newout{10,1} = ['uud']; + newout{11,1} = ['rrr']; + newout{12,1} = ['uudd']; + newout{13,1} = ['1/u']; + + yout = newout + +end diff --git a/Surface Vehicle/sparsifyDynamics.m b/Underwater Vehicle/sparsifyDynamics.m similarity index 100% rename from Surface Vehicle/sparsifyDynamics.m rename to Underwater Vehicle/sparsifyDynamics.m diff --git a/Surface Vehicle/sparsifyDynamicsIndependent.m b/Underwater Vehicle/sparsifyDynamicsIndependent.m similarity index 100% rename from Surface Vehicle/sparsifyDynamicsIndependent.m rename to Underwater Vehicle/sparsifyDynamicsIndependent.m diff --git a/Underwater Vehicle/step1_doc.m b/Underwater Vehicle/step1_doc.m deleted file mode 100644 index 75b22f5..0000000 --- a/Underwater Vehicle/step1_doc.m +++ /dev/null @@ -1,62 +0,0 @@ -set(0,'defaultfigurecolor','w') -%% Step1:load data - -%Use zigzag for the training set -load npsAUV_zigzag_1010_005p npsAUV_zigzag_1010_005p -load npsAUV_zigzag_2020_005p npsAUV_zigzag_2020_005p -load npsAUV_zigzag_2505_005 npsAUV_zigzag_2505_005 -load npsAUV_zigzag_2510_005 npsAUV_zigzag_2510_005 - -h= 0.05; sample= 2; %sampling step -%h is the set time interval - -data_raw = [npsAUV_zigzag_1010_005p,npsAUV_zigzag_2020_005p]'; -data = data_raw; - -num_tr = size(data,1); -t=linspace(0,num_tr,num_tr+1).*h; - -data_train = data; -u = data_train(:,1); -v = data_train(:,2); -r = data_train(:,3); -d = data_train(:,4)*pi/180; %Rudder angle - -%% Step2 Construct data -tic -u_x_doc = u(1:end-1); u_y = u(2:end); -v_x_doc = v(1:end-1); v_y = v(2:end); -r_x_doc = r(1:end-1); r_y = r(2:end); -d_x_doc = d(1:end-1); -Xm = [u_x_doc,v_x_doc,r_x_doc,d_x_doc]; -Ym = [u_y,v_y,r_y]; -% noise test -Xm2=[u_x_doc,v_x_doc,r_x_doc]; -Am = (Ym-Xm2)/h; -Am_d = wdenoise(Am); -t_t =t(1:sample:end-1); -Xm_t= Xm(1:sample:num_tr,:); -Am_t = Am(1:sample:num_tr,:); -Am_d_t = Am_d(1:sample:num_tr,:); - - - -Len1= Am_d_t(1:end-1,1); -Len2= Am_d_t(1:end-1,2); -Len3=Am_d_t(1:end-1,3); - -u_x_doc=u_x_doc(1:2:end); -v_x_doc=v_x_doc(1:2:end); -r_x_doc=r_x_doc(1:2:end); -d_x_doc=d_x_doc(1:2:end); - -for i=1:num_tr/2-1 - [X1_doc(i,:),X2_doc(i,:),X3_doc(i,:)]=ConstructAUV_doc(u_x_doc,v_x_doc,r_x_doc,d_x_doc,i); %和F8中pooldata 作用是一样的 -end - - -theta1 = pinv(X1_doc'*X1_doc)*X1_doc'*Len1; %Coefficients of the model -theta2 = pinv(X2_doc'*X2_doc)*X2_doc'*Len2; -theta3 = pinv(X3_doc'*X3_doc)*X3_doc'*Len3; - -t1 = toc; %Calculate training time diff --git a/Underwater Vehicle/step1_submarine.m b/Underwater Vehicle/step1_submarine.m deleted file mode 100644 index c6edceb..0000000 --- a/Underwater Vehicle/step1_submarine.m +++ /dev/null @@ -1,63 +0,0 @@ -set(0,'defaultfigurecolor','w') -%% Step1:load data - -%Use zigzag for the training set -load npsAUV_zigzag_1005 npsAUV_zigzag_1005 -load npsAUV_zigzag_2005 npsAUV_zigzag_2005 -load npsAUV_zigzag_3005 npsAUV_zigzag_3005 -load npsAUV_zigzag_1010_005p npsAUV_zigzag_1010_005p -load npsAUV_zigzag_2020_005p npsAUV_zigzag_2020_005p - -h= 0.05; sample= 2; %Sampling step -%h is the set time interval - -data_raw = [npsAUV_zigzag_1010_005p,npsAUV_zigzag_2020_005p]'; -data = data_raw; - -num_tr = size(data,1); -t=linspace(0,num_tr,num_tr+1).*h; - -data_train = data; -u = data_train(:,1); -v = data_train(:,2); -r = data_train(:,3); -d = data_train(:,4)*pi/180; %Rudder angle - -%% Step2 Construct data -tic -u_x = u(1:end-1); u_y = u(2:end); -v_x = v(1:end-1); v_y = v(2:end); -r_x = r(1:end-1); r_y = r(2:end); -d_x = d(1:end-1); -Xm = [u_x,v_x,r_x,d_x]; -Ym = [u_y,v_y,r_y]; -% noise test -Xm2=[u_x,v_x,r_x]; -Am = (Ym-Xm2)/h; -Am_d = wdenoise(Am); -t_t =t(1:sample:end-1); -Xm_t= Xm(1:sample:num_tr,:); -Am_t = Am(1:sample:num_tr,:); -Am_d_t = Am_d(1:sample:num_tr,:); - - - -Len1= Am_d_t(1:end-1,1); -Len2= Am_d_t(1:end-1,2); -Len3=Am_d_t(1:end-1,3); - -u_x=u_x(1:2:end); -v_x=v_x(1:2:end); -r_x=r_x(1:2:end); -d_x=d_x(1:2:end); - -for i=1:num_tr/2-1 - [X1(i,:),X2(i,:),X3(i,:)]=ConstructAUV_submarine(u_x,v_x,r_x,d_x,i); -end - - -theta1 = pinv(X1'*X1)*X1'*Len1; %Coefficients of the model -theta2 = pinv(X2'*X2)*X2'*Len2; -theta3 = pinv(X3'*X3)*X3'*Len3; - -t1 = toc; %Record training time diff --git a/Underwater Vehicle/step2_doc.m b/Underwater Vehicle/step2_doc.m deleted file mode 100644 index cca526f..0000000 --- a/Underwater Vehicle/step2_doc.m +++ /dev/null @@ -1,58 +0,0 @@ -load npsAUV_zigzag_2505 npsAUV_zigzag_2505 -load npsAUV_zigzag_3505 npsAUV_zigzag_3505 -load npsAUV_zigzag_1505 npsAUV_zigzag_1505 - -pre_data= npsAUV_zigzag_2505_005; -order=pre_data(4,:)*pi/180; - -%deg angle; rad radian -R2D=180/pi; -D2R=pi/180; -h=0.05; %Step length: seconds -m= size(pre_data,2); %Rhythm -tic -x = zeros(6,1); %Temporary status variables -TEMP_a = zeros(6,1); -T = zeros(m,1); -Y = zeros(m,14); -%Variable initialization -u0 = 1; v0 = 0; r0 = 0; x0 = 0; y0 = 0; psi0 = 0;d0 = 0; -Initial = [u0; v0; r0; x0; y0; psi0]; - - -x = Initial; -X1 = zeros(m,2);X2 = zeros(m,7);X3 = zeros(m,7); - -for i=1:1:m - t = h*i; - T(i,1)=t; - d_vali=order(i); - [TEMP_a]=Infante_doc(x,theta1,theta2,theta3,d_vali) ; - x= x + h.*TEMP_a; - Y(i,1) = x(1);%u - Y(i,2) = x(2);%v - Y(i,3) = x(3);%r - Y(i,4) = x(4);%x - Y(i,5) = x(5);%y - Y(i,6) = x(6);%psi-yaw -end - -U_pre_doc = Y(:,1); -V_pre_doc = Y(:,2); -R_pre_doc = Y(:,3); -Xp = Y(:,4); -Yp = Y(:,5); -psi = Y(:,6); -duo = Y(:,7); - -duo = duo*180/pi; -psi = psi *180/pi; -t2=toc; - -figure -subplot(311),plot(T,U_pre_doc,'linewidth',1.5),xlabel('time (s)'),ylabel('u (m/s)');grid on;hold on -subplot(312),plot(T,V_pre_doc,'linewidth',1.5),xlabel('time (s)'),ylabel('v (m/s)');grid on;hold on -subplot(313),plot(T,R_pre_doc,'linewidth',1.5),xlabel('time (s)'),ylabel('r (deg/s)');grid on;hold on - - - diff --git a/Underwater Vehicle/step2_submarine.m b/Underwater Vehicle/step2_submarine.m deleted file mode 100644 index 1ec3336..0000000 --- a/Underwater Vehicle/step2_submarine.m +++ /dev/null @@ -1,69 +0,0 @@ -load npsAUV_zigzag_2505_005 npsAUV_zigzag_2505_005 -load npsAUV_zigzag_3505 npsAUV_zigzag_3505 - -pre_data= npsAUV_zigzag_2505_005; -order=pre_data(4,:)*pi/180; - -%deg angle; rad radian -R2D=180/pi; -D2R=pi/180; -h=0.05; %Step length: seconds -m= size(pre_data,2); %Rhythm -tic -x = zeros(6,1); %Temporary status variables -TEMP_a = zeros(6,1); -T = zeros(m,1); %Time -Y = zeros(m,14); %Status Variables -%Variable initialization -u0 = 1; v0 = 0; r0 = 0; x0 = 0; y0 = 0; psi0 = 0;d0 = 0; -Initial = [u0; v0; r0; x0; y0; psi0]; - - - - -x = Initial; -X1 = zeros(m,2);X2 = zeros(m,7);X3 = zeros(m,7); - -for i=1:1:m - t = h*i; - T(i,1)=t; - d_vali=order(i); - [TEMP_a]=Infante_submarine(x,theta1,theta2,theta3,d_vali) ; - x= x + h.*TEMP_a; - Y(i,1) = x(1);%u - Y(i,2) = x(2);%v - Y(i,3) = x(3);%r - Y(i,4) = x(4);%x - Y(i,5) = x(5);%y - Y(i,6) = x(6);%psi-yaw -end - -U_pre = Y(:,1); -V_pre = Y(:,2); -R_pre = Y(:,3); -Xp = Y(:,4); -Yp = Y(:,5); -psi = Y(:,6); -duo = Y(:,7); - - - - -duo = duo*180/pi; -psi = psi *180/pi; -t2=toc; - -figure -subplot(311),plot(T,U_pre,'linewidth',1.5),xlabel('time (s)'),ylabel('u (m/s)');grid on;hold on -subplot(312),plot(T,V_pre,'linewidth',1.5),xlabel('time (s)'),ylabel('v (m/s)');grid on;hold on -subplot(313),plot(T,R_pre,'linewidth',1.5),xlabel('time (s)'),ylabel('r (deg/s)');grid on;hold on - -U_pre_submarine = U_pre; -V_pre_submarine = V_pre; -R_pre_submarine = R_pre; - -save U_pre_submarine U_pre_submarine -save V_pre_submarine V_pre_submarine -save R_pre_submarine R_pre_submarine - - diff --git a/Underwater Vehicle/step3_doc.m b/Underwater Vehicle/step3_doc.m deleted file mode 100644 index 0a0535a..0000000 --- a/Underwater Vehicle/step3_doc.m +++ /dev/null @@ -1,15 +0,0 @@ - -data = npsAUV_zigzag_2505; -set(0,'defaultfigurecolor','w') - -u = data(1,:); -v = data(2,:); -r = data(3,:); -d = data(4,:); -%Data Mapping -figure(1) -subplot(311),plot(T,u,'linewidth',2),xlabel('time (s)'),grid on;hold on -title('result of doc model') -legend('semi','real') -subplot(312),plot(T,v,'linewidth',2),xlabel('time (s)'),grid on;hold on -subplot(313),plot(T,r,'linewidth',2),xlabel('time (s)'),grid on;hold on diff --git a/Underwater Vehicle/step3_submarine.m b/Underwater Vehicle/step3_submarine.m deleted file mode 100644 index 0e8e363..0000000 --- a/Underwater Vehicle/step3_submarine.m +++ /dev/null @@ -1,14 +0,0 @@ - -data = npsAUV_zigzag_2505; -set(0,'defaultfigurecolor','w') - -u = data(1,:); -v = data(2,:); -r = data(3,:); -d = data(4,:); -%Data Mapping -figure(1) -subplot(311),plot(T,u,'linewidth',2),xlabel('time (s)'),grid on;hold on -title('result of submarine') -subplot(312),plot(T,v,'linewidth',2),xlabel('time (s)'),grid on;hold on -subplot(313),plot(T,r,'linewidth',2),xlabel('time (s)'),grid on;hold on diff --git a/Underwater Vehicle/zigzag.m b/Underwater Vehicle/zigzag.m deleted file mode 100644 index 2e2448d..0000000 --- a/Underwater Vehicle/zigzag.m +++ /dev/null @@ -1,25 +0,0 @@ -function[signal,x]=zigzag(rudder,d_psi,time) -rudder=rudder*pi/180; %Converts the input angle into radians -d_psi=d_psi*pi/180; -dt=0.05; %Time for small changes -x0 = [1;0;0;0;0;0;0;0;10;0;0;0]; -ui = [rudder;0;0;0;0;1200]; %Set the initial state of the navigator -x(:,1) = x0(1:12); %Assigning initial values to the robot's state -signal(1)=ui(1,:); -for k=1:time/dt %Expand the number of simulations - [xdot(:,k),U(:,k)] = npsauv(x(:,k),ui); - st = x(:,k); dif=xdot(:,k); - st_next = st+ (dt*dif); - x(:,k+1) = st_next; %Update next status - if x(12,end)>=d_psi - ui = [rudder;0;0;0;0;1200]; - elseif -x(12,end)>=d_psi - ui = [-rudder;0;0;0;0;1200]; - end - signal(k+1)=ui(1,:); -end - psi=x(12,2:end)*180/pi; %Output bow angle - signal=-signal*180/pi; - x=x; - -end