Range-Kutta Integration

RK4

• Saw from here https://web.casadi.org/blog/ocp/ while learning CasADI

for k in range(N):
    k1 = f(X[:, k], U[:, k])
    k2 = f(X[:, k] + dt/2 * k1, U[:, k])
    k3 = f(X[:, k] + dt/2 * k2, U[:, k])
    k4 = f(X[:, k] + dt * k3, U[:, k])
    x_next = X[:, k] + dt/6 * (k1 + 2*k2 + 2*k3 + k4)
    opti.subject_to(X[:, k+1] == x_next)  # Close the gaps

Simpler is to just use Euler Integration method

for k in range(N):
    opti.subject_to(X[:, k+1] == X[:, k] + f(X[:, k], U[:, k]) * dt)

Trey was saying RK2 is probs fine, but RK4 might run into some issues.