NaN shows up in matrix when solving my ODEs, how can I fix this?

5 views (last 30 days)

All of my matrices I am solving for have NaN after the third time line. How do I solved this issue?

Code:

clear
clc

%Menus/Variable Defintions/MEchanical Parameter Calculations

filename = 'Force Impact FBD.png';
I = imread('Force Impact FBD.png');
y = imread('Force Impact FBD.png', 'BackgroundColor', ["none"]);
imshow('Force Impact FBD.png')

%Impact on M1 FBD Menu
prompt={'Impact Force (N)','beta (degrees)','Time at which impulse will begin (seconds after zero)','Duration of Impact (seconds)'};
dlg_title='Impact on M1 FBD Menu';
num_lines=1;
defaultans={'3000','10','3','0.3'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);

%Variable Assignments from User Inputs
FimpM1=str2double(answer{1});
betaimp=str2double(answer{2});
tbeginimp=str2double(answer{3});
tdurimp=str2double(answer{4});
FimpM1x=-((FimpM1)*(cosd(betaimp)));
FimpM1z=((FimpM1)*(sind(betaimp)));

filename = 'FBD (with distances and dimensions and forces).png';
I = imread('FBD (with distances and dimensions and forces).png');
y = imread('FBD (with distances and dimensions and forces).png', 'BackgroundColor', ["none"]);
imshow('FBD (with distances and dimensions and forces).png')

%FBD Variable Defintion Menu 1
prompt={'x6i (m)','z6i (m)','x5i (m)','z5i (m)','x4i (m)','z4i (m)','x3i (m)','z3i (m)','x2i (m)','z2i (m)','x1i (m)','z1i (m)'};
dlg_title='FBD Variable Defintion Menu 1';
num_lines=1;
defaultans={'0.00','0.30','0.00','0.25','0.00','0.20','0.00','0.15','0.00','0.10','0.00','0.05'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);

%Variable Assignments from User Inputs
x6i=str2double(answer{1});
z6i=str2double(answer{2});
x5i=str2double(answer{3});
z5i=str2double(answer{4});
x4i=str2double(answer{5});
z4i=str2double(answer{6});
x3i=str2double(answer{7});
z3i=str2double(answer{8});
x2i=str2double(answer{9});
z2i=str2double(answer{10});
x1i=str2double(answer{11});
z1i=str2double(answer{12});

%FBD Variable Defintion Menu 2
prompt={'xw6 (m)','zw6 (m)','xw5 (m)','zw5 (m)','xw4 (m)','zw4 (m)','xw3 (m)','zw3 (m)'};
dlg_title='FBD Variable Defintion Menu 2';
num_lines=1;
defaultans={'0.10','0.30','0.10','0.25','0.10','0.20','0.10','0.15'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);

%Variable Assignments from User Inputs
xw6=str2double(answer{1});
zw6=str2double(answer{2});
xw5=str2double(answer{3});
zw5=str2double(answer{4});
xw4=str2double(answer{5});
zw4=str2double(answer{6});
xw3=str2double(answer{7});
zw3=str2double(answer{8});
zq6=zw6;
zq5=zw5;
zq4=zw4;
zq3=zw3;

%FBD Variable Defintion Menu 3
prompt={'xw2 (m)','zw2 (m)','xw1 (m)','zw1 (m)','xw0 (m)','zw0 (m)', 'xw7 (m)', 'zw7 (m)'};
dlg_title='FBD Variable Defintion Menu 3';
num_lines=1;
defaultans={'0.10','0.10','0.10','0.05','0.10','0.00','0.00','0.00'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);

%Variable Assignments from User Inputs
xw2=str2double(answer{1});
zw2=str2double(answer{2});
xw1=str2double(answer{3});
zw1=str2double(answer{4});
xw0=str2double(answer{5});
zw0=str2double(answer{6});
xw7=str2double(answer{7});
zw7=str2double(answer{8});
zq2=zw2;
zq1=zw1;
zq0=zw0;
xq1=xw0;
xq0=xw7;

%Car Seat Stiffnesses Menu
prompt={'Lower Seat Back (K12) (kN/m)','Lower Seat Back (K10) (kN/m)','Middle Seat Back (K8) (kN/m)','Middle Seat Back (K6) (kN/m)','Top Seat Back (K4) (kN/m)','Top Seat Back (K2) (kN/m)'};
dlg_title='Car Seat Stiffnesses Menu';
num_lines=1;
defaultans={'680.94','3565.77','1138.64','1138.64','666.55','2322.89'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);

%Variable Assignments from User Inputs
K12=str2double(answer{1});
K10=str2double(answer{2});
K8=str2double(answer{3});
K6=str2double(answer{4});
K4=str2double(answer{5});
K2=str2double(answer{6});

%Car Seat Damping Coefficients Menu
prompt={'Lower Seat Back (C12) (N-sec/m)','Lower Seat Back (C10) (N-sec/m)','Middle Seat Back (C8) (N-sec/m)','Middle Seat Back (C6) (N-sec/m)','Top Seat Back (C4) (N-sec/m)','Top Seat Back (C2) (N-sec/m)'};
dlg_title='Car Seat Damping Coefficients Menu';
num_lines=1;
defaultans={'4.10','0.75','24.45','24.45','772.68','3258.36'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);

%Variable Assignments from User Inputs
C12=str2double(answer{1});
C10=str2double(answer{2});
C8=str2double(answer{3});
C6=str2double(answer{4});
C4=str2double(answer{5});
C2=str2double(answer{6});

%Anthropomorphic Measurmeents Menu 1
prompt={'Standing Height (cm):','Shoulder Height (cm)','Head Length (cm)','Head Breadth (cm)','Neck Circumference (cm)','Chest Depth (cm)','Chest Breadth (cm)'};
dlg_title='Input Anthropomorphic Measurmeents (cm)';
num_lines=1;
defaultans={'168.67','146.53','19.86','15.57','37.95','23.32','32.89'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);

