# use compute_trim function to compute trim state and trim input
Va = 25.
gamma = 0. * np.pi / 180.
R = np.inf
display_transfer = True
display_ss = True
display_trim = True
trim_state, trim_input = compute_trim(mav, Va, gamma, R, display=display_trim)
mav._state = trim_state # set the initial state of the mav to the trim state
delta = trim_input # set input to constant constant trim input
# # compute the state space model linearized about trim
A_lon, B_lon, A_lat, B_lat = compute_ss_model(mav,
trim_state,
trim_input,
display=display_transfer)
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 \
= compute_tf_model(mav, trim_state, trim_input, display=display_ss)
# initialize the simulation time
sim_time = SIM.start_time
# main simulation loop
print(trim_input)
print("Press Command-Q to exit...")
while sim_time < SIM.end_time:
#-------physical system-------------
#current_wind = wind.update() # get the new wind vector
mav_view = mav_viewer() # initialize the mav viewer
data_view = data_viewer() # initialize view of data plots
# initialize elements of the architecture
wind = wind_simulation(SIM.ts_simulation)
mav = mav_dynamics(SIM.ts_simulation)
# use compute_trim function to compute trim state and trim input
Va = 25.
gamma = 0. * np.pi / 180.
trim_state, trim_input = compute_trim(mav, Va, gamma)
mav._state = trim_state # set the initial state of the mav to the trim state
delta = trim_input # set input to constant constant trim input
# # compute the state space model linearized about trim
A_lon, B_lon, A_lat, B_lat = compute_ss_model(mav, trim_state, trim_input)
print('A_lon: \n', A_lon)
print('B_lon: \n', B_lon)
print('A_lat: \n', A_lat)
print('B_lat: \n', B_lat)
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, T_v_delta_r \
= compute_tf_model(mav, trim_state, trim_input)
#print('T_phi_delta_a: \n',T_phi_delta_a)
#print('T_chi_phi: \n',T_chi_phi)
#print('T_theta_delta_e: \n',T_theta_delta_e)
#print('T_h_theta: \n',T_h_theta)
#print('T_h_Va: \n',T_h_Va)
delta_e_star = MAV.delta_e_star
alpha_star = MAV.alpha_star
a_v_1 = MAV.rho*MAV.Va0*MAV.S_prop/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*Va0
a_v_2 = MAV.rho*MAV.S_prop/MAV.mass*MAV.C_prop*MAV.k_motor**2*delta_t_star
airspeed_throttle_kp = wn_v**2/a_v_2
airspeed_throttle_ki = (2.*zeta_v*wn_v - a_v_1)/a_v_2
# --------------------------------------------------------------------------
# ------------------------ LQR PARAMETERS ----------------------------------
# --------------------------------------------------------------------------
mav = mav_dynamics(SIM.ts_simulation)
Va = 25.
gamma = 0.*np.pi/180.
R = np.inf
trim_state, trim_input = compute_trim(mav, Va, gamma, R)
A_lon, B_lon, A_lat, B_lat = compute_ss_model(mav, trim_state, trim_input, euler=True)
# big value implies more cost for variable, low means low cost
# Q_lat = np.diag((1.0/1.0, 1.0/1.0, 1.0/1.0, 1.0/2.0, 1.0/2.0, 1.0/40.0)) # v, p, r, phi, psi, psi_int
Q_lat = np.diag((1.0/1.0, 1.0/1.0, 1.0/1.0, 0.0/2.0, 1.0/10.0, 1.0/100.0)) # v, p, r, phi, psi, psi_int
R_lat = np.diag((50.0/1.0, 50.0/1.0)) # da, dr
H_lat = np.array([[0, 0, 0, 0, 1]])
A_lat_lqr = np.block([[A_lat, np.zeros((len(A_lat), len(H_lat)))],
[H_lat, np.zeros((len(H_lat), len(H_lat)))]])
B_lat_lqr = np.block([[B_lat],
[np.zeros((len(H_lat), len(B_lat[0])))]])
limit_lat_lqr = np.array([[np.radians(45.)], [-np.radians(45.)], [np.radians(45.)], [-np.radians(45.)]])
# u_tilde, w, q, theta, h_tilde, h_int, Va_int
Q_lon = np.diag((10000.0/1.0, 0.0/1.0, 1.0/10.0, 0.0/1.0, 20.0/1.0, 1.0/1.0, 10.0/1.0)) # u, w, q, theta, h, h_int, Va_int
# de, dt
R_lon = np.diag((600.0/1.0, 500.0/1.0)) # de, dt