www.pudn.com > SHIPCONTROL.rar > sixdegreesfreedomshipmotion.m, change:2015-12-02,size:1840b


 
close all; 
clear all; 
clc; 
global m L D B midu CB ZG H T  Dp np  tp b1 b2  P hR  epxlong hs delta kafang deltaomiga h13 nw  Cw Cp Zb; 
m=55000*10^3; 
L=280; 
D=9.45; 
B=35.5; 
midu=1000; 
CB=0.597; 
ZG=14; 
H=2.9; 
T=0.058*D; 
Dp=4.3; 
np=3; 
tp=0.08; 
b1=6.5; 
b2=4.6; 
P=3.7; 
hR=7.2; 
epxlong=0.92; 
hs=6.9; 
delta=pi/20; 
t0=0; 
tf=10; 
kafang=pi/20; 
deltaomiga=0.1; 
h13=1; 
nw=200; 
Cw=0.6; 
Cp=0.6180; 
Zb=9; 
 
tspan=[t0,tf]; 
Y0=[0 0 0 0 0 0 8 0 0 0 0 0]; 
[time,YY]=ode45(@DYDT20101201,tspan,Y0); 
x=YY(:,1);  
y=YY(:,2); 
z=YY(:,3); 
FAI=YY(:,4); 
sita=YY(:,5); 
bosai=YY(:,6); 
 
 
u=YY(:,7); 
v=YY(:,8); 
w=YY(:,9); 
p=YY(:,10); 
q=YY(:,11); 
r=YY(:,12); 
 
 
 
[hang,lie]=size(x); 
x0=zeros(1,hang); 
y0=zeros(1,hang); 
z0=zeros(1,hang); 
FAI0=zeros(1,hang); 
sita0=zeros(1,hang); 
bosai0=zeros(1,hang); 
 
for i=2:hang-1 
    x0(i+1)=(u(i)*cos(bosai(i))*cos(sita(i))+v(i)*cos(bosai(i))*sin(sita(i))*sin(FAI(i))-v(i)*cos(FAI(i))*sin(bosai(i))+... 
    w(i)*cos(bosai(i))*sin(sita(i))*cos(FAI(i))+w(i)*sin(bosai(i))*sin(FAI(i)))*(time(i)-time(i-1))+x0(i); 
    y0(i+1)=(u(i)*sin(bosai(i))*cos(sita(i))+v(i)*(cos(FAI(i))*cos(bosai(i))+sin(bosai(i))*sin(sita(i))*sin(FAI(i)))+... 
    w(i)*(sin(bosai(i))*sin(sita(i))*cos(FAI(i))-sin(FAI(i))*cos(bosai(i))))*(time(i)-time(i-1))+y0(i); 
    z0(i+1)=(-u(i)*sin(sita(i))+v(i)*cos(sita(i))*sin(FAI(i))+w(i)*cos(sita(i))*cos(FAI(i)))*(time(i)-time(i-1))+z0(i); 
    FAI0(i+1)=(p(i)+q(i)*tan(sita(i))*sin(FAI(i))+r(i)*tan(sita(i))*cos(FAI(i)))*(time(i)-time(i-1))+FAI0(i); 
    sita0(i+1)=(q(i)*cos(FAI(i))-r(i)*sin(FAI(i)))*(time(i)-time(i-1))+sita0(i); 
    bosai0(i+1)=(r(i)*cos(FAI(i))/cos(sita(i))+q(i)*sin(FAI(i))/cos(sita(i)))*(time(i)-time(i-1))+bosai0(i); 
     
end 
plot(x0,y0);  
figure 
plot(z0) 
figure 
plot(FAI0); 
figure 
plot(sita0) 
figure 
plot(bosai0);