%Variable Assignments from User Inputs
L1=str2double(answer{1});
L2=str2double(answer{2});
L3=str2double(answer{3});
L4=str2double(answer{4});
L5=str2double(answer{5});
L6=str2double(answer{6});
L7=str2double(answer{7});

%Anthropomorphic Measurmeents Menu 2
prompt={'Waist Depth (cm)','Waist Breadth (cm)','Buttock Depth (cm)','Hip Breadth, Standing (cm)','Shoulder to Elbow Length (cm)','Elbow-Wrist Length (cm)','Wrist-Fingertip Length (cm)'};
dlg_title='Input Anthropomorphic Measurmeents (cm)';
num_lines=1;
defaultans={'21.51','28.22','23.19','35.43','37.54','25.28','19.72'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);

%Variable Assignments from User Inputs
L8=str2double(answer{1});
L9=str2double(answer{2});
L10=str2double(answer{3});
L11=str2double(answer{4});
L12=str2double(answer{5});
L13=str2double(answer{6});
L14=str2double(answer{7});

%Miscellaneous Inputs Menu 1
prompt={'Segments Combined Mass (kg)','Elastic Modulus of Bones and Tissue (MN/m^2)','Effective Coefficient of Viscosity of Blood and Synovial Fluid (N-sec/m^2)'};
dlg_title='Miscellaneous Inputs Menu 1';
num_lines=1;
defaultans={'35','1.30','0.003'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);

%Variable Assignments from User Inputs
MassCom=str2double(answer{1});
E=str2double(answer{2});
mew=str2double(answer{3});

%Formulae for segment's ellipsoidal semi-axes
M6a = L4/2;
M6b = L4/2;
M6c = L3/2;
M5a = L5/(2*pi);
M5b = L5/(2*pi);
M5c = (L1-L2-L3)/2;
M4a = L6/2;
M4b = L7/2;
M4c = L12/2;
M3a = L6/2;
M3b = L7/2;
M3c = L12/2;
M2a = L8/2;
M2b = L9/2;
M2c = L13/2;
M1a = L8/2;
M1b = L9/2;
M1c = L14/2;

%Formulae for volume and mass of segments
VolumeM6 = pi*M6a*M6b*M6c;
VolumeM5 = pi*M5a*M5b*M5c;
VolumeM4 = pi*M4a*M4b*M4c;
VolumeM3 = pi*M3a*M3b*M3c;
VolumeM2 = pi*M2a*M2b*M2c;
VolumeM1 = pi*M1a*M1b*M1c;
VolumeCom = VolumeM6+VolumeM5+VolumeM4+VolumeM3+VolumeM2+VolumeM1;

MassM6 = (MassCom*VolumeM6)/VolumeCom;
MassM5 = (MassCom*VolumeM5)/VolumeCom;
MassM4 = (MassCom*VolumeM4)/VolumeCom;
MassM3 = (MassCom*VolumeM3)/VolumeCom;
MassM2 = (MassCom*VolumeM2)/VolumeCom;
MassM1 = (MassCom*VolumeM1)/VolumeCom;

%Formulae for segment stiffness
I = 0.366529188; %unitless
K11 = (pi*(E*1000)*(M6a/100)*(M6b/100))/((M6c/100)*I); %E*1000 converts from MN/m^2 to kN/m^2, each M../100 converts from cm to m
K9 = (pi*(E*1000)*(M5a/100)*(M5b/100))/((M5c/100)*I); %E*1000 converts from MN/m^2 to kN/m^2, each M../100 converts from cm to m
K7 = (pi*(E*1000)*(M4a/100)*(M4b/100))/((M4c/100)*I); %E*1000 converts from MN/m^2 to kN/m^2, each M../100 converts from cm to m
K5 = (pi*(E*1000)*(M3a/100)*(M3b/100))/((M3c/100)*I); %E*1000 converts from MN/m^2 to kN/m^2, each M../100 converts from cm to m
K3 = (pi*(E*1000)*(M2a/100)*(M2b/100))/((M2c/100)*I); %E*1000 converts from MN/m^2 to kN/m^2, each M../100 converts from cm to m
K1 = (pi*(E*1000)*(M1a/100)*(M1b/100))/((M1c/100)*I); %E*1000 converts from MN/m^2 to kN/m^2, each M../100 converts from cm to m

%Formulae for segment damping
AreaM6 = pi*(M6a/100)*(M6b/100);
LengthM6 = L3/100;
DiamM6 = L4/100;
ClearM6 = .07*DiamM6;
C11 = ((12*mew)/(pi))*((((AreaM6)^2)*(LengthM6))/((DiamM6)*((ClearM6)^3)));
AreaM5 = pi*(M5a/100)*(M5b/100);
LengthM5 = (L1-L2-L3)/100;
DiamM5 = L5/(pi*100);
ClearM5 = .06*DiamM5;
C9 = ((12*mew)/(pi))*((((AreaM5)^2)*(LengthM5))/((DiamM5)*((ClearM5)^3)));
AreaM4 = pi*(M4a/100)*(M4b/100);
LengthM4 = L12/100;
DiamM4 = L6/100;
ClearM4 = .06*DiamM4;
C7 = ((12*mew)/(pi))*((((AreaM4)^2)*(LengthM4))/((DiamM4)*((ClearM4)^3)));
AreaM3 = pi*(M3a/100)*(M3b/100);
LengthM3 = L12/100;
DiamM3 = L6/100;
ClearM3 = .06*DiamM3;
C5 = ((12*mew)/(pi))*((((AreaM3)^2)*(LengthM3))/((DiamM3)*((ClearM3)^3)));
AreaM2 = pi*(M2a/100)*(M2b/100);
LengthM2 = L13/100;
DiamM2 = L8/100;
ClearM2 = .01*DiamM2;
C3 = ((12*mew)/(pi))*((((AreaM2)^2)*(LengthM2))/((DiamM2)*((ClearM2)^3)));
AreaM1 = pi*(M1a/100)*(M1b/100);
LengthM1 = L14/100;
DiamM1 = L10/100;
ClearM1 = .01*DiamM1;
C1 = ((12*mew)/(pi))*((((AreaM1)^2)*(LengthM1))/((DiamM1)*((ClearM1)^3)));

