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

Tags:
1. Mar 17, 2016

### noelll

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. Mar 17, 2016

### 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.

3. Mar 17, 2016

### noelll

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

4. Mar 17, 2016

### 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.

5. Mar 18, 2016

### noelll

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.

6. Mar 25, 2016

### Twigg

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.

7. Mar 26, 2016

### noelll

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

8. Mar 26, 2016

### Twigg

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.

9. Mar 29, 2016

### noelll

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:

• ###### boef impact theory 2.pdf
File size:
938.6 KB
Views:
207
10. Mar 29, 2016

### Twigg

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.

11. Mar 30, 2016

### jasonRF

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.

Hope that helps.

Jason

12. Mar 30, 2016

### Twigg

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.

13. Mar 30, 2016

### noelll

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

14. Apr 2, 2016

### jasonRF

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

15. Apr 3, 2016

### noelll

Hi Jason,

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

File size:
938.6 KB
Views:
96