[3D Pendulum - MATLAB]

Calculated using the Runge Cutta Method. 

global kappa m g l Cd rho A

m = 1;

l = 1;

g = pi^2;

kappa = 4*(pi^2);

Cd = 0.4;

rho = 1.1;

A = 0.2;

% initial conditions

y0 = [0.4, 0, -1.4, 0, -0.4, 0];

h = 0.002;

tspan =  [0 50.3] ;

[t, output] = ivpsys_RKF(y0, h, tspan);

x = output(:, 1);

y = output(:, 2);

z = output(:, 3);

u = output(:,4);

v = output(:,5);

w = output(:,6);

figure(1); 

plot3(x, y, z, 'k'); 

grid on; 

axis([-0.4, 0.4 -0.4, 0.4  -2, 0])

view(3); 

xlabel('X (m)');

ylabel('Y (m)'); 

zlabel('Z (m)'); 

title('3D Trajectory of Pendulum');

function [t, y] = ivpsys_RKF(y0, h, tspan)

N = ceil( ( tspan(2) - tspan(1) ) /h );

num_eq = length(y0);

t = zeros(N,1);

y = zeros(N,num_eq);

t(1) = tspan(1);

y(1,:) = y0;

%% Time advancement:

for n = 1:N

   % RK 1st substep

   k1 = h * ivpsys_fun(t(n) ,  y(n,:) );

   % RK 2nd substep

    k2 = h * ivpsys_fun(t(n) + ((1/4) * h),  y(n,:) + ((1/4) * k1));

   % RK 3rd substep

   k3 = h * ivpsys_fun(t(n) + ((3/8) * h),  y(n,:) + ((3/32) * k1) + ((9/32) * k2));

   % RK 4th substep

   k4 = h * ivpsys_fun(t(n) + ((12/13) * h),  y(n,:) + ((1932/2197) * k1) - ((7200/2197) * k2) + ((7296/2197) * k3));

   % RK 5th substep

    k5 = h * ivpsys_fun(t(n) + h,  y(n,:) + ((439/216) * k1) -  (8 * k2) + ((3680/513) * k3) - ((845/4104) * k4));

   % RK 6th substep

   k6 = h * ivpsys_fun(t(n) + ((1/2) * h),  y(n,:) - ((8/27) * k1) + (2 * k2) - ((3544/2565) * k3) + ((1859/4104) * k4) - ((11/40) * k5));

   % Advance from tn to tn+h using weighted changes in y: (k1, k2, k3, k4, k5, k6)

   y(n+1,:) = y(n,:) + ((16/135) * k1) + ((6656/12825) * k3) + ((28561/56430) * k4) - ((9/50) * k5) + ((2/55) * k6);

   t(n+1) = t(n) + h;

end

end

function yp = ivpsys_fun(t, y)

% IVPSYS_FUN evaluate the right-hand-side of the ODE

global kappa m g l Cd rho A 

yp(1) = y(4); % u

yp(2) = y(5); % v

yp(3) = y(6); % w

r = sqrt((y(1)^2) + (y(2)^2) + (y(3)^2));

vmag = sqrt( (yp(1)^2) + (yp(2)^2) + (yp(3)^2) );

yp(4) = ( -(kappa/m)*( (r - l) / r )*y(1) ) - ( (Cd*rho*A*vmag*y(4)) / 2*m ); 

 %xpp/t

yp(5) = ( -(kappa/m)*( (r - l) / r )*y(2) ) - ( (Cd*rho*A*vmag*y(5)) / 2*m ); 

 %ypp/y

yp(6) = ( -(kappa/m)*( (r - l) / r )*y(3) ) - ( (Cd*rho*A*vmag*y(6)) / 2*m ) - g; 

 %zpp/t

end