tic

%Equations of Motion (X-direction and Z-direction)

% Other problem parameters
vx1i=2; % segement 1 initial x-velocity (m/s)
vz1i=2; % segement 1 initial z-velocity (m/s)
vx2i=2; % segement 2 initial x-velocity (m/s)
vz2i=2; % segement 2 initial z-velocity (m/s)
vx3i=2; % segement 3 initial x-velocity (m/s)
vz3i=2; % segement 3 initial z-velocity (m/s)
vx4i=2; % segement 4 initial x-velocity (m/s)
vz4i=2; % segement 4 initial z-velocity (m/s)
vx5i=2; % segement 5 initial x-velocity (m/s)
vz5i=2; % segement 5 initial z-velocity (m/s)
vx6i=2; % segement 6 initial x-velocity (m/s)
vz6i=2; % segement 6 initial z-velocity (m/s)

vxw1=3; % segement 1 initial x-velocity (m/s)
vzw1=2; % segement 1 initial z-velocity (m/s)
vxw2=2; % segement 2 initial x-velocity (m/s)
vzw2=2; % segement 2 initial z-velocity (m/s)
vxw3=2; % segement 3 initial x-velocity (m/s)
vzw3=2; % segement 3 initial z-velocity (m/s)
vxw4=2; % segement 4 initial x-velocity (m/s)
vzw4=2; % segement 4 initial z-velocity (m/s)
vxw5=2; % segement 5 initial x-velocity (m/s)
vzw5=2; % segement 5 initial z-velocity (m/s)
vxw6=2; % segement 6 initial x-velocity (m/s)
vzw6=2; % segement 6 initial z-velocity (m/s)

% Set time step stuff
simTime=10; % simulation time (s)
tStep=0.001; % simulation time step
iterations=simTime/tStep;
t=0:iterations;
% FimpM1=str2double(answer{1});
% betaimp=str2double(answer{2});
% tbeginimp=str2double(answer{3});
% tdurimp=str2double(answer{4});
% FimpM1x=-((FimpM1)*(cosd(betaimp)));
% FimpM1z=((FimpM1)*(sind(betaimp)));

if (tbeginimp <= t) & (t <= tbeginimp+tdurimp)
FimpM1xapplied = FimpM1x;
else
FimpM1xapplied = 0;
end

if (tbeginimp <= t) & (t <= tbeginimp+tdurimp)
FimpM1zapplied = FimpM1z;
else
FimpM1zapplied = 0;
end

% Pre-allocate variables for speed and add initial conditions
x1=zeros(iterations,1);
x1(1,:)=x1i;
x2=zeros(iterations,1);
x2(1,:)=x2i;
x3=zeros(iterations,1);
x3(1,:)=x3i;
x4=zeros(iterations,1);
x4(1,:)=x4i;
x5=zeros(iterations,1);
x5(1,:)=x5i;
x6=zeros(iterations,1);
x6(1,:)=x6i;

z1=zeros(iterations,1);
z1(1,:)=z1i;
z2=zeros(iterations,1);
z2(1,:)=z2i;
z3=zeros(iterations,1);
z3(1,:)=z3i;
z4=zeros(iterations,1);
z4(1,:)=z4i;
z5=zeros(iterations,1);
z5(1,:)=z5i;
z6=zeros(iterations,1);
z6(1,:)=z6i;

vx1=zeros(iterations,1);
vx1(1,:)=vx1i;
vx2=zeros(iterations,1);
vx2(1,:)=vx2i;
vx3=zeros(iterations,1);
vx3(1,:)=vx3i;
vx4=zeros(iterations,1);
vx4(1,:)=vx4i;
vx5=zeros(iterations,1);
vx5(1,:)=vx5i;
vx6=zeros(iterations,1);
vx6(1,:)=vx6i;

vz1=zeros(iterations,1);
vz1(1,:)=vz1i;
vz2=zeros(iterations,1);
vz2(1,:)=vz2i;
vz3=zeros(iterations,1);
vz3(1,:)=vz3i;
vz4=zeros(iterations,1);
vz4(1,:)=vz4i;
vz5=zeros(iterations,1);
vz5(1,:)=vz5i;
vz6=zeros(iterations,1);
vz6(1,:)=vz6i;

