Help on solving nonlinear coupled equations with Runge-Kutta approach

I have the following piece of code. I'm providing a "piece" here, since the full code is quite lengthy and I think the following would suffice. But I'm more than happy to share more, if needed.

x = [0, 5, 0, 0, 0, 0].';

% Simulation parameters
dt = 0.1;                               % time step (s)
tend = 10;                              % end time (s)
tspan = 0:dt:tend;                      % time vector
number_of_time_steps = tend/dt;         % number of time steps

for t=1:number_of_time_steps

    f = @(x,t) eomsys(x, data);         % I have more parameters than these two.
    [x, dxdt] = rungekutta4(f,x,u,dt);

end

My equations of motions are (nonlinear, coupled) quite similar to the ones I've posted in earlier posts (a quite old one here, and here), but with some additions. Basically, I have formulated the equations in following form, and the script follows. I'm sorry about the messy matrices!

function [xdot] = eomsys(x, data)

    u = x(1); v = x(2); w = x(3); p = x(4); q = x(5); r = x(6);
    
    massmat = [70.0532949500000	0	0.715271137500000	0	2.28968497500000	0
                0	93.4368680000000	0	0.445077447500000	0	-0.607912125000000
                1.33294997500000	0	138.803890750000	0	0.520733722500000	0
                0	0.444320895000000	0	10940.2695433200	0	0.0816402557500000
                5.67678722500000	0	55153.0589243300	0	12.7759792500000	0
                0	-0.608241150000000	0	0.0810982870000000	0	55162.3832866000];
    
    smat = [0, 86.9651*r, 4.1729*w, 0.9520*r, 0, 0.9520*r;
            (-2.7108*r-0*v-0*p-0*0*r), -2.3186, 0, -0.1296, 0, -11.1265;
            0, 0, 0.5241, 0, -76.0312, 0;
            (0*v-0*p-0*0*r), -0.3209, 0, -0.0103, 0, (1.0939e+04*q);
            0, 0, 13.0970, 0, -0.0382*q, -1.0940e+04*p;
            (2.7108*v - (65.4088+0*0)*v - (0.9520+0*0)*p +...
            (1.1414+0*0^2)*r), -25.5156, 0, -0.6808, -4.4212e+04*p, -2.2918];
    
    kmat = [1; 1; 1; 1; 1; 1]; 
    % I have added ones here arbitrarily. I will have a different, and a bigger matrix here.
    
    xdot = inv(massmat)*smat*x + inv(massmat)*kmat;

end

And my Runge-Kutta scheme,

function [xnxt, dxdt] = rungekutta4(f,x,u,dt)
    xo = x;
    
    dxdt = feval(f,xo,u);
    k1   = dt*dxdt;
    x    = xo+0.5*k1;
    k2   = dt*feval(f,x,u);
    x    = xo+0.5*k2;
    k3   = dt*feval(f,x,u);
    x    = xo+k3;
    k4   = dt*feval(f,x,u);
    xnxt = xo + (k1+2*(k2+k3)+k4)/6;
end

As you can see, I'm learning as I go, and I feel like my approach here is wrong. I'll start from the obvious one - Can I write my nonlinear coupled ODEs like this, in matrix form? (I read here that it's not possible, but I had found a few tutorials online that did otherwise). If that is the case, this post would be redundent, and I'd appreciate a suggestion to move forward.

If I am wrong here, what am I doing wrong? If, somehow, I am right, what can I do to improve?

Thank you in advance!

[Torsten]

Do you have a specific problem with the code ?

[Jake]

Hi @Torsten, sorry to be a nuisance - I should've been a bit clearer. My code runs without errors, but I have no way of knowing whether the results are correct or if I am doing this correctly. I understand that without knowing the complete problem it is not possible for anyone to tell if my results are correct or not. But, I just wanted to know whether my approach of solving nonlinear coupled ODEs with RK4 method is (or seems) correct or not. Thank you.

[Torsten]

@Sam Chak idea is the best you can do. If the results are different, apply your code to a differential equation you know the solution of in order to be able to compare results. You should calculate the inverse of your mass matrix once before calling the runge-kutta code and pass it to "eomsys".

[Jake]

Hi @Torsten, I agree - I hadn't thought about it before! That is the best option and I'm accepting that as the answer. I will try this soon too.

Could you please explain a bit more on what you meant by "You should calculate the inverse of your mass matrix once before calling the runge-kutta code and pass it to "eomsys" "?

[Torsten]

Could you please explain a bit more on what you meant by "You should calculate the inverse of your mass matrix once before calling the runge-kutta code and pass it to "eomsys" "?

Like this:

x = [0, 5, 0, 0, 0, 0].';
% Simulation parameters
dt = 0.1;                               % time step (s)
tend = 10;                              % end time (s)
tspan = 0:dt:tend;                      % time vector
number_of_time_steps = tend/dt;         % number of time steps
massmat = [70.0532949500000	0	0.715271137500000	0	2.28968497500000	0
    0	93.4368680000000	0	0.445077447500000	0	-0.607912125000000
    1.33294997500000	0	138.803890750000	0	0.520733722500000	0
    0	0.444320895000000	0	10940.2695433200	0	0.0816402557500000
    5.67678722500000	0	55153.0589243300	0	12.7759792500000	0
    0	-0.608241150000000	0	0.0810982870000000	0	55162.3832866000];
invmassmat = inv(massmat);
for t=1:number_of_time_steps
    f = @(x,t) eomsys(x, data, invmassmat);         % I have more parameters than these two.
    [x, dxdt] = rungekutta4(f,x,u,dt);
end

function [xnxt, dxdt] = rungekutta4(f,x,u,dt)
xo = x;

dxdt = feval(f,xo,u);
k1   = dt*dxdt;
x    = xo+0.5*k1;
k2   = dt*feval(f,x,u);
x    = xo+0.5*k2;
k3   = dt*feval(f,x,u);
x    = xo+k3;
k4   = dt*feval(f,x,u);
xnxt = xo + (k1+2*(k2+k3)+k4)/6;
end

function [xdot] = eomsys(x, data, invmassmat)
u = x(1); v = x(2); w = x(3); p = x(4); q = x(5); r = x(6);

smat = [0, 86.9651*r, 4.1729*w, 0.9520*r, 0, 0.9520*r;
    (-2.7108*r-0*v-0*p-0*0*r), -2.3186, 0, -0.1296, 0, -11.1265;
    0, 0, 0.5241, 0, -76.0312, 0;
    (0*v-0*p-0*0*r), -0.3209, 0, -0.0103, 0, (1.0939e+04*q);
    0, 0, 13.0970, 0, -0.0382*q, -1.0940e+04*p;
    (2.7108*v - (65.4088+0*0)*v - (0.9520+0*0)*p +...
    (1.1414+0*0^2)*r), -25.5156, 0, -0.6808, -4.4212e+04*p, -2.2918];

kmat = [1; 1; 1; 1; 1; 1]; 
% I have added ones here arbitrarily. I will have a different, and a bigger matrix here.

xdot = invmassmat*(smat*x + kmat);
end

[Jake]

Perfectly clear - this makes sense because now I don't have to get the inv of the mass matrix at every step. Thanks!

I have one last question. So, this formulation (it gets the job done correctly, as I've tested it following what Sam suggested) gives the x, and xdot values, which are the velocities and acceleration (since I'm passing velocities as the initial conditions, i.e., x vector.). Is it possible to get the displacements too, without having to derive the equations again accordingly?

[Torsten]

The displacement is the integral of the velocity.

If x, y and z are the displacements, you have to solve 3 additional equations for them:

dx/dt = u, x(0) = x0
dy/dt = v, y(0) = y0
dz/dt = w, z(0) = z0

Or what do you mean ?

[Jake]

I understand :) MY initial thought was whether I could simply pass the x vector as

x = [x xdot y ydot z zdot p pdot q qdot r rdot]; % 6dof

and obtain the displacement, velocity and acceleration vectors at the end, but after re-reading the Matlab example - Solve Equations of Motion for Baton Thrown into Air, I can see the sillyness of my thought. I think what I have to do is derive the equations again similar to that example.

Thank you for being patient and for your responses.

[Sam Chak]

That is a fully-coupled translational and rotational motion equations. I'm unsure, but the {u, v, w, p, q, r} states are commonly seen Aircraft and Marine Craft Dynamics textbooks.

[Torsten]

@Jake

What you solve for in the example Matlab example - Solve Equations of Motion for Baton Thrown into Air is displacement and velocity. Acceleration is what you prescribe: inv(M)*rhs .

[Jake]

You're right, @Sam Chak :) This relates to aircraft dynamics and I'm working with a system of fully-coupeld translational and rotational motion equations.

Thank you for the response, and I understand @Torsten. Unfortunately, I think I lack the knowledge to fully comprehend all these, and I'm going to go back to learning a bit more and deriving the equations again before I give up completely.

My equations are kind of similar to the ones shown in the example (Matlab example - Solve Equations of Motion for Baton Thrown into Air), but I have a bit more going on. For example, my motion equation in x direction is in this form. The system has 6 degrees of freedom.

I have tried several approaches, but I couldn't figure a proper approach. I tried with an ode solver, but I do need intermediate values of the coefficients (such as f_1 and k_1), so I chose Runge-Kutta with a time loop. As you can guess, the kmat I have included in my above code was supposed to represent the time and velocity dependent values.

I'm sorry if I'm taking too much of time, and I can open another post if you think it's best. But I'd gladly accept any feedback at this point.

Answer 1

[Sam Chak]

Hi @Jake

I am unfamiliar with your equations of motion. But you can check and compare if you get similar plots below as solved by ode45().

% Simulation parameters
dt    = 0.01;           % time step (s)
tend  = 10;             % end time (s)
tspan = 0:dt:tend;      % time vector
x0 = [0, 5, 0, 0, 0, 0];
[t, x] = ode45(@eomsys, tspan, x0);

for j = 1:6
    subplot(3, 2, j)
    plot(t, x(:,j)), grid on
    title(sprintf('x_{%d}', j))
end

function [xdot] = eomsys(t, x)
    xdot = zeros(6, 1);
    
    u = x(1); v = x(2); w = x(3); p = x(4); q = x(5); r = x(6);
    
    massmat = [70.0532949500000	0	0.715271137500000	0	2.28968497500000	0
                0	93.4368680000000	0	0.445077447500000	0	-0.607912125000000
                1.33294997500000	0	138.803890750000	0	0.520733722500000	0
                0	0.444320895000000	0	10940.2695433200	0	0.0816402557500000
                5.67678722500000	0	55153.0589243300	0	12.7759792500000	0
                0	-0.608241150000000	0	0.0810982870000000	0	55162.3832866000];
    
    smat = [0, 86.9651*r, 4.1729*w, 0.9520*r, 0, 0.9520*r;
            (-2.7108*r-0*v-0*p-0*0*r), -2.3186, 0, -0.1296, 0, -11.1265;
            0, 0, 0.5241, 0, -76.0312, 0;
            (0*v-0*p-0*0*r), -0.3209, 0, -0.0103, 0, (1.0939e+04*q);
            0, 0, 13.0970, 0, -0.0382*q, -1.0940e+04*p;
            (2.7108*v - (65.4088+0*0)*v - (0.9520+0*0)*p +...
            (1.1414+0*0^2)*r), -25.5156, 0, -0.6808, -4.4212e+04*p, -2.2918];
    
    kmat = [1; 1; 1; 1; 1; 1]; 
        
    xdot = inv(massmat)*smat*x + inv(massmat)*kmat;
end

[Jake]

Hi @Sam Chak, thank you for the idea! I don't know why I haven't thought about this already!