def getCoriolisMatrix(angVel_b, P):
"""TODO: Docstring for getCoriolisMatrix.
:returns: TODO
"""
c_rb_11 = P['mass'] * ng.Smtrx(angVel_b)
c_rb_12 = -P['mass'] * ng.Smtrx(angVel_b) @ ng.Smtrx(P['r_cg'])
c_rb_21 = P['mass'] * ng.Smtrx(P['r_cg']) @ ng.Smtrx(angVel_b)
c_rb_22 = -ng.Smtrx(P['I_cg'] @ angVel_b)
C_rb = np.vstack([np.hstack([c_rb_11, c_rb_12]),\
np.hstack([c_rb_21, c_rb_22])])
return C_rb
'C_n_delta_r' : 0,\
'rho' : 1.2250,\
'gravity' : 9.8100,\
'aileron_min' : -0.6109,\
'aileron_max' : 0.6109,\
'elevator_min' : -0.6109,\
'elevator_max' : 0.6109,\
'rudder_min' : 0,\
'rudder_max' : 0,\
'throttle_min' : 0,\
'throttle_max' : 1}
P['r_cg'] = np.zeros(3)
P['I_cg'] = np.array([[P['Jxx'],0,-P['Jxz']], [0, P['Jyy'], 0], [-P['Jxz'], 0, P['Jzz']] ])
P['M_rb'] = np.vstack([np.hstack([np.eye(3) * P['mass'], -P['mass'] * ng.Smtrx(P['r_cg'])]),
np.hstack([P['mass'] * ng.Smtrx(P['r_cg']), P['I_cg']])])
P['M_rb_inv'] = np.linalg.inv(P['M_rb'])
P['J'] = P['I_cg']
P['AR'] = P['b']**2/P['S_wing']
P['e'] = 0.9
P['M'] = 50
P['alpha_0'] = 0.4712
P['epsilon'] = 0.1592
def compute_perpendicular_projector(x):
# Assumes x has norm equal to one
# return np.eye(len(x)) - [email protected]
return -ng.Smtrx(x)@ng.Smtrx(x)
def compute_eOmega(Gamma, omega, omega_d):
Pi_perp = compute_perpendicular_projector(Gamma)
# return Pi_perp@omega - Pi_perp@omega_d
return -ng.Smtrx(Gamma)@ng.Smtrx(Gamma)@(omega - omega_d)
def loadParameters(fileName):
P = sio.loadmat(fileName)
del P['__header__'], P['__version__'], P['__globals__']
for key, value in P.items():
if P[key].size == 1:
P[key] = P[key][0][0]
P['r_cg'] = np.zeros(3)
P["rho"] = 1.225
P['gravity'] = 9.81
P['I_cg'] = np.array([[P['Jx'], 0, -P['Jxz']], [0, P['Jy'], 0],
[-P['Jxz'], 0, P['Jz']]])
P['M_rb'] = np.vstack([
np.hstack([np.eye(3) * P['mass'], -P['mass'] * ng.Smtrx(P['r_cg'])]),
np.hstack([P['mass'] * ng.Smtrx(P['r_cg']), P['I_cg']])
])
P['aileron_min'] = -np.pi / 3.0
P['aileron_min'] = -np.pi / 3.0
P['aileron_max'] = +np.pi / 3.0
P['elevator_min'] = -np.pi / 3.0
P['elevator_max'] = +np.pi / 3.0
P['throttle_min'] = 0.0
P['throttle_max'] = 1.0
P['kp_h'] = -0.025
P['ki_h'] = 0.0
P['kp_theta'] = 0.1
P['kd_theta'] = -0.01
P['ki_theta'] = 0
P['kp_V'] = -0.05
P['ki_V'] = -0.01
P['kp_phi'] = -0.5
P['ki_phi'] = 0
P['kd_phi'] = 0
P['kp_chi'] = -0.05
P['ki_chi'] = 0.0
# Sensor parameters
P['stdAcc'] = 1e-2
P['stdGyr'] = 1e-2
P['stdSP'] = 1e-2
P['stdDP'] = 1e-2
P['stdMag'] = 1e-2
gam = P['Jx'] * P['Jz'] - P['Jxz'] * P['Jxz']
P['Gamma'] = [gam, \
P['Jxz']*(P['Jx']-P['Jy']+P['Jz'])/gam, \
(P['Jz']*(P['Jz']-P['Jy'])+P['Jxz']*P['Jxz'])/gam,\
P['Jz']/gam,\
P['Jxz']/gam,\
(P['Jz']-P['Jx'])/P['Jy'],\
P['Jxz']/P['Jy'],\
((P['Jx']-P['Jy'])*P['Jx']+P['Jxz']*P['Jxz'])/gam,\
P['Jx']/gam]
return P