%Mass 1 Equations of Motion
d01i=((((z1i)^2)+((x1i-xq0)^2))^(1/2)); %2b
Fs10i=(-K1)*(d01i-zq1); %2a
d01dti=((z1i*vz1i)+((x1i-xq0)*vx1i))/d01i; %3b
Fd10i=(-C1)*(d01dti); %3a
d12i=((((z2i-z1i)^(2))+((x2i-x1i))^(2))^(1/2)); %4b
Fs12i=(K3)*(d12i-(zq2-zq1)); %4a
d12dti=(1/d12i)*(((z2i-z1i)*(vz2i))-((z2i-z1i)*(vz1i))+((x2i-x1i)*(vx2i))-((x2i-x1i)*(vx1i))); %5b
Fd12i=(C3)*(d12dti); %5a
d1w1i=((((zw1-z1i)^(2))+((xw1-x1i))^(2))^(1/2)); %6b
Fs1w1i=(K2)*(d1w1i-(xq1-xq0)); %6a
d1w1dti=(1/d1w1i)*(((zw1-z1i)*(vzw1))-((zw1-z1i)*(vz1i))+((xw1-x1i)*(vxw1))-((xw1-x1i)*(vx1i))); %7b
Fd1w1i=(C2)*(d1w1dti); %7a
axM1=zeros(iterations,1);
axM1(1,:)=((Fs10i)*((x1i-xq0)/(d01i)))+((Fd10i)*((x1i-xq0)/(d01i)))+(Fs12i*((x1i-x2i)/d12i))+(Fd12i*((x1i-x2i)/d12i))+(Fs1w1i*((x1i-xw1)/d1w1i))+(Fd1w1i*((x1i-xw1)/d1w1i))+(FimpM1xapplied)/MassM1; %1a
azM1=zeros(iterations,1);
azM1(1,:)=((Fs10i)*(z1i/d01i))+((Fd10i)*((z1i)/(d01i)))+(Fs12i*((z1i-z2i)/d12i))+(Fd12i*((z1i-z2i)/d12i))+(Fs1w1i*((z1i-zw1)/d1w1i))+(Fd1w1i*((z1i-zw1)/d1w1i))+(FimpM1zapplied)/MassM1; %1b
%Mass 2 Equations of Motion
d23i=((((z3i-z2i)^(2))+((x3i-x2i))^(2))^(1/2)); %11b
Fs23i=(K5)*(d23i-(zq3-zq2)); %11a
d23dti=(1/d23i)*(((z3i-z2i)*(vz3i))-((z3i-z2i)*(vz2i))+((x3i-x2i)*(vx3i))-((x3i-x2i)*(vx2i))); %12b
Fd23i=(C5)*(d23dti); %12a
d2w2i=((((zw2-z2i)^(2))+((xw2-x2i))^(2))^(1/2)); %13b
Fs2w2i=(K4)*(d2w2i-(xq1-xq0)); %13a
d2w2dti=(1/d2w2i)*(((zw2-z2i)*(vzw2))-((zw2-z2i)*(vz2i))+((xw2-x2i)*(vxw2))-((xw2-x2i)*(vx2i))); %14b
Fd2w2i=(C4)*(d2w2dti); %14a
axM2=zeros(iterations,1);
axM2(1,:)=(-Fs12i*((x1i-x2i)/d12i))+(-Fd12i*((x1i-x2i)/d12i))+(Fs23i*((x2i-x3i)/d23i))+(Fd23i*((x2i-x3i)/d23i))+(Fs2w2i*((x2i-xw2)/d2w2i))+(Fd2w2i*((x2i-xw2)/d2w2i))/MassM2; %8a
azM2=zeros(iterations,1);
azM2(1,:)=(-Fs12i*((z1i-z2i)/d12i))+(-Fd12i*((z1i-z2i)/d12i))+(Fs23i*((z2i-z3i)/d23i))+(Fd23i*((z2i-z3i)/d23i))+(Fs2w2i*((z2i-zw2)/d2w2i))+(Fd2w2i*((z2i-zw2)/d2w2i))/MassM2; %8b
%Mass 3 Equations of Motion
d34i=((((z4i-z3i)^(2))+((x4i-x3i))^(2))^(1/2)); %18b
Fs34i=(K7)*(d34i-(zq4-zq3)); %18a
d34dti=(1/d34i)*(((z4i-z3i)*(vz4i))-((z4i-z3i)*(vz3i))+((x4i-x3i)*(vx4i))-((x4i-x3i)*(vx3i))); %19b
Fd34i=(C7)*(d34dti); %19a
d3w3i=((((zw3-z3i)^(2))+((xw3-x3i))^(2))^(1/2)); %20b
Fs3w3i=(K6)*(d3w3i-(xq1-xq0)); %20a
d3w3dti=(1/d3w3i)*(((zw3-z3i)*(vzw3))-((zw3-z3i)*(vz3i))+((xw3-x3i)*(vxw3))-((xw3-x3i)*(vx3i))); %21b
Fd3w3i=(C6)*(d3w3dti); %21a
axM3=zeros(iterations,1);
axM3(1,:)=(-Fs23i*((x2i-x3i)/d23i))+(-Fd23i*((x2i-x3i)/d23i))+(Fs34i*((x3i-x4i)/d34i))+(Fd34i*((x3i-x4i)/d34i))+(Fs3w3i*((x3i-xw3)/d3w3i))+(Fd3w3i*((x3i-xw3)/d3w3i))/MassM3; %15a
azM3=zeros(iterations,1);
azM3(1,:)=(-Fs23i*((z2i-z3i)/d23i))+(-Fd23i*((z2i-z3i)/d23i))+(Fs34i*((z3i-z4i)/d34i))+(Fd34i*((z3i-z4i)/d34i))+(Fs3w3i*((z3i-zw3)/d3w3i))+(Fd3w3i*((z3i-zw3)/d3w3i))/MassM3; %15b
%Mass 4 Equations of Motion
d45i=((((z5i-z4i)^(2))+((x5i-x4i))^(2))^(1/2)); %25b
Fs45i=(K9)*(d45i-(zq5-zq4)); %25a
d45dti=(1/d45i)*(((z5i-z4i)*(vz5i))-((z5i-z4i)*(vz4i))+((x5i-x4i)*(vx5i))-((x5i-x4i)*(vx4i))); %26b
Fd45i=(C9)*(d45dti); %26a
d4w4i=((((zw4-z4i)^(2))+((xw4-x4i))^(2))^(1/2)); %27b
Fs4w4i=(K8)*(d4w4i-(xq1-xq0)); %27a
d4w4dti=(1/d4w4i)*(((zw4-z4i)*(vzw4))-((zw4-z4i)*(vz4i))+((xw4-x4i)*(vxw4))-((xw4-x4i)*(vx4i))); %28b
Fd4w4i=(C8)*(d4w4dti); %28a
axM4=zeros(iterations,1);
axM4(1,:)=(-Fs34i*((x3i-x4i)/d34i))+(-Fd34i*((x3i-x4i)/d34i))+(Fs45i*((x4i-x5i)/d34i))+(Fd45i*((x4i-x5i)/d34i))+(Fs4w4i*((x4i-xw4)/d4w4i))+(Fd4w4i*((x4i-xw4)/d4w4i))/MassM4; %22a
azM4=zeros(iterations,1);
azM4(1,:)=(-Fs34i*((z3i-z4i)/d34i))+(-Fd34i*((z3i-z4i)/d34i))+(Fs45i*((z4i-z5i)/d34i))+(Fd45i*((z4i-z5i)/d34i))+(Fs4w4i*((z4i-zw4)/d4w4i))+(Fd4w4i*((z4i-zw4)/d4w4i))/MassM4; %22b
%Mass 5 Equations of Motion
d56i=((((z6i-z5i)^(2))+((x6i-x5i))^(2))^(1/2)); %32b
Fs56i=(K11)*(d56i-(zq6-zq5)); %32a
d56dti=(1/d56i)*(((z6i-z5i)*(vz6i))-((z6i-z5i)*(vz5i))+((x6i-x5i)*(vx6i))-((x6i-x5i)*(vx5i))); %33b
Fd56i=(C11)*(d56dti); %33a
d5w5i=((((zw5-z5i)^(2))+((xw5-x5i))^(2))^(1/2)); %34b
Fs5w5i=(K10)*(d5w5i-(xq1-xq0)); %34a
d5w5dti=(1/d5w5i)*(((zw5-z5i)*(vzw5))-((zw5-z5i)*(vz5i))+((xw5-x5i)*(vxw5))-((xw5-x5i)*(vx5i))); %35b
Fd5w5i=(C10)*(d5w5dti); %35a
axM5=zeros(iterations,1);
axM5(1,:)=(-Fs45i*((x4i-x5i)/d34i))+(-Fd45i*((x4i-x5i)/d34i))+(Fs56i*((x5i-x6i)/d56i))+(Fd56i*((x5i-x6i)/d56i))+(Fs5w5i*((x5i-xw5)/d5w5i))+(Fd5w5i*((x5i-xw5)/d5w5i))/MassM5; %29a
azM5=zeros(iterations,1);
azM5(1,:)=(-Fs45i*((z4i-z5i)/d34i))+(-Fd45i*((z4i-z5i)/d34i))+(Fs56i*((z5i-z6i)/d56i))+(Fd56i*((z5i-z6i)/d56i))+(Fs5w5i*((z5i-zw5)/d5w5i))+(Fd5w5i*((z5i-zw5)/d5w5i))/MassM5; %29b
%Mass 6 Equations of Motion
d6w6i=((((zw6-z6i)^(2))+((xw6-x6i))^(2))^(1/2)); %41b
Fs6w6i=(K12)*(d6w6i-(xq1-xq0)); %42a
d6w6dti=(1/d6w6i)*(((zw6-z6i)*(vzw6))-((zw6-z6i)*(vz6i))+((xw6-x6i)*(vxw6))-((xw6-x6i)*(vx6i))); %43b
Fd6w6i=(C12)*(d6w6dti); %43a
axM6=zeros(iterations,1);
axM6(1,:)=(-Fs56i*((x5i-x6i)/d56i))+(-Fd56i*((x5i-x6i)/d56i))+(Fs6w6i*((x6i-xw6)/d6w6i))+(Fd6w6i*((x6i-xw6)/d6w6i))/MassM6; %36a
azM6=zeros(iterations,1);
azM6(1,:)=(-Fs56i*((z5i-z6i)/d56i))+(-Fd56i*((z5i-z6i)/d56i))+(Fs6w6i*((z6i-zw6)/d6w6i))+(Fd6w6i*((z6i-zw6)/d6w6i))/MassM6; %36b

