4th order Runge kutta with system of coupled 2nd order ode MATLAB need help i do not know where my algorithm gone wrong

MX''=Fn(cosΦ−usinΦ)

MZ''=Fn(sinΦ+ucosΦ)−Mg

IΦ''=Fn(Bxx+uBz)

I tried using Runge-Kutta methods to approximate motion equations in matlab but it turn out wrong. I also tired finding and researching forums and web for solution but to no avail.

Fn,M,θ,u is constant fn/M = 0.866

it seems that i did my coupled runge kutta wrongly. my phi and omega does not change with time. please help

clc;                                               % Clears the screen
clear all;

thete=30;
g= 9.81;                                             %Constant

h=0.01;                                              % step size
tfinal = 3;
N=ceil(tfinal/h);                                      %ceil is round up                              

t(1) = 0;% initial condition
Vx(1)=0;%initial accleration
X(1)=0;
Vz(1)=0;
Z(1)=20;%initial velocity
fn = 0.866;
Bz = 0.35;
Phi(1)= 30;
W(1)= 0;

%Bxx = ((Z-Z(1))+5.8*cos(thete)+Bz*(sin(Phi)-sin(thete)));
Bxx = 0.5;
%sliding phase
F_X  = @(t,X,Vx,Phi) Vx;
F_Z  = @(t,Z,Vz,Phi) Vz;
F_Phi= @(t,Phi,W) W;
F_Vx = @(t,X,Vx,Phi)(fn*(cos(Phi)-0.05*(sin(Phi))));
F_Vz = @(t,Z,Vz,Phi)(fn*(sin(Phi)+0.05*(cos(Phi)))-g);
F_W  = @(t,Phi,W)((fn*(Bxx+Bz*0.05))/20000);

for i=1:N;                                         % calculation loop

      %update time
      t(i+1)=t(i)+h;
      %update motions main equation
      %rotation phase
      k_1 = F_X (t(i)      ,X(i)          ,Vx(i)          ,Phi(i));
      L_1 = F_Vx(t(i)      ,X(i)          ,Vx(i)          ,Phi(i));
      k_2 = F_X (t(i)+0.5*h,X(i)+0.5*h*k_1,Vx(i)+0.5*h*L_1,Phi(i)+0.5*h*L_1);
      L_2 = F_Vx(t(i)+0.5*h,X(i)+0.5*h*k_1,Vx(i)+0.5*h*L_1,Phi(i)+0.5*h*L_1);
      k_3 = F_X (t(i)+0.5*h,X(i)+0.5*h*k_2,Vx(i)+0.5*h*L_2,Phi(i)+0.5*h*L_2);
      L_3 = F_Vx(t(i)+0.5*h,X(i)+0.5*h*k_2,Vx(i)+0.5*h*L_2,Phi(i)+0.5*h*L_2);
      k_4 = F_X (t(i)+h    ,X(i)+h    *k_3,Vx(i)+    h*L_3,Phi(i)+h*L_3);
      L_4 = F_Vx(t(i)+h    ,X(i)+h    *k_3,Vx(i)+    h*L_3,Phi(i)+h*L_3);

      k_11 = F_Z (t(i)      ,Z(i)          ,Vz(i)            ,Phi(i));
      L_11 = F_Vz(t(i)      ,Z(i)          ,Vz(i)            ,Phi(i));
      k_22 = F_Z (t(i)+0.5*h,Z(i)+0.5*h*k_11,Vz(i)+0.5*h*L_11,Phi(i)+0.5*h*L_11);
      L_22 = F_Vz(t(i)+0.5*h,Z(i)+0.5*h*k_11,Vz(i)+0.5*h*L_11,Phi(i)+0.5*h*L_11);
      k_33 = F_Z (t(i)+0.5*h,Z(i)+0.5*h*k_22,Vz(i)+0.5*h*L_22,Phi(i)+0.5*h*L_22);
      L_33 = F_Vz(t(i)+0.5*h,Z(i)+0.5*h*k_22,Vz(i)+0.5*h*L_22,Phi(i)+0.5*h*L_22);
      k_44 = F_Z (t(i)+    h,Z(i)+    h*k_33,Vz(i)+    h*L_33,Phi(i)+h*L_33);
      L_44 = F_Vz(t(i)+    h,Z(i)+    h*k_33,Vz(i)+    h*L_33,Phi(i)+h*L_33);

      k_5 = F_Phi (t(i)      ,Phi(i)           ,W(i));
      L_5 = F_W   (t(i)      ,Phi(i)           ,W(i));
      k_6 = F_Phi (t(i)+0.5*h,Phi(i)+0.5*h*k_5,W(i)+0.5*h*L_5);
      L_6 = F_W   (t(i)+0.5*h,Phi(i)+0.5*h*k_5,W(i)+0.5*h*L_5);
      k_7 = F_Phi (t(i)+0.5*h,Phi(i)+0.5*h*k_6,W(i)+0.5*h*L_6);
      L_7 = F_W   (t(i)+0.5*h,Phi(i)+0.5*h*k_6,W(i)+0.5*h*L_6);
      k_8 = F_Phi (t(i)+    h,Phi(i)+    h*k_7,W(i)+    h*L_7);
      L_8 = F_W   (t(i)+    h,Phi(i)+    h*k_7,W(i)+    h*L_7);

      X(i+1)  = X(i) + (h/6)*(k_1+2*k_2+2*k_3+k_4);
      Vx(i+1) = Vx(i) + (h/6)*(L_1+2*L_2+2*L_3+L_4);
      Z(i+1)  = Z(i) + (h/6)*(k_11+2*k_22+2*k_33+k_44);
      Vz(i+1) = Vz(i) + (h/6)*(L_11+2*L_22+2*L_33+L_44);
      Phi(i+1)= Phi(i) + (h/6)*(k_5+2*k_6+2*k_7+k_8);
      W(i+1)  = W(i) + (h/6)*(L_5+2*L_6+2*L_7+L_8);

