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_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 self.msg_true_state.Vg = np.sqrt((self._state.item(3))**2 + (self._state.item(4))**2 + (self._state.item(5))**2) Vg = np.array( [self._state.item(3), self._state.item(4), self._state.item(5)]) Vg_M = np.linalg.norm(Vg) Vg_h = np.array([self._state.item(3), self._state.item(4), 0.]) Vg_h_M = np.linalg.norm(Vg_h) Va_h = np.array([self._ur, self._vr, 0.]) Va_h_M = np.linalg.norm(Va_h) self.msg_true_state.gamma = np.arccos(Vg.dot(Vg_h) / (Vg_M * Vg_h_M)) num = Vg_h.dot(Va_h) den = (Vg_h_M * Va_h_M) frac = np.round(num / den, 8) self.msg_true_state.chi = psi + self._beta + np.arccos(frac) 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_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 = self.Euler2Rotation(phi, theta, psi) Vg_b = self.Va_b + self._wind Vg_i = R.T @ Vg_b self.msg_true_state.Vg = np.linalg.norm(Vg_b) self.msg_true_state.gamma = np.arctan2( -Vg_i[2], np.sqrt(Vg_i[0]**2 + Vg_i[1]**2)) self.msg_true_state.chi = -np.arctan2(Vg_i[1], 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 compute_tf_model( trim_state, trim_input): #computes the transfer function linearized at trim # trim values mav = mav_dynamics(SIM.ts_simulation) mav.set_state(trim_state) mav._update_velocity_data() Va_trim = mav._Va alpha_trim = mav._alpha beta_trim = mav._beta phi, theta_trim, psi = Quaternion2Euler(trim_state[6:10]) delta_t_trim = trim_input[3, 0] _dT_dVa = dT_dVa(Va_trim, delta_t_trim) _dT_ddelta_t = dT_ddelta_t(Va_trim, delta_t_trim) # define transfer function constants a_phi1 = -0.25 * MAV.rho * mav._Va * MAV.S_wing * (MAV.b**2) * MAV.C_p_p a_phi2 = .5 * MAV.rho * (mav._Va**2) * MAV.S_wing * MAV.b * MAV.C_p_delta_a a_theta1 = -MAV.rho * mav._Va * (MAV.c** 2) * MAV.S_wing * MAV.C_m_q / (4 * MAV.Jy) a_theta2 = -MAV.rho * (mav._Va**2) * MAV.c * MAV.S_wing * MAV.C_m_alpha / ( 2 * MAV.Jy) a_theta3 = MAV.rho * ( mav._Va**2) * MAV.c * MAV.S_wing * MAV.C_m_delta_e / (2 * MAV.Jy) a_V1 = (MAV.rho * Va_trim * MAV.S_wing / MAV.mass) * (MAV.C_D_0 + MAV.C_D_alpha * alpha_trim + MAV.C_D_delta_e) - _dT_dVa / MAV.mass a_V2 = _dT_ddelta_t / MAV.mass a_V3 = MAV.gravity * np.cos(theta_trim - alpha_trim) return Va_trim, alpha_trim, beta_trim, theta_trim, a_phi1, a_phi2, a_theta1, a_theta2, a_theta3, a_V1, a_V2, a_V3
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], self._state[7], self._state[8], self._state[9]) 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.Vs = self._Vs 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 = self._Vg self.msg_true_state.gamma = self._gamma self.msg_true_state.chi = self._chi 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) self.msg_true_state.e0 = self._state[6] self.msg_true_state.e1 = self._state[7] self.msg_true_state.e2 = self._state[8] self.msg_true_state.e3 = self._state[9]
def _update_velocity_data(self, wind=np.zeros((6, 1))): # self.wind = wind_simulation(SIM.ts_simulation) # how can I know there are six vector come here? wn_s = wind.item(0) we_s = wind.item(1) wd_s = wind.item(2) wind_ss = [wn_s, we_s, wd_s] phi, theta, psi = Quaternion2Euler(self._state[6:10]) angles = phi, theta, psi wind_ss_b = TransformationFormInertialToBody(wind_ss, angles) wn_b = wind_ss_b[0] we_b = wind_ss_b[1] wd_b = wind_ss_b[2] u_w = wn_b + wind.item(3) v_w = we_b + wind.item(4) w_w = wd_b + wind.item(5) u = self._state.item(3) v = self._state.item(4) w = self._state.item(5) u_r = u - u_w v_r = v - v_w w_r = w - w_w # compute airspeed self._Va = np.sqrt(u_r**2 + v_r**2 + w_r**2) # compute angle of attack self._alpha = np.arctan(w_r / u_r) # compute sideslip angle self._beta = np.arcsin(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 = self.Euler2Rotation(phi, th, psi) @ wind[0:3] self._wind = 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] - self._wind[0][0]], [self._state[4][0] - self._wind[1][0]], [self._state[5][0] - self._wind[2][0]]]) self._Va = np.linalg.norm(self.Va_b) Vg_b = self.Va_b + self._wind self._Vg = np.linalg.norm(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 _update_velocity_data(self, wind=np.zeros((6, 1))): e0 = self._state.item(6) e1 = self._state.item(7) e2 = self._state.item(8) e3 = self._state.item(9) phi, theta, psi = Quaternion2Euler(np.array([e0, e1, e2, e3])) Rvb = RotationVehicle2Body(phi, theta, psi) # calculate wind components self._wind = wind[0:3] wind_combined = Rvb @ wind[0:3] + wind[3:6] # compute relative airspeed components self._ur = self._state.item(3) - wind_combined.item(0) self._vr = self._state.item(4) - wind_combined.item(1) self._wr = self._state.item(5) - wind_combined.item(2) self.Vg = Rvb.transpose() @ self._state[3:6] # In vehicle frame self._Vg = np.linalg.norm(self.Vg) # compute airspeed self._Va = np.sqrt(self._ur**2 + self._vr**2 + self._wr**2) # compute angle of attack self._alpha = np.arctan2(self._wr, self._ur) # compute sideslip angle self._beta = np.arcsin(self._vr / self._Va)
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 self.msg_true_state.Vg = np.sqrt( self.Vg.item(0)**2 + self.Vg.item(1)**2 + self.Vg.item(2)**2) self.msg_true_state.gamma = np.arctan2( -self.Vg[2], np.sqrt(self.Vg[0]**2 + self.Vg[1]**2)) self.msg_true_state.chi = np.arctan2(self.Vg[1], self.Vg[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) self.msg_true_state.phi_g = 0.0 self.msg_true_state.theta_g = 0.0 self.msg_true_state.psi_g = 0.0
def set_state(self, state): self._state = state temp1, temp2, temp3 = Quaternion2Euler(self._state[6:10]) self.phi = temp1[0] # roll self.theta = temp2[0] # pitch self.psi = temp3[0] #yaw self.h = -self._state[2, 0]
def update_sensors(self): "Return value of sensors on MAV: gyros, accels, static_pressure, dynamic_pressure, GPS" pn = self._state.item(0) pe = self._state.item(1) h = -self._state.item(2) e0 = self._state.item(6) e1 = self._state.item(7) e2 = self._state.item(8) e3 = self._state.item(9) phi, theta, psi = Quaternion2Euler(np.array([e0, e1, e2, e3])) p = self._state.item(10) q = self._state.item(11) r = self._state.item(12) Va = self._Va wn = self._wind.item(0) we = self._wind.item(1) self.sensors.gyro_x = p + SENSOR.gyro_sigma * np.random.randn() self.sensors.gyro_y = q + SENSOR.gyro_sigma * np.random.randn() self.sensors.gyro_z = r + SENSOR.gyro_sigma * np.random.randn() self.sensors.accel_x = float(self._forces[0] / MAV.mass + MAV.gravity * np.sin(theta) + SENSOR.accel_sigma * np.random.randn()) self.sensors.accel_y = float(self._forces[1] / MAV.mass - MAV.gravity * np.cos(theta) * np.sin(phi) + SENSOR.accel_sigma * np.random.randn()) self.sensors.accel_z = float(self._forces[2] / MAV.mass - MAV.gravity * np.cos(theta) * np.cos(phi) + SENSOR.accel_sigma * np.random.randn()) self.sensors.static_pressure = MAV.rho * MAV.gravity * h + SENSOR.static_pres_sigma * np.random.randn( ) self.sensors.diff_pressure = MAV.rho * Va**2 / 2. + SENSOR.diff_pres_sigma * np.random.randn( ) if self._t_gps >= SENSOR.ts_gps: kGPS = 1.0 / 16000. self._gps_eta_n = np.exp( -kGPS * SENSOR.ts_gps ) * self._gps_eta_n + SENSOR.gps_n_sigma * np.random.randn() self._gps_eta_e = np.exp( -kGPS * SENSOR.ts_gps ) * self._gps_eta_e + SENSOR.gps_e_sigma * np.random.randn() self._gps_eta_h = np.exp( -kGPS * SENSOR.ts_gps ) * self._gps_eta_h + SENSOR.gps_h_sigma * np.random.randn() self.sensors.gps_n = pn + self._gps_eta_n self.sensors.gps_e = pe + self._gps_eta_e self.sensors.gps_h = h + self._gps_eta_h self.sensors.gps_Vg = np.sqrt( (Va * np.cos(psi) + wn)**2 + (Va * np.sin(psi) + we)**2) + SENSOR.gps_Vg_sigma * np.random.randn() self.sensors.gps_course = np.arctan2( Va * np.sin(psi) + we, Va * np.cos(psi) + wn) + SENSOR.gps_course_sigma * np.random.randn() self._t_gps = 0. else: self._t_gps += self._ts_simulation
def dtI_dq(mav, x_euler, delta): [phi, theta, psi] = Quaternion2Euler(x_euler[6:10]) dtI_dq = np.zeros([13, 12]) dtI_dq[0:6, 0:6] = np.eye(6) dtI_dq[10:, 9:] = np.eye(3) dtI_dq[6:10, 6:9] = jacobian(Euler2Quaternion, mav, (phi, theta, psi), delta, 3) return dtI_dq
def euler_state(x_quat): # convert state x with attitude represented by quaternion # to x_euler with attitude represented by Euler angles e = np.array([[x_quat[6], x_quat[7], x_quat[8], x_quat[9]]]) [phi, theta, psi] = Quaternion2Euler(e) x_euler = np.array([[x_quat[0][0]], [x_quat[1][0]], [x_quat[2][0]], [x_quat[3][0]], [x_quat[4][0]], [x_quat[5][0]], [phi], [theta], [psi], [x_quat[10][0]], [x_quat[11][0]], [x_quat[12][0]]]) return x_euler
def dtheta_dq(quat): h = 0.005 Jacobian = np.zeros((3, 4)) for i in range(4): q_p = np.copy(quat) q_p[i][0] += h q_m = np.copy(quat) q_m[i][0] -= h phi, theta, psi = Quaternion2Euler(q_p) f_p = np.array([[phi, theta, psi]]).T phi, theta, psi = Quaternion2Euler(q_m) f_m = np.array([[phi, theta, psi]]).T Jac_col_i = (f_p - f_m) / (2 * h) Jacobian[:, i] = Jac_col_i[:, 0] return Jacobian
def euler_state(x_quat): # convert state x with attitude represented by quaternion # to x_euler with attitude represented by Euler angles phi, theta, psi = Quaternion2Euler(x_quat[6:10]) x_euler = np.empty((12, 1)) x_euler[:6] = x_quat[:6] x_euler[6:9] = np.array([[phi], [theta], [psi]]) x_euler[9:12] = x_quat[10:13] return x_euler
def __init__(self, Ts): self._ts_simulation = Ts # set initial states based on parameter file # _state is the 13x1 internal state of the aircraft that is being propagated: # _state = [pn, pe, pd, u, v, w, e0, e1, e2, e3, p, q, r] # We will also need a variety of other elements that are functions of the _state and the wind. # self.true_state is a 19x1 vector that is estimated and used by the autopilot to control the aircraft: # true_state = [pn, pe, h, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi, gyro_bx, gyro_by, gyro_bz] self._state = np.array([ [MAV.pn0], # (0) # inertial north position [MAV.pe0], # (1) # inertial east position [MAV.pd0], # (2) # inertial down position, neg of altitude [MAV.u0], # (3) # Body frame velocity nose direction (i) [MAV.v0], # (4) # Body frame velocity right wing direction (j) [MAV.w0], # (5) # Body frame velocity down direction (l) # Quaternion rotation from inertial frame to the body frame [MAV.e0 ], # (6) # related to scalar part of rotation = cos(theta/2) [ MAV.e1 ], # (7) # related to vector we are rotating about = v*sin(theta/2) [MAV.e2], # (8) # " " [MAV.e3], # (9) # " " [MAV.p0], # (10) # roll rate in body frame [MAV.q0], # (11) # pitch rate in body frame [MAV.r0] ]) # (12) # yaw rate in body frame #rotation from vehicle to body self.h = -self._state[2, 0] self.Rbv = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) #rotation from vehicle to body # store wind data for fast recall since it is used at various points in simulation self._wind = np.array([[0.], [0.], [ 0. ]]) # wind in NED frame in meters/sec (velocity of wind [uw, vw, ww]) self._update_velocity_data() # store forces to avoid recalculation in the sensors function self._forces = np.array( [[0.], [0.], [0.]]) #forces acting on mav in body frame [fx,fy,fz] self._Va = MAV.Va0 # velocity magnitude of airframe relative to airmass self._alpha = 0 #angle of attack self._beta = 0 #sideslip angle temp1, temp2, temp3 = Quaternion2Euler(self._state[6:10]) self.phi = temp1[0] # roll self.theta = temp2[0] # pitch self.psi = temp3[0] #yaw self.gamma = 0 #flight path angle (pitch up from horizontal velocity) self.chi = 0 #course angle (heading) self.Vg = np.linalg.norm( np.dot(self.Rbv.T, np.array([[MAV.u0], [MAV.v0], [MAV.w0]]))) # initialize true_state message self.msg_true_state = msg_state() self.breakpoint = True
def euler_state(x_quat): # convert state x with attitude represented by quaternion # to x_euler with attitude represented by Euler angles x_euler = np.zeros((12, 1)) phi, theta, psi = Quaternion2Euler(x_quat[6:10]) x_euler[0:6] = x_quat[0:6] x_euler[6] = phi x_euler[7] = theta x_euler[8] = psi x_euler[9:12] = x_quat[10:13] return x_euler
def _update_msg_true_state(self): # update the true state message: 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.phi = phi self.msg_true_state.theta = theta self.msg_true_state.psi = psi 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)
def update_sensors(self): "Return value of sensors on MAV: gyros, accels, static_pressure, dynamic_pressure, GPS" phi, theta, psi = Quaternion2Euler(self._state[6:10]) cos, sin = np.cos, np.sin self.sensors.gyro_x = self._state.item(10) + np.random.normal( SENSOR.gyro_x_bias, SENSOR.gyro_sigma) self.sensors.gyro_y = self._state.item(11) + np.random.normal( SENSOR.gyro_y_bias, SENSOR.gyro_sigma) self.sensors.gyro_z = self._state.item(12) + np.random.normal( SENSOR.gyro_z_bias, SENSOR.gyro_sigma) self.sensors.accel_x = np.asscalar( (self._forces.item(0) / MAV.mass) + MAV.gravity * sin(theta) + np.random.normal(0, SENSOR.accel_sigma)) self.sensors.accel_y = np.asscalar( (self._forces.item(1) / MAV.mass) - MAV.gravity * cos(theta) * sin(phi) + np.random.normal(0, SENSOR.accel_sigma)) self.sensors.accel_z = np.asscalar( (self._forces.item(2) / MAV.mass) - MAV.gravity * cos(theta) * cos(phi) + np.random.normal(0, SENSOR.accel_sigma)) self.sensors.static_pressure = MAV.rho * MAV.gravity * self._state.item( 2) + np.random.normal(0, SENSOR.static_pres_sigma) self.sensors.diff_pressure = (MAV.rho * self._Va**2) / 2 + np.random.normal( 0, SENSOR.diff_pres_sigma) if self._t_gps >= SENSOR.ts_gps: # v[n+1] self._gps_eta_n = np.exp(-SENSOR.gps_beta * SENSOR.ts_gps ) * self._gps_eta_n + np.random.normal( 0, SENSOR.gps_n_sigma) self._gps_eta_e = np.exp(-SENSOR.gps_beta * SENSOR.ts_gps ) * self._gps_eta_e + np.random.normal( 0, SENSOR.gps_e_sigma) self._gps_eta_h = np.exp(-SENSOR.gps_beta * SENSOR.ts_gps ) * self._gps_eta_h + np.random.normal( 0, SENSOR.gps_h_sigma) # yGPS,_[n] = p_[n] + v_[n] 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 self.sensors.gps_Vg = np.asscalar( np.sqrt((self._Va * cos(psi) + self.msg_true_state.wn)**2 + (self._Va * sin(psi) + self.msg_true_state.we)**2) + np.random.normal(0, SENSOR.gps_Vg_sigma)) self.sensors.gps_course = np.asscalar( np.arctan2(self._Va * sin(psi) + self.msg_true_state.we, self._Va * cos(psi) + self.msg_true_state.wn) + np.random.normal(0, SENSOR.gps_Vg_sigma)) self._t_gps = 0. else: self._t_gps += self._ts_simulation
def _update_velocity_data(self, wind=np.zeros((6, 1))): # compute airspeed e0 = self._state.item(6) e1 = self._state.item(7) e2 = self._state.item(8) e3 = self._state.item(9) #print("EEEES=",e0,e1,e2,e3) phi, theta, psi = Quaternion2Euler(np.array([e0, e1, e2, e3])) #print("angles=",phi,theta,psi) Rv2b = RotationVehicle2Body(phi, theta, psi) wind_result = np.matmul(Rv2b, wind[0:3]) + wind[3:6] self._wind = np.matmul(Rv2b, wind_result) uw = wind_result.item(0) vw = wind_result.item(1) ww = wind_result.item(2) ur = self._state[3] - uw vr = self._state[4] - vw wr = self._state[5] - ww ur = ur.item(0) vr = vr.item(0) wr = wr.item(0) self._Va = sqrt(ur**2 + vr**2 + wr**2) #print("update_velocities =",self._Va) # compute angle of attack self._alpha = atan2(wr, ur) # compute sideslip angle self._beta = np.arcsin(vr / self._Va) # Vg, chi, gamma Rb2v = RotationBody2Vehicle(phi, theta, psi) Vg_result = np.matmul(Rb2v, self._state[3:6]) Vg_n = Vg_result.item(0) Vg_e = Vg_result.item(1) Vg_d = Vg_result.item(2) self._Vg = sqrt(Vg_n**2 + Vg_e**2 + Vg_d**2) self.gamma = atan2(-Vg_d, sqrt(Vg_n**2 + Vg_e**2)) self.chi = atan2(Vg_e, Vg_n) # angle wrapping for commanded phi help if self.chi_prev > 0.75 * np.pi and self.chi < 0.0: self.chi += 2. * np.pi if self.chi_prev < -0.75 * np.pi and self.chi > 0.0: self.chi -= 2. * np.pi self.chi_prev = self.chi
def showMeQuat(self): beginquat = np.array([[1, 0, 0, 0]]).T endquat = Euler2Quaternion(self.phi, self.theta, self.psi) dist = np.linalg.norm(beginquat - endquat) howmany = int(dist * 100 / 1.2) e0 = np.linspace(beginquat.item(0), endquat.item(0), num=howmany) e1 = np.linspace(beginquat.item(1), endquat.item(1), num=howmany) e2 = np.linspace(beginquat.item(2), endquat.item(2), num=howmany) e3 = np.linspace(beginquat.item(3), endquat.item(3), num=howmany) for i in range(howmany): quat = np.array([[e0[i], e1[i], e2[i], e3[i]]]).T phi, theta, psi = Quaternion2Euler(quat) self.plot2(phi, theta, psi) self.plot2(self.phi, self.theta, self.psi)
def euler_state(x_quat): # convert state x with attitude represented by quaternion # to x_euler with attitude represented by Euler angles e = np.array( [x_quat.item(6), x_quat.item(7), x_quat.item(8), x_quat.item(9)]) [phi, theta, psi] = Quaternion2Euler(e) x_euler = np.array([[x_quat.item(0)], [x_quat.item(1)], [x_quat.item(2)], [x_quat.item(3)], [x_quat.item(4)], [x_quat.item(5)], [phi], [theta], [psi], [x_quat.item(10)], [x_quat.item(11)], [x_quat.item(12)]]) return x_euler
def getALat(mav, trim_state, trim_input): # take partial of f_euler with respect to x_euler rho = MAV.rho m = MAV.mass u = mav._state.item(3) w = mav._state.item(5) Va = mav._Va S = MAV.S_wing b = MAV.b c = MAV.c Cpp = MAV.C_p_p Cpb = MAV.C_p_beta Cp0 = MAV.C_p_0 # b_2Va = b / (2*Va) # c_2Va = c / (2*Va) # beta = mav._beta # alpha = mav._alpha phi, theta, psi = Quaternion2Euler(trim_state[6:10]) frac = rho * S / 2 Y_v = frac * MAV.C_Y_beta / m * Va Y_p = w + frac * Va * b / (2 * m) * MAV.C_Y_p Y_r = -u + frac * Va * b / (2 * m) * MAV.C_Y_r Y_da = frac * (Va**2) / m * MAV.C_Y_delta_a Y_dr = frac * (Va**2) / m * MAV.C_Y_delta_r L_v = frac * b * Cpb * Va L_p = frac * Va * (b**2) / 2.0 * Cpp L_r = frac * Va * (b**2) / 2.0 * MAV.C_p_r L_da = frac * (Va**2) * b * MAV.C_p_delta_a L_dr = frac * (Va**2) * b * MAV.C_p_delta_r N_v = frac * b * MAV.C_r_beta * Va N_p = frac * Va * (b**2) / 2.0 * MAV.C_r_p N_r = frac * Va * (b**2) / 2.0 * MAV.C_r_r N_da = frac * (Va**2) * b * MAV.C_r_delta_a N_dr = frac * (Va**2) * b * MAV.C_r_delta_r A_lat = np.array([ [Y_v, Y_p, Y_r, MAV.gravity * np.cos(theta) * np.cos(phi), 0], [L_v, L_p, L_r, 0, 0], [N_v, N_p, N_r, 0, 0], [0, 1, np.cos(phi) * np.tan(theta), 0, 0], [0, 0, np.cos(phi) / np.cos(theta), 0, 0], ]) return A_lat
def _update_msg_true_state(self): # update the true state message: 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.phi = phi self.msg_true_state.theta = theta self.msg_true_state.psi = psi 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.Va = self._Va self.msg_true_state.alpha = self._alpha # see line 164 updateVelocityData self.msg_true_state.beta = self._beta # see line 167 updateVelocityData self.msg_true_state.Vg = np.linalg.norm(self._state[3:6]) self.calcGammaAndChi()
def _update_msg_true_state(self): # update the true state message: 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.phi = phi self.msg_true_state.theta = theta self.msg_true_state.psi = psi 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.Va = self._Va self.msg_true_state.alpha = self._alpha # see line 164 updateVelocityData self.msg_true_state.beta = self._beta # see line 167 updateVelocityData self.msg_true_state.Vg = np.linalg.norm(self._state[3:6]) self.gamma = np.arctan2(-self._state.item(5), self._state.item(3)) # is this right atan2(-w, u) self.chi = np.arctan2(self._state.item(4), self._state.item(3)) + psi # atan2(v, u)
def _update_msg_true_state(self): phi, theta, psi = Quaternion2Euler(self._state[6], self._state[7], self._state[8], self._state[9]) 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 = self._Vg self.msg_true_state.gamma = self._gamma self.msg_true_state.chi = self._chi 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_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 u = self._state.item(3) v = self._state.item(4) w = self._state.item(5) e0 = self._state.item(6) e1 = self._state.item(7) e2 = self._state.item(8) e3 = self._state.item(9) # position kinematics 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 self.msg_true_state.Vg = np.sqrt((self._state[3][0])**2 + (self._state[4][0])**2 + (self._state[5][0])**2) self.msg_true_state.gamma = np.arcsin( np.sqrt(pn_dot**2 + pe_dot**2) / np.sqrt(pn_dot**2 + pe_dot**2 + pd_dot**2)) self.msg_true_state.chi = np.arctan(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_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 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_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 self.msg_true_state.Vg = np.linalg.norm(self._state[3:6]) self.msg_true_state.gamma = np.arctan2(-self._state.item(5),self._state.item(3)) self.msg_true_state.chi = np.arctan2(self._state.item(4),self._state.item(3)) 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) self._update_gamma_chi()
def getALon(mav, trim_state, trim_input): rho = MAV.rho m = MAV.mass u = mav._state.item(3) w = mav._state.item(5) Va = mav._Va S = MAV.S_wing b = MAV.b c = MAV.c Cpp = MAV.C_p_p Cpb = MAV.C_p_beta Cp0 = MAV.C_p_0 # b_2Va = b / (2*Va) # c_2Va = c / (2*Va) de = trim_input.item(0) beta = mav._beta alpha = mav._alpha phi, theta, psi = Quaternion2Euler(trim_state[6:10]) # X_u = u*rho*S/m * (MAV.C_X_0 + MAV.C_X_alpha*alpha + MAV.C_X_delta_e*de) \ A_lon - 0 return A_lon