4th order Runge-Kutta with system of coupled 2nd order ODE in MATLAB

Question

I tried using Runge-Kutta methods to approximate motion equations in matlab but it turn out wrong. $$ M\left(\frac{d^2x}{dt^2}\right)=F_n(\cosΦ - u\sinΦ) \\ M\left(\frac{d^2z}{dt^2}\right)=F_n(\sinΦ + u\cosΦ ) - Mg\\ M\left(\frac{d^2Φ}{dt^2}\right)=F_n(Bxx + uBz ) $$ 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);

Answer 1

One basic principle in programming is to make algorithms available for as large a class of data structures as possible. Matlab makes this easy since vectors and matrices are elementary data structures. Thus make an implementation of RK4 for general vectors and translate your state into a state vector for integration.

The following demonstrates this on the example of the 3-dimensional Lorenz system (this is for scilab, which is close, but not identical to matlab, here the only change should be in the endfunction statement which should be end in matlab. Also with @ expressions one could shorten some parts)

function [t, y] = RK4(odefunc, y0, t0, tf, N)
    T = tf-t0;
    h=T/N;
    time=t0;
    state=y0;
    t = zeros(N+1);
    y = zeros(N+1,length(y0));
    t(1) = t0;
    y(1,:) = y0;

    for i=2:N+1
        k1 = odefunc(time        , state           );
        k2 = odefunc(time + 0.5*h, state + 0.5*h*k1);
        k3 = odefunc(time + 0.5*h, state + 0.5*h*k2);
        k4 = odefunc(time +     h, state +     h*k3);
        state = state + (h/6)*(k1+2*k2+2*k3+k4);
        time = time + h;
        t(i) = time;
        y(i,:) = state;
    end
endfunction

function doty = lorenz(t, y, params)
    a=params(1); b=params(2); c=params(3);
    doty = [ a * ( y(2) - y(1) )
            -b * y(1) + y(1) * y(3)
            -c * y(1) - b * y(3) ]'
endfunction

function doty=odefunc(t,y)
    doty = lorenz(t,y,[0.5, 0.66, 1.4])
endfunction

[T,Y] = RK4(odefunc, [-1.2 0.2 0.8], 0, 10, 400)