船舶动力定位模拟器.zip

function y=ShipModel(x) ksix=x(1); ksiy=x(2); ksipsi=x(3); xw=x(4);%%高频位置信息 yw=x(5);%%高频位置信息 psiw=x(6);%%高频航向信息 psi=x(7);%%固定坐标系低频航向信息 u=x(8);%%船体坐标系坐标系低频速度信息 v=x(9);%%船体坐标系坐标系低频速度信息 r=x(10);%%船体坐标系坐标系低频航向角速度信息 wx=x(11); wy=x(12); wpsi=x(13); X=x(14); Y=x(15); N=x(16); xw=[ksix;ksiy;ksipsi;xw;yw;psiw]; w0=0.8; ksi=0.1; Aw=[0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1; -w0^2 0 0 -2*ksi*w0 0 0; 0 -w0^2 0 0 -2*ksi*w0 0; 0 0 -w0^2 0 0 -2*ksi*w0]; Bw=[0 0 0; 0 0 0; 0 0 0; 1 0 0; 0 1 0; 0 0 1]; M=[5.3122*10^6 0 0; 0 8.2831*10^6 0; 0 0 3.7454*10^9]; M1= 1.0e-006*[ 0.1882 0 0; 0 0.1207 0; 0 0 0.0003]; D=[5.0242*10^4 0 0; 0 2.7229*10^5 -4.3933*10^6; 0 -4.3933*10^6 4.1894*10^8]; R=[cos(psi) -sin(psi) 0; sin(psi) cos(psi) 0; 0 0 1]; ywdot=Aw*xw+Bw*[wx;wy;wpsi]; yposdot=R*[u;v;r]; yspddot=-M1*D*[u;v; r]+M1*[X;Y;N];%zeros(3,1);% y=[ywdot;yposdot;yspddot]; end