it works ok till the first nsolver. the code is to trim an aircraft.
i found some errors in my code , and the corrected one is below,
but a new error happen, which is NameError: global name
'Derivative' is not defined
(i have removed the first nsolve since it is ok, only left the second
to trim an aircraft for steady turn,
sorry i can not give an simple example)
from sympy import *
import numpy as np
init_printing(use_unicode=False, wrap_line=True, no_global=True)
#flat earth, rigid ,symatric , body axis
theta,phi,psi=var('theta,phi,psi')
p,q,r=var('p,q,r')
u,v,w=var('u,v,w')
Xe,Ye,Ze=var('Xe,Ye,Ze')
deltAileron,deltRudder=var('deltAileron,deltRudder')
deltFlap,deltElevator,dotAlpha=var('deltFlap,deltElevator,dotAlpha')
fuel=var('fuel')
omega=var('omega')
thr=var('thr')
####################################
#g=var('g')
g=9.81#m/s**2
## DCM from inerria frame to body frame
H_I2B=Matrix([[cos(theta)*cos(psi),cos(theta)*sin(psi),-sin(theta)],
[(-cos(theta)*sin(psi)+sin(phi)*sin(theta)*cos(psi)),(cos(phi)*cos(psi)+sin(phi)*sin(theta)*cos(psi)),sin(phi)*cos(theta)],
[(sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi)),(-sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi)),cos(phi)*cos(theta)]])
gx,gy,gz=H_I2B*Matrix(3,1,[0,0,g])#gravity in body frame
#b,c,S=var('b,c,S')
#wing span
b=2.8956# m
#mean aerodynamics chord
c=0.189941 # %
#wing area
S=0.55# m**2
#m=var('m')
m=13.5# kg
#e,AR=var('e,AR')
#Oswald's coefficient
e=0.75
AR=b**2/S
################ISA for Troposphere #############
T0=288.15 # sea level tempreture ,k (15 centigree)
P_atm0=101325# N/m**2
R=287.04 #Characteristic gas constant (J/Kg/K)
gamma=1.4 #Ratio of specific heats
#altitude
h=-Ze
T=T0-6.5*h/1000
P_atm=P_atm0*(1-0.0065*h/T0)**5.2561
rho=P_atm/(R*T)
speedofsound=(T*R*gamma)**0.5
############### air data ##############33
#uw,vw,ww=var('uw,vw,ww')#wind speed
uw=0
vw=0
ww=0
ua,va,wa=Matrix(3,1,[u,v,w])-H_I2B*Matrix(3,1,[uw,vw,ww])
#alpha,beta,Va,Mach=var('alpha,beta,Va,Mach')
Va=(ua**2+va**2+wa**2)**0.5
alpha=atan(wa/ua)
beta=asin(va/Va)
Mach=Va/speedofsound
q_bar=rho*Va**2/2
########### aerodynamic doefficients ######################
#CL_0,CL_alpha,CL_deltFlap,CL_deltElevator,CL_dotAlpha,CL_q,CL_Mach=var('CL_0,CL_alpha,CL_deltFlap,CL_deltElevator,CL_dotAlpha,CL_q,CL_Mach')
####Lift coefficient######
#Zero-alpha lift
CL_0=0.23
CL_alpha=5.6106
CL_deltFlap=0.74
CL_deltElevator=0.13
CL_dotAlpha=1.9724
CL_q=7.9543
CL_Mach=0
#CD_0,CL_minD,CD_deltFlap,CD_deltElevator,CD_deltAileron,CD_deltRudder,CD_Mach=var('CD_0,CL_minD,CD_deltFlap,CD_deltElevator,CD_deltAileron,CD_deltRudder,CD_Mach')
CD_0=0.0434
CL_minD=0.23
CD_deltFlap=0.1467
CD_deltElevator=0.0135
CD_deltAileron=0.0302
CD_deltRudder=0.0303
CD_Mach=0
#CY_beta,CY_deltAileron,CY_deltRudder,CY_p,CY_r=var('CY_beta,CY_deltAileron,CY_deltRudder,CY_p,CY_r')
CY_beta=-0.83
CY_deltAileron=-0.075
CY_deltRudder=0.1914
CY_p=0
CY_r=0
#Cm_0,Cm_alpha,Cm_deltFlap,Cm_deltElevator,Cm_dotAlpha,Cm_q,Cm_Mach=var('Cm_0,Cm_alpha,Cm_deltFlap,Cm_deltElevator,Cm_dotAlpha,Cm_q,Cm_Mach')
Cm_0=0.135
Cm_alpha=-2.7397
Cm_deltFlap=0.0467
Cm_deltElevator=-0.9918
Cm_dotAlpha=-10.3796
Cm_q=-38.2067
Cm_Mach=0
#Cl_beta,Cl_deltAileron,Cl_deltRudder,Cl_p,Cl_r=var('Cl_beta,Cl_deltAileron,Cl_deltRudder,Cl_p,Cl_r')
Cl_beta=-0.13
Cl_deltAileron=-0.1695
Cl_deltRudder=0.0024
Cl_p=-0.5051
Cl_r=0.2519
#Cn_beta,Cn_deltAileron,Cn_deltRudder,Cn_p,Cn_r=var('Cn_beta,Cn_deltAileron,Cn_deltRudder,Cn_p,Cn_r')
Cn_beta=0.0726
Cn_deltAileron=0.0108
Cn_deltRudder=0.-0.0693
Cn_p=-0.069
Cn_r=-0.0946
### in total ###
#CL,CD,CY_w,Cm,Cl,Cn=var('CL,CD,CY,Cm,Cl,Cn')
CL=CL_0+CL_alpha*alpha+CL_deltFlap*deltFlap+CL_deltElevator*deltElevator\
+(CL_dotAlpha*dotAlpha+CL_q*q)*c/(2*Va)+CL_Mach*Mach
CD=CD_0+(CL-CL_minD)**2/(pi*e*AR)+CD_deltFlap*(deltFlap)+CD_deltElevator*(deltElevator)\
+CD_deltAileron*(deltAileron)+CD_deltRudder*(deltRudder)+CD_Mach*Mach
CY_w=CY_beta*beta+CY_deltAileron*deltAileron+CY_deltRudder*deltRudder+(CY_p*p+CY_r*r)*b/(2*Va)
Cm=Cm_0+Cm_alpha*alpha+Cm_deltFlap*deltFlap+Cm_deltElevator*deltElevator\
+(Cm_dotAlpha*dotAlpha+Cm_q*q)*c/(2*Va)+Cm_Mach*Mach
Cl=Cl_beta*beta+Cl_deltAileron*deltAileron+Cl_deltRudder*deltRudder+(Cl_p*p+Cl_r*r)*b/(2*Va)
Cn=Cn_beta*beta+Cn_deltAileron*deltAileron+Cn_deltRudder*deltRudder+(Cn_p*p+Cn_r*r)*b/(2*Va)
H_b2w=Matrix([[cos(alpha)*cos(beta),sin(beta),sin(alpha)*cos(beta)],
[-cos(alpha)*sin(beta),cos(beta),-sin(alpha)*sin(beta)],
[-sin(alpha),0,cos(alpha)]])
#CX,CY,CZ=var('CX,CY,CZ')
CX,CY,CZ=H_b2w.transpose()*Matrix(3,1,[-CD,CY_w,-CL])
##############Fixed-pitch propeller##############333
#Jar,CT,CP=var('Jar,CT,CP')
#propeller radius
R_prop=0.254# m
Jar=pi*Va/(omega*R_prop)
#coefficient of thrust (CT) and power (CP)
#the fomular is got after curve fitting using quadratic polynomial
CT=-0.02162763*Jar**2-0.03491274*Jar+0.03787723
CP=-0.01860428*Jar**2-0.01512463*Jar+0.02509024
F_prop=4*rho*R_prop**4*omega**2*CT/pi**2
M_prop=-4*rho*R_prop**5*omega**2*CP
##################Piston engine###################
MAPmin=60;
MAP=thr*(P_atm/1000-MAPmin)+MAPmin
RPM=omega*30/pi
#the fomulas of fuel flow and power of piston are got using polynomial
surface fitiing in Matlab
Fuelflow=(-3189+0.2615*RPM+119.6*MAP-0.00001329*RPM**2-0.005922*RPM*MAP-1.483*MAP**2\
+3.257/10**10*RPM**3+1.377/10**7*RPM**2*MAP+0.00003562*RPM*MAP**2+0.006149*MAP**3)/1000/3600
Power_P=-10600+1.38*RPM+367.1*MAP-0.0000768*RPM**2-0.03094*RPM*MAP-4.202*MAP**2\
+3.727/10**9*RPM**3+3.917/10**7*RPM**2*MAP+0.0002021*RPM*MAP**2+0.01572*MAP**3
power_corrected=(T0/T)**0.5*Power_P
M_eng=power_corrected/omega
###### engine total force and moment#########
J_eng=0.001# engine shaft moment of inertia
J_prop=0.002# propeller moment of inertia
MomentofInertiaofEngAndProp=(M_eng+M_prop)/(J_eng+J_prop) # which is omega
dot
######## mass,inertia and their derivatives and cg lacation
#################
#empty aricraft mass
m_empty=8.5
#gross aricraft mass
m_gross=13.5
#empty CG location [x y z]
cg_empty=Matrix([0.156,0,0.079])
#gross CG location [x y z]
cg_gross=Matrix([0.159,0,0.090])
#empty moment of inertia
Ixx_empty=0.7795 #kg*m^2
Iyy_empty=1.122
Izz_empty=1.752
Ixz_empty=0.1211
#empty inertia matrix
I_empty=Matrix([[Ixx_empty,0,-Ixz_empty],
[0,Iyy_empty,0],
[-Ixz_empty,0,Izz_empty]])
#gross moment of inertia
Ixx_gross=0.8244# kg*m**2
Iyy_gross=1.135# kg*m**2
Izz_gross=1.759# kg*m**2
Ixz_gross=0.1204# kg*m**2
#gross inertia matrix
I_gross=Matrix([[Ixx_gross,0,-Ixz_gross],
[0,Iyy_gross,0],
[-Ixz_gross,0,Izz_gross]])
# current cg m and I using interplation based on fuel
cg=(fuel/(m_gross-m_empty))*(cg_gross-cg_empty)+cg_empty
m=m_empty+fuel
m_dot=-Fuelflow
I=(fuel/(m_gross-m_empty))*(I_gross-I_empty)+I_empty
I_dot=m_dot*((I_gross-I_empty)/(m_gross-m_empty))
###########
rHub=Matrix([0,0,0])
#cg=Matrix([0.159,0,0.090])
cp=Matrix([0.1425,0,0])
#### cross product function
def cross(a, b):
c = [a[1]*b[2] - a[2]*b[1],
a[2]*b[0] - a[0]*b[2],
a[0]*b[1] - a[1]*b[0]]
return c
######## total propulsion forces and moemnts #########3
Tx=F_prop
Ty=0
Tz=0
M_Tx_temp=-M_eng
M_Ty_temp=0
M_Tz_temp=0
#propulsion force caused moments based on the application point (usually
the propeller hub)
M_Tx_prop,M_Ty_prop,M_Tz_prop=Matrix(cross(Matrix([Tx,Ty,Tz]),(cg-rHub)))
M_Tx=M_Tx_temp+M_Tx_prop
M_Ty=M_Ty_temp+M_Ty_prop
M_Tz=M_Tz_temp+M_Tz_prop
############ aerodynamics forces and moments ############
X_aero=CX*q_bar*S
Y_aero=CY*q_bar*S
Z_aero=CZ*q_bar*S
L_temp=Cl*q_bar*S*b
M_temp=Cm*q_bar*S*c
N_temp=Cn*q_bar*S*b
# since cross() return a list, it is required to transform to Matrix to
perform the math opration
L_aero,M_aero,N_aero=Matrix(cross(Matrix([X_aero,Y_aero,Z_aero]),(cg-cp)))+Matrix([L_temp,M_temp,N_temp])
####### total aerodynamic and propulsion forces and moments #############3
X=X_aero+Tx
Y=Y_aero+Ty
Z=Z_aero+Tz
L=L_aero+M_Tx
M=M_aero+M_Ty
N=N_aero+M_Tz
#############equations of motion################
## derived after the Simulink block : Custom Variable Mass 6DoF (Euler
Angles)
## since Aerosonde model use this block
Vb=Matrix([u,v,w])
p_q_r=Matrix([p,q,r])
#total forces matrix
F=Matrix([X+m*gx,Y+m*gy,Z+m*gz])-Vb*m_dot
# base on the equtions from Matlab help which is
# Fb=m*(Vb_dot+cross(p_q_r,Vb))+m_dot*Vb
# where Vb_dot=[u_dot,v_dot,w_dot]
# then we can get u_dot,v_dot,w_dot as follows:
u_dot,v_dot,w_dot=F/m+Matrix(cross(Vb,p_q_r))
# total moments matrix
M=Matrix([L,M,N])
# base on the equtions from Matlab help which is
# M=I*p_q_r_dot+cross(p_q_r,I*p_q_r)+I-dot*p_q_r
# where p_q_r_dot=[p_dot,q_dot,r_dot]
# then we can get p_dot,q_dot,r_dot as follows:
p_dot,q_dot,r_dot=I.inv()*(M-Matrix(cross(p_q_r,I*p_q_r))-I_dot*p_q_r)
# the kinematic equations
x_I_dot=cos(theta)*cos(psi)*u+(-cos(phi)*sin(psi)+sin(phi)*sin(theta)*cos(psi))*v\
+(sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi))*w
y_I_dot=cos(theta)*sin(psi)*u+(cos(phi)*cos(psi)+sin(phi)*sin(theta)*sin(psi))*v\
+(-sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi))*w
z_I_dot=-sin(theta)*u+sin(phi)*cos(theta)*v+cos(phi)*cos(theta)*w
phi_dot=p+(q*sin(phi)+r*cos(phi))*tan(theta)
theta_dot=q*cos(phi)-r*sin(phi)
psi_dot=(q*sin(phi)+r*cos(phi))*sec(theta)
############### trim for steady turn #############
###steady flght constrians
#u_dot=0,v_dot=0,w_dot=0,p_dot=0,q_dot=0,r_dot
### additional constrains
#phi_dot, theta_dot is zero, psi_dot is turn rate
#take psi_dot = 0.02 as an example
u_dot_=u_dot.subs({deltFlap:0,dotAlpha:0,Ze:-1000,fuel:2})
v_dot_=v_dot.subs({deltFlap:0,dotAlpha:0,Ze:-1000,fuel:2})
w_dot_=w_dot.subs({deltFlap:0,dotAlpha:0,Ze:-1000,fuel:2})
p_dot_=p_dot.subs({deltFlap:0,dotAlpha:0,Ze:-1000,fuel:2})
q_dot_=q_dot.subs({deltFlap:0,dotAlpha:0,Ze:-1000,fuel:2})
r_dot_=r_dot.subs({deltFlap:0,dotAlpha:0,Ze:-1000,fuel:2})
phi_dot_=phi_dot
theta_dot_=theta_dot
psi_dot_=psi_dot
Fuelflow_=Fuelflow.subs({deltFlap:0,dotAlpha:0,Ze:-1000,fuel:2})
MomentofInertiaofEngAndProp_=MomentofInertiaofEngAndProp.subs({deltFlap:0,dotAlpha:0,Ze:-1000,fuel:2})
#the relationshiop between p,q,r and eular angles are given as
# p=-psi_dot*theta
# q=psi_dot*sin(phi)*cos(theta)
# r=psi_dot*cos(phi)*cos(theta)
# which are expressed as
p_equ=(p+psi_dot*theta)
q_equ=(q-psi_dot*sin(phi)*cos(theta))
r_equ=(r-psi_dot*cos(phi)*cos(theta))
result_steadyturn=nsolve([u_dot_,v_dot_,w_dot_,p_dot_,q_dot_,r_dot_,phi_dot_,theta_dot_,psi_dot_-0.02,
p_equ,q_equ,r_equ,Fuelflow_,MomentofInertiaofEngAndProp_],
[u,v,w,p,q,r,phi,theta,psi,deltElevator,deltAileron,deltRudder,thr,omega],
[10,3,0,0,0,0,0,0,0,0,0,0,0.5,500])
--
You received this message because you are subscribed to the Google Groups
"sympy" group.
To post to this group, send email to [email protected].
To unsubscribe from this group, send email to
[email protected].
Visit this group at http://groups.google.com/group/sympy?hl=en.
For more options, visit https://groups.google.com/groups/opt_out.
