Angle derivatives via track body
@@ -54,6 +54,7 @@ class Derivatives:
"""
cls = DerivativeSet
+ if cls.quat is None:
cls.quat = quat
cls.cga = algebra.quat2rotation(cls.quat)
cls.v0 = v0
@@ -69,9 +70,10 @@ class Derivatives:
cls.steady_forces = steady_forces
- H0 = state_space.freqresp(np.array([1e-4]))[:, :, 0]
- # A, B, C, D = ss.get_mats()
- # H0 = C.dot(np.linalg.inv(np.eye(ss.states) - A).dot(B)) + D
+ H0 = state_space.freqresp(np.array([1e-5]))[:, :, 0]
+ # A, B, C, D = state_space.get_mats()
+ # H0 = C.dot(np.linalg.inv(np.eye(state_space.states) - A).dot(B)) + D
+ # H0 = C.dot(-np.linalg.inv(A).dot(B)) + D
# np.savetxt('./nodal_aeroelastic_static_manual.txt', H0.real)
if cls.modal:
vel_inputs_variables = state_space.input_variables.get_variable_from_name('q_dot')
@@ -352,6 +354,42 @@ class DerivativeSet:
self.apply_coefficients()
+ def angle_derivatives_tb(self):
+ self.name = 'Force/Angle derivatives via angle input'
+ self.labels_in = ['phi', 'alpha', 'beta']
+ self.labels_out = ['CD', 'CY', 'CL', 'Cl', 'Cm', 'Cn']
+ self.matrix = np.zeros((6, 3))
+
+ # Get free stream velocity direction
+ v0 = self.v0
+
+ f0a = self.steady_forces[:3]
+ m0a = self.steady_forces[-3:]
+
+ print(f0a)
+ print(m0a)
+
+ euler0 = algebra.quat2euler(self.quat)
+ cga = self.cga
+
+ # first term in the stability derivative expression
+ stab_der_trans = algebra.der_Ceuler_by_v(euler0, f0a)
+ stab_der_mom = algebra.der_Ceuler_by_v(euler0, m0a)
+
+ # second term in the stability derivative expression
+ if self.modal:
+ delta_nodal_forces = self.inv_phi_forces.dot(self.transfer_function[:6, 6:9].real)
+ else:
+ delta_nodal_forces = self.transfer_function[:6, 6:9].real
+
+ stab_der_trans2 = cga.dot(delta_nodal_forces[:3, :])
+ stab_der_mom2 = cga.dot(delta_nodal_forces[3:, :])
+
+ self.matrix[:3, :] = stab_der_trans + stab_der_trans2
+ self.matrix[3:6, :] = stab_der_mom + stab_der_mom2
+
+ self.apply_coefficients()
+
def body_derivatives(self):
self.name = 'Force derivatives to rigid body velocities - Body derivatives'
self.labels_in = ['uA', 'vA', 'wA', 'pA', 'qA', 'rA']
