Plot graph

Dear all,
I need a help. I can run my program below, but the problem is not all the figure plotted come out, only figure 1 and figure 2. Why this situation happen? Tq.

function[sb_test]=sb_test(x,dx,y,dy,z,dz,phi,dphi,theta,dtheta,psi,dpsi,s1,s2,l1,l2,l3,l4,k1,k2,k3,k4,t)

i = %i;
omega=0;

ns=12;
ni=4;
delta_t=0.01;

x0=[x;dx;y;dy;phi;dphi;theta;dtheta;psi;dpsi;z;dz];
tf=15;
j1=0;
j2=0;

for i=1:t
t_current=(i-1)*delta_t;
tspan=i*delta_t;

t0=t_current;
tspan2=t0:delta_t:tspan;

// y=[x,dx,psi,dpsi,y,dy]
y=[x0(1,1);x0(2,1);x0(3,1);x0(4,1);x0(5,1);x0(6,1);x0(7,1);x0(8,1);x0(9,1);x0(10,1);x0(11,1);x0(12,1)];

y2=sb_trans_y(y(1,1),y(2,1),y(3,1),y(4,1),y(11,1),y(12,1),y(5,1),y(6,1),y(7,1),y(8,1),y(9,1),y(10,1));

y3=sb_trans_x(y2(1,1),y2(2,1),y2(3,1),y2(4,1),y2(5,1),y2(6,1));

y4=sb_trans_z(y3(1,1),y3(2,1),y3(3,1),y3(4,1),y3(5,1),y3(6,1));

//(w1,w2)
y5=sb_input(y4(1,1),y4(2,1),y4(3,1),y4(4,1),y4(5,1),y4(6,1),s1,s2,l1,l2,l3,l4);

//v1,v2
y6=sb_input_ori(y(1,1),y(2,1),y(3,1),y(4,1),y(11,1),y(12,1),y(5,1),y(6,1),y(7,1),y(8,1),y(9,1),y(10,1),y5(1,1),y5(2,1));

//
y7=sb_input_ori2(y(1,1),y(2,1),y(3,1),y(4,1),y(11,1),y(12,1),y(5,1),y(6,1),y(7,1),y(8,1),y(9,1),y(10,1),y6(1,1),y6(2,1),k1,k2,k3,k4,omega);

//
z(i,1)=t_current;
for j2=1:ns
z(i,j2+1)=y(j2);
end

//(v_1,v_2,hat v_3,hat v_4)
v(i,1)=t_current;
for j2=1:ni
v(i,j2+1)=y7(j2);
end

//omega‚
omega = sb_omega(y7(1,1),y7(2,1),y7(3,1),y7(4,1));

//
deff("[xdot]=f(t,x)","xdot=sb_plant_6(y(1,1),y(2,1),y(3,1),y(4,1),y(11,1),y(12,1),y(5,1),y(6,1),y(7,1),y(8,1),y(9,1),y(10,1),y7(1,1),y7(2,1),y7(3,1),y7(4,1),omega);")

//
x=ode(y,t0,tspan2,f);
n=size(x,2);

for j2=1:ns
x0(j2,1)=x(j2,n);
end

end

sb_test = [x0(1,1);x0(2,1);x0(3,1);x0(4,1);x0(5,1);x0(6,1);x0(7,1);x0(8,1);x0(9,1);x0(10,1);y7(1,1);y7(2,1);y7(3,1);y7(4,1)];

figure(1)
plot(z(:,1),z(:,2),'-',z(:,1),z(:,4),'--',z(:,1),z(:,12),':');
word_1 = legend('x','y','z');
xlabel('Time t [s]','FontSize',4,'FontName','Times');
ylabel('Position [m]','FontSize',4,'FontName','Times');

figure(2)
plot(z(:,1),z(:,6),'-',z(:,1),z(:,8),'--',z(:,1),z(:,10),':');
word_1 = legend('phi','theta','psi');
xlabel('Time t [s]','FontSize',4,'FontName','Times');
ylabel('Attitude [rad]','FontSize',4,'FontName','Times');

figure(3)
plot(z(:,1),z(:,3),'-',z(:,1),z(:,5),'--',z(:,1),z(:,13),'---');
word_1 = legend('dx','dy','dz');
xlabel('Time t [s]','FontSize',4,'FontName','Times');
ylabel('Velocity [m/s]','FontSize',4,'FontName','Times');

figure(4)
plot(z(:,1),z(:,7),'-',z(:,1),z(:,9),'--',z(:,1),z(:,11),':');
word_1 = legend('dphi','dtheta','dpsi');
xlabel('Time t [s]','FontSize',4,'FontName','Times');
ylabel('Anglar velocity [rad/s]','FontSize',4,'FontName','Times');

