>
> 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.