end

figure
plot(X,Z,'--', 'Linewidth', 1.5, 'color', 'red')
xlabel('time')
ylabel('height')
legend('position')
figure
plot(Phi,W,'--', 'Linewidth', 1.5, 'color', 'blue')
xlabel('time')
ylabel('height')
legend('rotation')
figure
plot(t,Vx,'--', 'Linewidth', 1.5, 'color', 'black')
hold on
plot(t,Vz,'--', 'Linewidth', 1.5, 'color', 'yellow')
hold on
plot(t,W,'--','Linewidth',1.5,'color','green')
xlabel('time')
ylabel('height')
legend('Vx','Vz','W')

fprintf('time %.3f\n x %.3f\n z %.3f\n ',t,X,Z);
fprintf('W %.3f\n',W);
fprintf('Phi %.3f\n',Phi);

[Torsten]

Use one of the ODE integrators, e.g. ODE45.

Best wishes

Torsten.

Answer 1

[Ced]

Why are you dividing F_W by 20000? What kind of system is this? I think your ODEs are working fine, they are just reeeeeally slow.

May I suggest using an integrator function? I think the resulting code is much more readable. Here is your code, rewritten using my own little rk4 integrator so you see what I mean:

function solve_coupled_ode()

clc;                                               % Clears the screen
clear all;

theta =30;
g= 9.81;                                             %Constant

h=0.01;                                              % step size
tfinal = 3;
N=ceil(tfinal/h);                                      %ceil is round up

% parameters
fnM = 0.866; % fn / M
Bz = 0.35;
u = 0.05;
%Bxx = ((Z-Z(1))+5.8*cos(theta)+Bz*(sin(Phi)-sin(thete)));
Bxx = 0.5;

% initial values
x0 = 0;
dx0 = 0;
z0 = 20;
dz0 = 0;
Phi0 = 30;
dPhi0 = 0;

% state vector: y = [ X dX Z dZ Phi dPhi ]'
y0 = [ x0 dx0 z0 dz0 Phi0 dPhi0 ]';

% dynamics
dynamics = @(t,y)f(t,y,fnM,u,Bz,Bxx,g);

% result matrices
y_out = zeros(N,6);
t = zeros(N,1);

% set initial condition and integrate
y_out(1,:) = y0';

for i= 1:N

      % update time and dynamics
      [ t(i+1), y_out(i+1,:) ] = integrator_rk4(h,dynamics,t(i),y_out(i,:)');

end

% Extract result
X   = y_out(:,1);
Vx  = y_out(:,2);
Z   = y_out(:,3);
Vz  = y_out(:,4);
Phi = y_out(:,5);
W   = y_out(:,6);

% Plot
figure
plot(X,Z,'--', 'Linewidth', 1.5, 'color', 'red')
xlabel('time')
ylabel('height')
legend('position')

figure
plot(Phi,W,'--', 'Linewidth', 1.5, 'color', 'blue')
xlabel('time')
ylabel('height')
legend('rotation')

figure
plot(t,Vx,'--', 'Linewidth', 1.5, 'color', 'black')
hold on
plot(t,Vz,'--', 'Linewidth', 1.5, 'color', 'yellow')
hold on
plot(t,W,'--','Linewidth',1.5,'color','green')
xlabel('time')
ylabel('height')
legend('Vx','Vz','W')

% fprintf('time %.3f\n x %.3f\n z %.3f\n ',t,X,Z);
% fprintf('W %.3f\n',W);
% fprintf('Phi %.3f\n',Phi);

end

%%%%%%%%%%%%%
%%Dynamics of motion
function dy = f(t,y,fnM,u,Bz,Bxx,g)
% state vector: y = [ X dX Z dZ Phi dPhi ]'

dy = zeros(6,1);

% x, dx
dy(1) = y(2);
dy(2) = fnM*(cos(y(5)) - u*sin(y(5)));

% z, dz
dy(3) = y(4);
dy(4) = fnM*(sin(y(5)) + u*cos(y(5))) - g;

% phi, dphi
dy(5) = y(6);
dy(6) = fnM*(Bxx + u*Bz);

end

%%%%%%%%%%%%%
function [ tout, xout ] = integrator_rk4(dt,dx,t,x)
% function xnext = integrator_rk4(dt,dx,t,x)
%
% This function performs a single Runge-Kutta update step
%
% input: 
% dt: timestep
% dx: function handle to dx(t,x)
% t: current time
% x: current state (column vector)
%
% output:
% tout: t + dt
% xout: state at time t + dt
%

tout = t + dt;

dt_half = 0.5*dt;
k1 = dx(t,x);
k2 = dx(t+dt_half,x+dt_half*k1);
k3 = dx(t+dt_half,x+dt_half*k2);
k4 = dx(tout,x+dt*k3);

xout = x + dt*(k1+2*k2+2*k3+k4)/6;

end

Cheers