figure(5)
plot(v(:,1),v(:,2),'-',v(:,1),v(:,3),'--',v(:,1),v(:,4),'---',v(:,1),v(:,5),':');
word_3 = legend('u_1','u_2','u_3','u_4');
xlabel('Time t [s]','FontSize',4,'FontName','Times');
ylabel('Control inputs','FontSize',4,'FontName','Times');

endfunction

function[sb_trans_z] = sb_trans_z(x1,x2,x3,x4,x5,x6)

z1 = x1;
z2 = x2;
z3 = x3;
z4 = x4;
z5 = (x5/x1);
z6 = (x6/x1);

sb_trans_z = [z1;
z2;
z3;
z4;
z5;
z6;
];

endfunction

function[sb_trans_y] = sb_trans_y(x,dx,y,dy,z,dz,phi,dphi,theta,dtheta,psi,dpsi)
mb = 21.43; // mass of the vehicle
rho = 1023.0; //% fluid density
L = 0.1; //% distance
r = 0.1; //% radius
b = 0.068; //% thrust factor
d = 3.617e-4;// % drag factor
Jx = 0.0857; //%roll inertia of the vehicle
Jy = 1.1143; //% pitch inertia of the vehicle
Jz = 1.1143; //% yaw inertia of the vehicle
J_t = 1.1941e-4;// % thruster inertia

m1 = mb + 0.394*rho*%pi*r^3; //%total mass in the x-direction
m2 = mb + 5.96*rho*%pi*r^3; //%total mass in the y-direction
m3 = mb + 5.96*rho*%pi*r^3; //%total mass in the z-direction

Ix = Jx + 0;// %total inertia in the x-direction
Iy = Jy + 24.2648*rho*%pi*r^5;// %total inertia in the y-direction
Iz = Jz + 24.2648*rho*%pi*r^5;// %total inertia in the z-direction

y1 = x ;
y2 = m1/m2*tan(psi);
y3 = y ;

dy1 = dx ;
dy2 = m1/m2*dpsi/(cos(psi)^2);
dy3 = dy ;

sb_trans_y = [y1;
y2;
y3;
dy1;
dy2;
dy3];

endfunction

function[sb_trans_x] = sb_trans_x(y1,y2,y3,dy1,dy2,dy3)

x1 = y1;
x2 = dy1;
x3 = y2;
x4 = dy2;
x5 = y3;
x6 = dy3;

sb_trans_x = [x1;
x2;
x3;
x4;
x5;
x6;
];

endfunction

function [sb_plant_6] = sb_plant_6(x,dx,y,dy,z,dz,phi,dphi,theta,dtheta,psi,dpsi,u1,u2,u3,u4,omega)

mb = 21.43; // mass of the vehicle
rho = 1023.0; //% fluid density
L = 0.1; //% distance
r = 0.1; //% radius
b = 0.068; //% thrust factor
d = 3.617e-4;// % drag factor
Jx = 0.0857; //%roll inertia of the vehicle
Jy = 1.1143; //% pitch inertia of the vehicle
Jz = 1.1143; //% yaw inertia of the vehicle
J_t = 1.1941e-4;// % thruster inertia

m1 = mb + 0.394*rho*%pi*r^3; //%total mass in the x-direction
m2 = mb + 5.96*rho*%pi*r^3; //%total mass in the y-direction
m3 = mb + 5.96*rho*%pi*r^3; //%total mass in the z-direction

Ix = Jx + 0;// %total inertia in the x-direction
Iy = Jy + 24.2648*rho*%pi*r^5;// %total inertia in the y-direction
Iz = Jz + 24.2648*rho*%pi*r^5;// %total inertia in the z-direction

f_x = [ (dx);
0;
(dy);
0;
(dphi);
0;
(dtheta);
0;
(dpsi);
0;
(dz);
0;
];

g1_x = [ 0;
(cos(theta)*cos(psi)) /m1;
0;
(cos(theta)*sin(psi)) /m2;
0;
0;
0;
0;
0;
0;
0;
(-sin(theta)) /m3;
];

g2_x = [ 0;
0;
0;
0;
0;
(1/Ix);
0;
0;
0;
0;
0;
0;
];

g3_x = [ 0;
0;
0;
0;
0;
0;
0;
(L/Iy);
0;
0;
0;
0;
];

g4_x = [ 0;
0;
0;
0;
0;
0;
0;
0;
0;
(L/Iz);
0;
0;
];

g5_x = [ 0;
0;
0;
0;
0;
(dtheta)*(dpsi)*(Iy-Iz)/Ix;
0;
(dphi)*(dpsi)*((Iz-Ix)/Iy)-(J_t)*(dpsi)*(omega)/Iy;
0;
(dphi)*(dtheta)*(Ix-Iy)/Iz+J_t*(dtheta)*(omega)/Iz;
0;
0;
];

sb_plant_6=f_x + g1_x*u1 + g2_x*u2 + g3_x*u3 + g4_x*u4 + g5_x;

endfunction

