A method of simulating (numerical analysis / calculation) of a physical model in the form of an ordinary differential equation such as a flight simulator using python. What I'm doing is using odeint in Scipy.integrate. It seems that it uses Fortran Odepack lsode, so the calculation is fast.
As an example, let's port the flight simulation of an airplane published as Octave (Matlab compatible) code to python. The link is detailed for the contents of the calculation. Butterfly_Effect( ) 6DOF Flight Simulation
If you use for to simulate something that moves over time, even a simple numerical calculation method such as the Euler method is quite slow in python, so you should use a library such as scipy as much as possible.
Note that when porting from matlab, the * operator of the matrix (ndarray) is different from matlab: inner product and numpy: element product. Also, note that in python, if the decimal point is not specified, integer division will occur.
# -*- coding: utf-8 -*-
import numpy as np
from scipy.integrate import odeint
from scipy.linalg import block_diag
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
plt.close('all')
# import time
# start = time.time()
def Rotation_X(Psi):
R_x = np.array([[np.cos(Psi), np.sin(Psi), 0],
[-np.sin(Psi), np.cos(Psi), 0],
[0, 0, 1]])
return R_x
def Rotation_Y(Theta):
R_y = np.array([[np.cos(Theta), 0, -np.sin(Theta)],
[0, 1, 0],
[np.sin(Theta), 0, np.cos(Theta)]])
return R_y
def Rotation_Z(Phi):
R_z = np.array([[1, 0, 0],
[0, np.cos(Phi), np.sin(Phi)],
[0, -np.sin(Phi), np.cos(Phi)]])
return R_z
#Equation of motion
def dynamical_system(x, t, A, U0):
# x = [u,alpha,q,theta,beta,p,r,phi,psi,x,y,z]
dx = A.dot(x)
u = x[0]+U0 #speed
UVW = np.array([u, u*x[4], u*x[1]]) #Velocity vector[U,V,W]
Rotation = Rotation_X(-x[8]).dot(Rotation_Y(-x[3])).dot(Rotation_Z(-x[7]))
dX = Rotation.dot(UVW)
dx[9] = dX[0]
dx[10] = dX[1]
dx[11] = dX[2]
return dx
#Dimensional stability fine coefficient
Xu = -0.01; Zu = -0.1; Mu = 0.001
Xa = 30.0; Za = -200.0; Ma = -4.0
Xq = 0.3; Zq = -5.0; Mq = -1.0
Yb = -45.0; Lb_= -2.0; Nb_= 1.0
Yp = 0.5; Lp_= -1.0; Np_= -0.1
Yr = 3.0; Lr_= 0.2; Nr_=-0.2
#Other parameters
W0 = 0.0; U0 = 100.0; theta0 = 0.05
g = 9.8 #Gravitational acceleration
#Vertical system
A_lat = np.array([[Xu, Xa, -W0, -g*np.cos(theta0)],
[Zu/U0, Za/U0, (U0+Zq)/U0, -g*np.sin(theta0)/U0],
[Mu, Ma, Mq, 0],
[0, 0, 1, 0]])
#Lateral / directional system
A_lon = np.array([[Yb, (W0+Yp), -(U0-Yr), g*np.cos(theta0), 0],
[Lb_, Lp_, Lr_, 0, 0],
[Nb_, Np_, Nr_, 0, 0],
[0, 1, np.tan(theta0), 0, 0],
[0, 0, 1/np.cos(theta0), 0, 0]])
#Combine systems as diagonal blocks
A = block_diag(A_lat, A_lon)
#In addition, secure space for the flight trajectory
A = block_diag(A, np.zeros([3,3]))
#Setting calculation conditions
endurance = 200 #Flight time[sec]
step = 10 # 1.0[sec]Number of time steps per
t = np.linspace(0,endurance,endurance*step)
#Initial value x0= [u,alpha,q,theta, beta,p,r,phi,psi]
#Variables to be included in ordinary differential equations must be one-dimensional.
x0_lat = np.array([10, 0.1, 0.4, 0.2]) #Vertical initial value
x0_lon = np.array([0.0, 0.6, 0.4, 0.2, 0.2]) #Horizontal / directional initial values
x0_pos = np.array([0, 0, -1000]) #Initial position of the plane
x0 = np.hstack((x0_lat, x0_lon, x0_pos))
x = odeint(dynamical_system, x0, t, args=(A,U0,))
print u"run successfully."
#Visualization
plt.ion()
fig = plt.figure()
ax = Axes3D(fig)
ax.plot(x[:,9], x[:,10], x[:,11])
ax.set_xlabel(u"x");ax.set_ylabel(u"y");ax.set_zlabel(u"z")
ax.set_xlim([-5000,2000])
ax.set_ylim([-2000,5000])
ax.set_zlim([-5000,2000])
plt.show()
In scipy.integrate, there is not only odeint but also a general-purpose interface for numerical calculation of ordinary differential equations called ode, which is made object-oriented. Unlike odeint, you can specify the calculation method, so if you want to decide the contents of the calculation, this is good. Also, it seems possible to finish the calculation in the middle. However, since the calculation is performed while while, the calculation speed is quite slow depending on the number of steps. It was about 10 times slower in my environment. Also, note that the order of the arguments of the function of the differential equation is reversed from that of (time, variable) and odeint, and that it must be the argument of the tuple.
# -*- coding: utf-8 -*-
import numpy as np
from scipy.integrate import odeint
from scipy.integrate import ode
from scipy.linalg import block_diag
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
plt.close('all')
def Rotation_X(Psi):
R_x = np.array([[np.cos(Psi), np.sin(Psi), 0],
[-np.sin(Psi), np.cos(Psi), 0],
[0, 0, 1]])
return R_x
def Rotation_Y(Theta):
R_y = np.array([[np.cos(Theta), 0, -np.sin(Theta)],
[0, 1, 0],
[np.sin(Theta), 0, np.cos(Theta)]])
return R_y
def Rotation_Z(Phi):
R_z = np.array([[1, 0, 0],
[0, np.cos(Phi), np.sin(Phi)],
[0, -np.sin(Phi), np.cos(Phi)]])
return R_z
#Equation of motion
#Unlike the case of odeint(time, x, args*)Note that args are tuples
def dynamical_system(t, x, (A, U0)):
# x = [u,alpha,q,theta,beta,p,r,phi,psi,x,y,z]
dx = A.dot(x)
u = x[0]+U0 #speed
UVW = np.array([u, u*x[4], u*x[1]]) #Velocity vector[U,V,W]
Rotation = Rotation_X(-x[8]).dot(Rotation_Y(-x[3])).dot(Rotation_Z(-x[7]))
dX = Rotation.dot(UVW)
dx[9] = dX[0]
dx[10] = dX[1]
dx[11] = dX[2]
return dx
#Dimensional stability fine coefficient
Xu = -0.01; Zu = -0.1; Mu = 0.001
Xa = 30.0; Za = -200.0; Ma = -4.0
Xq = 0.3; Zq = -5.0; Mq = -1.0
Yb = -45.0; Lb_= -2.0; Nb_= 1.0
Yp = 0.5; Lp_= -1.0; Np_= -0.1
Yr = 3.0; Lr_= 0.2; Nr_=-0.2
#Other parameters
W0 = 0.0; U0 = 100.0; theta0 = 0.05
g = 9.8 #Gravitational acceleration
#Vertical system
A_lat = np.array([[Xu, Xa, -W0, -g*np.cos(theta0)],
[Zu/U0, Za/U0, (U0+Zq)/U0, -g*np.sin(theta0)/U0],
[Mu, Ma, Mq, 0],
[0, 0, 1, 0]])
#Lateral / directional system
A_lon = np.array([[Yb, (W0+Yp), -(U0-Yr), g*np.cos(theta0), 0],
[Lb_, Lp_, Lr_, 0, 0],
[Nb_, Np_, Nr_, 0, 0],
[0, 1, np.tan(theta0), 0, 0],
[0, 0, 1/np.cos(theta0), 0, 0]])
#Combine systems as diagonal blocks
A = block_diag(A_lat, A_lon)
#In addition, secure space for the flight trajectory
A = block_diag(A, np.zeros([3,3]))
#Setting calculation conditions
endurance = 200 #Flight time[sec]
step = 10 # 1.0[sec]Number of time steps per
dt = 1.0 / step #Width of time step[sec]
t0 = 0.0 #Start time[sec]
t = np.linspace(0,endurance,endurance*step)
#Initial value x0= [u,alpha,q,theta, beta,p,r,phi,psi,x,y,z]
#Variables to be included in ordinary differential equations must be one-dimensional.
x0_lat = np.array([10, 0.1, 0.4, 0.2]) #Vertical initial value
x0_lon = np.array([0.0, 0.6, 0.4, 0.2, 0.2]) #Horizontal / directional initial values
x0_pos = np.array([0, 0, -1000]) #Initial position of the plane
x0 = np.hstack((x0_lat, x0_lon, x0_pos))
#ODE settings
solver = ode(dynamical_system).set_integrator('vode', method='bdf')
solver.set_initial_value(x0, t0)
solver.set_f_params((A, U0))
dt = 1.0 / step
x = np.zeros([int(endurance/dt)+1, 12])
t = np.zeros([int(endurance/dt)+1])
index = 0
while solver.successful() and solver.t < endurance:
solver.integrate(solver.t+dt)
x[index] = solver.y
t[index] = solver.t
index += 1
print u"run successfully."
#Visualization
plt.ion()
fig = plt.figure()
ax = Axes3D(fig)
ax.plot(x[:,9], x[:,10], x[:,11])
ax.set_xlim([-5000,2000])
ax.set_ylim([-2000,5000])
ax.set_zlim([-5000,2000])
plt.show()
Recommended Posts