Dismiss Notice
Join Physics Forums Today!
The friendliest, high quality science and math community on the planet! Everyone who loves science is here!

I 4th order Runge-Kutta with system of coupled 2nd order ode motion equations

  1. Mar 17, 2016 #1
    MX''=Fn(cosΦ−usinΦ)
    MZ''=Fn(sinΦ+ucosΦ)−Mg
    MΦ''=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

    Code (Matlab M):

    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);



     
     
  2. jcsd
  3. Mar 17, 2016 #2

    jedishrfu

    Staff: Mentor

    Try placing print statements inside your loop and/or use a debugger to step thru your code as you inspect each statements results.

    There's a lot going on and you may have more than one thing wrong.
     
  4. Mar 17, 2016 #3
    Hi thanks for your reply,

    tried de-bugging it and the code seems fine, so i was thinking it should be my runge kutta algorithm.
    I start learning runge kutta just last week and i still do not really understand how the coupled equations work.

    base on what i did above, was to find the value of omega(W) and pass it into Phi and then pass Phi into X and Z respectively, i would like to ask is my understanding correct?

    Thank you once again for your help
     
  5. Mar 17, 2016 #4

    jedishrfu

    Staff: Mentor

    So you're saying that printing t and phi and omega in the for loop the phi and omega don't change.

    If so then that means the variables used to compute phi and omega aren't changing either is that right?

    So I'd check them next and you should be able to find it.

    The equations look right but I've never actually implemented RK instead I've done Euler variants which are decidedly simpler.
     
  6. Mar 18, 2016 #5
    Hi thank for your reply,

    Yes my phi and omega did not change it still remain the same as what i have input as the initial parameters, however but for my X and Z it change according to time.

    Therefore, I feel that my phi and omega is not passing in to the runge kutta loop. Then again my X and Z get pass in the loop.

    is my function wrong? i split up the three 2nd order ode into six 1st order ode.
    Code (Text):

    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);
     
    Thank you so much for helping me.
     
  7. Mar 25, 2016 #6

    Twigg

    User Avatar
    Gold Member

    Run your code again. Go into your workspace window and double click on the 1x301 double variable W. It's changing, it's just small. Phi changes in the least significant digit of the mantissa as well. Double check that you calculated your coefficients correctly. I wager that 20000 or that 0.05 in the expression for w is off by a couple orders of magnitude, or that W and phi just aren't supposed to change much.
     
  8. Mar 26, 2016 #7
    Hi Twigg,

    Thank you so much for your reply, yes i went to check and phi is changing by very small step, i also check my coefficients and the value i get is correct. Could it be my runge kutta algorithm were wrong? Because base on the motion equations i shared above,
    equations does not have any X or Z variables.

    Thank you again for helping
    Noel
     
  9. Mar 26, 2016 #8

    Twigg

    User Avatar
    Gold Member

    I played with your code a bit, and sure enough you get a linearly increasing omega vs. time if you make Bxx a lot bigger. It seems to me like the Runge-Kutta algorithm is doing something that looks to me like integrating a constant angular acceleration, i.e. ##\Phi''##. Whether or not the data is accurate is another story. Is ##\omega(t)## being nearly constant not what you were expecting? I wouldn't change your RK code if I were you, until you're absolutely certain that the data it gives you is unmistakably false.

    Also, using RK to solve for phi is overkill. The way you've defined it, it's just a constant angular acceleration. Why not just create a function handle to evaluate the displacement in terms of phi(1), W(1), and Fn*(Bxx+uBz)/M? It'll make troubleshooting a lot easier.
     
  10. Mar 29, 2016 #9
    Hi Twigg,

    Thank you so much for your reply, I am currently following a journal paper(attached) base on the paper during rotation phase ##\Phi''## and ##\omega(t)## should constantly change with time. I am not sure if the problem due to my runge kutta algorithm. Therefore, i am not absolutely certain if my data is false or not.

    I updated my previous code and i got a error, i know the error is cause by different size of vector, k_1 is 1x1, k_5 is 301x1, k_6 is 301x1 and lastly k_7 is 301x1. is the error due to k_1?
    Hope you could help me, thank you so much!
    Code (Text):
    In an assignment  A(I) = B, the number of elements in B and I must be the same.

    Error in rotation2 (line 95)
        X(i+1)  = X(i) + (h/6)*(k_1+2*k_5+2*k_6+k_7);
     
    Code (Text):



    clc;                                               % Clears the screen
    clear all;

    % Constants
    g    = 9.81;
    degree=55;
    alpha=(degree*(pi/180));
    M    = 6000;
    fr   = 0.05;
    Bx1  = 1.95;
    Bx2  = 3.20;
    I    = 20000;
    Bz   = 0.35;
    L    = 8.5;
    SL   = 5.8;

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

    % Simulation sections
    % Pre-allocate vectors
    t = zeros(N+1,1);
    X = zeros(N+1,1);
    Z = zeros(N+1,1);
    Vx= zeros(N+1,1);
    Vz= zeros(N+1,1);
    Phi = zeros(N+1,1);
    W = zeros(N+1,1);

    % Initial Conditions
    t(1) = 0;
    Vx(1)=0;
    X(1)=0;
    Vz(1)=0;
    Z(1)=19;
    Phi(1) = 0;
    W(1) = 0;

    Bxx = ((Z-Z(1))+SL*cos(alpha)+Bz*(sin(Phi)-sin(alpha)));
    dBxx = ((Vz+Bz).*((sec(Phi)).^2).*(W-Bz.*(sin(alpha))).*(1./(cos(Phi)).^2).*(sin(Phi)).*W);
    fn1 = ((W.*(Vx.*sin(Phi)-Vz.*cos(Phi)-dBxx)+(M.*g.*sin(Phi))./M)./((Bxx+Bz.*fr)./I+1./M));
    RotationEnd = (SL*cos(alpha)+Bz*(sin(Phi)-sin(alpha))+Bx2*cos(Phi));

    F_X  = @(t,X,Vx) Vx;
    F_Z  = @(t,Z,Vz) Vz;
    F_Vx = @(t,X,Vx)(fn1.*(cos(Phi)-fr*(sin(Phi))))/M;
    F_Vz = @(t,Z,Vz)((fn1./M).*(sin(Phi)+fr*(cos(Phi)))-g);
    F_Phi = @(t,W)W;
    F_W = @(t,Z,Vx,Vz,Phi,W)((fn1.*(Bxx+Bz*fr))/I);

    i = 1;

    %% NOT REACHED TERMINAL CONDITION OR END OF TIME %%
    while (Z(1)-Z(i) < RotationEnd | i <= N )
       
        % update time and state (integrate)
        t(i+1)=t(i)+h;
       
        %update motions main equation
        %Rotation phase
        k_1 = F_X (t(i)      ,X(i)          ,Vx(i));
        L_1 = F_Vx(t(i)      ,X(i)          ,Vx(i));
        k_5 = F_X (t(i)+0.5*h,X(i)+0.5*h*k_1,Vx(i)+0.5*h*L_1);
        L_5 = F_Vx(t(i)+0.5*h,X(i)+0.5*h*k_1,Vx(i)+0.5*h*L_1);
        k_6 = F_X (t(i)+0.5*h,X(i)+0.5*h*k_5,Vx(i)+0.5*h*L_5);
        L_6 = F_Vx(t(i)+0.5*h,X(i)+0.5*h*k_5,Vx(i)+0.5*h*L_5);
        k_7 = F_X (t(i)+h    ,X(i)+h    *k_6,Vx(i)+    h*L_6);
        L_7 = F_Vx(t(i)+h    ,X(i)+h    *k_6,Vx(i)+    h*L_6);
       
     
        k_11 = F_Z (t(i)     ,Z(i)          ,Vz(i));
        L_11 = F_Vz(t(i)     ,Z(i)          ,Vz(i));
        k_55 = F_Z (t(i)+0.5*h,Z(i)+0.5*h*k_11,Vz(i)+0.5*h*L_11);
        L_55 = F_Vz(t(i)+0.5*h,Z(i)+0.5*h*k_11,Vz(i)+0.5*h*L_11);
        k_66 = F_Z (t(i)+0.5*h,Z(i)+0.5*h*k_55,Vz(i)+0.5*h*L_55);
        L_66 = F_Vz(t(i)+0.5*h,Z(i)+0.5*h*k_55,Vz(i)+0.5*h*L_55);
        k_77 = F_Z (t(i)+    h,Z(i)+    h*k_66,Vz(i)+    h*L_66);
        L_77 = F_Vz(t(i)+    h,Z(i)+    h*k_66,Vz(i)+    h*L_66);
       
        k1_Phi  = F_Phi (t(i), W(i));
        k1_W    = F_W   (t(i), Z(i), Vx(i), Vz(i), Phi(i), W(i));
        k5_Phi  = F_Phi (t(i)+0.5*h,W(i)+0.5*h*k1_W);
        k5_W    = F_W   (t(i)+0.5*h,Z(i)+0.5*h*k_11,Vx(i)+0.5*h*L_1,Vz(i)+0.5*h*L_11,Phi(i)+0.5*h*k1_Phi,W(i)+0.5*h*k1_W);
        k6_Phi  = F_Phi (t(i)+0.5*h,W(i)+0.5*h*k5_W);
        k6_W    = F_W   (t(i)+0.5*h,Z(i)+0.5*h*k_55,Vx(i)+0.5*h*L_5,Vz(i)+0.5*h*L_55,Phi(i)+0.5*h*k5_Phi,W(i)+0.5*h*k5_W);
        k7_Phi  = F_Phi (t(i)+    h,W(i)+    h*k6_W);
        k7_W    = F_W   (t(i)+    h,Z(i)+    h*k_66,Vx(i)+    h*L_6,Vz(i)+    h*L_66,Phi(i)+    h*k6_Phi,W(i)+    h*k6_W);

        X(i+1)  = X(i) + (h/6)*(k_1+2*k_5+2*k_6+k_7);
        Vx(i+1) = Vx(i) + (h/6)*(L_1+2*L_5+2*L_6+L_7);
        Z(i+1)  = Z(i) + (h/6)*(k_11+2*k_55+2*k_66+k_77);
        Vz(i+1) = Vz(i) + (h/6)*(L_11+2*L_55+2*L_66+L_77);
        Phi(i+1)= Phi(i) + (h/6)*(k1_Phi+2*k5_Phi+2*k6_Phi+k7_Phi);
        W(i+1)  = W(i) + (h/6)*(k1_W+2*k5_W+2*k6_W+k7_W);
       
        i = i+1;

    end

    keyboard
    figure(2)
    plot(X,Z,'--', 'Linewidth', 1.5, 'color', 'red')
    xlabel('x-distance')
    ylabel('height')
    legend('position')
    figure(3)
    plot(t,Vx,'--', 'Linewidth', 1.5, 'color', 'black')
    hold on
    plot(t,Vz,'--', 'Linewidth', 1.5, 'color', 'yellow')
    xlabel('time')
    ylabel('height')
    legend('Vx','Vz')

    fprintf('time %.3f\n x %.3f\n z %.3f\n ',t,X,Z);
    fprintf('Vx %.3f\n',Vx);
    fprintf('Vz %.3f\n',Vz);
    fprintf('pi %.3f\n',alpha);
    fprintf('fn %.3f\n',fn);
     

    Attached Files:

  11. Mar 29, 2016 #10

    Twigg

    User Avatar
    Gold Member

    The error you posted is about a mismatch between x(i) and k5, k6, and k7. x(i+1) is a scalar, so MATLAB expects a scalar value on the right side. x(i) and k1 are scalars, but the other k's (k5,k6,k7) are all 1x301 arrays. You have to index those as well. MATLAB assumes that any scalar plus a 1xn vector is another 1xn vector whose j-th entry is the j-th entry of the original vector plus the scalar value. That's why it throws a dimension mismatch error there.

    I haven't had time to read the paper yet, but is there a set of initial conditions for which you know what the solution should look like? If so, see if the simulation matches the known solution. Is there a set of initial values for which you could solve the differential equations by hand, and then compare your results with your code's? I'll read through the paper and attempt to verify the algorithm this weekend.
     
  12. Mar 30, 2016 #11

    jasonRF

    User Avatar
    Science Advisor
    Gold Member

    Your Runge-Kutta implementation is definitely wrong. You cannot decouple this system of six equations into three systems of two equations each.

    Looking at your first implementation you at least have the correct functional dependence for F_Vx, etc. But in your first step you should be computing all of: k_1, L_1, k_11, L_11, k_5 and L_5, then using all of those for the second step to compute k_2, L_2, k_22, etc.

    Your second implementation has additional problems that are even worse.

    Hope that helps.

    Jason
     
  13. Mar 30, 2016 #12

    Twigg

    User Avatar
    Gold Member

    Finally noticed this in the first implementation thanks to JasonRF's comment:

    L_1 isn't what you should add to Phi(i), as L_1 is a change in the Vx variable. What I think JasonRF is suggesting is to calculate all your RK coefficients at the same time, so you can do

    Code (Text):

    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*k_5)
    One neat way you could implement this with minimal hassle is just to vectorize all your k's and L's (k_1 through k_4 being vectors with x,z,phi components, and L_1 through L_4 being vectors with x',z',w components). Then, for example, you could write:

    Code (Text):

    k_1 = F_traj(t(i),            trajectory(:,i),                   velocity(:.i)                    );
    L_1 = F_vel(t(i),            trajectory(:,i),                   velocity(:,i)                    );
    k_2 = F_traj(t(i)+0.5*h, trajectory(:,i)+0.5*h*k_1, velocity(:,i) + 0.5*h*L_1);
    %where...
    %trajectory(:,i) = [X(i);Z(i);Phi(i)]
    %velocity(:,i) = [Vx(i); Vz(i); W(i)]
    %F_traj = @(t(i),    trajectory(:,i),       velocity(:,i)     )      [ F_X( t(i),X(i),Vx(i),Phi(i) ); F_Z( t(i),Z(i),Vz(i),Phi(i) ); F_Phi( t(i),Phi(i),W(i) ) ]
    %F_vel = @(t(i),    trajectory(:,i),       velocity(:,i)      )     [ F_Vx(t(i),X(i),Vx(i),Phi(i) ); F_Z( t(i),Z(i),Vz(i),Phi(i) ); F_W( t(i),Phi(i),W(i),Z(i) ) ]
    or something along those lines.

    Another solution you can consider is trying to change coordinates to an uncoupled system.
     
  14. Mar 30, 2016 #13
    Hi Twigg and Jason, thank you so much for your reply.

    I took Jason advise and i re-do my RK algorithm.

    Code (Text):

    clc;                                               % Clears the screen
    clear all;

    % Constants
    g    = 9.81;
    degree=55;
    alpha=(degree*(pi/180));
    M    = 6000;
    fr   = 0.05;
    Bx1  = 1.95;
    Bx2  = 3.20;
    I    = 20000;
    Bz   = 0.35;
    L    = 8.5;
    SL   = 5.8;

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

    % Simulation sections
    % Pre-allocate vectors
    t = zeros(N+1,1);
    X = zeros(N+1,1);
    Z = zeros(N+1,1);
    Vx= zeros(N+1,1);
    Vz= zeros(N+1,1);
    Phi = zeros(N+1,1);
    W = zeros(N+1,1);

    % Initial Conditions
    t(1) = 0;
    Vx(1)=0;
    X(1)=0;
    Vz(1)=0;
    Z(1)=19;
    Phi(1) = 0;
    W(1) = 0;


    %% 1. Sliding Phase
    fn   =(M*g*sin(alpha))-cos(alpha);           % normal force
    SlidingCondition = ((SL+Bz*fr)*cos(alpha));  % terminal condition for sliding phase

    F_X  = @(t,X,Vx) Vx;
    F_Z  = @(t,Z,Vz) Vz;
    F_Vx = @(t,X,Vx)(fn*(cos(alpha)-fr*(sin(alpha))))/M;
    F_Vz = @(t,Z,Vz)(fn/M*(sin(alpha)+fr*(cos(alpha)))-g);
    F_Phi = @(t,W)W;
    F_W = @(t)0;

    i = 1;
    while(Z(1)-Z(i) < SlidingCondition && i <= N) % calculation loop
       
        %update time
        t(i+1)=t(i)+h;
       
        %update motions main equation
        %Sliding phase
        k_1 = F_X (t(i)      ,X(i)          ,Vx(i));
        L_1 = F_Vx(t(i)      ,X(i)          ,Vx(i));
        k_2 = F_X (t(i)+0.5*h,X(i)+0.5*h*k_1,Vx(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);
        k_3 = F_X (t(i)+0.5*h,X(i)+0.5*h*k_2,Vx(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);
        k_4 = F_X (t(i)+h    ,X(i)+h    *k_3,Vx(i)+    h*L_3);
        L_4 = F_Vx(t(i)+h    ,X(i)+h    *k_3,Vx(i)+    h*L_3);
       
     
        k_11 = F_Z (t(i)     ,Z(i)          ,Vz(i));
        L_11 = F_Vz(t(i)     ,Z(i)          ,Vz(i));
        k_22 = F_Z (t(i)+0.5*h,Z(i)+0.5*h*k_11,Vz(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);
        k_33 = F_Z (t(i)+0.5*h,Z(i)+0.5*h*k_22,Vz(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);
        k_44 = F_Z (t(i)+    h,Z(i)+    h*k_33,Vz(i)+    h*L_33);
        L_44 = F_Vz(t(i)+    h,Z(i)+    h*k_33,Vz(i)+    h*L_33);
       
        k1_Phi  = F_Phi (t(i), W(i));
        k1_W    = F_W   (t(i));
        k2_Phi  = F_Phi (t(i)+0.5*h,W(i)+0.5*h*k1_W);
        k2_W    = F_W   (t(i)+0.5*h);
        k3_Phi  = F_Phi (t(i)+0.5*h,W(i)+0.5*h*k2_W);
        k3_W    = F_W   (t(i)+0.5*h);
        k4_Phi  = F_Phi (t(i)+    h,W(i)+    h*k3_W);
        k4_W    = F_W   (t(i)+    h);

        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)*(k1_Phi+2*k2_Phi+2*k3_Phi+k4_Phi);
        W(i+1)  = W(i) + (h/6)*(k1_W+2*k2_W+2*k3_W+k4_W);
       
        i = i+1;
       
    end

    % Intermediate plotting
    ind_sliding = 1:i; % select indices up to current time
    figure(1)
    plot(X(ind_sliding),Z(ind_sliding),'--', 'LineWidth', 1.5, 'color', 'red')
    grid on
    ylim([ 0 Z(1) ])
    xlabel('x distance [m]')
    ylabel('height [m]')
    title('Sliding Phase')


    %% 2. Rotation Phase

    %%% DEFINE EQUATIONS OF MOTION AND TERMINAL CONDITION FOR ROTATION PHASE HERE %%%


    Bxx = @(Z,Phi)((Z-Z(1))+SL*cos(alpha)+Bz*(sin(Phi)-sin(alpha)));
    dBxx =@(Vz,Phi,W)((Vz+Bz).*((sec(Phi)).^2).*(W-Bz.*(sin(alpha))).*(1./(cos(Phi)).^2).*(sin(Phi)).*W);
    fn1 = @(Z,Vx,Vz,Phi,W)((W.*(Vx.*sin(Phi)-Vz.*cos(Phi)-dBxx(Vz,Phi,W))+(M.*g.*sin(Phi))./M)./((Bxx(Z,Phi)+Bz.*fr)./I+1./M));
    RotationEnd =@(Phi) (SL*cos(alpha)+Bz*(sin(Phi)-sin(alpha))+Bx2*cos(Phi));

    F_Xr  = @(t,X,Vx) Vx;
    F_Zr  = @(t,Z,Vz) Vz;
    F_Phir= @(t,W)W;
    F_Vxr = @(t,X,Z,Vx,Vz,Phi,W)(fn1(Z,Vx,Vz,Phi,W)*(cos(Phi)-fr*(sin(Phi))))/M;
    F_Vzr = @(t,X,Z,Vx,Vz,Phi,W)(fn1(Z,Vx,Vz,W,Phi)/M*(sin(Phi)+fr*(cos(Phi)))-g);
    F_Wr  = @(t,X,Z,Vx,Vz,Phi,W)((fn1(Z,Vx,Vz,W,Phi).*(Bxx(Z,Phi)+Bz*fr))/I);


    % continue integration where we stopped last time
    %% NOT REACHED TERMINAL CONDITION OR END OF TIME %%
    while (Z(1)-Z(i) < RotationEnd(Phi) | i <= N )
       
        % update time and state (integrate)
        t(i+1)=t(i)+h;
       
        %update motions main equation
        %Rotation phase
        k_5     = F_Xr (t(i)      ,X(i)          ,Vx(i));
        L_5     = F_Vxr(t(i)      ,X(i)          ,Z(i)           ,Vx(i)          ,Vz(i)           ,Phi(i)             ,W(i));
        k_55    = F_Zr (t(i)      ,Z(i)          ,Vz(i));
        L_55    = F_Vzr(t(i)      ,X(i)          ,Z(i)           ,Vx(i)          ,Vz(i)           ,Phi(i)             ,W(i));
        k5_Phi  = F_Phir (t(i)    ,W(i));
        k5_W    = F_Wr   (t(i)    ,X(i)          ,Z(i)           ,Vx(i)          ,Vz(i)           ,Phi(i)             ,W(i));
       

        k_6    = F_Xr (t(i)+0.5*h,X(i)+0.5*h*k_5,Vx(i)+0.5*h*L_5);
        L_6    = F_Vxr(t(i)+0.5*h,X(i)+0.5*h*k_5,Z(i) +0.5*h*k_55,Vx(i)+0.5*h*L_5,Vz(i)+0.5*h*L_55,Phi(i)+0.5*h*k5_Phi,W(i)+0.5*h*k5_W);
        k_66   = F_Zr (t(i)+0.5*h,Z(i)+0.5*h*k_55,Vz(i)+0.5*h*L_55);
        L_66   = F_Vzr(t(i)+0.5*h,X(i)+0.5*h*k_5,Z(i) +0.5*h*k_55,Vx(i)+0.5*h*L_5,Vz(i)+0.5*h*L_55,Phi(i)+0.5*h*k5_Phi,W(i)+0.5*h*k5_W);
        k6_Phi = F_Phir (t(i)+0.5*h,W(i)+0.5*h*k5_W);
        k6_W   = F_Wr   (t(i)+0.5*h,X(i)+0.5*h*k_5,Z(i)+0.5*h*k_55,Vx(i)+0.5*h*L_5,Vz(i)+0.5*h*L_55,Phi(i)+0.5*h*k5_Phi,W(i)+0.5*h*k5_W);
       
       
        k_7 = F_Xr (t(i)+0.5*h,X(i)+0.5*h*k_6,Vx(i)+0.5*h*L_6);
        L_7 = F_Vxr(t(i)+0.5*h,X(i)+0.5*h*k_6,Z(i) +0.5*h*k_66,Vx(i)+0.5*h*L_6,Vz(i)+0.5*h*L_66,Phi(i)+0.5*h*k6_Phi,W(i)+0.5*h*k6_W);
        k_77 = F_Zr (t(i)+0.5*h,Z(i)+0.5*h*k_66,Vz(i)+0.5*h*L_66);
        L_77 = F_Vzr(t(i)+0.5*h,X(i)+0.5*h*k_6,Z(i) +0.5*h*k_66,Vx(i)+0.5*h*L_6,Vz(i)+0.5*h*L_66,Phi(i)+0.5*h*k6_Phi,W(i)+0.5*h*k6_W);
        k7_Phi  = F_Phir (t(i)+0.5*h,W(i)+0.5*h*k6_W);
        k7_W    = F_Wr   (t(i)+0.5*h,X(i)+0.5*h*k_6,Z(i)+0.5*h*k_66,Vx(i)+0.5*h*L_6,Vz(i)+0.5*h*L_66,Phi(i)+0.5*h*k6_Phi,W(i)+0.5*h*k6_W);
       
       
        k_8 = F_Xr (t(i)+h    ,X(i)+h    *k_7,Vx(i)+h    *L_7);
        L_8 = F_Vxr(t(i)+h    ,X(i)+h    *k_7,Z(i) +h    *k_77,Vx(i)+h    *L_7,Vz(i)+h    *L_77,Phi(i)+    h*k7_Phi,W(i)+h    *k7_W);
        k_88 = F_Zr (t(i)+h    ,Z(i)+h    *k_77,Vz(i)+h    *L_77);
        L_88 = F_Vzr(t(i)+h    ,X(i)+h    *k_7,Z(i) +h    *k_77,Vx(i)+h    *L_7,Vz(i)+h    *L_77,Phi(i)+    h*k7_Phi,W(i)+h    *k7_W);
        k8_Phi  = F_Phir (t(i)+    h,W(i)+    h*k7_W);
        k8_W    = F_Wr   (t(i)+0.5*h,X(i)+0.5*h*k_7,Z(i)+0.5*h*k_77,Vx(i)+0.5*h*L_7,Vz(i)+0.5*h*L_77,Phi(i)+0.5*h*k7_Phi,W(i)+0.5*h*k7_W);

        X(i+1)  = X(i) + (h/6)*(k_5+2*k_6+2*k_7+k_8);
        Vx(i+1) = Vx(i) + (h/6)*(L_5+2*L_6+2*L_7+L_8);
        Z(i+1)  = Z(i) + (h/6)*(k_55+2*k_66+2*k_77+k_88);
        Vz(i+1) = Vz(i) + (h/6)*(L_55+2*L_66+2*L_77+L_88);
        Phi(i+1)= Phi(i) + (h/6)*(k5_Phi+2*k6_Phi+2*k7_Phi+k8_Phi);
        W(i+1)  = W(i) + (h/6)*(k5_W+2*k6_W+2*k7_W+k8_W);
       
        i = i+1;

    end

    % Intermediate plotting
    ind_rotation = 1:i; % select indices up to current time
    figure(2)
    plot(X(ind_rotation),Z(ind_rotation),'--', 'LineWidth', 1.5, 'color', 'red')
    grid on
    ylim([ 0 Z(1) ])
    xlabel('x distance [m]')
    ylabel('height [m]')
    title('Rotation Phase')

    figure(3)
    plot(X,Z,'--','Linewidth', 1.5, 'color', 'blue')
    xlabel('x-distance')
    ylabel('height')
    legend('position')
    Is this method of doing my RK correct?
    One last thing now my sliding phase and rotation phase graph are separated, is there anyway i could plot them together into one graph?

    Thanks again for helping me!
    Best Regards,
    Noel
     
  15. Apr 2, 2016 #14

    jasonRF

    User Avatar
    Science Advisor
    Gold Member

    Structurally it looks okay, I guess, but it is really impossible to tell since you are clearly solving a different problem than you were earlier, and you never stated what problem it is! What is $$ \alpha $$? Also, you are solving two problems in this new code? The second problem has extra factors of 0.5 in k_8W, but otherwise has the right idea, I think. Again, without clear descriptions of what equations you are solving I cannot tell much of anything. Good luck.

    jason
     
  16. Apr 3, 2016 #15
    Hi Jason,

    Thank you so much for your reply, sorry for my bad explanation.

    Currently I am using this journal paper (attach) to do my coding. Hence, the equations and solving two problem are all base on the equations in the paper.
    alpha is the angle of launch and by using the equations in the paper i am required to solve for sliding phase, rotation phase and free-fall phase, also thank you so much for helping to check my codes, the extra factors of 0.5 in k_8 are totally my mistake, thank you for sounding it out.

    I hope after seeing the equations from the paper, you could still guide and help me.
    Thank you once again for your time,
    Best Regards,
    Noel
     

    Attached Files:

Know someone interested in this topic? Share this thread via Reddit, Google+, Twitter, or Facebook

Have something to add?
Draft saved Draft deleted



Similar Discussions: 4th order Runge-Kutta with system of coupled 2nd order ode motion equations
Loading...