% Solve the ODE's with Euler's Method
for n=2:(iterations+1)
x1(n,:)=x1(n-1,:)+vx1(n-1,:)*tStep; % segment 1 x-position
x2(n,:)=x2(n-1,:)+vx2(n-1,:)*tStep; % segment 2 x-position
x3(n,:)=x3(n-1,:)+vx3(n-1,:)*tStep; % segment 3 x-position
x4(n,:)=x4(n-1,:)+vx4(n-1,:)*tStep; % segment 4 x-position
x5(n,:)=x5(n-1,:)+vx5(n-1,:)*tStep; % segment 5 x-position
x6(n,:)=x6(n-1,:)+vx6(n-1,:)*tStep; % segment 6 x-position
z1(n,:)=z1(n-1,:)+vz1(n-1,:)*tStep; % segment 1 z-position
z2(n,:)=z2(n-1,:)+vz2(n-1,:)*tStep; % segment 2 z-position
z3(n,:)=z3(n-1,:)+vz3(n-1,:)*tStep; % segment 3 z-position
z4(n,:)=z4(n-1,:)+vz4(n-1,:)*tStep; % segment 4 z-position
z5(n,:)=z5(n-1,:)+vz5(n-1,:)*tStep; % segment 5 z-position
z6(n,:)=z6(n-1,:)+vz6(n-1,:)*tStep; % segment 6 z-position
vx1(n,:)=vx1(n-1,:)+axM1(n-1,:)*tStep; % segment 1 x-velocity
vx2(n,:)=vx2(n-1,:)+axM2(n-1,:)*tStep; % segment 2 x-velocity
vx3(n,:)=vx3(n-1,:)+axM3(n-1,:)*tStep; % segment 3 x-velocity
vx4(n,:)=vx4(n-1,:)+axM4(n-1,:)*tStep; % segment 4 x-velocity
vx5(n,:)=vx5(n-1,:)+axM5(n-1,:)*tStep; % segment 5 x-velocity
vx6(n,:)=vx6(n-1,:)+axM6(n-1,:)*tStep; % segment 6 x-velocity
vz1(n,:)=vz1(n-1,:)+azM1(n-1,:)*tStep; % segment 1 z-velocity
vz2(n,:)=vz2(n-1,:)+azM2(n-1,:)*tStep; % segment 2 z-velocity
vz3(n,:)=vz3(n-1,:)+azM3(n-1,:)*tStep; % segment 3 z-velocity
vz4(n,:)=vz4(n-1,:)+azM4(n-1,:)*tStep; % segment 4 z-velocity
vz5(n,:)=vz5(n-1,:)+azM5(n-1,:)*tStep; % segment 5 z-velocity
vz6(n,:)=vz6(n-1,:)+azM6(n-1,:)*tStep; % segment 6 z-velocity
%Mass 1 Equations of Motion
d01(n,:)=((((z1(n-1,:))^2)+((x1(n-1,:)-xq0)^2))^(1/2)); %2b
Fs10(n,:)=(-K1)*(d01(n-1,:)-zq1); %2a
d01dt(n,:)=((z1(n-1,:)*vz1(n-1,:))+((x1(n-1,:)-xq0)*vx1(n-1,:)))/d01(n-1,:); %3b
Fd10(n,:)=(-C1)*(d01dt(n-1,:)); %3a
d12(n,:)=((((z2(n-1,:)-z1(n-1,:))^(2))+((x2(n-1,:)-x1(n-1,:)))^(2))^(1/2)); %4b
Fs12(n,:)=(K3)*(d12(n-1,:)-(zq2-zq1)); %4a
d12dt(n,:)=(1/d12(n-1,:))*(((z2(n-1,:)-z1(n-1,:))*(vz2(n-1,:)))-((z2(n-1,:)-z1(n-1,:))*(vz1(n-1,:)))+((x2(n-1,:)-x1(n-1,:))*(vx2(n-1,:)))-((x2(n-1,:)-x1(n-1,:))*(vx1(n-1,:)))); %5b
Fd12(n,:)=(C3)*(d12dt(n-1,:)); %5a
d1w1(n,:)=((((zw1-z1(n-1,:))^(2))+((xw1-x1(n-1,:)))^(2))^(1/2)); %6b
Fs1w1(n,:)=(K2)*(d1w1(n-1,:)-(xq1-xq0)); %6a
d1w1dt(n,:)=(1/d1w1(n-1,:))*(((zw1-z1(n-1,:))*(vzw1))-((zw1-z1(n-1,:))*(vz1(n-1,:)))+((xw1-x1(n-1,:))*(vxw1))-((xw1-x1(n-1,:))*(vx1(n-1,:)))); %7b
Fd1w1(n,:)=(C2)*(d1w1dt(n-1,:)); %7a
%Mass 2 Equat(n,:)ons of Mot(n,:)on
d23(n,:)=((((z3(n-1,:)-z2(n-1,:))^(2))+((x3(n-1,:)-x2(n-1,:)))^(2))^(1/2)); %11b
Fs23(n,:)=(K5)*(d23(n-1,:)-(zq3-zq2)); %11a
d23dt(n,:)=(1/d23(n-1,:))*(((z3(n-1,:)-z2(n-1,:))*(vz3(n-1,:)))-((z3(n-1,:)-z2(n-1,:))*(vz2(n-1,:)))+((x3(n-1,:)-x2(n-1,:))*(vx3(n-1,:)))-((x3(n-1,:)-x2(n-1,:))*(vx2(n-1,:)))); %12b
Fd23(n,:)=(C5)*(d23dt(n-1,:)); %12a
d2w2(n,:)=((((zw2-z2(n-1,:))^(2))+((xw2-x2(n-1,:)))^(2))^(1/2)); %13b
Fs2w2(n,:)=(K4)*(d2w2(n-1,:)-(xq1-xq0)); %13a
d2w2dt(n,:)=(1/d2w2(n-1,:))*(((zw2-z2(n-1,:))*(vzw2))-((zw2-z2(n-1,:))*(vz2(n-1,:)))+((xw2-x2(n-1,:))*(vxw2))-((xw2-x2(n-1,:))*(vx2(n-1,:)))); %14b
Fd2w2(n,:)=(C4)*(d2w2dt(n-1,:)); %14a
%Mass 3 Equat(n,:)ons of Mot(n,:)on
d34(n,:)=((((z4(n-1,:)-z3(n-1,:))^(2))+((x4(n-1,:)-x3(n-1,:)))^(2))^(1/2)); %18b
Fs34(n,:)=(K7)*(d34(n-1,:)-(zq4-zq3)); %18a
d34dt(n,:)=(1/d34(n-1,:))*(((z4(n-1,:)-z3(n-1,:))*(vz4(n-1,:)))-((z4(n-1,:)-z3(n-1,:))*(vz3(n-1,:)))+((x4(n-1,:)-x3(n-1,:))*(vx4(n-1,:)))-((x4(n-1,:)-x3(n-1,:))*(vx3(n-1,:)))); %19b
Fd34(n,:)=(C7)*(d34dt(n-1,:)); %19a
d3w3(n,:)=((((zw3-z3(n-1,:))^(2))+((xw3-x3(n-1,:)))^(2))^(1/2)); %20b
Fs3w3(n,:)=(K6)*(d3w3(n-1,:)-(xq1-xq0)); %20a
d3w3dt(n,:)=(1/d3w3(n-1,:))*(((zw3-z3(n-1,:))*(vzw3))-((zw3-z3(n-1,:))*(vz3(n-1,:)))+((xw3-x3(n-1,:))*(vxw3))-((xw3-x3(n-1,:))*(vx3(n-1,:)))); %21b
Fd3w3(n,:)=(C6)*(d3w3dt(n-1,:)); %21a
%Mass 4 Equat(n,:)ons of Mot(n,:)on
d45(n,:)=((((z5(n-1,:)-z4(n-1,:))^(2))+((x5(n-1,:)-x4(n-1,:)))^(2))^(1/2)); %25b
Fs45(n,:)=(K9)*(d45(n-1,:)-(zq5-zq4)); %25a
d45dt(n,:)=(1/d45(n-1,:))*(((z5(n-1,:)-z4(n-1,:))*(vz5(n-1,:)))-((z5(n-1,:)-z4(n-1,:))*(vz4(n-1,:)))+((x5(n-1,:)-x4(n-1,:))*(vx5(n-1,:)))-((x5(n-1,:)-x4(n-1,:))*(vx4(n-1,:)))); %26b
Fd45(n,:)=(C9)*(d45dt(n-1,:)); %26a
d4w4(n,:)=((((zw4-z4(n-1,:))^(2))+((xw4-x4(n-1,:)))^(2))^(1/2)); %27b
Fs4w4(n,:)=(K8)*(d4w4(n-1,:)-(xq1-xq0)); %27a
d4w4dt(n,:)=(1/d4w4(n-1,:))*(((zw4-z4(n-1,:))*(vzw4))-((zw4-z4(n-1,:))*(vz4(n-1,:)))+((xw4-x4(n-1,:))*(vxw4))-((xw4-x4(n-1,:))*(vx4(n-1,:)))); %28b
Fd4w4(n,:)=(C8)*(d4w4dt(n-1,:)); %28a
%Mass 5 Equat(n,:)ons of Mot(n,:)on
d56(n,:)=((((z6(n-1,:)-z5(n-1,:))^(2))+((x6(n-1,:)-x5(n-1,:)))^(2))^(1/2)); %32b
Fs56(n,:)=(K11)*(d56(n-1,:)-(zq6-zq5)); %32a
d56dt(n,:)=(1/d56(n-1,:))*(((z6(n-1,:)-z5(n-1,:))*(vz6(n-1,:)))-((z6(n-1,:)-z5(n-1,:))*(vz5(n-1,:)))+((x6(n-1,:)-x5(n-1,:))*(vx6(n-1,:)))-((x6(n-1,:)-x5(n-1,:))*(vx5(n-1,:)))); %33b
Fd56(n,:)=(C11)*(d56dt(n-1,:)); %33a
d5w5(n,:)=((((zw5-z5(n-1,:))^(2))+((xw5-x5(n-1,:)))^(2))^(1/2)); %34b
Fs5w5(n,:)=(K10)*(d5w5(n-1,:)-(xq1-xq0)); %34a
d5w5dt(n,:)=(1/d5w5(n-1,:))*(((zw5-z5(n-1,:))*(vzw5))-((zw5-z5(n-1,:))*(vz5(n-1,:)))+((xw5-x5(n-1,:))*(vxw5))-((xw5-x5(n-1,:))*(vx5(n-1,:)))); %35b
Fd5w5(n,:)=(C10)*(d5w5dt(n-1,:)); %35a
%Mass 6 Equat(n,:)ons of Mot(n,:)on
d6w6(n,:)=((((zw6-z6(n-1,:))^(2))+((xw6-x6(n-1,:)))^(2))^(1/2)); %41b
Fs6w6(n,:)=(K12)*(d6w6(n-1,:)-(xq1-xq0)); %42a
d6w6dt(n,:)=(1/d6w6(n-1,:))*(((zw6-z6(n-1,:))*(vzw6))-((zw6-z6(n-1,:))*(vz6(n-1,:)))+((xw6-x6(n-1,:))*(vxw6))-((xw6-x6(n-1,:))*(vx6(n-1,:)))); %43b
Fd6w6(n,:)=(C12)*(d6w6dt(n-1,:)); %43a
% Find segment accelerations
axM1(n,:)=((Fs10(n,:))*((x1(n,:)-xq0)/(d01(n,:))))+((Fd10(n,:))*((x1(n,:)-xq0)/(d01(n,:))))+(Fs12(n,:)*((x1(n,:)-x2(n,:))/d12(n,:)))+(Fd12(n,:)*((x1(n,:)-x2(n,:))/d12(n,:)))+(Fs1w1(n,:)*((x1(n,:)-xw1)/d1w1(n,:)))+(Fd1w1(n,:)*((x1(n,:)-xw1)/d1w1(n,:)))+(FimpM1xapplied)/MassM1; % segment 1 x-acceleration
azM1(n,:)=((Fs10(n,:))*(z1(n,:)/d01(n,:)))+((Fd10(n,:))*((z1(n,:))/(d01(n,:))))+(Fs12(n,:)*((z1(n,:)-z2(n,:))/d12(n,:)))+(Fd12(n,:)*((z1(n,:)-z2(n,:))/d12(n,:)))+(Fs1w1(n,:)*((z1(n,:)-zw1)/d1w1(n,:)))+(Fd1w1(n,:)*((z1(n,:)-zw1)/d1w1(n,:)))+(FimpM1zapplied)/MassM1; % segment 1 z-acceleration
axM2(n,:)=(-Fs12(n,:)*((x1(n,:)-x2(n,:))/d12(n,:)))+(-Fd12(n,:)*((x1(n,:)-x2(n,:))/d12(n,:)))+(Fs23(n,:)*((x2(n,:)-x3(n,:))/d23(n,:)))+(Fd23(n,:)*((x2(n,:)-x3(n,:))/d23(n,:)))+(Fs2w2(n,:)*((x2(n,:)-xw2)/d2w2(n,:)))+(Fd2w2(n,:)*((x2(n,:)-xw2)/d2w2(n,:)))/MassM2; % segment 2 x-acceleration
azM2(n,:)=(-Fs12(n,:)*((z1(n,:)-z2(n,:))/d12(n,:)))+(-Fd12(n,:)*((z1(n,:)-z2(n,:))/d12(n,:)))+(Fs23(n,:)*((z2(n,:)-z3(n,:))/d23(n,:)))+(Fd23(n,:)*((z2(n,:)-z3(n,:))/d23(n,:)))+(Fs2w2(n,:)*((z2(n,:)-zw2)/d2w2(n,:)))+(Fd2w2(n,:)*((z2(n,:)-zw2)/d2w2(n,:)))/MassM2; % segment 2 z-acceleration
axM3(n,:)=(-Fs23(n,:)*((x2(n,:)-x3(n,:))/d23(n,:)))+(-Fd23(n,:)*((x2(n,:)-x3(n,:))/d23(n,:)))+(Fs34(n,:)*((x3(n,:)-x4(n,:))/d34(n,:)))+(Fd34(n,:)*((x3(n,:)-x4(n,:))/d34(n,:)))+(Fs3w3(n,:)*((x3(n,:)-xw3)/d3w3(n,:)))+(Fd3w3(n,:)*((x3(n,:)-xw3)/d3w3(n,:)))/MassM3; % segment 3 x-acceleration
azM3(n,:)=(-Fs23(n,:)*((z2(n,:)-z3(n,:))/d23(n,:)))+(-Fd23(n,:)*((z2(n,:)-z3(n,:))/d23(n,:)))+(Fs34(n,:)*((z3(n,:)-z4(n,:))/d34(n,:)))+(Fd34(n,:)*((z3(n,:)-z4(n,:))/d34(n,:)))+(Fs3w3(n,:)*((z3(n,:)-zw3)/d3w3(n,:)))+(Fd3w3(n,:)*((z3(n,:)-zw3)/d3w3(n,:)))/MassM3; % segment 3 z-acceleration
axM4(n,:)=(-Fs34(n,:)*((x3(n,:)-x4(n,:))/d34(n,:)))+(-Fd34(n,:)*((x3(n,:)-x4(n,:))/d34(n,:)))+(Fs45(n,:)*((x4(n,:)-x5(n,:))/d34(n,:)))+(Fd45(n,:)*((x4(n,:)-x5(n,:))/d34(n,:)))+(Fs4w4(n,:)*((x4(n,:)-xw4)/d4w4(n,:)))+(Fd4w4(n,:)*((x4(n,:)-xw4)/d4w4(n,:)))/MassM4; % segment 4 x-acceleration
azM4(n,:)=(-Fs34(n,:)*((z3(n,:)-z4(n,:))/d34(n,:)))+(-Fd34(n,:)*((z3(n,:)-z4(n,:))/d34(n,:)))+(Fs45(n,:)*((z4(n,:)-z5(n,:))/d34(n,:)))+(Fd45(n,:)*((z4(n,:)-z5(n,:))/d34(n,:)))+(Fs4w4(n,:)*((z4(n,:)-zw4)/d4w4(n,:)))+(Fd4w4(n,:)*((z4(n,:)-zw4)/d4w4(n,:)))/MassM4; % segment 4 z-acceleration
axM5(n,:)=(-Fs45(n,:)*((x4(n,:)-x5(n,:))/d34(n,:)))+(-Fd45(n,:)*((x4(n,:)-x5(n,:))/d34(n,:)))+(Fs56(n,:)*((x5(n,:)-x6(n,:))/d56(n,:)))+(Fd56(n,:)*((x5(n,:)-x6(n,:))/d56(n,:)))+(Fs5w5(n,:)*((x5(n,:)-xw5)/d5w5(n,:)))+(Fd5w5(n,:)*((x5(n,:)-xw5)/d5w5(n,:)))/MassM5; % segment 5 x-acceleration
azM5(n,:)=(-Fs45(n,:)*((z4(n,:)-z5(n,:))/d34(n,:)))+(-Fd45(n,:)*((z4(n,:)-z5(n,:))/d34(n,:)))+(Fs56(n,:)*((z5(n,:)-z6(n,:))/d56(n,:)))+(Fd56(n,:)*((z5(n,:)-z6(n,:))/d56(n,:)))+(Fs5w5(n,:)*((z5(n,:)-zw5)/d5w5(n,:)))+(Fd5w5(n,:)*((z5(n,:)-zw5)/d5w5(n,:)))/MassM5; % segment 5 z-acceleration
axM6(n,:)=(-Fs56(n,:)*((x5(n,:)-x6(n,:))/d56(n,:)))+(-Fd56(n,:)*((x5(n,:)-x6(n,:))/d56(n,:)))+(Fs6w6(n,:)*((x6(n,:)-xw6)/d6w6(n,:)))+(Fd6w6(n,:)*((x6(n,:)-xw6)/d6w6(n,:)))/MassM6; % segment 6 x-acceleration
azM6(n,:)=(-Fs56(n,:)*((z5(n,:)-z6(n,:))/d56(n,:)))+(-Fd56(n,:)*((z5(n,:)-z6(n,:))/d56(n,:)))+(Fs6w6(n,:)*((z6(n,:)-zw6)/d6w6(n,:)))+(Fd6w6(n,:)*((z6(n,:)-zw6)/d6w6(n,:)))/MassM6; % segment 6 z-acceleration
end

