Related
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
I'm trying to solve a set of transient flow equations of a body firing from a pressurised tube
from scipy.integrate import odeint
import numpy as np
# parameters
Db = 0.5 # body diameter
Lb = 2.0 # body length
mb = 1000 # body mass
Ai = 0.5 # inlet area
Ki = 1.0 # inlet loss coefficient
Li = 1.5 # inlet position
Dt = 0.6 # tube diameter
Lt = 3.0 # tube length
epsilon = 0.0001 # roughness
rho = 1000 # density
mu = 0.0011 # dynamic viscosity
par = [Db, Lb, mb, Ai, Ki, Li, Dt, Lt, epsilon, rho, mu]
# boundary conditions
pp = 5e5 # pump pressure
patm = 0.0 # atmosphere pressure
bound = [pp, patm]
# time
ts = 0.001 # time step
tmax = 2.0 # total time
t = np.linspace(0, tmax, int(tmax/ts) + 1)
# initial conditions
v0 = 0.0 # initial velocity
s0 = 0.5 # initial displacement
Qlr0 = 0.0 # initial leakage to rear
Qlf0 = 0.0 # initial leakage to front
Qf0 = 0.0 # initial front flow
initVar = [v0, s0, Qlr0, Qlf0, Qf0]
# friction factor calculation
def ff(Q, A, L):
D = 2.0 * (A / np.pi)**0.5 # hydraulic diameter
Re = (rho * (Q / A) * L) / mu # reynolds number
ff = 0.25 / (np.log10((epsilon / D)+(5.74 / Re**0.9)))**2.0 # friction factor
return ff
# main calculation
def calc_main(var, t, val):
par, bound = val
Db, Lb, mb, Ai, Ki, Li, Dt, Lt, epsilon, rho, mu = par
pp, patm = bound
vb, sb, Qlr, Qlf, Qf = var
Ab = np.pi * (Db / 2.0)**2.0 # body area
At = np.pi * (Dt / 2.0)**2.0 # tube area
Alr = At - Ab # rear leak area
Alf = At - Ab # front leak area
Af = At # front flow area
Dlr = 2.0 * (Alr / np.pi)**0.5 # hydraulic diameter of rear leak
Dlf = 2.0 * (Alf / np.pi)**0.5 # hydraulic diameter of front leak
Df = Dt # hydraulic diameter of front flow
Lr = sb # rear position
Lf = Lr + Lb # front position
Llr = max(0.0, Li - Lr) # length of rear leak
Llf = min(Lt - Li, Lf - Li) # length of front leak
Lf = max(0.0, Lt - Lf) # length of front flow
Qi = (1 / Ki)**(0.5) * Ab * ((2 * (pp - pi)) / rho)**(0.5) # inlet flow rate
dQlrdt = Alr / (rho * Llr) * (pi - pr) * ((ff(Qlr, Alr, Llr) * Qlr**2.0) / (2 * Dlr * Alr)) # rear leak derivative
dQlfdt = Alf / (rho * Llf) * (pi - pf) * ((ff(Qlf, Alf, Llf) * Qlf**2.0) / (2 * Dlf * Alf)) # front leak derivative
dQfdt = Af / (rho * Lf) * (pr - patm) * ((ff(Qf, Af, Lf) * Qf**2.0) / (2 * Df * Af)) # front flow derivative
Qf = Qi # flow continuity
Qf = Qlr + Qlf # flow continuity
Qlr = Ab * vb # flow continuity
Fb = (pr - pf) * (pi * (Db / 2.0) ** 2.0) # force on body
ab = Fb / mb # body acceleration
dvbdt = ab # body velocity derivative
dsbdt = vb # body displacement derivative
return dvbdt, dsbdt, dQlrdt, dQlfdt, dQfdt
# solve
res = odeint(calc_main, initVar, t, args=([par, bound],))
If I take out the flow equations and have pr and pf equal to pp and patm respectively, then the motion equations work fine (results below), but with the flow equations as above it complains that the pressures are undefined (understandably, I have nothing saying pf=[] or pr=[])
What I've attempted is to use a combination of fsolve and odeint to solve the simultaneous flow equations, but also solve the ODEs, to little success
from scipy.optimize import fsolve
# main calculation
def calc_main(var, t, val):
par, bound = val
Db, Lb, mb, Ai, Ki, Li, Dt, Lt, epsilon, rho, mu = par
pp, patm = bound
vb, sb, Qlr, Qlf, Qf = var
Ab = np.pi * (Db / 2.0)**2.0 # body area
At = np.pi * (Dt / 2.0)**2.0 # tube area
Alr = At - Ab # rear leak area
Alf = At - Ab # front leak area
Af = At # front flow area
Dlr = 2.0 * (Alr / np.pi)**0.5 # hydraulic diameter of rear leak
Dlf = 2.0 * (Alf / np.pi)**0.5 # hydraulic diameter of front leak
Df = Dt # hydraulic diameter of front flow
Lr = sb # rear position
Lf = Lr + Lb # front position
Llr = max(0.0, Li - Lr) # length of rear leak
Llf = min(Lt - Li, Lf - Li) # length of front leak
Lf = max(0.0, Lt - Lf) # length of front flow
def calc_fluid (var):
Qi, dQlrdt, dQlfdt, dQfdt, pi, pr, pf = var
eq1 = (1 / Ki)**(0.5) * Ab * ((2 * (pp - pi)) / rho)**(0.5) - Qi # inlet flow rate
eq2 = Alr / (rho * Llr) * (pi - pr) * ((ff(Qlr, Alr, Llr) * Qlr**2.0) / (2 * Dlr * Alr)) - dQlrdt # rear leak derivative
eq3 = Alf / (rho * Llf) * (pi - pf) * ((ff(Qlf, Alf, Llf) * Qlf**2.0) / (2 * Dlf * Alf)) - dQlfdt # front leak derivative
eq4 = Af / (rho * Lf) * (pr - patm) * ((ff(Qf, Af, Lf) * Qf**2.0) / (2 * Df * Af)) - dQfdt # front flow derivative
eq5 = Qi - Qf # flow continuity
eq6 = Qlr + Qlf - Qf # flow continuity
eq7 = Ab * vb - Qlr # flow continuity
return eq1, eq2, eq3, eq4, eq5, eq6, eq7
Qi, dQlrdt, dQlfdt, dQfdt, pi, pr, pf = fsolve(calc_fluid, [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
Fb = (pr - pf) * (pi * (Db / 2.0) ** 2.0) # force on body
ab = Fb / mb # body acceleration
dvbdt = ab # body velocity derivative
dsbdt = vb # body displacement derivative
return dvbdt, dsbdt, dQlrdt, dQlfdt, dQfdt
I get the error "RuntimeWarning: invalid value encountered in double_scalars" for a few different lines while it runs, but I don't get told specifically which time step that happens at
I'm not convinced that the results I get are right, as I seem to now not move the body at all
Can anyone see what I've done wrong, or know of a better way to solve the simultaneous equations alongside the derivatives?
I've been shooting in the dark a little having not done anything quite like this before so it's possible I've gone completely off the rails with how these functions are meant to be used, any explanation is much appreciated!
Your example does has some undefined variables, so I didn't try it here.
What you have to do is to write the constraints dependent variables in a separate function and then use solve_bvp to integrate the differential equation with the boundary conditions.
The bc function is evaluated with the ya and yb being the value of your dependent variables at the beginning and the end of your interval. It will return an array of values that are 0 for the expected solution, and non-zero for other solutions.
Note
Also, you may like to know that
has a closed form solution
I am what varies in your equations, but the coefficients of your differential equations for Q then you can use the closed form, and solving the boundary conditions for the free parameters.
I have defined the following functions in python:
from math import *
import numpy as np
import cmath
def BSM_CF(u, s0, T, r, sigma):
realp = -0.5*u**2*sigma**2*T
imagp = u*(s0+(r-0.5*sigma**2)*T)
zc = complex(realp, imagp)
return cmath.exp(zc)
def BSM_characteristic_function(v, x0, T, r, sigma):
cf_value = np.exp(((x0 / T + r - 0.5 * sigma ** 2) * 1j * v -
0.5 * sigma ** 2 * v ** 2) * T)
return cf_value
Parameters:
alpha = 1.5
K = 90
S0 = 100
T = 1
r = 0.05
sigma = 0.2
k = np.log(K / S0)
s0 = np.log(S0 / S0)
g = 1 # factor to increase accuracy
N = 2 ** 2
eta = 0.15
eps = (2*np.pi)/(N*eta)
b = 0.5 * N * eps - k
u = np.arange(1, N + 1, 1)
vo = eta * (u - 1)
v = vo - (alpha + 1) * 1j
BSMCF = BSM_characteristic_function(v, s0, T, r, sigma)
BSMCF_v2 = BSM_CF(0, s0, T, r, sigma)
print(BSMCF)
print(BSMCF_v2)
Both are the same functions. But, I get different results. How can I fix the function BSM_CF to get the same result from the function BSM_characteristic_function? The idea is get an array with len 4 values as in the funtion BSM_characteristic_function
Your calls are not identical. You are passing v in the first call and 0 in the second call. If I pass 0 for both, the results are identical. If I pass v, it complains because you can't call complex on a vector.
Numeric computation is Not always identical to symbolic algebra. For the first formula, you use complex computation as an alternative, which could result rounding errors in complex part. I came across such mistakes quite often as I used Mathematica, which loves to transfer a real formula to a complex one before doing the numeric computation.
Apologies for the (maybe misleading) title and the probably confusing question itself, i struggle a lot with wording my problem and especially compressing it into one sentence for the title. I want to find the roots of a function f(w, t, some_other_args) with two variables, w and t, using python. The real function structure is really long and complicated, you can find it on the end of this post. The important thing is that it contains the following line:
k = 1.5 * m.sqrt((1.0 - w) / (1.0 - 0.25 * w))
This means that w can't exceed 1, because that would lead to calculating the square root of a negative number, which, of course, is impossible. I have algorithms for calculating the approximate values of w and t using other values in my function, but they are very inaccurate.
So, i try to calculate the roots with scipy.optimize.fsolve (after trying literally every root finding algorithm i could find online, i found this one to be the best for my function) using these approximate values as starting points, which would look like this:
solution = optimize.fsolve(f, x0=np.array([t_approx, w_approx]), args=(some_other_args))
For most values, this works perfectly fine. If w is too close to 1, however, there always comes a point when fsolve tries some value bigger than 1 for w, which, in turn, raises a ValueError(because calculating the root of a negative number is mathematically impossible). This is an example printing out the values that fsolveis using, where w should be somewhere around 0.997:
w_approx: 0.9960090844989311
t_approx: 24.26777844720981
Values: t:24.26777844720981, w:0.9960090844989311
Values: t:24.26777844720981, w:0.9960090844989311
Values: t:24.26777844720981, w:0.9960090844989311
Values: t:24.267778808827888, w:0.9960090844989311
Values: t:24.26777844720981, w:0.996009099340623
Values: t:16.319554685876746, w:1.0096680915775516
solution = optimize.fsolve(f, x0=np.array([t_approx, w_approx]), args=(some_other_args))
File "C:\Users\...\venv\lib\site-packages\scipy\optimize\minpack.py", line 148, in fsolve
res = _root_hybr(func, x0, args, jac=fprime, **options)
File "C:\Users\...\venv\lib\site-packages\scipy\optimize\minpack.py", line 227, in _root_hybr
ml, mu, epsfcn, factor, diag)
File "C:\Users\...\algorithm.py", line 9, in f
k = 1.5 * m.sqrt((1.0 - w) / (1.0 - 0.25 * w))
ValueError: math domain error
So, how can i tell optimize.fsolve that w can't get bigger than 1? Or what are alternative algorithms for doing something like this (i know about brentq and so on, but all of those require giving an interval for both roots, which i don't want to do.)?
Code for testing (What's important to note here: even though func theoretically is supposed to calculate R and T given t and w, i have to use it the other way around. It's a bit clunky, but i simply don't manage to rewrite the function so that it accepts T, R to calculate t, w - it's a bit too much for my mediocre mathematical expertise ;)) :
import math as m
from scipy import optimize
import numpy as np
def func(t, w, r_1, r_2, r_3):
k = 1.5 * m.sqrt((1.0 - w) / (1.0 - 0.25 * w))
k23 = 2 * k / 3
z1 = 1 / (1 + k23)
z2 = 1 / (1 - k23)
z3 = 3 * ((1 / 5 + r_1 - r_2 - 1 / 5 * r_1 * r_2) / (z1 - r_2 * z2)) * m.exp(t * (k - 1))
z4 = -(z2 - r_2 * z1) / (z1 - r_2 * z2) * m.exp(2 * k * t)
z5 = -(z1 - r_2 * z2) / (z2 - r_2 * z1)
z6 = 3 * (1 - r_2 / 5) / (z2 - r_2 * z1)
beta_t = r_3 / (z2 / z1 * m.exp(2 * k * t) + z5) * (z6 - 3 / (5 * z1) * m.exp(t * (k - 1)))
alpha_t = beta_t * z5 - r_3 * z6
beta_r = (z3 - r_1 / 5 / z2 * m.exp(-2 * t) * 3 - 3 / z2) / (z1 / z2 + z4)
alpha_r = -z1 / z2 * beta_r - 3 / z2 - 3 / 5 * r_1 / z2 * m.exp(-2 * t)
It_1 = 1 / 4 * w / (1 - 8 / 5 * w) * (alpha_t * z2 * m.exp(-k * t) + beta_t * z1 * m.exp(k * t) + 3 * r_3 * m.exp(-t))
Ir_1 = (1 / 4 * w / (1 - 8 / 5 * w)) * (z1 * alpha_r + z2 * beta_r + 3 / 5 + 3 * r_1 * m.exp(-2 * t))
T = It_1 + m.exp(-t) * r_3
R = Ir_1 + m.exp(-2 * t) * r_1
return [T, R]
def calc_1(t, w, T, R, r_1, r_2, r_3):
t_begin = float(t[0])
T_new, R_new = func(t_begin, w, r_1, r_2, r_3)
a = abs(-1 + T_new/T)
b = abs(-1 + R_new/R)
return np.array([a, b])
def calc_2(x, T, R, r_1, r_2, r_3):
t = x[0]
w = x[1]
T_new, R_new = func(t, w, r_1, r_2, r_3)
a = abs(T - T_new)
b = abs(R - R_new)
return np.array([a, b])
def approximate_w(R):
k = (1 - R) / (R + 2 / 3)
w_approx = (1 - ((2 / 3 * k) ** 2)) / (1 - ((1 / 3 * k) ** 2))
return w_approx
def approximate_t(w, T, R, r_1, r_2, r_3):
t = optimize.root(calc_1, x0=np.array([10, 0]), args=(w, T, R, r_1, r_2, r_3))
return t.x[0]
def solve(T, R, r_1, r_2, r_3):
w_x = approximate_w(R)
t_x = approximate_t(w_x, T, R, r_1, r_2, r_3)
sol = optimize.fsolve(calc_2, x0=np.array([t_x, w_x]), args=(T, R, r_1, r_2, r_3))
return sol
# Values for testing:
T = 0.09986490557943692
R = 0.8918728343037964
r_1 = 0
r_2 = 0
r_3 = 1
print(solve(T, R, r_1, r_2, r_3))
What about logisticing the argument that you want to constrain? I mean, inside f, you could do
import numpy as np
def f(free_w, ...):
w = 1/(1 + np.exp(-free_w)) # w will always lie between 0 and 1
...
return zeros
And then, you would just have to apply the same logistic-transformation to the solution value of free_w to get w*. See
solution = optimize.fsolve(f, x0=np.array([t_approx, w_approx]), args=(some_other_args))
free_w = solution[0]
w = 1/(1 + np.exp(-free_w))
Your reported error occurs as fsolve can not deal with the implicit restrictions in the conversion of w to k. This can be solved radically by inverting that dependence, making func dependent on t and k instead.
def w2k(w): return 3 * m.sqrt((1.0 - w) / (4.0 - w))
#k = 1.5 * m.sqrt((1.0 - w) / (1.0 - 0.25 * w))
# (k/3)**2 * (4-w)= 1-w
def k2w(k): return 4 - 3/(1-(k/3)**2)
def func(t, k, r_1, r_2, r_3):
w = k2w(k)
print "t=%20.15f, k=%20.15f, w=%20.15f"%(t,k,w)
...
Then remove the absolute values from the function values in calc1 and calc2. This only renders your solutions as non-differentiable points which is bad for any root-finding algorithm. Sign changes and smooth roots are good for Newton-like methods.
def calc_2(x, T, R, r_1, r_2, r_3):
t = x[0]
k = x[1]
T_new, R_new = func(t, k, r_1, r_2, r_3)
a = T - T_new
b = R - R_new
return np.array([a, b])
It makes not much sense to find the value for t by solving the equation keeping w resp. k fixed, it just doubles the computational effort.
def approximate_k(R):
k = (1 - R) / (R + 2 / 3)
return k
def solve(T, R, r_1, r_2, r_3):
k_x = approximate_k(R)
t_x = 10
sol = optimize.fsolve(calc_2, x0=np.array([t_x, k_x]), args=(T, R, r_1, r_2, r_3))
return sol
t,k = solve(T, R, r_1, r_2, r_3)
print "t=%20.15f, k=%20.15f, w=%20.15f"%(t, k, k2w(k))
With these modifications the solution
t= 14.860121342410327, k= 0.026653140486605, w= 0.999763184675043
is found within 15 function evaluations.
You should try defining explicitly your function before optimizing it, that way you can check for domain more easily.
Essentially you have a function of T and R. this worked for me:
def func_to_solve(TR_vector, r_1, r_2, r_3):
T, R = TR_vector # what you are trying to find
w_x = approximate_w(R)
t_x = approximate_t(w_x, T, R, r_1, r_2, r_3)
return (calc_2([t_x, w_x], T, R, r_1, r_2, r_3))
def solve(TR, r_1, r_2, r_3):
sol = optimize.fsolve(func_to_solve, x0=TR, args=(r_1, r_2, r_3))
return sol
Also, replace m.exp by np.exp
Similar to many tutorials on the web, I've tried implementing a windowed-sinc lowpass filter using the following python functions:
def black_wind(w):
''' blackman window of width w'''
samps = np.arange(w)
return (0.42 - 0.5 * np.cos(2 * np.pi * samps/ (w-1)) + 0.08 * np.cos(4 * np.pi * samps/ (w-1)))
def lp_win_sinc(tw, fc, n):
''' lowpass sinc impulse response
Parameters:
tw = approximate transition width [fraction of nyquist freq]
fc = cutoff freq [fraction of nyquest freq]
n = length of output.
Returns:
s = impulse response of windowed-sinc filter appended zero-padding
to make len(s) = n
'''
m = int(np.ceil( 4./tw / 2) * 2)
samps = np.arange(m+1)
shift = samps - m/2
shift[m/2] = 1
h = np.sin(2 * np.pi * fc * shift)/shift
h[m/2] = 2 * np.pi * fc
h = h * black_wind(m+1)
h = h / h.sum()
s = np.zeros(n)
s[:len(h)] = h
return s
For input: 'tw = 0.05', 'fc = 0.2', 'n = 6000', the magnitude of the fft seems reasonable.
tw = 0.05
fc = 0.2
n = 6000
lp = lp_win_sinc(tw, fc, n)
f_lp = np.fft.rfft(lp)
plt.figure()
x = np.linspace(0, 0.5, len(f_lp))
plt.plot(x, np.abs(f_lp))
magnitude of lowpass filter response
however, the phase is non-linear above ~fc.
plt.figure()
x = np.linspace(0, 0.5, len(f_lp))
plt.plot(x, np.unwrap(np.angle(f_lp)))
phase of lowpass filter response
Given the symmetry of the non-zero-padded portion of the impulse response, I would expect the resulting phase to be linear. Can someone explain what is going on? Perhaps I'm using a numpy function incorrectly, or maybe my expectations are incorrect. I'm very grateful for any help.
***********************EDIT***********************
based on some of the helpful comments to this question and some more work, I wrote a function that produces zero phase delay and is therefore a bit easier to interpret the np.angle() results.
def lp_win_sinc(tw, fc, n):
m = int(np.ceil( 2./tw) * 2)
samps = np.arange(m+1)
shift = samps - m/2
shift[m/2] = 1
h = np.sin(2 * np.pi * fc * shift)/shift
h[m/2] = 2 * np.pi * fc
h = h * np.blackman(m+1)
h = h / h.sum()
s = np.zeros(n)
s[:len(h)] = h
return np.roll(s, -m/2)
The main change here is using np.roll() to place the line of symmetry at t=0.
The magnitudes in the stop band are crossing zero. The phase of the coefficient after a zero crossing will jump by 180 degrees, which is confusing np.angle()/np.unwrap(). -1*180° = 1*0°
The phase as shown in your graph is in fact linear. It's a constant slope in the passband, corresponding to a constant delay in the time domain. It's a much steeper slope, which renders as wrapping around at 2pi boundaries, in the stopband. But the value of the phase in the stopband is not particularly important since those frequencies aren't going to come through the filter anyway.