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