% % Plot results
% subplot(3,1,1)
% hold on;
% plot(t',x1,'r')
% plot(t',x2,'m')
% ylabel('Position (m)')
% title('Position, Velocity, & Acceleration as a Function of Time')
% legend('Cart 1','Cart 2')
% subplot(3,1,2)
% hold on;
% plot(t',v1,'b')
% plot(t',v2,'c')
% ylabel('Velocity (m/s)')
% legend('Cart 1','Cart 2')
% subplot(3,1,3)
% hold on;
% plot(t',a1,'g')
% plot(t',a2,'y')
% ylabel('Acceleration (m/s^2)')
% xlabel('time (iterations)')
% legend('Cart 1','Cart 2')

toc

Accepted Answer

James Tursa
James Tursa on 10 Dec 2021
Edited: James Tursa on 10 Dec 2021
"All of my matrices I am solving for have NaN after the third time line. How do I solved this issue?"
Set up a breakpoint somewhere in your Euler stepping code (i.e., click to the left of the line number in the editor). Then run your code, which will pause at that line with all variables intact. Hit the run button a couple of times until the NaNs start appearing. Examine which ones are NaN and look for a 0/0 or inf-inf situation. Then backtrack in your code and figure out why that is happening. Or you could insert these lines of code:
if( any(isnan(some variable that you noticed has NaNs in it)) )
disp('NaN'); % <-- set a breakpoint on this line
end
and your code will run until it hits the NaN values and then pause when it gets inside the if block.

More Answers (0)

Categories

Find more on Eigenvalues & Eigenvectors in Help Center and File Exchange

Tags

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!