You should look at why your error is occuring before posting. The value of zeta_dot is [0;0;0;0] and you're using i to index it to the 631st column. I tried to clean it up for you, but I don't know if this is what you want.
clc,
close all
dt=0.1;
ts=63;
%span
mw=2;
ms1 = 1;
ms2 = 1;
ms3 = 1;
ms4=1;
ms5=1;
ms6=1;
w=1;
Izzw = 0.02;
Ixxw = 0.02;
g=9.8;
a = 0.1;
xd=0.5;
yd=0.5;
th=90*pi/180
th_1 =90*pi/180
%zeta = [xd;yd;th;th_1]
zeta_dot = zeros(4,1);eai = zeros(4,1);
gamma_i = 1; gamma_p = 4; gamma_d = 4;
t = 0:dt:ts;
for i=1:length(t)
%% Desired values
zeta(:,i) = [xd;yd;th;th_1];
zeta_d(:,i) = [xd+0.2*sin(0.2*t(i));yd+0.2*sin(0.1*t(i));th+0.1*sin(0.1*t(i));th_1+0.1*sin(0.1*t(i))];
% zeta_d(:,i) = [xd+0.3*sin(0.1*t(i));yd-0.3*cos(0.1*t(i));yd+0.3*sin(0.1*t(i))];
e(:,i) = zeta_d(:,i) - zeta(:,i); % simple position error
% error derivatives
if i ==1
edot = zeros(4,1);
else
edot = (e(:,i)-e(:,i-1))/dt;
end
end
%% Dynamic aspects
%% slider masses and end-effector
%r1d = zeta_dot(1,i);
%r2d = zeta_dot(2,i);
%r3d = zeta_dot(3,i);
%r4d = zeta_dot(4,i);
r1 = zeta(1,i);
r2 = zeta(2,i);
r3 = zeta(3,i);
r4 = zeta(4,i);
r1p= xd+a/2*cos(th);
r2p= xd-a/2*cos(th);
r3p= yd+a/2*cos(th);
r4p= yd-a/2*cos(th);
rd(:,i) = [r1p;r2p;r3p;r4p];
mur(:,i) = [xd;yd;th;th_1];
m_11 = 2*ms1 + mw/4 + (ms3*mw)/8 - (a*ms3)/(2*a^2 + 2*r1 - 2*r2 + 2) + (Izzw*((a^2 + r1 - r2 + 1)^(1/2))*(a^2 + r1 - r2 + 1)^(1/2))/(a*(a^2 + r1 - r2)^(1/2)) + (Izzw*((a^2 + r1 - r2 + 1)^(1/2))*(a^2 + r1 - r2 + 1)^(1/2))/(a*((a^2 + r1 - r2)^(1/2))*(a)*(a^2 + r1 - r2)^(1/2));
m_12 = ms3/4 - (a*ms3)/(2*a^2 + 2*r1 - 2*r2+ 2) - ((a^2 + r1 - r2)^(1/2)*(a)*(a^2 + r1 - r2 + 1)^(1/2)/(8*a*(a^2 + r1 - r2)^(1/2) - (Izzw*((a^2 + r1 - r2 + 1)^(1/2)*(a^2 + r1 - r2 + 1)^(1/2))/(a*((a^2 + r1 - r2)^(1/2))*(a)*(a^2 + r1 - r2)^(1/2) + 1/4))));
m_13 = -(a*ms3)/(2*a^2 + 2*r1 - 2*r2 + 2);
m_14 = -(a*ms3)/(2*a^2 + 2*r1 - 2*r2 + 2);
m_21 = mw/4 + (ms3*mw)/8 - (a*ms3)/(2*a^2 + 2*r1 - 2*r2 + 2) - (Izzw*((a^2 + r1 - r2 + 1)^(1/2)))/(8*((a^2 + r1 - r2)^(1/2))*(a)*(a*(a^2 + r1 - r2)^(1/2) - (Izzw*a^2 +r1 - r2+ 1))/(((a^2 + r1 - r2)^(1/2))^2*(a)^2));
m_22 = (Izzw*((a^2 + r1 - r2 + 1)^(1/2))*(a^2 + r1 - r2 + 1)^(1/2))/(8*a*((a^2 + r1 - r2)^(1/2))*(a*(a^2 + r1 - r2)^(1/2) + (Izzw*((a^2 + r1 - r2 + 1)^(1/2)))/(((a^2 + r1 - r2)^(1/2))^2*(a)^2))) - (a*ms3)/(2*a^2 + 2*r1 - 2*r2 + 2) - (3*ms2)/2 + 1/4;
m_23 = -(a*ms3)/(2*a^2 + 2*r1 - 2*r2 + 2);
m_24 = -(a*ms3)/(2*a^2 + 2*r1 - 2*r2 + 2);
m_31=g*ms4;
m_32=g*ms4;
m_33 = (5*ms4)/4 + ms6 + mw/4 + g*ms4 + (ms3*mw)/8 + (Ixxw*((a^2 + r3 - r4 + 1)^(1/2))*(a^2 + r3 - r4 + 1)^(1/2))/(a*(a^2 + r3 - r4)^(1/2)) + (Ixxw*((a^2 + r3 - r4 + 1)^(1/2))*(a^2 + r3 - r4 + 1)^(1/2))/(a*((a^2 + r3 - r4)^(1/2))*(a)*(a^2 + r3 - r4)^(1/2));
m_34 = ms3/4 + mw/4 + g*ms4 - ((a^2 + r3 - r4)^(1/2)*(a)*(a^2 + r3 - r4 + 1)^(1/2)/(8*a*(a^2 + r3 - r4)^(1/2) - (Ixxw*((a^2 + r3 - r4 + 1)^(1/2))*(a^2 + r3 - r4 + 1)^(1/2))/(a*conj((a^2 + r3 - r4)^(1/2))*(a)*(a^2 + r3 - r4)^(1/2))));
m_41= Ixxw*ms3*((a^2*ms6)/(8*(a^2 + r3 - r4 + 1)^2) - (a*ms6)/(4*a^2 + 4*r3 - 4*r4 + 4) - (Ixxw*atan((a^2 + r3 - r4)^(1/2)))/((a^2 + r3 - r4)^(1/2)*(a^2 + r3 - r4 + 1)) + (a*ms6*(a^2 + r3 - r4))/(4*(a^2 + r3 - r4 + 1)^2))/8 - g*ms5;
m_42 = (Ixxw*ms3*((a^2*ms6)/(8*(a^2 + r3 - r4 + 1)^2) - (a*ms6)/(4*a^2 + 4*r3 - 4*r4 + 4) - (Ixxw*atan((a^2 + r3 - r4)^(1/2)))/((a^2 + r3 - r4)^(1/2)*(a^2 + r3 - r4 + 1)) + (a*ms6*(a^2 + r3 - r4))/(4*(a^2 + r3 - r4 + 1)^2)))/8 - g*ms5;
m_43 = (ms3*mw)/8 - g*ms5 + (Ixxw*ms3*((a^2*ms6)/(8*(a^2 + r3 - r4 + 1)^2) - (a*ms6)/(4*a^2 + 4*r3 - 4*r4 + 4) - (Ixxw*atan((a^2 + r3 - r4)^(1/2)))/((a^2 + r3 - r4)^(1/2)*(a^2 + r3 - r4 + 1)) + (a*ms6*(a^2 + r3 - r4))/(4*(a^2 + r3 - r4 + 1)^2)))/8;
m_44 = ms3/4 - g*ms5 + (Ixxw*ms3*((a^2*ms6)/(8*(a^2 + r3 - r4 + 1)^2) - (a*ms6)/(4*a^2 + 4*r3 - 4*r4 + 4) - (Ixxw*atan((a^2 + r3 - r4)^(1/2)))/((a^2 + r3 - r4)^(1/2)*(a^2 + r3 - r4 + 1)) + (a*ms6*(a^2 + r3 - r4))/(4*(a^2 + r3 - r4 + 1)^2)))/8;
c_11 = (Izzw*conj((a^2 + r1 - r2 + 1)^(1/2))*(1/(2*a*(a^2 + r1 - r2)^(1/2)*(a^2 + r1 - r2 + 1)^(1/2) - (a^2 + r1 - r2 + 1)^(1/2)/(2*a*(a^2 + r1 - r2)^(3/2))) + Izzw*((Izzw*atan(1/(a^2 + r1 - r2 + 1)))/8 + (a^2*ms3)/(32*(a^2 + r1 - r2 + 1)^2) - (a*(a^2 + r1 - r2))/(2*(a^2 + r1 - r2 + 1)^2) - ((conj((a^2 + r1 - r2 + 1)^(1/2))/(2*conj((a^2 + r1 - r2)^(1/2))^3*conj(a)) - 1/(2*conj((a^2 + r1 - r2)^(1/2))*conj((a^2 + r1 - r2 + 1)^(1/2))*conj(a)))*(a^2 + r1 - r2 + 1)^(1/2))/(a*(a^2 + r1 - r2)^(1/2)))))/(conj((a^2 + r1 - r2)^(1/2))*conj(a)) - (a*ms3)/(2*a^2 + 2*r1 - 2*r2 + 2);
c_12 = - (a*ms3)/(2*a^2 + 2*r1 - 2*r2 + 2) - (Izzw^2*conj((a^2 + r1 - r2 + 1)^(1/2))*((Izzw*atan(1/(a^2 + r1 - r2 + 1)))/8 - (a*(a^2 + r1 - r2))/(2*(a^2 + r1 - r2 + 1)^2) - ((conj((a^2 + r1 - r2 + 1)^(1/2))/(2*conj((a^2 + r1 - r2)^(1/2))^3*conj(a)) - 1/(2*conj((a^2 + r1 - r2)^(1/2))*conj((a^2 + r1 - r2 + 1)^(1/2))*conj(a)))*(a^2 + r1 - r2 + 1)^(1/2))/(a*(a^2 + r1 - r2)^(1/2)) + (a*ms3*(a/(2*(a^2 + r1 - r2 + 1)^(1/2)) + 1))/(16*(a^2 + r1 - r2 + 1)^(3/2))))/(conj((a^2 + r1 - r2)^(1/2))*conj(a));
c_13 = -(a*ms3)/(2*a^2 + 2*r1 - 2*r2 + 2);
c_14 = -(a*ms3)/(2*a^2 + 2*r1 - 2*r2 + 2);
c_21 = -ms3*(a/(2*a^2 + 2*r1 - 2*r2 + 2) + (Izzw*(a^2 + r1 - r2 + 1)^(1/2))/(a*(a^2 + r1 - r2)^(1/2)) + (Izzw*conj((a^2 + r1 - r2 + 1)^(1/2))*(1/(2*a*(a^2 + r1 - r2)^(1/2)*(a^2 + r1 - r2 + 1)^(1/2)) - (a^2 + r1 - r2 + 1)^(1/2)/(2*a*(a^2 + r1 - r2)^(3/2))))/(conj((a^2 + r1 - r2)^(1/2))*conj(a)));
c_22 = -ms3*(a/(2*a^2 + 2*r1 - 2*r2 + 2) + (Izzw*conj((a^2 + r1 - r2 + 1)^(1/2))*(1/(2*a*(a^2 + r1 - r2)^(1/2)*(a^2 + r1 - r2 + 1)^(1/2)) - (a^2 + r1 - r2 + 1)^(1/2)/(2*a*(a^2 + r1 - r2)^(3/2))))/(conj((a^2 + r1 - r2)^(1/2))*conj(a)) + (Izzw*ms3*(a/(2*(a^2 + r1 - r2 + 1)^(1/2)) + 1))/(16*(a^2 + r1 - r2)^(1/2)*(a^2 + r1 - r2 + 1)));
c_23 = -(a*ms3)/(2*a^2 + 2*r1 - 2*r2 + 2);
c_24 = -(a*ms3)/(2*a^2 + 2*r1 - 2*r2 + 2);
c_31 = g*ms4;
c_32 = g*ms4;
c_33 = g*ms4;
c_34 = g*ms4;
c_41 = (Ixxw*ms3*((a^2*ms6)/(8*(a^2 + r3 - r4 + 1)^2) - (a*ms6)/(4*a^2 + 4*r3 - 4*r4 + 4) - (Ixxw*atan((a^2 + r3 - r4)^(1/2)))/((a^2 + r3 - r4)^(1/2)*(a^2 + r3 - r4 + 1)) + (a*ms6*(a^2 + r3 - r4))/(4*(a^2 + r3 - r4 + 1)^2)))/8 - g*ms5;
c_42 = (Ixxw*ms3*((a^2*ms6)/(8*(a^2 + r3 - r4 + 1)^2) - (a*ms6)/(4*a^2 + 4*r3 - 4*r4 + 4) - (Ixxw*atan((a^2 + r3 - r4)^(1/2)))/((a^2 + r3 - r4)^(1/2)*(a^2 + r3 - r4 + 1)) + (a*ms6*(a^2 + r3 - r4))/(4*(a^2 + r3 - r4 + 1)^2)))/8 - g*ms5;
c_43 = mw/2 - g*ms5 + (Ixxw*ms3*(conj((a^2 + r3 - r4 + 1)^(1/2))/(conj((a^2 + r3 - r4)^(1/2))*conj(a)) + (a^2*ms6)/(8*(a^2 + r3 - r4 + 1)^2) - (a*ms6)/(4*a^2 + 4*r3 - 4*r4 + 4) - (Ixxw*atan((a^2 + r3 - r4)^(1/2)))/((a^2 + r3 - r4)^(1/2)*(a^2 + r3 - r4 + 1)) + (a*ms6*(a^2 + r3 - r4))/(4*(a^2 + r3 - r4 + 1)^2) + (Ixxw*(conj((a^2 + r3 - r4 + 1)^(1/2))/(2*conj((a^2 + r3 - r4)^(1/2))^3*conj(a)) - 1/(2*conj((a^2 + r3 - r4)^(1/2))*conj((a^2 + r3 - r4 + 1)^(1/2))*conj(a)))*(a^2 + r3 - r4 + 1)^(1/2))/(a*(a^2 + r3 - r4)^(1/2))))/8 - (Ixxw*conj((a^2 + r3 - r4 + 1)^(1/2)))/(8*conj((a^2 + r3 - r4)^(1/2))*conj(a));
c_44 = 2*ms4 + ms5 + ms6 - g*ms5 + (Ixxw*ms3*((a*ms6*(a^2 + r3 - r4))/(4*(a^2 + r3 - r4 + 1)^2) - (Ixxw*atan((a^2 + r3 - r4)^(1/2)))/((a^2 + r3 - r4)^(1/2)*(a^2 + r3 - r4 + 1)) - (a*ms6)/(4*a^2 + 4*r3 - 4*r4 + 4) + (conj((a^2 + r3 - r4 + 1)^(1/2))*(1/(2*a*(a^2 + r3 - r4)^(1/2)*(a^2 + r3 - r4 + 1)^(1/2)) - (a^2 + r3 - r4 + 1)^(1/2)/(2*a*(a^2 + r3 - r4)^(3/2))))/(a*(a^2 + r3 - r4)^(1/2)) + (a*ms6*(a/(2*(a^2 + r3 - r4 + 1)^(1/2)) + 1))/(4*(a^2 + r3 - r4 + 1)^(3/2)) + (Ixxw*(conj((a^2 + r3 - r4 + 1)^(1/2))/(2*conj((a^2 + r3 - r4)^(1/2))^3*conj(a)) - 1/(2*conj((a^2 + r3 - r4)^(1/2))*conj((a^2 + r3 - r4 + 1)^(1/2))*conj(a)))*(a^2 + r3 - r4 + 1)^(1/2))/(a*(a^2 + r3 - r4)^(1/2))))/8 + (conj((a^2 + r3 - r4 + 1)^(1/2))*(a^2 + r3 - r4)^(1/2))/(8*conj((a^2 + r3 - r4)^(1/2))^2*conj(a)*(a^2 + r3 - r4 + 1)^(1/2)) + 1/4;
g_11 = -(a*ms3)/(2*a^2 + 4);
g_12 = -ms3/(2*a);
g_13 = -(a*ms3)/(2*a^2 + 2);
g_13 = -(a*ms3)/(2*a^2 + 2);
g_21 = -(a*ms3)/(2*a^2 + 4);
g_22 = -ms3/(2*a);
g_23 = -(a*ms3)/(2*a^2 + 2);
g_24 = -(a*ms3)/(2*a^2 + 2);
g_31 = g*ms4;
g_32 = g*ms4;
g_33 = g*ms4;
g_34 = g*ms4;
g_41 = - g*ms5 - (Ixxw*ms3*((a*ms6)/(4*a^2 + 4) - (a^2*ms6)/(8*(a^2 + 1)^2) - (a^3*ms6)/(4*(a^2 + 1)^2) + (Ixxw*atan((a^2)^(1/2)))/((a^2 + 1)*(a^2)^(1/2))))/8;
g_42 = - g*ms5 - (Ixxw*ms3*((a*ms6)/(4*a^2 + 4) - (a^2*ms6)/(8*(a^2 + 1)^2) - (a^3*ms6)/(4*(a^2 + 1)^2) + (Ixxw*atan((a^2)^(1/2)))/((a^2 + 1)*(a^2)^(1/2))))/8;
g_43 = - g*ms5 - (Ixxw*ms3*((a*ms6)/(4*a^2 + 8) - (a^2*ms6)/(8*(a^2 + 2)^2) + (Ixxw*atan((a^2 + 1)^(1/2)))/((a^2 + 1)^(1/2)*(a^2 + 2)) - (a*ms6*(a^2 + 1))/(4*(a^2 + 2)^2)))/8;
g_44 = - g*ms5 - (Ixxw*ms3*(ms6/(4*a) - ms6/(8*a^2) - (ms6*(a^2 - 1))/(4*a^3) + (Ixxw*atan((a^2 - 1)^(1/2)))/(a^2*(a^2 - 1)^(1/2))))/8;
Mr = [m_11 m_12 m_13 m_14; m_21 m_22 m_23 m_24; m_31 m_32 m_33 m_34 ;m_41 m_42 m_43 m_44];
Nr = [c_11 c_12 c_13 c_14; c_21 c_22 c_23 c_24;c_31 c_32 c_33 c_34;c_41 c_42 c_43 c_44];
% gr = [g_11 g_12 g_13 g_41;g_21 g_22 g_23 g_24;g_31 g_32 g_33 g_34;g_41 g_42 g_43 g_44];
tau(:,i) = 10*(zeta_dot(:) - zeta(:,i)) - 4*zeta_dot(:);
%% f(:,i) = (Mr*(gamma_p*e(:,i)+gamma_d*edot));
zeta_ddot = inv(Mr) *(tau(:,i) - Nr -10*diag([0.1 -0.1 0.1 0.1])*zeta_dot(:) + 10*[0.1 0.2 0.2 0.2]');
%zeta_ddot = inv(Mr) *(f(:,i) - Nr -B_friction-10*diag([0.1 -0.1 0.1 0.1]) * zeta_dot(:,i) + 10*[0.1 0.2 0.2 0.2]');
% zeta_dot(:,i+1) = zeta_dot(:,i) + (zeta_ddot + 0*0.05*randn(4,1))*dt
%zeta_dot(:,i+1) = zeta_dot(:,i) + (xddot + 0*0.05*randn(4,1)) * dt;
zeta(:,i+1) = zeta(:,i) + zeta_dot(:) * dt;
%zeta(:,i+1) = (zeta(:,i) + zeta_dot(:,i)+zeta_ddot*dt)*dt
phio=atan((sqrt(a^2-(zeta(2,i)-zeta(1,i))^2/zeta(2,i)-zeta(1,i)+ atan((sqrt(a^2-(zeta(4,i)-zeta(3,i))^2/zeta(4,i)-zeta(3,i)))))));
phi_1= atan((sqrt(a^2-(zeta(2,i)-zeta(1,i))^2/zeta(2,i)-zeta(1,i)+ atan((sqrt(a^2-(zeta(4,i)-zeta(3,i))^2/zeta(4,i)-zeta(3,i)))))));
xpo = zeta(2,i) + a/2 * cos(phio);
ypo = zeta(4,i) + a/2*cos(phi_1);
mu(:,i) = [xpo;ypo;phio;phi_1];
%e(:,i)=mu(:,i)-zeta_dot(:,i);
plot(t,zeta(:,1:length(t)))
hold on
plot(mur(1,:),mur(2,:),'r--',mu(1,:),mu(2,:),'b-')
figure
% plot(x(1,1:length(t)),xdot(1,1:length(t)))
xbx = [-0.1 0.1 0.1 -0.1 -0.1]*0.2;
ybx = [-0.1 -0.1 0.1 0.1 -0.1]*0.2;
xcc = 0.02 * cos(0:2*pi);
ycc = 0.02 * cos(0:2*pi);
x1 = 0.5+0.3*cos(0.1*t);
y1 = 0.5+0.3*cos(0.1*t);
phi_1=(pi/2*(cos(0.1*t)));
phi_2=(pi/2*(cos(0.1*t)));
x1 = mu(1,:);
y1 = mu(2,:);
phi = mu(3,:);
phi_1=mu(4,:);
figure
for i = 1:10:length(t)
% Inverse Kinematics
s1(i) = x1(i) + a/2 * cos(phi_1(i));
s2(i) = x1(i) - a/2 * cos(phi_1(i));
s3(i) = y1(i) + a/2 * cos(phi_2(i));
s4(i) = y1(i) - a/2 * cos(phi_2(i));
r1(i) = x1(i) + a/2 * cos(phi_1(i));
r2(i) = x1(i) - a/2 * cos(phi_1(i));
r3(i) = y1(i) + a/2 * cos(phi_2(i));
r4(i) = y1(i) - a/2 * cos(phi_2(i));
% Forward Kinematics
phia(i) = atan(sqrt(a^2-(s2(i)-s1(i))^2/(s2(i)-s1(i))))+ atan(sqrt(a^2-(s4(i)-s3(i))^2/(s4(i)-s3(i))));
x2= r2(i)+a/2*cos(phia(i));
y2= r4(i)+a/2*cos(phia(i));
x2 = s1(i) + a/2 * cos(phia(i));
y2 = r1(i) + a/2* cos(phia(i));
% Animation
fill([s1(i) r2(i) r3(i) s1(i)],[r1(i) s2(i) s3(i) r1(i)],'r')
axis(0.05*[-2 22 -2 22]);
axis square
% grid on
hold on
%link
plot([0 s1(i) s1(i)],[0 0 r1(i)],'r-');
plot([w w r1(i)],[0 s1(i) r1(i)],'k-');
plot([w w r2(i)],[0 s2(i) r2(i)],'k-');
plot([0 0 r3(i)],[0 s3(i) r3(i)],'b-');
plot([0 0 r4(i)],[0 s4(i) r4(i)],'b-');
% plot([w/2 w/2 r4(i)],[0 s4(i) s4(i)],'b-');
%block
fill(xbx+s1(i),ybx,'r');
fill(xbx+w,ybx+s2(i),'k');
fill(xbx,ybx+s3(i),'b');
fill(xbx+w,ybx+s4(i),'k');
fill(xbx+s1(i),ybx+r1(i),'r');
fill(xbx+r2(i),ybx+s2(i),'k');
fill(xbx+r3(i),ybx+s3(i),'b');
fill(xbx+r4(i),ybx+r4(i),'b');
fill(xcc+s1(i),ycc+r1(i),'y');
fill(xcc+r2(i),ycc+s2(i),'y');
fill(xcc+r3(i),ycc+s3(i),'y');
fill(xcc+r4(i),ycc+s4(i),'y');
fill(xbx,ybx,'k');
fill(xbx+w,ybx,'k');
plot(x1(1:i),y1(1:i));
plot(x2,y2,'y*');
%set(gca,'fontsize',r2,'fontname','Times');
xlabel('x in units');
ylabel('y in units');
title('Platform motion in xy plane');
hold off
pause(0.1)
end
figure
subplot(4,3,2,1)
plot(t,zeta_d(1,:),'r--')
set(gca,'fontsize',12,'fontname','Times')
xlabel('t,[s]');
ylabel('$r_{1d}$,[m]','interpreter','Latex')
grid on
subplot(3,2,3,4)
plot(t,zeta_d(2,:),'b-')
set(gca,'fontsize',12,'fontname','Times')
xlabel('t,[s]');
ylabel('$r_{2d}$,[m]','interpreter','Latex')
grid on
subplot(3,2,5,3)
plot(t,zeta_d(3,:),'b-')
set(gca,'fontsize',12,'fontname','Times')
xlabel('t,[s]');
ylabel('$r_{3d}$,[m]','interpreter','Latex')
grid on
subplot(3,2,2)
plot(t,e(1,:),'r--')
set(gca,'fontsize',12,'fontname','Times')
xlabel('t,[s]');
ylabel('$\tilde{r}_{1}$,[m]','interpreter','Latex')
grid on
subplot(3,2,4,2)
plot(t,e(2,:),'r--')
set(gca,'fontsize',12,'fontname','Times')
xlabel('t,[s]');
ylabel('$\tilde{r}_{2}$,[m]','interpreter','Latex')
grid on
subplot(3,2,6,1)
plot(t,e(3,:),'b-')
set(gca,'fontsize',12,'fontname','Times')
xlabel('t,[s]');
ylabel('$\tilde{r}_{3}$,[m]','interpreter','Latex')
grid on
subplot(3,2,4,2)
plot(t,e(4,:),'r--')
set(gca,'fontsize',12,'fontname','Times')
xlabel('t,[s]');
ylabel('$\tilde{r}_{4}$,[m]','interpreter','Latex')
grid on
figure
plot(t,tau(1,:),'r--')
set(gca,'fontsize',12,'fontname','Times')
xlabel('t,[s]');
ylabel('$tau_{1}$,[m]','interpreter','Latex')
figure
grid on
plot(t,tau(2,:),'r--')
set(gca,'fontsize',12,'fontname','Times')
xlabel('t,[s]');
ylabel('$tau_{2}$,[m]','interpreter','Latex')
grid on
figure
plot(t,tau(3,:),'r--')
set(gca,'fontsize',12,'fontname','Times')
xlabel('t,[s]');
plot(t,tau(4,:),'r--')
set(gca,'fontsize',12,'fontname','Times')
xlabel('t,[s]');
ylabel('$tau_{3}$,[m]','interpreter','Latex')
grid on
