Applying runge kutta for coupled equations

Question

so I have 2 second order nonlinear ODE and after applying the state-space theorm I have 4 first order ODE. I'm trying to apply RK4 but I think I'm doing it wrong because the graphs diverge. I'm getting a hard time applying it because the equations are coupled. Those are the main equations. L and Fa also have state-space variables in them but it doesn't make a diffrence for my qustion. Equations image

After applying the state-space theorm, Those are my equations:

f1 = @(x2) x2; % = x1'

f2 = @(x1, x2, x3) K/m*(l_0/sqrt((X_d(t, t1, t2, a_x0, X_d0, X_d0_tag)-x1).^2+(Z_d(t, t0, a_z0, Z_d0, Z_d0_tag)-x3).^2)-1)*(x1-X_d(t, t1, t2, a_x0, X_d0, X_d0_tag)) ...
    - 0.5*Rho*A*C_d*(x2-Interpolation(Z_d(t, t0, a_z0, Z_d0, Z_d0_tag), data)).^2*sgn(x2, Z_d(t, t0, a_z0, Z_d0, Z_d0_tag), data)/m;
% = x2'

f3 = @(x1) x1; % = x3'

f4 = @(x1, x3) K/m*(l_0/sqrt((X_d(t, t1, t2, a_x0, X_d0, X_d0_tag)-x1).^2+(Z_d(t, t0, a_z0, Z_d0, Z_d0_tag)-x3).^2)-1)*(x3-Z_d(t, t0, a_z0, Z_d0, Z_d0_tag))-g;
% x4'

Than I tried to apply RK4. Heads up, it might be a complete nonsense. I also applied initial conditions but I don't want to make it messy.

h=0.2; % step size
t_array = 0:h:10;

w = zeros(1,length(t_array)); 
x = zeros(1,length(t_array)); 
y = zeros(1,length(t_array)); 
z = zeros(1,length(t_array));

for i=1:(length(t_array)-1) % calculation loop
    t = 0 +h*i; % A parameter needed for the interpolation in f2
    
    k_1 = f1(x(i));
    k_2 = f1(x(i)+0.5*h*k_1);
    k_3 = f1(x(i)+0.5*h*k_2);
    k_4 = f1(x(i)+k_3*h);
    x(i+1) = x(i) + (1/6)*(k_1+2*k_2+2*k_3+k_4)*h;  
    disp(x(i+1));
    
    m_1 = f3(z(i));
    m_2 = f3(z(i)+0.5*h*k_1);
    m_3 = f3(z(i)+0.5*h*k_2);
    m_4 = f3(z(i)+k_3*h);
    z(i+1) = z(i) + (1/6)*(m_1+2*m_2+2*m_3+m_4)*h;  

    n_1 = f2(x(i), z(i), w(i));
    n_2 = f2(x(i), z(i) ,w(i)+0.5*h*k_1);
    n_3 = f2(x(i), z(i) ,w(i)+0.5*h*k_2);
    n_4 = f2(x(i), z(i) ,w(i)+k_3*h);
    w(i+1) = w(i) + (1/6)*(k_1+2*k_2+2*k_3+k_4)*h;  

    l_1 = f4(x(i), z(i));
    l_2 = f4(x(i), z(i));
    l_3 = f4(x(i), z(i));
    l_4 = f4(x(i), z(i));
    y(i+1) = y(i) + (1/6)*(k_1+2*k_2+2*k_3+k_4)*h;  
    
end

As I said my graphs are divering (they souldn't be) so I suspect my code is wrong. Please help me fix the algorithm. Thank you very much!

Answer 1

What are XD and ZD? Why don't they have differential equations associated with them? Can you give more details about this?

Also, it would help if you used vectors for the state variable, and had one single function handle that produced a vector output. That cuts down on the code you write, and allows you to compare your results with ode45( ) since it can take the same function handle as input.

It appears that the fundamental flaw in your code is that although you state the equations are coupled, you are attempting to integrate them piecemeal. E.g., you do the RK4 scheme for one variable using k_1, k_2, k_3, k_4 to propagate x(i) to x(i+1). But during this process all the other coupled variables z(i) and w(i) and y(i) remain static in your code. This is a flaw. All of the coupled variables need to propagate at the same time through those intermediate calculations. I.e., you need to generate all of the k_1, m_1, n_1, and l_1 first, then using those results calculate all of the k_2, m_2, n_2, and l_2. Then using those results calculate all of the k_3, m_3, n_3, and l_3. Then using all of those results calculate k_4, m_4, n_4, and l_4. And finally use all of this to propagate all your variables forward one step. This is where a vector function handle can greatly help you. By making one function handle that takes a vector input (each element of the vector representing one of your variables) and returns a vector output, you can boil your code down to writing only one set of RK4 equations that automatically propagates all the variables forward at the same time because they are all part of the same vector. This will also make your code easier to debug.

Finally, you are mixing variables and derivatives. x's should go with k's, z's should go with m's, w's should go with n's, and y's should go with l's. In particular, the l's don't even have RK4 scheme implemented.