def _update_true_state(self): # update the class structure for the true state: # [pn, pe, h, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi, gyro_bx, gyro_by, gyro_bz] phi, theta, psi = Quaternion2Euler(self._state[6:10]) pdot = Quaternion2Rotation(self._state[6:10]) @ self._state[3:6] self.true_state.pn = self._state.item(0) self.true_state.pe = self._state.item(1) self.true_state.h = -self._state.item(2) self.true_state.Va = self._Va self.true_state.alpha = self._alpha self.true_state.beta = self._beta self.true_state.phi = phi self.true_state.theta = theta self.true_state.psi = psi self.true_state.Vg = np.linalg.norm(pdot) self.true_state.gamma = np.arcsin(pdot.item(2) / self.true_state.Vg) self.true_state.chi = np.arctan2(pdot.item(1), pdot.item(0)) self.true_state.p = self._state.item(10) self.true_state.q = self._state.item(11) self.true_state.r = self._state.item(12) self.true_state.wn = self._wind.item(0) self.true_state.we = self._wind.item(1) self.true_state.bx = SENSOR.gyro_x_bias self.true_state.by = SENSOR.gyro_y_bias self.true_state.bz = SENSOR.gyro_z_bias
def _update_msg_true_state(self): # update the class structure for the true state: # [pn, pe, h, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi, gyro_bx, gyro_by, gyro_bz] phi, theta, psi = Quaternion2Euler(self._state[6:10]) self.msg_true_state.pn = self._state.item(0) self.msg_true_state.pe = self._state.item(1) self.msg_true_state.h = -self._state.item(2) self.msg_true_state.Va = self._Va self.msg_true_state.alpha = self._alpha self.msg_true_state.beta = self._beta self.msg_true_state.phi = phi self.msg_true_state.theta = theta self.msg_true_state.psi = psi R = Euler2Rotation(phi, theta, psi) self.Vg_i = R.T @ self.Vg_b self.msg_true_state.Vg = self._Vg self.msg_true_state.gamma = np.arctan2( -self.Vg_i[2], np.sqrt(self.Vg_i[0]**2 + self.Vg_i[1]**2)) self.msg_true_state.chi = -np.arctan2(self.Vg_i[1], self.Vg_i[0])[0] self.msg_true_state.p = self._state.item(10) self.msg_true_state.q = self._state.item(11) self.msg_true_state.r = self._state.item(12) self.msg_true_state.wn = self._wind.item(0) self.msg_true_state.we = self._wind.item(1)
def _forces_moments(self, delta): """ return the forces on the UAV based on the state, wind, and control surfaces :param delta: np.matrix(delta_a, delta_e, delta_r, delta_t) :return: Forces and Moments on the UAV np.matrix(Fx, Fy, Fz, Ml, Mn, Mm) """ phi, theta, psi = Quaternion2Euler(self._state[6:10]) p = self._state.item(10) q = self._state.item(11) r = self._state.item(12) delta_e = delta.item(0) delta_a = delta.item(1) delta_r = delta.item(2) delta_t = delta.item(3) fx = fy = fz = Mx = My = Mz = self._forces[0] = fx self._forces[1] = fy self._forces[2] = fz return np.array([[fx, fy, fz, Mx, My, Mz]]).T
def _update_velocity_data(self, wind=np.zeros((6, 1))): # compute airspeed phi, theta, psi = Quaternion2Euler(self._state[6:10]) Wnb = Inertial2Body(phi, theta, psi, wind.item(0), wind.item(1), wind.item(2))[0] Web = Inertial2Body(phi, theta, psi, wind.item(0), wind.item(1), wind.item(2))[1] Wdb = Inertial2Body(phi, theta, psi, wind.item(0), wind.item(1), wind.item(2))[2] u = self._state[3] v = self._state[4] w = self._state[5] u_w = Wnb + wind.item(3) v_w = Web + wind.item(4) w_w = Wdb + wind.item(5) u_r = u - u_w v_r = v - v_w w_r = w - w_w self._Va = math.sqrt(u_r**2 + v_r**2 + w_r**2) # compute angle of attack self._alpha = np.arctan2(w_r, u_r) # compute sideslip angle self._beta = math.asin(v_r / self._Va)
def _update_velocity_data(self, wind=np.zeros((6, 1))): e0 = self._state[6] e1 = self._state[7] e2 = self._state[8] e3 = self._state[9] e_array = np.array([e0, e1, e2, e3]) [phi, th, psi] = Quaternion2Euler(e_array) # compute airspeed wind_constant = Euler2Rotation(phi, th, psi) @ wind[0:3] wind_b = np.array([[wind_constant[0][0] + wind[3][0]], [wind_constant[1][0] + wind[4][0]], [wind_constant[2][0] + wind[5][0]]]) self.Va_b = np.array([[self._state[3][0] - wind_b[0][0]], [self._state[4][0] - wind_b[1][0]], [self._state[5][0] - wind_b[2][0]]]) self._Va = np.linalg.norm(self.Va_b) self.Vg_b = self.Va_b + wind_b self._Vg = np.linalg.norm(self.Vg_b) if self._Va == 0.0: self._alpha = 0.0 self._beta = 0.0 else: # compute angle of attack self._alpha = np.arctan2(self.Va_b[2], self.Va_b[0])[0] # compute sideslip angle self._beta = np.arcsin(self.Va_b[1][0] / self._Va)
def compute_tf_model(mav, trim_state, trim_input): # trim values mav._state = trim_state mav._update_velocity_data() Va_trim = mav._Va alpha_trim = mav._alpha phi, theta_trim, psi = Quaternion2Euler(trim_state[6:10]) Va_star = trim_state[3] alpha_star = MAV.alpha0 delta_e_star = trim_input[0].item(0) delta_t_star = trim_input[1].item(0) # define transfer function constants a_phi1 = -1 / 2 * MAV.rho * mav._Va**2 * MAV.S_wing * MAV.b * MAV.C_p_p * MAV.b / ( 2 * mav._Va) a_phi2 = 1 / 2 * MAV.rho * mav._Va**2 * MAV.S_wing * MAV.b * MAV.C_p_delta_a a_theta1 = -MAV.rho * mav._Va**2 * MAV.c * MAV.S_wing / ( 2 * MAV.Jy) * MAV.C_m_q * MAV.c / (2 * mav._Va) a_theta2 = -MAV.rho * mav._Va**2 * MAV.c * MAV.S_wing / ( 2 * MAV.Jy) * MAV.C_m_alpha a_theta3 = MAV.rho * mav._Va**2 * MAV.c * MAV.S_wing / ( 2 * MAV.Jy) * MAV.C_m_delta_e a_V1 = MAV.rho * Va_star * MAV.S_wing / MAV.mass * ( MAV.C_D_0 + MAV.C_D_alpha * alpha_star + MAV.C_D_delta_e * delta_e_star) + MAV.rho * MAV.S_prop / MAV.mass * MAV.C_prop * Va_star a_V1 = a_V1.item(0) a_V2 = MAV.rho * MAV.S_prop / MAV.mass * MAV.C_prop * MAV.k_motor**2 * delta_t_star a_V3 = MAV.gravity return Va_trim, alpha_trim, theta_trim, a_phi1, a_phi2, a_theta1, a_theta2, a_theta3, a_V1, a_V2, a_V3
def _update_true_state(self): # update the true state message: phi, theta, psi = Quaternion2Euler(self._state[6:10]) self.true_state.pn = self._state.item(0) self.true_state.pe = self._state.item(1) self.true_state.h = -self._state.item(2) self.true_state.phi = phi self.true_state.theta = theta self.true_state.psi = psi self.true_state.p = self._state.item(10) self.true_state.q = self._state.item(11) self.true_state.r = self._state.item(12)
def compute_tf_model(mav, trim_state, trim_input): # trim values rho = MAV.rho S = MAV.S_wing Va = mav._Va b = MAV.b beta = mav._beta alpha = mav._alpha c = MAV.c Jy = MAV.Jy g = MAV.gravity mass = MAV.mass a_phi_1 = 0.5 * rho * (Va**2) * S * b * MAV.C_p_p * b / (2 * Va) a_phi_2 = 0.5 * rho * (Va**2) * S * b * MAV.C_p_delta_a T_phi_delta_a = TF([a_phi_2], [1, -a_phi_1, 0]) T_chi_phi = TF(np.array([g / Va]), [1, 0]) beta_dr = (rho * Va * S) / (2 * MAV.mass * np.cos(beta)) a_beta1 = -beta_dr * MAV.C_Y_beta a_beta2 = beta_dr * MAV.C_Y_delta_r T_beta_delta_r = TF([a_beta2], [1, a_beta1]) theta_de = rho * Va**2 * c * S / (2 * Jy) a_theta1 = -theta_de * MAV.C_m_q * c / (2 * Va) a_theta2 = -theta_de * MAV.C_m_alpha a_theta3 = theta_de * MAV.C_m_delta_e T_theta_delta_e = TF([a_theta3], [1, a_theta1, a_theta2]) T_h_theta = TF([Va], [1, 0]) _, theta, _ = Quaternion2Euler(trim_state[6:10]) T_h_Va = TF([theta], [1, 0]) C_Ds = MAV.C_D_0 + MAV.C_D_alpha * alpha + MAV.C_D_delta_e * trim_input[0] a_V1 = ((rho * Va * S * C_Ds) - dT_dVa(mav, Va, trim_input[3])) / mass a_V2 = dT_ddelta_t(mav, Va, trim_input[3]) / mass a_V3 = g * np.cos(theta - alpha) T_Va_delta_t = TF([a_V2], [1, a_V1]) T_Va_theta = TF([-a_V3], [1, a_V1]) with open("trim.pkl", 'wb') as f: vals = [ trim_state, trim_input, a_phi_1, a_phi_2, a_beta1, a_beta2, a_theta1, a_theta2, a_theta3 ] pkl.dump(vals, f) data = [] with open("trim.pkl", 'rb') as f: data = pkl.load(f) return T_phi_delta_a, T_chi_phi, T_theta_delta_e, T_h_theta, T_h_Va, T_Va_delta_t, T_Va_theta, T_beta_delta_r
def _update_true_state(self): phi, theta, psi = Quaternion2Euler(self._state[6:10]) self.true_state.pn = self._state[0] self.true_state.pe = self._state[1] self.true_state.h = -self._state[2] self.true_state.Va = self._Va self.true_state.alpha = self._alpha self.true_state.beta = self._beta self.true_state.phi = phi self.true_state.theta = theta self.true_state.psi = psi self.true_state.Vg = np.linalg.norm(self._state[3:6]) self.true_state.gamma, self.true_state.chi = self.calc_gamma_chi() self.true_state.p = self._state[10] self.true_state.q = self._state[11] self.true_state.r = self._state[12] self.true_state.wn = self._wind[0] self.true_state.we = self._wind[1]
def compute_tf_model(mav, trim_state, trim_input): # trim values mav._state = trim_state mav._update_velocity_data() Va_trim = mav._Va alpha_trim = mav._alpha phi, theta_trim, psi = Quaternion2Euler(trim_state[6:10]) # define transfer function constants a_phi1 = NotImplemented a_phi2 = NotImplemented a_theta1 = NotImplemented a_theta2 = NotImplemented a_theta3 = NotImplemented a_V1 = NotImplemented a_V2 = NotImplemented a_V3 = NotImplemented return Va_trim, alpha_trim, theta_trim, a_phi1, a_phi2, a_theta1, a_theta2, a_theta3, a_V1, a_V2, a_V3
def _update_true_state(self): # update the class structure for the true state: # [pn, pe, h, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi, gyro_bx, gyro_by, gyro_bz] phi, theta, psi = Quaternion2Euler(self._state[6:10]) self.msg_true_state.pn = self._state.item(0) self.msg_true_state.pe = self._state.item(1) self.msg_true_state.h = -self._state.item(2) self.msg_true_state.Va = self._Va self.msg_true_state.alpha = self._alpha self.msg_true_state.beta = self._beta self.msg_true_state.phi = phi self.msg_true_state.theta = theta self.msg_true_state.psi = psi self.msg_true_state.Vg = math.sqrt((self._state[3][0])**2 + (self._state[4][0])**2 + (self._state[5][0])**2) e0 = self._state[6][0] e1 = self._state[7][0] e2 = self._state[8][0] e3 = self._state[9][0] u = self._state[3][0] v = self._state[4][0] w = self._state[5][0] pn_dot = (e1**2 + e0**2 - e2**2 - e3**2) * u + 2 * ( e1 * e2 - e3 * e0) * v + 2 * (e1 * e3 + e2 * e0) * w pe_dot = 2 * (e1 * e2 + e3 * e0) * u + ( e2**2 + e0**2 - e1**2 - e3**2) * v + 2 * (e2 * e3 - e1 * e0) * w pd_dot = 2 * (e1 * e3 - e2 * e0) * u + 2 * (e2 * e3 + e1 * e0) * v + ( e3**2 + e0**2 - e1**2 - e2**2) * w # pn_dot, pe_dot, vg_k = Body2Inertia(phi, theta, psi, [self.state[3][0], self.state[4][0], self.state[5][0]] ) self.msg_true_state.gamma = math.asin( -pe_dot / math.sqrt(pn_dot**2 + pe_dot**2 + pd_dot**2)) self.msg_true_state.chi = math.atan(pe_dot / pn_dot) self.msg_true_state.p = self._state.item(10) self.msg_true_state.q = self._state.item(11) self.msg_true_state.r = self._state.item(12) self.msg_true_state.wn = self._wind.item(0) self.msg_true_state.we = self._wind.item(1)
def _update_true_state(self): # update the class structure for the true state: # [pn, pe, h, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi, gyro_bx, gyro_by, gyro_bz] phi, theta, psi = Quaternion2Euler(self._state[6:10]) self.true_state.pn = self._state[0] self.true_state.pe = self._state[1] self.true_state.h = -self._state[2] self.true_state.Va = self._Va self.true_state.alpha = self._alpha self.true_state.beta = self._beta self.true_state.phi = phi self.true_state.theta = theta self.true_state.psi = psi self.true_state.Vg = np.linalg.norm(self._state[3:6]) self.true_state.gamma = np.arctan2(-self._state[5], self._state[3]) self.true_state.chi = np.arctan2(self._state[4], self._state[3]) + psi self.true_state.p = self._state[10] self.true_state.q = self._state[11] self.true_state.r = self._state[12] self.true_state.wn = self._wind[0] self.true_state.we = self._wind[1]
def _forces_moments(self, delta): """ :param delta: np.matrix(delta_a, delta_e, delta_r, delta_t) :return: Forces and Moments on the UAV np.matrix(Fx, Fy, Fz, Ml, Mn, Mm) """ # longitudinal coefficients C_L_0 = MAV.C_L_0 C_L_alpha = MAV.C_L_alpha C_L_q = MAV.C_L_q C_L_delta_e = MAV.C_L_delta_e C_D_0 = MAV.C_D_0 C_D_alpha = MAV.C_D_alpha C_D_p = MAV.C_D_p C_D_q = MAV.C_D_q C_D_delta_e = MAV.C_D_delta_e C_m_0 = MAV.C_m_0 C_m_alpha = MAV.C_m_alpha C_m_q = MAV.C_m_q C_m_delta_e = MAV.C_m_delta_e C_prop = MAV.C_prop M = MAV.M alpha0 = MAV.alpha0 epsilon = MAV.epsilon # lateral coefficients C_Y_0 = MAV.C_Y_0 C_Y_beta = MAV.C_Y_beta C_Y_p = MAV.C_Y_p C_Y_r = MAV.C_Y_r C_Y_delta_a = MAV.C_Y_delta_a C_Y_delta_r = MAV.C_Y_delta_r C_ell_0 = MAV.C_ell_0 C_ell_beta = MAV.C_ell_beta C_ell_p = MAV.C_ell_p C_ell_r = MAV.C_ell_r C_ell_delta_a = MAV.C_ell_delta_a C_ell_delta_r = MAV.C_ell_delta_r C_n_0 = MAV.C_n_0 C_n_beta = MAV.C_n_beta C_n_p = MAV.C_n_p C_n_r = MAV.C_n_r C_n_delta_a = MAV.C_n_delta_a C_n_delta_r = MAV.C_n_delta_r # common terms al = self._alpha beta = self._beta Va = self._Va s_al = np.sin(al) c_al = np.cos(al) [phi, th, psi] = Quaternion2Euler(self._state[6:10]) p = self._state[10][0] q = self._state[11][0] r = self._state[12][0] delta_a = delta[0][0] delta_e = delta[1][0] delta_r = delta[2][0] delta_t = delta[3][0] # coefficients sig_a = (1 + np.exp(-M * (al - alpha0)) + np.exp(M * (al + alpha0))) / ( (1 + np.exp(-M * (al - alpha0))) * (1 + np.exp(M * (al + alpha0)))) CL = (1 - sig_a) * (C_L_0 + C_L_alpha * al) + sig_a * ( 2 * np.sign(al) * s_al**2 * c_al) CD = C_D_p + (C_L_0 + C_L_alpha * al)**2 / (np.pi * MAV.e * MAV.AR) # CL = C_L_0+C_L_alpha*al # CD = C_D_0+C_D_alpha*al Cx_a = -CD * c_al + CL * s_al Cxq_a = -C_D_q * c_al + C_L_q * s_al Cx_de_a = -C_D_delta_e * c_al + C_L_delta_e * s_al Cz_a = -CD * s_al - CL * c_al Czq_a = -C_D_q * s_al - C_L_q * c_al Cz_de_a = -C_D_delta_e * s_al - C_L_delta_e * c_al Tp, Qp = self._motor_thrust_torque(Va, delta_t) # forces fg = np.array([[-MAV.mass * MAV.gravity * np.sin(th)], [MAV.mass * MAV.gravity * np.cos(th) * np.sin(phi)], [MAV.mass * MAV.gravity * np.cos(th) * np.cos(phi)]]) if Va == 0.0: fa = np.array([[0.0], [0.0], [0.0]]) else: fa = 0.5 * MAV.rho * Va**2 * MAV.S_wing * np.array([ [Cx_a + Cxq_a * MAV.c / (2.0 * Va) * q + Cx_de_a * delta_e], [ C_Y_0 + C_Y_beta * beta + C_Y_p * MAV.b / (2.0 * Va) * p + C_Y_r * MAV.b / (2.0 * Va) * r + C_Y_delta_a * delta_a + C_Y_delta_r * delta_r ], [Cz_a + Czq_a * MAV.c / (2.0 * Va) * q + Cz_de_a * delta_e] ]) fp = np.array([[Tp], [0.0], [0.0]]) if Va == 0.0: Ma = np.array([[0.0], [0.0], [0.0]]) else: Ma = 0.5 * MAV.rho * Va**2 * MAV.S_wing * np.array( [[ MAV.b * (C_ell_0 + C_ell_beta * beta + C_ell_p * MAV.b / (2 * Va) * p + C_ell_r * MAV.b / (2.0 * Va) * r + C_ell_delta_a * delta_a + C_ell_delta_r * delta_r) ], [ MAV.c * (C_m_0 + C_m_alpha * al + C_m_q * MAV.c / (2.0 * Va) * q + C_m_delta_e * delta_e) ], [ MAV.b * (C_n_0 + C_n_beta * beta + C_n_p * MAV.b / (2.0 * Va) * p + C_n_r * MAV.b / (2.0 * Va) * r + C_n_delta_a * delta_a + C_n_delta_r * delta_r) ]]) Mt = np.array([[Qp], [0.0], [0.0]]) fx = fg[0][0] + fa[0][0] + fp[0][0] fy = fg[1][0] + fa[1][0] + fp[1][0] fz = fg[2][0] + fa[2][0] + fp[2][0] Mx = Ma[0][0] + Mt[0][0] My = Ma[1][0] + Mt[1][0] Mz = Ma[2][0] + Mt[2][0] # fx = 0.0 # fy = 0.0 # fz = 0.0 # Mx = 0.0 # My = 0.0 # Mz = 0.0 self._forces[0] = fx self._forces[1] = fy self._forces[2] = fz return np.array([[fx, fy, fz, Mx, My, Mz]]).T
def update_sensors(self, delta): "Return value of sensors on MAV: gyros, accels, static_pressure, dynamic_pressure, GPS" # Rate Gyros p = self._state.item(10) q = self._state.item(11) r = self._state.item(12) self.sensors.gyro_x = p + np.random.normal( scale=SENSOR.gyro_sigma) + SENSOR.gyro_x_bias self.sensors.gyro_y = q + np.random.normal( scale=SENSOR.gyro_sigma) + SENSOR.gyro_y_bias self.sensors.gyro_z = r + np.random.normal( scale=SENSOR.gyro_sigma) + SENSOR.gyro_z_bias # Accelerometers forces = self._forces_moments(delta) phi, theta, psi = Quaternion2Euler([ self._state.item(6), self._state.item(7), self._state.item(8), self._state.item(9) ]) m, g = MAV.mass, MAV.gravity self.sensors.accel_x = forces.item(0) / m + g * np.sin( theta) + np.random.normal(scale=SENSOR.accel_sigma) self.sensors.accel_y = forces.item(1) / m - g * np.cos(theta) * np.sin( phi) + np.random.normal(scale=SENSOR.accel_sigma) self.sensors.accel_z = forces.item(2) / m - g * np.cos(theta) * np.cos( phi) + np.random.normal(scale=SENSOR.accel_sigma) # Static Pressure Sensor h_ASL = -self._state.item(2) self.sensors.static_pressure = MAV.rho * g * h_ASL + np.random.normal( scale=SENSOR.static_pres_sigma) # Differencial Presssure Sensor self.sensors.diff_pressure = 0.5 * MAV.rho * ( self._Va)**2 + np.random.normal(scale=SENSOR.diff_pres_sigma) if self._t_gps >= SENSOR.ts_gps: self._gps_eta_n = np.exp(-SENSOR.gps_k * SENSOR.ts_gps ) * self._gps_eta_n + np.random.normal( scale=SENSOR.gps_n_sigma) self._gps_eta_e = np.exp(-SENSOR.gps_k * SENSOR.ts_gps ) * self._gps_eta_e + np.random.normal( scale=SENSOR.gps_e_sigma) self._gps_eta_h = np.exp(-SENSOR.gps_k * SENSOR.ts_gps ) * self._gps_eta_h + np.random.normal( scale=SENSOR.gps_h_sigma) self.sensors.gps_n = self._state.item(0) + self._gps_eta_n self.sensors.gps_e = self._state.item(1) + self._gps_eta_e self.sensors.gps_h = -self._state.item(2) + self._gps_eta_h # V_a and course measurements Va = self._Va w_n, w_e, w_d = self._wind.item(0), self._wind.item( 1), self._wind.item(2) self.sensors.gps_Vg = np.sqrt( (Va * np.cos(psi) + w_n)**2 + (Va * np.sin(psi) + w_e)**2) + np.random.normal( scale=SENSOR.gps_Vg_sigma) self.sensors.gps_course = np.arctan2( Va * np.sin(psi) + w_e, Va * np.cos(psi) + w_n) + np.random.normal(scale=SENSOR.gps_course_sigma) self._t_gps = 0. else: self._t_gps += self._ts_simulation
elev_doublet = sigs.signals(amplitude=0.2, frequency=1.0, start_time=1, duration=1, dc_offset=chap5.u_trim[0]) for ii in range(n): delta_e = elev_doublet.doublet(dt * ii) delta = np.array( [delta_e, chap5.u_trim[1], chap5.u_trim[2], chap5.u_trim[3]]) wind = np.array([[0.], [0.], [0.], [0.], [0.], [0.]]) uav.update(delta, wind) alpha.append(uav._alpha * 180 / np.pi) beta.append(uav._beta * 180 / np.pi) t_history.append(ii * dt) phi_t, theta_t, psi_t = Quaternion2Euler(uav._state[6:10]) phi.append(phi_t * 180 / np.pi) theta.append(theta_t * 180 / np.pi) psi.append(psi_t * 180 / np.pi) gamma.append(uav.true_state.gamma * 180 / np.pi) # close all figures print(uav._state) plt.close('all') plt.figure() plt.plot(t_history, alpha, 'r', label='aoa') plt.xlabel('t') plt.ylabel('angle [deg]') plt.legend()
def _forces_moments(self, delta): """ return the forces on the UAV based on the state, wind, and control surfaces :param delta: np.matrix(delta_e, delta_a, delta_r, delta_t) :return: Forces and Moments on the UAV np.matrix(Fx, Fy, Fz, Ml, Mn, Mm) """ phi, theta, psi = Quaternion2Euler(self._state[6:10]) p = self._state.item(10) q = self._state.item(11) r = self._state.item(12) delta_e = delta.item(0) delta_a = delta.item(1) delta_r = delta.item(2) delta_t = delta.item(3) # Gravitational Components of Force, Moments = 0 mg = MAV.mass*MAV.gravity fx_grav = -mg*np.sin(theta) fy_grav = mg* np.cos(theta) * np.sin(phi) fz_grav = mg* np.cos(theta) * np.cos(phi) # Thrust Components of Force and Moments fx_thrust,Mx_thrust = self.thrust_from_prop(delta_t) fy_thrust = 0 fz_thrust = 0 My_thrust = 0 Mz_thrust = 0 # Aerodynamic Components of Forces and Moments b = MAV.b cyp = MAV.C_Y_p cyr = MAV.C_Y_r cydeltaa = MAV.C_Y_delta_a cydeltar = MAV.C_Y_delta_r aero_coef = 0.5*MAV.rho*self._Va**2*MAV.S_wing fx_aero = aero_coef * (self.Cx(self._alpha) + self.Cx_q(self._alpha)*MAV.c/(2*self._Va)*q + self.Cx_deltae(self._alpha)*delta_e) fy_aero = aero_coef * (MAV.C_Y_0 + MAV.C_Y_beta*self._beta + MAV.C_Y_p*b/(2*self._Va)*p + cyr * b/(2*self._Va)*r + cydeltaa * delta_a + cydeltar* delta_r) fz_aero = aero_coef * (self.Cz(self._alpha) + self.Cz_q(self._alpha)*MAV.c/(2*self._Va)*q + self.Cz_deltae(self._alpha)*delta_e) Mx_aero = aero_coef * MAV.b * (MAV.C_ell_0 + MAV.C_ell_beta*self._beta + MAV.C_ell_p*b/(2*self._Va)*p + MAV.C_ell_r*b/(2*self._Va)*r + MAV.C_ell_delta_a*delta_a + MAV.C_ell_delta_r*delta_r) My_aero = aero_coef * MAV.c * (MAV.C_m_0 + MAV.C_m_alpha*self._alpha + MAV.C_m_q*MAV.c/(2*self._Va)*q + MAV.C_m_delta_e*delta_e) Mz_aero = aero_coef * MAV.b * (MAV.C_n_0 + MAV.C_n_beta*self._beta + MAV.C_n_p*MAV.b/(2*self._Va)*p + MAV.C_n_r*MAV.b/(2*self._Va)*r + MAV.C_n_delta_a*delta_a + MAV.C_n_delta_r*delta_r) fx = fx_grav + fx_aero + fx_thrust fy = fy_grav + fy_aero + fy_thrust fz = fz_grav + fz_aero + fz_thrust # print('fx = ',fx) # print('fy = ',fy) # print('fz = ',fz) Mx = Mx_aero + Mx_thrust My = My_aero + My_thrust Mz = Mz_aero + Mz_thrust # print('Mx = ',Mx) # print('My = ',My) # print('Mz = ',Mz) self._forces[0] = fx self._forces[1] = fy self._forces[2] = fz fm = np.reshape(np.array([fx, fy, fz, Mx, My, Mz]),[6,1]) return fm
def _forces_moments(self, delta): """ return the forces on the UAV based on the state, wind, and control surfaces :param delta: message with attributes [aileron, elevator, rudder, and throttle] :return: Forces and Moments on the UAV np.matrix(Fx, Fy, Fz, Ml, Mn, Mm) """ phi, theta, psi = Quaternion2Euler(self._state[6:10]) e0 = self._state.item(6) e1 = self._state.item(7) e2 = self._state.item(8) e3 = self._state.item(9) p = self._state.item(10) q = self._state.item(11) r = self._state.item(12) delta_a = delta.aileron delta_e = delta.elevator delta_r = delta.rudder delta_t = delta.throttle # compute gravitaional forces f_g = MAV.mass * MAV.gravity * np.array([[ 2 * (e1 * e3 - e2 * e0), 2 * (e2 * e3 + e1 * e0), e3**2 + e0**2 - e1**2 - e2**2 ]]) # compute Lift and Drag coefficients sigma = (1. + np.exp(-MAV.M * (self._alpha - MAV.alpha0)) + np.exp(MAV.M * (self._alpha + MAV.alpha0))) / ( (1 + np.exp(-MAV.M * (self._alpha - MAV.alpha0))) * (1. + np.exp(MAV.M * (self._alpha + MAV.alpha0)))) sa = np.sin(self._alpha) ca = np.cos(self._alpha) CL = (1. - sigma) * (MAV.C_L_0 + MAV.C_L_alpha * self._alpha) + sigma * ( 2 * np.sign(self._alpha) * sa**2 * ca) CD = MAV.C_D_p + (MAV.C_L_0 + MAV.C_L_alpha * self._alpha)**2 / ( np.pi * MAV.e * MAV.AR) # CL = MAV.C_L_0 + MAV.C_L_alpha*self._alpha # CD = MAV.C_D_0 + MAV.C_D_alpha*self._alpha # compute Lift and Drag Forces scalar = 0.5 * MAV.rho * (self._Va**2) * MAV.S_wing F_lift = scalar * (CL + MAV.C_L_q * MAV.c * q / (2 * self._Va) + MAV.C_L_delta_e * delta_e) F_drag = scalar * (CD + MAV.C_D_q * MAV.c * q / (2 * self._Va) + MAV.C_D_delta_e * delta_e) # compute propeller thrust and torque thrust_prop, torque_prop = self._motor_thrust_torque(self._Va, delta_t) # # rotate lift and drag to body frame # Cx = -CD*ca + CL*sa # Cxq = -MAV.C_D_q*ca + MAV.C_L_q*sa # Cxde = -MAV.C_D_delta_e*ca + MAV.C_L_delta_e*sa # Cz = -CD*sa - CL*ca # Czq = -MAV.C_D_q*sa - MAV.C_L_q*ca # Czde = -MAV.C_D_delta_e*sa - MAV.C_L_delta_e # compute longitudinal forces in body frame fx = -F_drag * ca + F_lift * sa + thrust_prop + f_g[0, 0] fz = -F_drag * sa - F_lift * ca + f_g[0, 2] # compute lateral forces in body frame b2Va = MAV.b / (2 * self._Va) fy = f_g[0, 1] + scalar * ( MAV.C_Y_0 + MAV.C_Y_beta * self._beta + MAV.C_Y_p * p * b2Va + MAV.C_Y_r * b2Va * r + MAV.C_Y_delta_a * delta_a + MAV.C_Y_delta_r * delta_r) # compute logitudinal torque in body frame My = scalar * MAV.c * (MAV.C_m_0 + MAV.C_m_alpha * self._alpha + MAV.C_m_q * MAV.c * q / (2 * self._Va) + MAV.C_m_delta_e * delta_e) # compute lateral torques in body frame Mx = scalar * MAV.b * (MAV.C_ell_0 + MAV.C_ell_beta * self._beta + MAV.C_ell_p * b2Va * p + MAV.C_ell_r * b2Va * r + MAV.C_ell_delta_a * delta_a + MAV.C_ell_delta_r * delta_r) + torque_prop Mz = scalar * MAV.b * (MAV.C_n_0 + MAV.C_n_beta * self._beta + MAV.C_n_p * b2Va * p + MAV.C_n_r * b2Va * r + MAV.C_n_delta_a * delta_a + MAV.C_n_delta_r * delta_r) # print(thrust_prop, "\t", fx) # from IPython import embed; embed() self._forces[0] = fx self._forces[1] = fy self._forces[2] = fz return np.array([[fx, fy, fz, Mx, My, Mz]]).T
pd0 = -100.0 # initial down position u0 = 24.971443 # initial velocity along body x-axis v0 = 0.0 # initial velocity along body y-axis w0 = 1.194576 # initial velocity along body z-axis e = np.array([0.993827, 0.000000, 0.110938, 0.000000]) p0 = 0 # initial roll rate q0 = 0 # initial pitch rate r0 = 0 # initial yaw rate Va0 = np.sqrt(u0**2 + v0**2 + w0**2) # Quaternion State e0 = e.item(0) e1 = e.item(1) e2 = e.item(2) e3 = e.item(3) phi0, theta0, psi0 = Quaternion2Euler(e) ###################################################################################### # Physical Parameters ###################################################################################### mass = 11. #kg Jx = 0.8244 #kg m^2 Jy = 1.135 Jz = 1.759 # Jxz = 0.1204 Jxz = 0.0 J = np.matrix([[Jx, 0, -Jxz], [0, Jy, 0], [-Jxz, 0, Jz]]) S_wing = 0.55 b = 2.8956 c = 0.18994 S_prop = 0.2027
def update_sensors(self): "Return value of sensors on MAV: gyros, accels, static_pressure, dynamic_pressure, GPS" Fx = self._forces[0] Fy = self._forces[1] Fz = self._forces[2] m = MAV.mass g = MAV.gravity e = self._state[6:10] phi, th, psi = Quaternion2Euler(e) p, q, r = self._state[10:13] rho = MAV.rho h_AGL = -self._state[2] Va = self._Va K_gps = SENSOR.K_gps Ts = SENSOR.ts_gps pn = self._state[0] pe = self._state[1] ph = -self._state[2] Vg_n = self.Vg_i[0] Vg_e = self.Vg_i[1] n_gyro_x = np.random.normal(0.0, SENSOR.gyro_sigma) n_gyro_y = np.random.normal(0.0, SENSOR.gyro_sigma) n_gyro_z = np.random.normal(0.0, SENSOR.gyro_sigma) n_accel_x = np.random.normal(0.0, SENSOR.accel_sigma) n_accel_y = np.random.normal(0.0, SENSOR.accel_sigma) n_accel_z = np.random.normal(0.0, SENSOR.accel_sigma) n_abs_pres = np.random.normal(0.0, SENSOR.static_pres_sigma) n_dif_pres = np.random.normal(0.0, SENSOR.diff_pres_sigma) n_gps_n = np.random.normal(0.0, SENSOR.gps_n_sigma) n_gps_e = np.random.normal(0.0, SENSOR.gps_e_sigma) n_gps_h = np.random.normal(0.0, SENSOR.gps_h_sigma) n_gps_v = np.random.normal(0.0, SENSOR.gps_Vg_sigma) n_gps_chi = np.random.normal(0.0, SENSOR.gps_course_sigma) B_gyro_x = SENSOR.gyro_x_bias B_gyro_y = SENSOR.gyro_y_bias B_gyro_z = SENSOR.gyro_z_bias B_abs_pres = SENSOR.static_pres_beta B_dif_pres = SENSOR.diff_pres_beta self._sensors.gyro_x = (p + B_gyro_x + n_gyro_x)[0] self._sensors.gyro_y = (q + B_gyro_y + n_gyro_y)[0] self._sensors.gyro_z = (r + B_gyro_z + n_gyro_z)[0] self._sensors.accel_x = (Fx / m + g * np.sin(th) + n_accel_x)[0] self._sensors.accel_y = (Fy / m - g * np.cos(th) * np.sin(phi) + n_accel_y)[0] self._sensors.accel_z = (Fz / m - g * np.cos(th) * np.cos(phi) + n_accel_z)[0] self._sensors.static_pressure = (rho * g * h_AGL + B_abs_pres + n_abs_pres)[0] self._sensors.diff_pressure = rho * Va**2 / 2.0 + B_dif_pres + n_dif_pres if self._t_gps >= Ts: self._gps_eta_n = math.exp( -K_gps * self._t_gps) * self._gps_eta_n + n_gps_n self._gps_eta_e = math.exp( -K_gps * self._t_gps) * self._gps_eta_n + n_gps_e self._gps_eta_h = math.exp( -K_gps * self._t_gps) * self._gps_eta_n + n_gps_h self._sensors.gps_n = (pn + self._gps_eta_n)[0] self._sensors.gps_e = (pe + self._gps_eta_e)[0] self._sensors.gps_h = (ph + self._gps_eta_h)[0] self._sensors.gps_Vg = (np.sqrt(Vg_n**2 + Vg_e**2) + n_gps_v)[0] self._sensors.gps_course = -math.atan2(Vg_e, Vg_n) + n_gps_chi self._t_gps = 0. else: self._t_gps += self._ts_simulation
def sensors(self): "Return value of sensors on MAV: gyros, accels, absolute_pressure, dynamic_pressure, GPS" phi, theta, psi = Quaternion2Euler(self._state[6:10]) u = self._state.item(3) v = self._state.item(4) w = self._state.item(5) uvw = np.array([[u, v, w]]).T Rib = Quaternion2Rotation(self._state[6:10]) pdot = Rib @ uvw p = self._state.item(10) q = self._state.item(11) r = self._state.item(12) # simulate rate gyros(units are rad / sec) self._sensors.gyro_x = p + np.random.normal(SENSOR.gyro_x_bias, SENSOR.gyro_sigma) self._sensors.gyro_y = q + np.random.normal(SENSOR.gyro_y_bias, SENSOR.gyro_sigma) self._sensors.gyro_z = r + np.random.normal(SENSOR.gyro_z_bias, SENSOR.gyro_sigma) # simulate accelerometers(units of g) self._sensors.accel_x = self._forces[ 0, 0] / MAV.mass + MAV.gravity * np.sin(theta) + np.random.normal( 0.0, SENSOR.accel_sigma) self._sensors.accel_y = self._forces[ 1, 0] / MAV.mass - MAV.gravity * np.cos(theta) * np.sin( phi) + np.random.normal(0.0, SENSOR.accel_sigma) self._sensors.accel_z = self._forces[ 2, 0] / MAV.mass - MAV.gravity * np.cos(theta) * np.cos( phi) + np.random.normal(0.0, SENSOR.accel_sigma) # simulate magnetometers # magnetic field in provo has magnetic declination of 12.5 degrees # and magnetic inclination of 66 degrees iota = np.radians(66) delta = np.radians(12.5) R_mag = Euler2Rotation(0.0, -iota, delta).T # magnetic field in inertial frame: unit vector mag_inertial = R_mag @ np.array([1, 0, 0]) R = Rib.T # body to inertial # magnetic field in body frame: unit vector mag_body = R @ mag_inertial self._sensors.mag_x = mag_body[0] + np.random.normal( SENSOR.mag_beta, SENSOR.mag_sigma) self._sensors.mag_y = mag_body[0] + np.random.normal( SENSOR.mag_beta, SENSOR.mag_sigma) self._sensors.mag_z = mag_body[0] + np.random.normal( SENSOR.mag_beta, SENSOR.mag_sigma) # simulate pressure sensors self._sensors.abs_pressure = MAV.rho * MAV.gravity * -self._state.item( 2) + np.random.normal(0, SENSOR.abs_pres_sigma) self._sensors.diff_pressure = MAV.rho * self._Va**2 / 2 + np.random.normal( 0.0, SENSOR.diff_pres_sigma) # simulate GPS sensor if self._t_gps >= SENSOR.ts_gps: self._gps_nu_n = self._gps_nu_n * np.exp( -SENSOR.ts_gps * SENSOR.gps_k) + np.random.normal( 0.0, SENSOR.gps_n_sigma) self._gps_nu_e = self._gps_nu_e * np.exp( -SENSOR.ts_gps * SENSOR.gps_k) + np.random.normal( 0.0, SENSOR.gps_e_sigma) self._gps_nu_h = self._gps_nu_h * np.exp( -SENSOR.ts_gps * SENSOR.gps_k) + np.random.normal( 0.0, SENSOR.gps_h_sigma) self._sensors.gps_n = self._state.item(0) + self._gps_nu_n self._sensors.gps_e = self._state.item(1) + self._gps_nu_e self._sensors.gps_h = -self._state.item(2) + self._gps_nu_h self._sensors.gps_Vg = np.sqrt( (self._Va * np.cos(psi) + self.true_state.wn)**2 + (self._Va * np.sin(psi) + self.true_state.we)**2 ) + np.random.normal(0, SENSOR.gps_Vg_sigma) self._sensors.gps_course = np.arctan2( self._Va * np.sin(psi) + self.true_state.we, self._Va * np.cos(psi) + self.true_state.we) + np.random.normal(0.0, SENSOR.gps_course_sigma) self._t_gps = 0. else: self._t_gps += self._ts_simulation return self._sensors