I'm sorry for my ignorance in using NumPy and python. I'm much more comfortable in Matlab however I've been required to convert my code from Matlab to Python. To run my script in Python takes 113 seconds meanwhile it takes 7 seconds in MATLAB. Both produce correct results albeit different due to stochasticity. The big culprit I believe is my derivative function. I profiled my python code and it says that this function accounts for 76% of the run time (It doesn't take up nearly as much percent-wise in MATLAB). I understand there is a lot of code so if anyone could help me with making my python derivative code perform more similarly to MATLAB that would be greatly appreciated! If you have any questions feel free to ask. Thank you!
Derivative6DOF.py
## Derivative6DOF Variables
# Input Variables
# x - vehicle state = [x, y, z, u, v, w, phi, theta, psi, p, q, r, omega1, omega2, omega3, omega4]'
# dt - time step
# bigOmegasTarget - commanded rotor speeds = [omega1, omega2, omega3, omega4]'
# batterySOC - percent battery charge remaining
# m - mass of vehicle
# g - gravitational acceleration constant
# I - moment of inertia = eye([I11, I22, I33]')
# Cd0 - Profile drag coefficient of each rotor
# pitch - pitch of blade
# rho - density of air
# a - lift curve slope of the airfoil
# b - number of blades
# c - chord length of blade
# R - total length of the rotor blade
# viGuessInitial - guess induced velocity
# posProps - position of propellers from center of gravity = [x, y, z]'
# A - area of propeller
# maxCapacity - max battery capacity
# powerDrawComponents - power drawn by components from battery
# Kv - voltage coefficient of motor [rad/sec/V]
# Kt - motor torque coefficent [N*m/A]
# voltMax - max voltage supplied
# resistanceMotor - resistance of motor
# PeMax - max power possible
# Jp - inertia about each motor (includes propellor and motor) [kg*m^2]
# Output Variables
# xdot - vehicle state derivative = [u, v, w, udot, vdot, zdot, p, q, r, pdot, qdot, rdot, omegadot1 omegadot2 omegadot3 omegadot4]'
# battery - battery state = [percentRemaining, dischargeRate]'
# k - thrust-to-omega coefficient (https://andrew.gibiansky.com/blog/physics/quadcopter-dynamics/#:~:text=Each#20rotor#20contributes#20some#20torque,CDAv2.)
# b_coeff - yaw-to-omega coefficient (https://andrew.gibiansky.com/blog/physics/quadcopter-dynamics/#:~:text=Each#20rotor#20contributes#20some#20torque,CDAv2.)
# Intermediate Variables
# DCM - rotation matrix from body frame to inertial frame
# UVWp - translational velocity at each propeller = [u, v, w]'
# theta0 - root blade angle
# theta1 - linear twist of blade
# T - thrust
# vi - induced velocity iterated guess
# vPrime - effective speed of flow at the rotor
# viCalc - induced velocity calculated
# Pinduced - induced power
# Pprofile - profile power
# Qp - rotor aerodynamic torque (current motor torque)
# totalThrust - sum of each rotor thrust
# QeMax - max electric motor torque possible
# PeMax - max electric power available
# Qe - electric motor torque (desired motor torque)
# ind - index
# Maero - aerodynamic pitching, rolling and yawing moments = [x, y, z]'
# H - hub forces = [x, y, z]'
# Mgyro - gyroscopic moments = [x, y, z]'
# Cl - coefficient of lift
# AR - aspect ratio
# Cd - coefficient of drag
# lx - distance of props in x-direction
# ly - distance of props in y-direction
# LScale - pitch scalar coefficient
# MScale - roll scalar coefficient
# NScale - yaw scalar coefficient
# MThrust - differential thrust = [x, y, z]'
# F - sum of all forces acting on vehicle
# M - sum of all moments acting on vehicle
# powerDrawMotors - sum of all power draw from all 4 motors
# totalPowerDraw - total power load on battery from all components (motors, camera, electronics, etc.)
# maxBatteryPower = maxCapacity*voltMax/1000; # (Whr) Max possible power from battery at start
# State Element Vector
# x - x-position
# y - y-position
# z - z-position
# u - x-velocity
# v - y-velocity
# w - z-velocity
# udot - x-acceleration
# vdot - y-acceleration
# wdot - z-acceleration
# phi - angle about y-axis
# theta - angle about x-axis
# psi - angle about z-axis
# p - angular velocity about y-axis
# q - angular velocity about x-axis
# r - angular velocity about z-axis
# pdot - angular acceleration about y-axis
# qdot - angular acceleration about x-axis
# rdot - angular acceleration about z-axis
# omega1 - angular speed of front left rotor
# omega2 - angular speed of back right rotor
# omega3 - angular speed of front right rotor
# omega4 - angular speed of back left rotor
# omegadot1 - angular acceleration of front left rotor
# omegadot2 - angular acceleration of back right rotor
# omegadot3 - angular acceleration of front right rotor
# omegadot4 - angular acceleration of back left rotor
# percentRemaining - percent battery charge remaining
# dischargeRate - discharge rate of battery
# Additional Notes
# Mgyro Derivation Code (Page 639)
# syms p q r Omega Jp
# omega = [0; 0; Jp*Omega]; rotation about positive z-axis
# skewMatrix = [0 -r q; r 0 -p; -q p 0];
# Mgyro = -skewMatrix*omega
import numpy as np
def Derivative6DOF(x,dt,bigOmegasTarget,batterySOC,m,g,I,Cd0,pitch,rho,a,b,c,R,viGuessInitial,posProps,A,maxCapacity,powerDrawComponents,Kv,Kt,voltMax,resistanceMotor,PeMax,Jp,processNoise,theta0,theta1):
# direction cosine matrix (from bodyframe to NED)
DCM = np.array([[np.cos(x[8]) * np.cos(x[7]),- np.sin(x[8]) * np.cos(x[6]) + np.cos(x[8]) * np.sin(x[7]) * np.sin(x[6]),np.sin(x[8]) * np.sin(x[6]) + np.cos(x[8]) * np.cos(x[6]) * np.sin(x[7])],[np.cos(x[7]) * np.sin(x[8]),np.cos(x[8]) * np.cos(x[6]) + np.sin(x[8]) * np.sin(x[7]) * np.sin(x[6]),- np.cos(x[8]) * np.sin(x[6]) + np.sin(x[8]) * np.cos(x[6]) * np.sin(x[7])],[- np.sin(x[7]),np.sin(x[6]) * np.cos(x[7]),np.cos(x[7]) * np.cos(x[6])]])
# propeller velocity
UVWp = np.zeros((4,3))
for i in range(0,4):
UVWp[i,:] = x[3:6] + np.cross(x[9:12],posProps[i,:])
T = np.zeros((4,))
vi = np.ones((4,)) * viGuessInitial
# induced velocity for each rotor
for i in range(0,4):
# induced velocity
vPrime = np.sqrt(UVWp[i,0] ** 2 + UVWp[i,1] ** 2 + (UVWp[i,2] - vi[i]) ** 2)
T[i] = rho * a * b * c * R / 4 * ((UVWp[i,2] - vi[i]) * x[12 + i] * R + 2 / 3 * (x[12 + i] * R) ** 2 * (theta0 + 0.75 * theta1) + (UVWp[i,0] ** 2 + (UVWp[i,1] ** 2)) * (theta0 + 0.5 * theta1))
viCalc = T[i] / (2 * rho * A * vPrime)
# converge iterated induced velocity and calculated induced velocity
while np.abs(vi[i] - viCalc) > 1e-05:
# induced velocity
vPrime = np.sqrt(UVWp[i,0] ** 2 + UVWp[i,1] ** 2 + (UVWp[i,2] - vi[i]) ** 2)
T[i] = rho * a * b * c * R / 4 * ((UVWp[i,2] - vi[i]) * x[12 + i] * R + 2 / 3 * (x[12 + i] * R) ** 2 * (theta0 + 0.75 * theta1) + (UVWp[i,0] ** 2 + (UVWp[i,1] ** 2)) * (theta0 + 0.5 * theta1))
viCalc = T[i] / (2 * rho * A * vPrime)
# increment the induced velocity guess
vi[i] = vi[i] + 0.1 * (viCalc - vi[i])
# calculate induced power, profile power, and rotor aerodynamic torque for each rotor
Pinduced = T*(vi-UVWp[:,2])
Pprofile = (rho*Cd0*b*c*x[12:16]*R**2/8 * ((x[12:16]*R)**2 + UVWp[:,0]**2 + UVWp[:,1]**2))
Qp = 1/x[12:16]*(Pinduced + Pprofile)
# calculate total thrust
totalThrust = np.array([[0],[0],[-np.sum(T)]]).reshape((3,))
# calculate electric motor torque
QeMax = PeMax / x[12:16]
Qe = (Kt / resistanceMotor) * (bigOmegasTarget - x[12:16]) / Kv
# confirm electric motor torque isn't exceeding limit
np.where(Qe > QeMax, Qe, QeMax)
# calculate aerodynamic pitching, rolling, and yawing moments
Maero = np.zeros((3,))
Maero[0] = np.sum(np.array([[- 1],[- 1],[1],[1]]) * rho * a * b * c * R**2 * (x[12:16] * R**2 / 16 * x[9] + ((UVWp[:,2] - vi) / 8 + x[12:16] * R * theta0 / 6 + x[12:16] * R * theta1 / 8) * UVWp[:,0]))
Maero[1] = np.sum(np.array([[- 1],[- 1],[1],[1]]) * rho * a * b * c * R**2 * (x[12:16] * R**2 / 16 * x[10] + ((UVWp[:,2] - vi) / 8 + x[12:16] * R * theta0 / 6 + x[12:16] * R * theta1 / 8) * UVWp[:,1]))
Maero[2] = np.sum(np.array([[- 1],[- 1],[1],[1]]) * Qe)
# calculate hub force
H = -np.sign(x[3:6]) * np.sum(rho * Cd0 * b * c * x[12:16] * R**2 / 4 * np.array([UVWp[:,0],UVWp[:,1],np.array([[0],[0],[0],[0]]).reshape(4,)]))
# calculate gyroscopic moments
Mgyro = np.sum(x[12:16] * np.array([[-1],[-1],[1],[1]]).reshape(4,)) * Jp * np.array([-x[10], x[9], 0]).reshape((3,))
# calculate thrust-to-omega coefficient
k = T / x[12:16]**2
# calculate yaw-to-omega coefficient
Cl = 2 * T / (rho * R**2 * x[12:16]**2)
AR = R**2 / A
Cd = Cd0 + Cl**2 / (np.pi * AR * 1)
b_coeff = 0.5 * R ** 3 * rho * Cd * A
# calculate motor-mixer scalar coefficients
lx = np.abs(posProps[:,0])
ly = np.abs(posProps[:,1])
LScale = k * ly
MScale = k * lx
NScale = b_coeff
# calculate differential thrust
MThrust = np.zeros((3,))
MThrust[0] = np.sum(x[12:16]**2 * np.array([1,-1,-1,1]).reshape((4,)) * LScale)
MThrust[1] = np.sum(x[12:16]**2 * np.array([1,-1,1,-1]).reshape((4,)) * MScale)
MThrust[2] = np.sum(x[12:16]**2 * np.array([-1,-1,1,1]).reshape((4,)) * NScale)
# total force and moment acting on quadcopter in inertial frame
F = DCM @ (totalThrust + H)
M = DCM @ (Maero + Mgyro + MThrust)
# calculate state derivative
xdot = np.zeros((16,))
xdot[0:3] = np.array([x[3],x[4],x[5]])
xdot[3] = F[0] / m
xdot[4] = F[1] / m
xdot[5] = g + F[2] / m
xdot[6] = x[9] + np.tan(x[7]) * np.sin(x[6]) * x[10] + np.tan(x[7]) * np.cos(x[6]) * x[11]
xdot[7] = np.cos(x[6]) * x[10] - np.sin(x[6]) * x[11]
xdot[8] = 1/np.cos(x[7]) * np.sin(x[6]) * x[10] + 1/np.cos(x[7]) * np.cos(x[6]) * x[11]
xdot[9] = (M[0] + x[10] * x[11] * (I[1,1] - I[2,2])) / I[0,0]
xdot[10] = (M[1] + x[9] * x[11] * (I[2,2] - I[0,0])) / I[1,1]
xdot[11] = (M[2] + x[9] * x[10] * (I[0,0] - I[1,1])) / I[2,2]
xdot[12:16] = 1/Jp * (Qe - Qp)
xdot = xdot + np.random.randn(16,) * processNoise
# calculate battery state
powerDrawMotors = np.sum(Qp * x[12:16])
totalPowerDraw = powerDrawMotors + powerDrawComponents
maxBatteryPower = maxCapacity * voltMax / 1000
discharge = totalPowerDraw * dt / 3600 / maxBatteryPower * 100
batterySOC = batterySOC - discharge
battery = np.array([[batterySOC],[discharge]]).reshape((2,))
return xdot,battery,k,b_coeff,vi
