01. Training For New Users » 1_02 Simplify Tutorial of Python

# -*- coding: utf-8 -*-

"""

Created on Thu Mar 23 02:05:44 2017

RK4 Method

University of Sao Paulo

@author: Gabriel

"""

def rk4(ini,end,step,x0,u0,P,deltap,u,j):

    from numpy import *

    import copy

    t = ini

    x = x0.T

#    print x

#    x = array([[P[6]],[P[7]],[x[2,0]]])

#    print x

    p = copy.copy(P)

    if j != -1:

        p[j] += deltap[j]

    A,B,C,D = Matrix(p, x0,u0)

    y = dot(C,x) + dot(D,u)

    output = append(t, y.T)

    while t < end:

        K1 = step*(dot(A,x) + dot(B,u))

        K2 = step*(dot(A,x + K1) + dot(B,u)) #x.T é a transposta de x

        K3 = step*(dot(A,x + K2) + dot(B,u))

        K4 = step*(dot(A,x + K3) + dot(B,u))

        t += step

        x = x + (K1 + 2*K2 + 2*K3 + K4)/6

        y = dot(C,x) + dot(D,u)

#        if t == ini + step:

#            print "\n\nC*x\n"

#            print C, x

#            print dot(C,x)

#            print "\n\nD*u\n"

#            print D, u

#            print dot(D,u)

#            print "\n\ny\n"

#            print y

        output = vstack((output, append(t, y.T)))

    return output