clc
clear all
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%The Infante AUV model parameters
M=2234.5;I_z=2000;X_du=-142;Y_dv=-1715;N_dr=-1350;X_uu=-35.4;X_vv=-128.4;Y_r=435;
Y_v=-346;Y_vv=-667;N_v=-686;N_vv=443;N_r=-1427;
M_u=M-X_du;M_v=M-Y_dv;M_r=I_z-N_dr;M_ur=M-Y_r;
% du=-X_uu*u^2-X_vv*v^2;
% dv=-Y_v*u*v-Y_vv*v*abs(v);
% dr=-N_v*u*v-N_vv*v*abs(v)-N_r*u*r;
%%%%%%%%%%%%%%%%%%%%%%%%%%%
X=[4;-4;0];U=[0;0;0];
F=100;T=50;
h=0.05;
n=10000;
plot_time=zeros(n,1);
plot_X=zeros(n,3);
plot_U=zeros(n,3);
for i=1:n
X(3,:)=rad2pipi(X(3,:));
plot_time(i)=h*i;
plot_X(i,:)=X;
plot_U(i,:)=U;
k11=U(1,:)*cos(X(3,:))-U(2,:)*sin(X(3,:));
k21=U(1,:)*sin(X(3,:))+U(2,:)*cos(X(3,:));
k31=U(3,:);
k41=(X_uu*U(1,:)^2+X_vv*U(2,:)^2)/M_u+F/M_u;
k51=(Y_v*U(1,:)*U(2,:)+Y_vv*U(2,:)*abs(U(2,:)))/M_v-M_ur*U(1,:)*U(3,:)/M_v;
k61=(N_v*U(1,:)*U(2,:)+N_vv*U(2,:)*abs(U(2,:))+N_r*U(1,:)*U(3,:))/M_r+T/M_r;
k12=(U(1,:)+h/2*k41)*cos(X(3,:)+h/2*k31)-(U(2,:)+h/2*k51)*sin(X(3,:)+h/2*k31);
k22=(U(1,:)+h/2*k41)*sin(X(3,:)+h/2*k31)+(U(2,:)+h/2*k51)*cos(X(3,:)+h/2*k31);
k32=U(3,:)+h/2*k61;
k42=(X_uu*(U(1,:)+h/2*k41)^2+X_vv*(U(2,:)+h/2*k51)^2)/M_u+F/M_u;
k52=(Y_v*(U(1,:)+h/2*k41)*(U(2,:)+h/2*k51)+Y_vv*(U(2,:)+h/2*k51)*abs(U(2,:)+h/2*k51))/M_v-M_ur*(U(1,:)+h/2*k41)*(U(3,:)+h/2*k61)/M_v;
k62=(N_v*(U(1,:)+h/2*k41)*(U(2,:)+h/2*k51)+N_vv*(U(2,:)+h/2*k51)*abs(U(2,:)+h/2*k51)+N_r*(U(1,:)+h/2*k41)*(U(3,:)+h/2*k61))/M_r+T/M_r;
k13=(U(1,:)+h/2*k42)*cos(X(3,:)+h/2*k32)-(U(2,:)+h/2*k52)*sin(X(3,:)+h/2*k32);
k23=(U(1,:)+h/2*k42)*sin(X(3,:)+h/2*k32)+(U(2,:)+h/2*k52)*cos(X(3,:)+h/2*k32);
k33=U(3,:)+h/2*k62;
k43=(X_uu*(U(1,:)+h/2*k42)^2+X_vv*(U(2,:)+h/2*k52)^2)/M_u+F/M_u;
k53=(Y_v*(U(1,:)+h/2*k42)*(U(2,:)+h/2*k52)+Y_vv*(U(2,:)+h/2*k52)*abs(U(2,:)+h/2*k52))/M_v-M_ur*(U(1,:)+h/2*k42)*(U(3,:)+h/2*k62)/M_v;
k63=(N_v*(U(1,:)+h/2*k42)*(U(2,:)+h/2*k52)+N_vv*(U(2,:)+h/2*k52)*abs(U(2,:)+h/2*k52)+N_r*(U(1,:)+h/2*k42)*(U(3,:)+h/2*k62))/M_r+T/M_r;
k14=(U(1,:)+h*k43)*cos(X(3,:)+h*k33)-(U(2,:)+h*k53)*sin(X(3,:)+h*k33);
k24=(U(1,:)+h*k43)*sin(X(3,:)+h*k33)+(U(2,:)+h*k53)*cos(X(3,:)+h*k33);
k34=U(3,:)+h*k63;
k44=(X_uu*(U(1,:)+h*k43)^2+X_vv*(U(2,:)+h*k53)^2)/M_u+F/M_u;
k54=(Y_v*(U(1,:)+h*k43)*(U(2,:)+h*k53)+Y_vv*(U(2,:)+h*k53)*abs(U(2,:)+h*k53))/M_v-M_ur*(U(1,:)+h*k43)*(U(3,:)+h*k63)/M_v;
k64=(N_v*(U(1,:)+h*k43)*(U(2,:)+h*k53)+N_vv*(U(2,:)+h*k53)*abs(U(2,:)+h*k53)+N_r*(U(1,:)+h*k43)*(U(3,:)+h*k63))/M_r+T/M_r;
X(1,:)=X(1,:)+h/6*(k11+k12+k13+k14);
X(2,:)=X(2,:)+h/6*(k21+k22+k23+k24);
X(3,:)=X(3,:)+h/6*(k31+k32+k33+k34);
U(1,:)=U(1,:)+h/6*(k41+k42+k43+k44);
U(2,:)=U(2,:)+h/6*(k51+k52+k53+k54);
U(3,:)=U(3,:)+h/6*(k61+k62+k63+k64);
end
figure(1)
plot(plot_X(:,1),plot_X(:,2),'r');
figure(2)
plot(plot_time,plot_X(:,3),'r');
hold on
plot(plot_time,plot_U(:,3),'g');
figure(3)
plot(plot_time,plot_U(:,1),'r');
hold on
plot(plot_time,plot_U(:,2),'g');