function[sb_omega] = sb_omega(u1,u2,u3,u4)

mb = 21.43; // mass of the vehicle
rho = 1023.0; //% fluid density
L = 0.1; //% distance
r = 0.1; //% radius
b = 0.068; //% thrust factor
d = 3.617e-4;// % drag factor
Jx = 0.0857; //%roll inertia of the vehicle
Jy = 1.1143; //% pitch inertia of the vehicle
Jz = 1.1143; //% yaw inertia of the vehicle
J_t = 1.1941e-4;// % thruster inertia

m1 = mb + 0.394*rho*%pi*r^3; //%total mass in the x-direction
m2 = mb + 5.96*rho*%pi*r^3; //%total mass in the y-direction
m3 = mb + 5.96*rho*%pi*r^3; //%total mass in the z-direction

Ix = Jx + 0;// %total inertia in the x-direction
Iy = Jy + 24.2648*rho*%pi*r^5;// %total inertia in the y-direction
Iz = Jz + 24.2648*rho*%pi*r^5;// %total inertia in the z-direction

f1 = (u1 + (b/d)*u2 - 2*u3)/4;
f2 = (u1 - (b/d)*u2 + 2*u4)/4;
f3 = ((u1 + (b/d)*u2)/4) + (1/2)*u3;
f4 = ((u1 - (b/d)*u2)/4) - (1/2)*u4;

W1 = sqrt((-1/b)*f1);
W2 = sqrt((-1/b)*f2);
W3 = sqrt((-1/b)*f3);
W4 = sqrt((-1/b)*f4);

omega = W2 + W4 - W1 - W3;

sb_omega = abs(omega);

endfunction

function[sb_input_ori2] = sb_input_ori2(x,dx,y,dy,z,dz,phi,dphi,theta,dtheta,psi,dpsi,w1,w4,k1,k2,k3,k4,omega)

mb = 21.43; // mass of the vehicle
rho = 1023.0; //% fluid density
L = 0.1; //% distance
r = 0.1; //% radius
b = 0.068; //% thrust factor
d = 3.617e-4;// % drag factor
Jx = 0.0857; //%roll inertia of the vehicle
Jy = 1.1143; //% pitch inertia of the vehicle
Jz = 1.1143; //% yaw inertia of the vehicle
J_t = 1.1941e-4;// % thruster inertia

m1 = mb + 0.394*rho*%pi*r^3; //%total mass in the x-direction
m2 = mb + 5.96*rho*%pi*r^3; //%total mass in the y-direction
m3 = mb + 5.96*rho*%pi*r^3; //%total mass in the z-direction

Ix = Jx + 0;// %total inertia in the x-direction
Iy = Jy + 24.2648*rho*%pi*r^5;// %total inertia in the y-direction
Iz = Jz + 24.2648*rho*%pi*r^5;// %total inertia in the z-direction

w2= - (k1)*(phi) - (k2)*(dphi);
w3= - (k3)*(theta) - (k4)*(dtheta);

u1 = m1*w1/(cos(theta)*cos(psi));
u2 = Ix*w2 - dtheta*dpsi*(Iy-Iz);
u3 = Iy/L*w3 - (dphi*dpsi*(Iz-Ix)-J_t*dpsi*omega)/L;
u4 = Iz/L*w4 - (dphi*dtheta*(Ix-Iy)+J_t*dtheta*omega)/L;

sb_input_ori2 = [u1;
u2;
u3;
u4;
];

endfunction

function[sb_input_ori] = sb_input_ori(x,dx,y,dy,z,dz,phi,dphi,theta,dtheta,psi,dpsi,v1,v2)

mb = 21.43; // mass of the vehicle
rho = 1023.0; //% fluid density
L = 0.1; //% distance
r = 0.1; //% radius
b = 0.068; //% thrust factor
d = 3.617e-4;// % drag factor
Jx = 0.0857; //%roll inertia of the vehicle
Jy = 1.1143; //% pitch inertia of the vehicle
Jz = 1.1143; //% yaw inertia of the vehicle
J_t = 1.1941e-4;// % thruster inertia

m1 = mb + 0.394*rho*%pi*r^3; //%total mass in the x-direction
m2 = mb + 5.96*rho*%pi*r^3; //%total mass in the y-direction
m3 = mb + 5.96*rho*%pi*r^3; //%total mass in the z-direction

w1 = v1 ;
w4 = 2*m2/m1*((cos(psi))^2)*v2-2*(tan(psi))*dpsi^2;
sb_input_ori = [w1;
w4;
];

endfunction

function[sb_input] = sb_input(z1,z2,z3,z4,z5,z6,s1,s2,l1,l2,l3,l4)

v1 = -(s1+s2)*z2 - s1*s2*z1;
v2 = l1*z3 + l2*z4 + l3*z5 + l4*z6;

sb_input = [v1;
v2;
];

endfunction

Taxonomy upgrade extras: