Manual RK4 method for solving IVP (data formatting problem)-CodePudding

Currently I'm attempting to solve 4 coupled ODE's to stabilize an inverted pendulum on a cart. I have no problem doing it with ODEINT from Scipy, however, I can't make it work with a manual implementation. Most likely this is due to some weird data formatting done in the 'model' function in the code.

I have tried multiple things to no avail, thus I won't post my error codes, since they range from the size not fitting when adding all the calculated steps in the RK4 method.

My current code with ODEINT working is down below. What I'm asking is whether someone can help me, so that the function 'model' is properly made, so that I can implement the RK4 solver (which I can do for other ODE's without any problem).

import numpy as np
from scipy.integrate import solve_ivp
from scipy import signal
g = 9.82
l = 0.281 
mc = 6.28
alpha = 0.4
mp = 0.175
t_start = 0.
t_end = 12.
tol = 10**(-1)
# Define A and B and the poles we want
A = np.array([[0., 1., 0., 0.], [(mc mp)*g/(l*mc), 0., 0., (-alpha)/(l*mc)], [0., 0., 0., 1.], [(g*mp)/mc, 0., 0., (-alpha)/mc]])
B = np.array([[0.], [1./(l*mc)], [0.], [1./mc]])
Poles = np.array([complex(-1.,2.), complex(-1.,-2.), complex(-2.,1.), complex(-2.,-1.)])

# Determine K
signal = signal.place_poles(A, B, Poles)
K = signal.gain_matrix
# print(signal.computed_poles) # To verify if the computes poles are correct

# Define the model
def model(t,x):
    x1, x2, x3, x4 = x
    u = -np.matmul(K,x)
    dx1dt = x2
    dx2dt = (np.cos(x1.astype(float))*(u-alpha*x4-mp*l*x2**2*np.sin(x1.astype(float))) (mc mp)*g*np.sin(x1.astype(float)))/(l*(mc mp*(1-np.cos(x1.astype(float))**2)))
    dx3dt = x4
    dx4dt = (u-alpha*x4-mp*l*x2**2*np.sin(x1.astype(float)) mp*g*np.sin(x1.astype(float))*np.cos(x1.astype(float)))/(mc mp*(1-np.cos(x1.astype(float))**2))
    return np.array([dx1dt, dx2dt, dx3dt, dx4dt])

# Solve the system
N = 10000 # Number of steps
t = np.linspace(t_start, t_end, N)
t_span = (t_start, t_end)
x0 = np.array([0.2, 0., 0., 0.])
sol = solve_ivp(model,t_span,x0, t_eval=t, method='RK45')
index = np.argmin(sol.y[2,:]) # Max displacement from the origin
print(f' The biggest deviation from the origin is: {abs(sol.y[2, index])} meters.')

#This doesn't work
def RK4(fcn,a ,b ,y0 ,N):
    h = (b-a)/N
    x = a   np.arange(N 1)*h
    y = np.zeros((x.size,y0.size))
    y[0,:] = y0
    for k in range(N):
        k1 = fcn(x[k], y[k,:])
        k2 = fcn(x[k]   h/2, y[k,:]   h*k1/2)
        k3 = fcn(x[k]   h/2, y[k,:]   h*k2/2)
        k4 = fcn(x[k]   h, y[k,:]   h*k3)
        y[k 1,:] = y[k,:]   h/6*(k1   2*(k2   k3)   k4)
    
    return x,y

a,b = RK4(model, 0, 12, x0, 1000)

Which yields the following error:

runcell(0, 'C:/Users/Nikolai Lund Kühne/OneDrive - Aalborg Universitet/Uni/3. semester/P3 - Dynamiske Systemer/manualRK4.py')
 The biggest deviation from the origin is: 0.48256054833140316 meters.
Traceback (most recent call last):

  File "C:\Users\Nikolai Lund Kühne\OneDrive - Aalborg Universitet\Uni\3. semester\P3 - Dynamiske Systemer\manualRK4.py", line 57, in <module>
    a,b = RK4(model, 0, 12, x0, 1000)

  File "C:\Users\Nikolai Lund Kühne\OneDrive - Aalborg Universitet\Uni\3. semester\P3 - Dynamiske Systemer\manualRK4.py", line 53, in RK4
    y[k 1,:] = y[k,:]   h/6*(k1   2*(k2   k3)   k4)

ValueError: could not broadcast input array from shape (4,4,4) into shape (4)

Edit 2: Attempt to implement RK4 manually results in some weird errors.

Edit 1: Based on a comment the code is now implemented with solve_ivp.

CodePudding user response：

I did not completely debug this, and you could also reduce the data to a state where the expected happens. So some speculation.

Numpy is halping in the style of Matlab. The constructed format of K is an array in the shape of a row vector, [[K1,K2,K3,K4]]. Now the matrix-vector multiplication in any form, K@x, has a one-dimensional result. Mathematically, one would expect either a scalar or a 1x1 matrix [[u1]]. Following the Matlab philosophy it is neither, it is a simple array u=[u1]. Any further scalar operation that has u inside will also result in 1-element arrays. Putting the derivatives together, this has the effect of producing a column vector. Now further operations with arrays have the potential to broadcast that to a 4x4 matrix-shaped array. How the 4x4x4 shaped tensor occurs I did not follow-up on, but it seems quite possible.