def __init__(self, Ts):
# steady state wind defined in the inertial frame
self._steady_state = np.array([[3.0], [2.0], [0.0]])
#given low altitude and light turbulance
Lu = 200
Lv = Lu
Lw = 50
sig_u = 1.06
sig_v = sig_u
sig_w = 0.7
Va = 0.0
a1 = sig_u * np.sqrt(2 * Va / Lu)
a2 = sig_v * np.sqrt(3 * Va / Lv)
a3 = sig_v * np.power(Va / Lv, 3.0 / 2.0)
a4 = sig_w * np.sqrt(3 * Va / Lw)
a5 = sig_w * np.power(Va / Lw, 3.0 / 2.0)
b1 = Va / Lu
b2 = Va / Lv
b3 = Va / Lw
self.u_w = transfer_function(num=np.array([[a1]]),
den=np.array([[1, b1]]),
Ts=Ts)
self.v_w = transfer_function(num=np.array([[a2, a3]]),
den=np.array([[1, 2 * b2, b2**2.0]]),
Ts=Ts)
self.w_w = transfer_function(num=np.array([[a4, a5]]),
den=np.array([[1, 2 * b3, b3**2.0]]),
Ts=Ts)
self._Ts = Ts
def __init__(self, Ts):
# steady state wind defined in the inertial frame
self._steady_state = np.array([0, 0, 0]) #NED
sig_u = 1.06
sig_v = sig_u
sig_w = 0.7
Va = 20
Lu = 200
Lv = Lu
Lw = 50
a1 = sig_u*np.sqrt(2*Va/Lu)
a2 = sig_v*np.sqrt(3*Va/Lv)
a3 = sig_v*np.sqrt(3*Va/Lv) * Va/(np.sqrt(3)*Lv)
a4 = sig_w*np.sqrt(3*Va/Lw)
a5 = sig_w*np.sqrt(3*Va/Lw) * Va/(np.sqrt(3)*Lw)
b1 = Va/Lu
b2 = Va/Lv
b3 = Va/Lw
self.u_w = transfer_function(num=np.array([[a1]]),
den=np.array([[1, b1]]),
Ts=Ts)
self.v_w = transfer_function(num=np.array([[a2, a3]]),
den=np.array([[1, 2*b2, b2**2.0]]),
Ts=Ts)
self.w_w = transfer_function(num=np.array([[a4, a5]]),
den=np.array([[1, 2*b3, b3**2.0]]),
Ts=Ts)
self._Ts = Ts
def __init__(self, Ts):
# steady state wind defined in the inertial frame
Va = 20.
Lu = 200.
Lv = Lu
Lw = 50.
sigma_u = 2.12
sigma_v = sigma_u
sigma_w = 1.4
a1 = sigma_u*np.sqrt(2.0*Va/(np.pi*Lu))
a2 = sigma_v*np.sqrt(3.0*Va/(np.pi*Lv))
a3 = (Va/(np.sqrt(3.0)*Lv))*a2
a4 = sigma_w*np.sqrt(3.0*Va/(np.pi*Lw))
a5 = (Va/(np.sqrt(3.0)*Lw))*a4
b1 = Va/Lu
b2 = Va/Lv
b3 = Va/Lw
self._steady_state = [[1],[-0.5],[0.1]]
self.u_w = transfer_function(num=np.array([[a1]]),
den=np.array([[1, b1]]),
Ts=Ts)
self.v_w = transfer_function(num=np.array([[a2, a3]]),
den=np.array([[1, 2*b2, b2**2.0]]),
Ts=Ts)
self.w_w = transfer_function(num=np.array([[a4, a5]]),
den=np.array([[1, 2*b3, b3**2.0]]),
Ts=Ts)
self._Ts = Ts
def __init__(self, Ts):
# steady state wind defined in the inertial frame
# a1, b1, a2, a3, b2, a4, a5, b3 = pc()
Lu, Lv, Lw = 200, 200, 50
sigma_u, sigma_v = 1.06, 1.06
sigma_w = .7
V_a = 20
c_u = sigma_u * root(2 * V_a / Lu / np.pi)
c_v = sigma_v * root(3 * V_a / Lv / np.pi)
c_w = sigma_w * root(3 * V_a / Lw / np.pi)
a1 = c_u
b1 = V_a / Lu
a2 = c_v
a3 = c_v * V_a / Lv / root(3)
b2 = V_a / Lv
a4 = c_w
a5 = c_w * V_a / Lw / root(3)
b3 = V_a / Lw
self._steady_state = np.array([[0, 0, 0]])
self.u_w = transfer_function(num=np.array([[a1]]),
den=np.array([[1, b1]]),
Ts=Ts)
self.v_w = transfer_function(num=np.array([[a2, a3]]),
den=np.array([[1, 2 * b2, b2**2.0]]),
Ts=Ts)
self.w_w = transfer_function(num=np.array([[a4, a5]]),
den=np.array([[1, 2 * b3, b3**2.0]]),
Ts=Ts)
self._Ts = Ts
def __init__(self, Ts, Va0=0):
# steady state wind defined in the inertial frame
self._steady_state = np.array([[0], [0], [0]])
#medium altitude, light turbulence (alitutde 600 meters)
sigma_u = 1.5 #m/s
sigma_v = sigma_u
sigma_w = 1.5 #m/s
Lw = 533 #m
Lu = 533 #m
Lv = Lu
a1 = sigma_u * np.sqrt(2 * Va0 / (np.pi * Lu))
b1 = Va0 / Lu
a2 = sigma_v * np.sqrt(3 * Va0 / (np.pi * Lv))
a3 = a2 * Va0 / (np.sqrt(3) * Lv)
b2 = 2 * Va0 / Lv
b3 = (Va0 / Lv)**2
a4 = sigma_w * np.sqrt(3 * Va0 / (np.pi * Lw))
a5 = a4 * Va0 / (np.sqrt(3) * Lw)
b4 = 2 * Va0 / Lw
b5 = (Va0 / Lw)**2
self.u_w = transfer_function(num=np.array([[a1]]),
den=np.array([[1, b1]]),
Ts=Ts)
self.v_w = transfer_function(num=np.array([[a2, a3]]),
den=np.array([[1, b2, b3]]),
Ts=Ts)
self.w_w = transfer_function(num=np.array([[a4, a5]]),
den=np.array([[1, b4, b5]]),
Ts=Ts)
self._Ts = Ts
def __init__(self, Ts):
# steady state wind defined in the inertial frame
self._steady_state = np.array([[windy.Vw_ss_n], [windy.Vw_ss_e],
[windy.Vw_ss_d]])
Va = 25
a1 = windy.sig_u * np.sqrt(2 * Va / (np.pi * windy.L_u))
b1 = Va / windy.L_u
a2 = windy.sig_v * np.sqrt(3 * Va / (np.pi * windy.L_v))
a3 = Va / (np.sqrt(3) * windy.L_v)
b2 = Va / windy.L_v
a4 = windy.sig_w * np.sqrt(3 * Va / (np.pi * windy.L_w))
a5 = Va / (np.sqrt(3) * windy.L_v)
b3 = Va / windy.L_w
# Dryden Gust Params:
self.u_w = transfer_function(num=np.array([[a1]]),
den=np.array([[1, b1]]),
Ts=Ts)
self.v_w = transfer_function(num=np.array([[a2, a3]]),
den=np.array([[1, 2 * b2, b2**2.0]]),
Ts=Ts)
self.w_w = transfer_function(num=np.array([[a4, a5]]),
den=np.array([[1, 2 * b3, b3**2.0]]),
Ts=Ts)
self._Ts = Ts
def __init__(self, Ts):
# steady state wind defined in the inertial frame
self._steady_state =
self.u_w = transfer_function(num=np.array([[a1]]),
den=np.array([[1, b1]]),
Ts=Ts)
self.v_w = transfer_function(num=np.array([[a2, a3]]),
den=np.array([[1, 2*b2, b2**2.0]]),
Ts=Ts)
self.w_w = transfer_function(num=np.array([[a4, a5]]),
den=np.array([[1, 2*b3, b3**2.0]]),
Ts=Ts)
self._Ts = Ts
def __init__(self, ts_control):
# instantiate lateral controllers
self.roll_from_aileron = pd_control_with_rate(kp=AP.roll_kp,
kd=AP.roll_kd,
limit=np.radians(45))
self.course_from_roll = pi_control(kp=AP.course_kp,
ki=AP.course_ki,
Ts=ts_control,
limit=np.radians(30))
self.sideslip_from_rudder = pi_control(kp=AP.sideslip_kp,
ki=AP.sideslip_ki,
Ts=ts_control,
limit=np.radians(45))
self.yaw_damper = transfer_function(
num=np.array([[AP.yaw_damper_kp, 0]]),
den=np.array([[1, 1 / AP.yaw_damper_tau_r]]),
Ts=ts_control)
# instantiate lateral controllers
self.pitch_from_elevator = pd_control_with_rate(kp=AP.pitch_kp,
kd=AP.pitch_kd,
limit=np.radians(45))
self.altitude_from_pitch = pi_control(kp=AP.altitude_kp,
ki=AP.altitude_ki,
Ts=ts_control,
limit=np.radians(30))
self.airspeed_from_throttle = pi_control(kp=AP.airspeed_throttle_kp,
ki=AP.airspeed_throttle_ki,
Ts=ts_control,
limit=1.0)
self.commanded_state = msg_state()
def __init__(self, ts_control):
# instantiate lateral controllers
self.roll_from_aileron = pdControlWithRate(kp=AP.roll_kp,
kd=AP.roll_kd,
limit=AP.delta_a_max)
self.course_from_roll = piControl(kp=AP.course_kp,
ki=AP.course_ki,
Ts=ts_control,
limit=AP.roll_max)
self.yaw_damper = transfer_function(
num=np.array([[AP.yaw_damper_kp, 0]]),
den=np.array([[1, 1 / AP.yaw_damper_tau_r]]),
Ts=ts_control)
# instantiate lateral controllers
self.pitch_from_elevator = pdControlWithRate(kp=AP.pitch_kp,
kd=AP.pitch_kd,
limit=AP.delta_e_max)
self.altitude_from_pitch = piControl(kp=AP.altitude_kp,
ki=AP.altitude_ki,
Ts=ts_control,
limit=AP.pitch_max)
self.airspeed_from_throttle = piControl(kp=AP.airspeed_throttle_kp,
ki=AP.airspeed_throttle_ki,
Ts=ts_control,
limit=AP.throttle_max)
self.commanded_state = msg_state()
def __init__(self, Ts):
# steady state wind defined in the inertial frame
# wn we wd
self._steady_state = np.array([[0], [0], [0]])
sigma_u = 1.06
sigma_v = 1.06
sigma_w = 0.7
Lu = 200
Lv = 200
Lw = 50
Va = 25
a1 = sigma_u * math.sqrt(2 * Va / Lu / np.pi)
b1 = Va / Lu
a2 = sigma_v * math.sqrt(3 * Va / Lv / np.pi)
a3 = sigma_v * math.sqrt(3 * Va / Lv / np.pi) * (Va /
(math.sqrt(3) * Lv))
b2 = Va / Lv
a4 = sigma_w * math.sqrt(3 * Va / Lw / np.pi)
a5 = sigma_w * math.sqrt(3 * Va / Lw / np.pi) * (Va /
(math.sqrt(3) * Lw))
b3 = Va / Lw
self.u_w = transfer_function(num=np.array([[a1]]),
den=np.array([[1, b1]]),
Ts=Ts)
self.v_w = transfer_function(num=np.array([[a2, a3]]),
den=np.array([[1, 2 * b2, b2**2.0]]),
Ts=Ts)
self.w_w = transfer_function(num=np.array([[a4, a5]]),
den=np.array([[1, 2 * b3, b3**2.0]]),
Ts=Ts)
self._Ts = Ts
def __init__(self, ts_control):
#Can i just use the PID control class with 0 for certain gain?
# instantiate lateral controllers
self.roll_from_aileron = pid_control( #was pd_control_with_rate
kp=AP.roll_kp,
kd=AP.roll_kd,
limit_h=np.radians(45),
limit_l=-np.radians(45))
self.course_from_roll = pid_control( # was pi
kp=AP.course_kp,
ki=AP.course_ki,
Ts=ts_control,
limit_h=np.radians(30),
limit_l=-np.radians(30))
self.sideslip_from_rudder = pid_control( # was pi
kp=AP.sideslip_kp,
ki=AP.sideslip_ki,
Ts=ts_control,
limit_h=np.radians(45),
limit_l=-np.radians(45))
self.yaw_damper = transfer_function(
num=np.array([[AP.yaw_damper_kp, 0]]),
den=np.array([[1, 1 / AP.yaw_damper_tau_r]]),
Ts=ts_control)
# instantiate longitudinal controllers
self.pitch_from_elevator = pid_control( # was pd_control with rate
kp=AP.pitch_kp,
kd=AP.pitch_kd,
limit_h=np.radians(45),
limit_l=-np.radians(45))
self.altitude_from_pitch = pid_control( # was pi
kp=AP.altitude_kp,
ki=AP.altitude_ki,
Ts=ts_control,
limit_h=np.radians(30),
limit_l=-np.radians(30))
self.airspeed_from_throttle = pid_control( # was pi
kp=AP.airspeed_throttle_kp,
ki=AP.airspeed_throttle_ki,
Ts=ts_control,
limit_h=1.0,
limit_l=0.0)
self.commanded_state = StateMsg()
def compute_tf_model(mav, trim_state, trim_input, display=False):
# euler states
e_state = euler_state(trim_state)
theta = e_state.item(7)
# control surface inputs
delta_e_star = trim_input[0, 0]
delta_t_star = trim_input[3, 0]
# additional mav variables
Va = mav._Va
Va_star = mav._Va
Vg = mav._Vg
alpha_star = mav._alpha
b = MAV.b
rho = MAV.rho
S = MAV.S_wing
mass = MAV.mass
c = MAV.c
Jy = MAV.Jy
g = MAV.gravity
# -------------- Transfer Functions --------------
# phi to delta_a
a_phi_1 = -0.5 * rho * Va**2 * S * b * MAV.C_p_p * b / (2.0 * Va)
a_phi_2 = 0.5 * rho * Va**2 * S * b * MAV.C_p_delta_a
T_phi_delta_a = transfer_function(num=np.array([[a_phi_2]]),
den=np.array([[1, a_phi_1, 0]]),
Ts=Ts)
# chi to phi
T_chi_phi = transfer_function(num=np.array([[g / Vg]]),
den=np.array([[1, 0]]),
Ts=Ts)
# beta to delta_r
a_b_1 = -rho * Va * S / (2 * mass) * MAV.C_Y_beta
a_b_2 = rho * Va * S / (2 * mass) * MAV.C_Y_delta_r
T_beta_delta_r = transfer_function(num=np.array([[a_b_2]]),
den=np.array([[1, a_b_1]]),
Ts=Ts)
#theta to delta_e
a_t_1 = -rho * Va**2 * c * S / (2 * Jy) * MAV.C_m_q * c / (2.0 * Va)
a_t_2 = -rho * Va**2 * c * S / (2 * Jy) * MAV.C_m_alpha
a_t_3 = rho * Va**2 * c * S / (2 * Jy) * MAV.C_m_delta_e
T_theta_delta_e = transfer_function(num=np.array([[a_t_3]]),
den=np.array([[1, a_t_1, a_t_2]]),
Ts=Ts)
# h to theta
T_h_theta = transfer_function(num=np.array([[Va]]),
den=np.array([[1, 0]]),
Ts=Ts)
# h to Va
T_h_Va = transfer_function(num=np.array([[theta]]),
den=np.array([[1, 0]]),
Ts=Ts)
# Va to delta_t
# book approach (non-finite difference)
# a_v_1 = rho*Va_star*S/mass*(MAV.C_D_0 + MAV.C_D_alpha*alpha_star + MAV.C_D_delta_e*delta_e_star) + rho*MAV.S_prop/mass*MAV.C_prop*Va_star
# a_v_2 = rho*MAV.S_prop/mass*MAV.C_prop*MAV.k_motor**2*delta_t_star
# a_v_3 = g
# finite difference approach
a_v_1 = rho * Va_star * S / mass * (
MAV.C_D_0 + MAV.C_D_alpha * alpha_star + MAV.C_D_delta_e *
delta_e_star) - 1.0 / mass * dT_dVa(mav, Va, delta_t_star)
a_v_2 = 1.0 / mass * dT_ddelta_t(mav, Va, delta_t_star)
a_v_3 = g * np.cos(theta - alpha_star)
T_Va_delta_t = transfer_function(num=np.array([[a_v_2]]),
den=np.array([[1, a_v_1]]),
Ts=Ts)
# Va to theta
T_Va_theta = transfer_function(num=np.array([[-a_v_3]]),
den=np.array([[1, a_v_1]]),
Ts=Ts)
if display:
print("Calculated Transfer Function Forms")
print("T_phi_delta_a: ")
T_phi_delta_a.print()
print("T_chi_phi: ")
T_chi_phi.print()
print("T_beta_delta_r: ")
T_beta_delta_r.print()
print("T_theta_delta_e: ")
T_theta_delta_e.print()
print("T_h_theta: ")
T_h_theta.print()
print("T_h_Va: ")
T_h_Va.print()
print("T_Va_delta_t: ")
T_Va_delta_t.print()
print("T_Va_theta: ")
T_Va_theta.print()
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 compute_tf_model(mav, trim_state, trim_input):
# trim values
[e0, e1, e2, e3] = trim_state[6:10]
[phi, theta, psi] = Quaternion2Euler(e0, e1, e2, e3)
Va_star = mav._Va
alpha_star = mav._alpha
theta_star = theta
chi_star = mav._chi
delta_e_star = trim_input[1]
delta_t_star = trim_input[2]
#T_phi_delta_a
a_phi_1 = -1 / 2 * p.rho * mav._Va**2 * p.S_wing * p.b * p.C_p_p * p.b / (
2 * mav._Va)
a_phi_2 = 1 / 2 * p.rho * mav._Va**2 * p.S_wing * p.b * p.C_p_delta_a
num = np.array([[a_phi_2]])
den = np.array([[1, a_phi_1, 0]])
T_phi_delta_a = transfer_function(num, den, Ts)
#T_chi_phi
num = np.array([[p.gravity / mav._Vg]])
den = np.array([[1, 0]])
T_chi_phi = transfer_function(num, den, Ts)
#T_beta_delta_r
a_beta_1 = -p.rho * mav._Va * p.S_wing / (2 * p.mass) * p.C_Y_beta
a_beta_2 = p.rho * mav._Va * p.S_wing / (2 * p.mass) * p.C_Y_delta_r
num = np.array([[a_beta_2]])
den = np.array([[1, a_beta_1]])
T_beta_delta_r = transfer_function(num, den, Ts)
#T_theta_delta_e
a_theta_1 = -p.rho * mav._Va**2 * p.c * p.S_wing / (
2 * p.Jy) * p.C_m_q * p.c / (2 * mav._Va)
a_theta_2 = -p.rho * mav._Va**2 * p.c * p.S_wing / (2 * p.Jy) * p.C_m_alpha
a_theta_3 = p.rho * mav._Va**2 * p.c * p.S_wing / (2 *
p.Jy) * p.C_m_delta_e
num = np.array([[a_theta_3]])
den = np.array([[1, a_theta_1, a_theta_2]])
T_theta_delta_e = transfer_function(num, den, Ts)
#T_h_theta
num = np.array([[mav._Vg]])
den = np.array([[1, 0]])
T_h_theta = transfer_function(num, den, Ts)
#T_h_Va
num = np.array([[theta]])
den = np.array([[1, 0]])
T_h_Va = transfer_function(num, den, Ts)
#T_Va_delta_t
a_v_1 = p.rho*Va_star*p.S_wing/p.mass*(p.C_D_0 + p.C_D_alpha*alpha_star + p.C_D_delta_e*delta_e_star) \
+ p.rho*p.S_prop/p.mass*p.C_prop*Va_star
a_v_2 = p.rho * p.S_prop / p.mass * p.C_prop * p.k_motor**2 * delta_t_star
num = np.array([[a_v_2]])
den = np.array([[1, a_v_1]])
T_Va_delta_t = transfer_function(num, den, Ts)
#T_Va_theta
a_v_3 = p.gravity * np.cos(theta_star - chi_star)
num = np.array([[-a_v_3]])
den = np.array([[1, a_v_1]])
T_Va_theta = transfer_function(num, den, Ts)
# finite difference approach
a_v_1 = p.rho*Va_star*p.S_wing/p.mass*(p.C_D_0 + p.C_D_alpha*alpha_star + p.C_D_delta_e*delta_e_star) \
- 1.0/p.mass*dT_dVa(mav, mav._Va, delta_t_star)
a_v_2 = 1.0 / p.mass * dT_ddelta_t(mav, mav._Va, delta_t_star)
a_v_3 = p.gravity * np.cos(theta - alpha_star)
T_Va_delta_t = transfer_function(num=np.array([[a_v_2]]),
den=np.array([[1, a_v_1]]),
Ts=Ts)
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 compute_tf_model(self, mav, trim_state, trim_input):
# trim values
u_star = trim_state[3]
v_star = trim_state[4]
w_star = trim_state[5]
e_vec = trim_state[6:10]
[phi, theta, psi] = Quaternion2Euler(e_vec)
delta_e_star = trim_input[1][0]
delta_t_star = trim_input[3][0]
th_star = theta #is this right?
#parameters
rho = MAV.rho
S = MAV.S_wing
b = MAV.b
C_p_p = MAV.C_p_p
Va = mav._Va
C_p_delta_a = MAV.C_p_delta_a
g = MAV.gravity
Vg = mav._Vg #this is in the body frame. Is that right? (its a magnitude so it shouldn't matter right?)
c = MAV.c
Jy = MAV.Jy
C_m_q = MAV.C_m_q
C_m_a = MAV.C_m_alpha #is this the right variable?
C_m_delta_e = MAV.C_m_delta_e
Va_star = Va #is this right since it is right after the trim function?
mass = MAV.mass
C_D_0 = MAV.C_D_0
C_D_al = MAV.C_D_alpha
al_star = mav._alpha
C_D_delta_e = MAV.C_D_delta_e
S_prop = MAV.S_prop
C_prop = MAV.C_prop
k_motor = MAV.k_motor
chi_star = mav.msg_true_state.chi
C_Y_B = MAV.C_Y_beta
C_Y_delta_r = MAV.C_Y_delta_r
#phi to delta_a
self.a_phi_1 = -0.5 * rho * Va**2 * S * b * C_p_p * b / (2.0 * Va)
self.a_phi_2 = 0.5 * rho * Va**2 * S * b * C_p_delta_a
num = np.array([[self.a_phi_2]])
den = np.array([[1, self.a_phi_1, 0]])
T_phi_delta_a = transfer_function(num, den, Ts)
#chi to phi
num = np.array([[g / Vg]])
den = np.array([[1, 0]])
T_chi_phi = transfer_function(num, den, Ts)
#theta to delta_e
self.a_th_1 = -rho * Va**2 * c * S / (2.0 * Jy) * C_m_q * c / (2.0 *
Va)
self.a_th_2 = -rho * Va**2 * c * S / (2.0 * Jy) * C_m_a
self.a_th_3 = rho * Va**2 * c * S / (2.0 * Jy) * C_m_delta_e
num = np.array([[self.a_th_3, 0.0]])
den = np.array([[1, self.a_th_1, self.a_th_2]])
T_theta_delta_e = transfer_function(num, den, Ts)
#h to theta
num = np.array([[Va]])
den = np.array([[1, 0]])
T_h_theta = transfer_function(num, den, Ts)
#h to Va
num = np.array([[theta]])
den = np.array([[1, 0]])
T_h_Va = transfer_function(num, den, Ts)
#Va to delta_t
self.a_v_1 = rho * Va_star * S / mass * (
C_D_0 + C_D_al * al_star + C_D_delta_e *
delta_e_star) + rho * S_prop / mass * C_prop * Va_star
self.a_v_2 = rho * S_prop / mass * C_prop * k_motor**2 * delta_t_star
num = np.array([[self.a_v_2]])
den = np.array([[1, self.a_v_1]])
T_Va_delta_t = transfer_function(num, den, Ts)
#Va to theta
self.a_v_3 = g * np.cos(th_star - chi_star)
num = np.array([[self.a_v_3]])
den = np.array([[1, self.a_v_1]])
T_Va_theta = transfer_function(num, den, Ts)
#beta to delta_r
self.a_B_1 = -rho * Va * S / (2.0 * mass) * C_Y_B
self.a_B_2 = rho * Va * S / (2.0 * mass) * C_Y_delta_r
num = np.array([[self.a_B_2]])
den = np.array([[1, self.a_B_1]])
T_beta_delta_r = transfer_function(num, den, Ts)
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
