diff --git a/README.md b/README.md index 0b582056..67467cd4 100644 --- a/README.md +++ b/README.md @@ -112,6 +112,11 @@ As an example to get started you estimate the parameters of a quadrotor model wi make estimate-model model=quadrotor_model log=resources/quadrotor_model.ulg ``` +For fixedwing AVL parameters, +``` +make estimate-model model=fixedwing_avl_model log=resources/orange_plane.ulg +``` + ## Generating a Model Prediction for Given Parameters and Log It is also possible to test the obtained parameters for a certain model on a different log using: diff --git a/Tools/parametric_model/configs/fixedwing_avl_model.yaml b/Tools/parametric_model/configs/fixedwing_avl_model.yaml new file mode 100644 index 00000000..acbda03a --- /dev/null +++ b/Tools/parametric_model/configs/fixedwing_avl_model.yaml @@ -0,0 +1,169 @@ +# AVL-based Fixed-Wing Model Configuration +# This configuration uses the AVL (Athena Vortex Lattice) aerodynamic model +# for comprehensive fixed-wing system identification + +model_name: "AVL Fixed Wing Model" +model_type: "Fixed Wing with AVL Aerodynamics" +model_class: "FixedWingAVLModel" +extractor_class: "FixedWingExtractorModel" + +extractor_config: + vmin: 8.0 + vmax: 20.0 + +model_config: + mass: 1.45 # kg + moment_of_inertia: + Ixx: 0.107 # kg*m^2 + Iyy: 0.256 # kg*m^2 + Izz: 0.356 # kg*m^2 + + actuators: + rotors: + puller_: + - rotor: + description: "puller rotor" + rotor_type: "LinearRotorModel" + dataframe_name: "throttle" + + aerodynamics: + type: "AVLModel" + + # Reference geometry + area: 0.41 # Wing reference area [m^2] + chord: 0.19 # Mean aerodynamic chord [m] + span: 2.16 # Wing span [m] + AR: 11.37 # Aspect ratio (can be computed from span and area) + eff: 0.95 # Oswald efficiency factor + + # Air properties + air_density: 1.225 # kg/m^3 at sea level + + # Stall parameters + alpha_stall: 0.262 # Stall angle of attack [rad] (~15 degrees) + M: 15.0 # Sigmoid blending parameter for stall transition + + # Flat plate drag model parameters (for post-stall) + CD_fp_k1: -3.0 # Flat plate drag sigmoid parameter 1 + CD_fp_k2: 0.5 # Flat plate drag sigmoid parameter 2 + + # Control surfaces + num_ctrl_surfaces: 3 + ctrl_surface_names: + - "elevator" + - "aileron" + - "rudder" + +dynamics_model_config: + optimizer_config: + optimizer_class: "LinearRegressor" + + estimate_forces: True + estimate_moments: True + resample_freq: 50.0 + estimate_angular_acceleration: False + + data: + required_ulog_topics: + actuator_outputs: + id: 1 + ulog_name: + - "timestamp" + - "output[0]" + - "output[1]" + - "output[3]" + - "output[4]" + - "output[6]" + dataframe_name: + - "timestamp" + - "u0" + - "u1" + - "u3" + - "u4" + - "u6" + actuator_type: + - "timestamp" + - "control_surface" + - "control_surface" + - "control_surface" + - "motor" + - "control_surface" + + vehicle_local_position: + ulog_name: + - "timestamp" + - "vx" + - "vy" + - "vz" + + vehicle_attitude: + ulog_name: + - "timestamp" + - "q[0]" + - "q[1]" + - "q[2]" + - "q[3]" + dataframe_name: + - "timestamp" + - "q0" + - "q1" + - "q2" + - "q3" + + vehicle_angular_velocity: + ulog_name: + - "timestamp" + - "xyz[0]" + - "xyz[1]" + - "xyz[2]" + - "xyz_derivative[0]" + - "xyz_derivative[1]" + - "xyz_derivative[2]" + dataframe_name: + - "timestamp" + - "ang_vel_x" + - "ang_vel_y" + - "ang_vel_z" + - "ang_acc_b_x" + - "ang_acc_b_y" + - "ang_acc_b_z" + + sensor_combined: + ulog_name: + - "timestamp" + - "accelerometer_m_s2[0]" + - "accelerometer_m_s2[1]" + - "accelerometer_m_s2[2]" + dataframe_name: + - "timestamp" + - "acc_b_x" + - "acc_b_y" + - "acc_b_z" + + vehicle_thrust_setpoint: + ulog_name: + - "timestamp" + - "xyz[0]" + dataframe_name: + - "timestamp" + - "throttle" + + vehicle_torque_setpoint: + ulog_name: + - "timestamp" + - "xyz[0]" + - "xyz[1]" + - "xyz[2]" + dataframe_name: + - "timestamp" + - "aileron" # Combined aileron input + - "elevator" # Elevator input + - "rudder" # Rudder input + + vehicle_land_detected: + ulog_name: + - "timestamp" + - "landed" + dataframe_name: + - "timestamp" + - "landed" diff --git a/Tools/parametric_model/src/models/__init__.py b/Tools/parametric_model/src/models/__init__.py index ce89a445..a8a37fe8 100644 --- a/Tools/parametric_model/src/models/__init__.py +++ b/Tools/parametric_model/src/models/__init__.py @@ -1,4 +1,5 @@ from . import aerodynamic_models +from .fixedwing_avl_model import FixedWingAVLModel from . import rotor_models from . import model_plots from . import extractor_models diff --git a/Tools/parametric_model/src/models/aerodynamic_models/__init__.py b/Tools/parametric_model/src/models/aerodynamic_models/__init__.py index 5f18f6c6..6c672fc7 100644 --- a/Tools/parametric_model/src/models/aerodynamic_models/__init__.py +++ b/Tools/parametric_model/src/models/aerodynamic_models/__init__.py @@ -2,3 +2,4 @@ from .phiaerodynamics_model import PhiAerodynamicsModel from .control_surface_model import ControlSurfaceModel from .linear_wing_model import LinearWingModel +from .avl_model import AVLModel diff --git a/Tools/parametric_model/src/models/aerodynamic_models/avl_model.py b/Tools/parametric_model/src/models/aerodynamic_models/avl_model.py new file mode 100644 index 00000000..8d6c30e1 --- /dev/null +++ b/Tools/parametric_model/src/models/aerodynamic_models/avl_model.py @@ -0,0 +1,396 @@ +""" +Full-fidelity AVL aerodynamic model with 36 + 6N parameters. + +Implements complete AVL-style aerodynamics with proper feature matrix structure +for system identification. All parameters are affine (linear in the coefficients). + +Copyright (c) 2024 Jaeyoung Lim, ETH Zurich ASL +License: BSD 3-Clause +""" + +import numpy as np +from scipy.spatial.transform import Rotation +from progress.bar import Bar + + +class AVLModel: + """ + Full AVL (Athena Vortex Lattice) aerodynamic model. + + Implements 36 + 6N parameters where N is the number of control surfaces: + - 36 base aerodynamic parameters + - 6 parameters per control surface (CD, CY, CL, Cell, Cem, Cen) + + All parameters are affine (linear) in the regression, computed from + known flight states (velocities, angles, rates). + """ + + def __init__(self, config_dict): + self.air_density = config_dict.get("air_density", 1.225) + self.gravity = 9.81 + + # Reference geometry + self.area = config_dict["area"] + self.chord = config_dict.get("chord", 0.0) + self.span = config_dict.get("span", 0.0) + self.AR = config_dict.get("AR", 0.0) + self.eff = config_dict.get("eff", 0.95) + + # Calculate missing geometry + if self.AR > 0 and self.span == 0: + self.span = np.sqrt(self.area * self.AR) + if self.chord == 0 and self.span > 0: + self.chord = self.area / self.span + + # Stall parameters + self.alpha_stall = config_dict.get("alpha_stall", np.deg2rad(15)) + self.M = config_dict.get("M", 15.0) + + # Flat plate drag parameters + self.CD_fp_k1 = config_dict.get("CD_fp_k1", -3.0) + self.CD_fp_k2 = config_dict.get("CD_fp_k2", 0.5) + + # Control surfaces + self.num_ctrl_surfaces = config_dict.get("num_ctrl_surfaces", 0) + self.ctrl_surface_names = config_dict.get("ctrl_surface_names", + [f"ctrl_{i}" for i in range(self.num_ctrl_surfaces)]) + + def compute_force_features_single(self, v_airspeed, alpha, beta, omega, ctrl): + """ + Compute force features in (3, n_params) matrix format. + + Each column represents one parameter's contribution to [Fx, Fy, Fz]. + All 36 + 6N base parameters are included. + + Returns: + Flattened array in format [p0_x, p0_y, p0_z, p1_x, p1_y, p1_z, ...] + """ + # Velocity and dynamic pressure + v_x, v_y, v_z = v_airspeed[0], v_airspeed[1], v_airspeed[2] + v_xz = np.sqrt(v_x**2 + v_z**2) + q = 0.5 * self.air_density * v_xz**2 + half_rho_v = 0.5 * self.air_density * v_xz + + # Non-dimensional rates + if v_xz > 1e-6: + p_hat = omega[0] * self.span / (2 * v_xz) + q_hat = omega[1] * self.chord / (2 * v_xz) + r_hat = omega[2] * self.span / (2 * v_xz) + else: + p_hat = q_hat = r_hat = 0.0 + + # Stall blending + sigma = self._sigmoid_blend(alpha) + sin_a, cos_a = np.sin(alpha), np.cos(alpha) + sign_a = 1.0 if alpha >= 0 else -1.0 + + # Flat plate drag for post-stall + CD_fp = 2 / (1 + np.exp(self.CD_fp_k1 + self.CD_fp_k2 * max(self.AR, 1/self.AR))) + + # Rotation from stability to body frame + R = Rotation.from_rotvec([0, alpha, 0]).as_matrix() + + # Build feature matrix: each column is one parameter's [Fx, Fy, Fz] + features = [] + + # ========== FORCE PARAMETERS (18 base) ========== + + # === Drag coefficients (6) === + # CD0 - parasitic drag + features.append(R @ np.array([-q * self.area * (1 - sigma), 0, 0])) + + # CD_induced - induced drag (approximated as alpha^2) + features.append(R @ np.array([-q * self.area * (1 - sigma) * alpha**2, 0, 0])) + + # CD_poststall - flat plate drag + features.append(R @ np.array([-q * self.area * sigma * np.abs(CD_fp * (0.5 - 0.5*np.cos(2*alpha))), 0, 0])) + + # CDp - drag due to roll rate + features.append(R @ np.array([-half_rho_v * self.area * self.span/2 * p_hat, 0, 0])) + + # CDq - drag due to pitch rate + features.append(R @ np.array([-half_rho_v * self.area * self.chord/2 * q_hat, 0, 0])) + + # CDr - drag due to yaw rate + features.append(R @ np.array([-half_rho_v * self.area * self.span/2 * r_hat, 0, 0])) + + # === Side force coefficients (5) === + # CYa - sideforce due to alpha + features.append(R @ np.array([0, q * self.area * alpha, 0])) + + # CYb - sideforce due to sideslip + features.append(R @ np.array([0, q * self.area * beta, 0])) + + # CYp - sideforce due to roll rate + features.append(R @ np.array([0, half_rho_v * self.area * self.span/2 * p_hat, 0])) + + # CYq - sideforce due to pitch rate + features.append(R @ np.array([0, half_rho_v * self.area * self.chord/2 * q_hat, 0])) + + # CYr - sideforce due to yaw rate + features.append(R @ np.array([0, half_rho_v * self.area * self.span/2 * r_hat, 0])) + + # === Lift coefficients (7) === + # CL0 - zero-alpha lift + features.append(R @ np.array([0, 0, -q * self.area * (1 - sigma)])) + + # CLa - lift curve slope + features.append(R @ np.array([0, 0, -q * self.area * (1 - sigma) * alpha])) + + # CL_poststall - post-stall lift + features.append(R @ np.array([0, 0, -q * self.area * sigma * sign_a * sin_a**2 * cos_a])) + + # CLb - lift due to sideslip + features.append(R @ np.array([0, 0, -q * self.area * beta])) + + # CLp - lift due to roll rate + features.append(R @ np.array([0, 0, -half_rho_v * self.area * self.span/2 * p_hat])) + + # CLq - lift due to pitch rate + features.append(R @ np.array([0, 0, -half_rho_v * self.area * self.chord/2 * q_hat])) + + # CLr - lift due to yaw rate + features.append(R @ np.array([0, 0, -half_rho_v * self.area * self.span/2 * r_hat])) + + # === Control surface force effects (3 per surface) === + for i in range(self.num_ctrl_surfaces): + delta = np.rad2deg(ctrl[i]) if i < len(ctrl) else 0.0 + + # CD_ctrl - drag due to control deflection + features.append(R @ np.array([-q * self.area * delta * 0.001, 0, 0])) + + # CY_ctrl - sideforce due to control deflection + features.append(R @ np.array([0, q * self.area * delta * 0.001, 0])) + + # CL_ctrl - lift due to control deflection + features.append(R @ np.array([0, 0, -q * self.area * delta * 0.01])) + + # Stack into (3, n_params) matrix and flatten + X = np.column_stack(features) + return X.T.flatten() + + def compute_moment_features_single(self, v_airspeed, alpha, beta, omega, ctrl): + """ + Compute moment features in (3, n_params) matrix format. + + Each column represents one parameter's contribution to [Mx, My, Mz]. + All 19 + 3N base parameters are included. + + Returns: + Flattened array in format [p0_x, p0_y, p0_z, p1_x, p1_y, p1_z, ...] + """ + v_xz = np.sqrt(v_airspeed[0]**2 + v_airspeed[2]**2) + q = 0.5 * self.air_density * v_xz**2 + half_rho_v = 0.5 * self.air_density * v_xz + + # Non-dimensional rates + if v_xz > 1e-6: + p_hat = omega[0] * self.span / (2 * v_xz) + q_hat = omega[1] * self.chord / (2 * v_xz) + r_hat = omega[2] * self.span / (2 * v_xz) + else: + p_hat = q_hat = r_hat = 0.0 + + # Stall check for pitch moment + in_stall_pos = alpha > self.alpha_stall + in_stall_neg = alpha < -self.alpha_stall + + features = [] + + # ========== MOMENT PARAMETERS (19 base + 3N ctrl) ========== + + # === Roll moment (Cell) - 5 params === + # Cella - roll due to alpha + features.append(np.array([q * self.area * self.span * alpha, 0, 0])) + + # Cellb - roll due to beta (dihedral effect) + features.append(np.array([q * self.area * self.span * beta, 0, 0])) + + # Cellp - roll damping + features.append(np.array([half_rho_v * self.area * self.span * self.span/2 * p_hat, 0, 0])) + + # Cellq - roll due to pitch rate + features.append(np.array([half_rho_v * self.area * self.span * self.chord/2 * q_hat, 0, 0])) + + # Cellr - roll due to yaw rate + features.append(np.array([half_rho_v * self.area * self.span * self.span/2 * r_hat, 0, 0])) + + # === Pitch moment (Cem) - 8 params === + # Cem0 - zero-alpha pitch moment + features.append(np.array([0, q * self.area * self.chord, 0])) + + # Cema - pitch moment slope (pre-stall) + if not in_stall_pos and not in_stall_neg: + features.append(np.array([0, q * self.area * self.chord * alpha, 0])) + elif in_stall_pos: + features.append(np.array([0, q * self.area * self.chord * self.alpha_stall, 0])) + else: + features.append(np.array([0, -q * self.area * self.chord * self.alpha_stall, 0])) + + # Cema_stall - pitch moment slope (post-stall) + if in_stall_pos: + features.append(np.array([0, q * self.area * self.chord * (alpha - self.alpha_stall), 0])) + elif in_stall_neg: + features.append(np.array([0, q * self.area * self.chord * (alpha + self.alpha_stall), 0])) + else: + features.append(np.zeros(3)) + + # Cemb - pitch moment due to sideslip + features.append(np.array([0, q * self.area * self.chord * beta, 0])) + + # Cemp - pitch moment due to roll rate + features.append(np.array([0, half_rho_v * self.area * self.chord * self.span/2 * p_hat, 0])) + + # Cemq - pitch damping + features.append(np.array([0, half_rho_v * self.area * self.chord * self.chord/2 * q_hat, 0])) + + # Cemr - pitch moment due to yaw rate + features.append(np.array([0, half_rho_v * self.area * self.chord * self.span/2 * r_hat, 0])) + + # === Yaw moment (Cen) - 6 params === + # Cena - yaw due to alpha + features.append(np.array([0, 0, q * self.area * self.span * alpha])) + + # Cenb - yaw due to beta (directional stability) + features.append(np.array([0, 0, q * self.area * self.span * beta])) + + # Cenp - yaw due to roll rate + features.append(np.array([0, 0, half_rho_v * self.area * self.span * self.span/2 * p_hat])) + + # Cenq - yaw due to pitch rate + features.append(np.array([0, 0, half_rho_v * self.area * self.span * self.chord/2 * q_hat])) + + # Cenr - yaw damping + features.append(np.array([0, 0, half_rho_v * self.area * self.span * self.span/2 * r_hat])) + + # === Control surface moment effects (3 per surface) === + for i in range(self.num_ctrl_surfaces): + delta = np.rad2deg(ctrl[i]) if i < len(ctrl) else 0.0 + + # Cell_ctrl - roll moment due to control + features.append(np.array([q * self.area * self.span * delta * 0.001, 0, 0])) + + # Cem_ctrl - pitch moment due to control + features.append(np.array([0, q * self.area * self.chord * delta * 0.01, 0])) + + # Cen_ctrl - yaw moment due to control + features.append(np.array([0, 0, q * self.area * self.span * delta * 0.001])) + + # Stack and flatten + X = np.column_stack(features) + return X.T.flatten() + + def compute_aero_force_features(self, v_airspeed_mat, alpha_vec, beta_vec, omega_mat, ctrl_mat=None): + """Batch compute force features for all timesteps.""" + n_samples = v_airspeed_mat.shape[0] + + if ctrl_mat is None: + ctrl_mat = np.zeros((n_samples, self.num_ctrl_surfaces)) + + print("Computing AVL aerodynamic force features (full model)...") + features_bar = Bar("Force Features", max=n_samples) + + X_aero = self.compute_force_features_single( + v_airspeed_mat[0, :], alpha_vec[0], beta_vec[0], + omega_mat[0, :], ctrl_mat[0, :] + ) + + for i in range(1, n_samples): + X_curr = self.compute_force_features_single( + v_airspeed_mat[i, :], alpha_vec[i], beta_vec[i], + omega_mat[i, :], ctrl_mat[i, :] + ) + X_aero = np.vstack((X_aero, X_curr)) + features_bar.next() + features_bar.finish() + + coef_dict, col_names = self._generate_force_coef_names() + return X_aero, coef_dict, col_names + + def compute_aero_moment_features(self, v_airspeed_mat, alpha_vec, beta_vec, omega_mat, ctrl_mat=None): + """Batch compute moment features for all timesteps.""" + n_samples = v_airspeed_mat.shape[0] + + if ctrl_mat is None: + ctrl_mat = np.zeros((n_samples, self.num_ctrl_surfaces)) + + print("Computing AVL aerodynamic moment features (full model)...") + features_bar = Bar("Moment Features", max=n_samples) + + X_aero = self.compute_moment_features_single( + v_airspeed_mat[0, :], alpha_vec[0], beta_vec[0], + omega_mat[0, :], ctrl_mat[0, :] + ) + + for i in range(1, n_samples): + X_curr = self.compute_moment_features_single( + v_airspeed_mat[i, :], alpha_vec[i], beta_vec[i], + omega_mat[i, :], ctrl_mat[i, :] + ) + X_aero = np.vstack((X_aero, X_curr)) + features_bar.next() + features_bar.finish() + + coef_dict, col_names = self._generate_moment_coef_names() + return X_aero, coef_dict, col_names + + def _sigmoid_blend(self, alpha): + """Sigmoid blending function for pre/post-stall transition.""" + return (1 + np.exp(-self.M * (alpha - self.alpha_stall)) + + np.exp(self.M * (alpha + self.alpha_stall))) / ( + (1 + np.exp(-self.M * (alpha - self.alpha_stall))) * + (1 + np.exp(self.M * (alpha + self.alpha_stall)))) + + def _generate_force_coef_names(self): + """Generate coefficient names for force features.""" + base_params = [ + "CD0", "CD_induced", "CD_poststall", "CDp", "CDq", "CDr", + "CYa", "CYb", "CYp", "CYq", "CYr", + "CL0", "CLa", "CL_poststall", "CLb", "CLp", "CLq", "CLr" + ] + + # Add control surface parameters (3 per surface) + for name in self.ctrl_surface_names: + base_params.extend([f"CD_{name}", f"CY_{name}", f"CL_{name}"]) + + # Generate column names: [p0_x, p0_y, p0_z, p1_x, p1_y, p1_z, ...] + col_names = [] + for param in base_params: + col_names.extend([f"{param}_x", f"{param}_y", f"{param}_z"]) + + # Coefficient dictionary + coef_dict = {} + for param in base_params: + coef_dict[param] = {"lin": { + "x": f"{param}_x", + "y": f"{param}_y", + "z": f"{param}_z" + }} + + return coef_dict, col_names + + def _generate_moment_coef_names(self): + """Generate coefficient names for moment features.""" + base_params = [ + "Cella", "Cellb", "Cellp", "Cellq", "Cellr", + "Cem0", "Cema", "Cema_stall", "Cemb", "Cemp", "Cemq", "Cemr", + "Cena", "Cenb", "Cenp", "Cenq", "Cenr" + ] + + # Add control surface parameters (3 per surface) + for name in self.ctrl_surface_names: + base_params.extend([f"Cell_{name}", f"Cem_{name}", f"Cen_{name}"]) + + col_names = [] + for param in base_params: + col_names.extend([f"{param}_x", f"{param}_y", f"{param}_z"]) + + coef_dict = {} + for param in base_params: + coef_dict[param] = {"rot": { + "x": f"{param}_x", + "y": f"{param}_y", + "z": f"{param}_z" + }} + + return coef_dict, col_names diff --git a/Tools/parametric_model/src/models/dynamics_model.py b/Tools/parametric_model/src/models/dynamics_model.py index 0c95047a..06f3d17a 100644 --- a/Tools/parametric_model/src/models/dynamics_model.py +++ b/Tools/parametric_model/src/models/dynamics_model.py @@ -463,6 +463,13 @@ def save_result_dict_to_yaml( print( "-------------------------------------------------------------------------------" ) + + # Call sanity check for AVL models + if hasattr(self, 'sanity_check_parameters'): + try: + self.sanity_check_parameters(self.result_dict.get("coefficients", {})) + except Exception as e: + print(f"Note: Sanity check not available or failed: {e}") def load_dataframes(self, data_frame): self.data_df = data_frame diff --git a/Tools/parametric_model/src/models/fixedwing_avl_model.py b/Tools/parametric_model/src/models/fixedwing_avl_model.py new file mode 100644 index 00000000..0794c908 --- /dev/null +++ b/Tools/parametric_model/src/models/fixedwing_avl_model.py @@ -0,0 +1,396 @@ +""" + * + * Copyright (c) 2024 Jaeyoung Lim + * 2024 Autonomous Systems Lab ETH Zurich + * All rights reserved. + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name Data Driven Dynamics nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * +""" + +__author__ = "Jaeyoung Lim" +__maintainer__ = "Jaeyoung Lim" +__license__ = "BSD 3" + + +import numpy as np + +from . import aerodynamic_models +from .dynamics_model import DynamicsModel +from .model_config import ModelConfig +from matplotlib import pyplot as plt +from scipy.spatial.transform import Rotation + + +class FixedWingAVLModel(DynamicsModel): + """ + Fixed-wing aircraft model using AVL (Athena Vortex Lattice) aerodynamics. + + This model extends the basic fixed-wing model to support the comprehensive + AVL aerodynamic model with: + - Multiple control surfaces (not just elevator) + - Body rate effects on forces (not just moments) + - Sideslip effects on forces (not just moments) + - Full 6-DOF aerodynamic derivatives + """ + + def __init__( + self, config_file, normalization=True, model_name="avl_fixedwing_model" + ): + self.config = ModelConfig(config_file) + super(FixedWingAVLModel, self).__init__( + config_dict=self.config.dynamics_model_config, normalization=normalization + ) + self.mass = self.config.model_config["mass"] + self.moment_of_inertia = np.diag( + [ + self.config.model_config["moment_of_inertia"]["Ixx"], + self.config.model_config["moment_of_inertia"]["Iyy"], + self.config.model_config["moment_of_inertia"]["Izz"], + ] + ) + + self.model_name = model_name + + self.rotor_config_dict = self.config.model_config["actuators"]["rotors"] + self.aerodynamics_dict = self.config.model_config["aerodynamics"] + + # Initialize AVL aerodynamic model + try: + self.aero_model = getattr( + aerodynamic_models, self.aerodynamics_dict["type"] + )(self.aerodynamics_dict) + except AttributeError: + error_str = ( + "Aerodynamics Model '{0}' not found, is it added to models " + "directory and models/__init__.py?".format(self.aerodynamics_dict["type"]) + ) + raise AttributeError(error_str) + + # Verify it's actually an AVL model + if self.aerodynamics_dict["type"] != "AVLModel": + print(f"Warning: FixedWingAVLModel expected AVLModel, got {self.aerodynamics_dict['type']}") + + # Get control surface configuration + self.num_ctrl_surfaces = self.aerodynamics_dict.get("num_ctrl_surfaces", 0) + self.ctrl_surface_names = self.aerodynamics_dict.get("ctrl_surface_names", []) + + print(f"Initialized AVL model with {self.num_ctrl_surfaces} control surfaces: {self.ctrl_surface_names}") + + def prepare_control_deflections(self): + """ + Prepare control surface deflection matrix from dataframe. + + Returns control deflections for all configured control surfaces. + """ + n_samples = len(self.data_df) + control_deflections = np.zeros((n_samples, self.num_ctrl_surfaces)) + + # Map control surface names to dataframe columns + for i, ctrl_name in enumerate(self.ctrl_surface_names): + if ctrl_name.lower() in self.data_df.columns: + control_deflections[:, i] = self.data_df[ctrl_name.lower()].to_numpy() + else: + print(f"Warning: Control surface '{ctrl_name}' not found in dataframe. Available: {list(self.data_df.columns)}") + print(f"Using zero deflection for {ctrl_name}") + + return control_deflections + + def prepare_force_regression_matrices(self): + """ + Prepare regression matrices for force estimation using AVL model. + + The AVL model requires: + - Airspeed (3D vector) + - Angle of attack + - Angle of sideslip (needed for forces in AVL model) + - Angular velocities (needed for forces in AVL model) + - Control surface deflections (all surfaces) + """ + accel_mat = self.data_df[["acc_b_x", "acc_b_y", "acc_b_z"]].to_numpy() + force_mat = accel_mat * self.mass + self.y_forces = (force_mat).flatten() + self.data_df[ + ["measured_force_x", "measured_force_y", "measured_force_z"] + ] = force_mat + + # Prepare AVL model inputs + airspeed_mat = self.data_df[ + ["V_air_body_x", "V_air_body_y", "V_air_body_z"] + ].to_numpy() + aoa_vec = self.data_df["angle_of_attack"].to_numpy() + sideslip_vec = self.data_df["angle_of_sideslip"].to_numpy() + angular_vel_mat = self.data_df[ + ["ang_vel_x", "ang_vel_y", "ang_vel_z"] + ].to_numpy() + + # Get control deflections for all surfaces + control_deflections_mat = self.prepare_control_deflections() + + # Compute AVL aerodynamic force features + ( + X_aero, + coef_dict_aero, + col_names_aero, + ) = self.aero_model.compute_aero_force_features( + airspeed_mat, + aoa_vec, + sideslip_vec, + angular_vel_mat, + control_deflections_mat + ) + + self.data_df[col_names_aero] = X_aero + self.coef_dict.update(coef_dict_aero) + self.y_dict.update( + { + "lin": { + "x": "measured_force_x", + "y": "measured_force_y", + "z": "measured_force_z", + } + } + ) + + def prepare_moment_regression_matrices(self): + """ + Prepare regression matrices for moment estimation using AVL model. + + The AVL model requires: + - Airspeed (3D vector) + - Angle of attack + - Angle of sideslip + - Angular velocities + - Control surface deflections (all surfaces) + """ + # Angular acceleration + moment_mat = np.matmul( + self.data_df[["ang_acc_b_x", "ang_acc_b_y", "ang_acc_b_z"]].to_numpy(), + self.moment_of_inertia, + ) + self.y_moments = moment_mat.flatten() + self.data_df[ + ["measured_moment_x", "measured_moment_y", "measured_moment_z"] + ] = moment_mat + + # Prepare AVL model inputs + airspeed_mat = self.data_df[ + ["V_air_body_x", "V_air_body_y", "V_air_body_z"] + ].to_numpy() + aoa_vec = self.data_df["angle_of_attack"].to_numpy() + sideslip_vec = self.data_df["angle_of_sideslip"].to_numpy() + angular_vel_mat = self.data_df[ + ["ang_vel_x", "ang_vel_y", "ang_vel_z"] + ].to_numpy() + + # Get control deflections for all surfaces + control_deflections_mat = self.prepare_control_deflections() + + # Compute AVL aerodynamic moment features + ( + X_aero, + coef_dict_aero, + col_names_aero, + ) = self.aero_model.compute_aero_moment_features( + airspeed_mat, + aoa_vec, + sideslip_vec, + angular_vel_mat, + control_deflections_mat + ) + + self.data_df[col_names_aero] = X_aero + self.coef_dict.update(coef_dict_aero) + + self.y_dict.update( + { + "rot": { + "x": "measured_moment_x", + "y": "measured_moment_y", + "z": "measured_moment_z", + } + } + ) + + def sanity_check_parameters(self, identified_params): + """ + Perform sanity checks comparing estimated parameters with theoretical values. + + Compares: + 1. CD_induced vs theoretical induced drag from CLa + 2. Post-stall parameters vs theoretical nonlinear functions + 3. Overall parameter reasonableness + """ + print("\n" + "="*79) + print(" AVL Model Sanity Checks") + print("="*79) + + # Extract relevant parameters + params = identified_params + + # Get aerodynamic parameters + CLa = params.get('CLa', 0) + CL0 = params.get('CL0', 0) + CD0 = params.get('CD0', 0) + CD_induced = params.get('CD_induced', 0) + CD_poststall = params.get('CD_poststall', 0) + CL_poststall = params.get('CL_poststall', 0) + + # Get geometry from config + AR = self.aerodynamics_dict.get('AR', 10.0) + eff = self.aerodynamics_dict.get('eff', 0.95) + alpha_stall = np.rad2deg(self.aerodynamics_dict.get('alpha_stall', np.deg2rad(15))) + + print("\n1. Induced Drag Check") + print("-" * 79) + print(f" Oswald efficiency (e): {eff:.3f}") + print(f" Aspect ratio (AR): {AR:.2f}") + print(f" Identified CLa: {CLa:.4f} /rad") + + # Theoretical induced drag coefficient at typical cruise alpha (5 deg) + alpha_cruise = np.deg2rad(5.0) + CL_cruise_theory = CL0 + CLa * alpha_cruise + CD_induced_theory = CL_cruise_theory**2 / (np.pi * AR * eff) + + # Compare with identified parameter (which is multiplied by alpha^2) + # So we need to account for alpha^2 scaling + CD_induced_theory_param = CD_induced_theory / (alpha_cruise**2) + + print(f"\n At α = 5°:") + print(f" - Theoretical CL: {CL_cruise_theory:.4f}") + print(f" - Theoretical CD_induced: {CD_induced_theory:.5f}") + print(f" - Theoretical CD_induced parameter: {CD_induced_theory_param:.4f}") + print(f" - Identified CD_induced parameter: {CD_induced:.4f}") + + if abs(CD_induced) > 0.01: + ratio = CD_induced / CD_induced_theory_param + print(f" - Ratio (identified/theoretical): {ratio:.2f}") + if 0.5 < ratio < 2.0: + print(f" ✓ Reasonable match (within 2x)") + else: + print(f" ⚠ Large discrepancy - check data quality or model assumptions") + else: + print(f" ⚠ CD_induced very small - may not be well identified") + + print("\n2. Stall Parameters Check") + print("-" * 79) + print(f" Configured stall angle: {alpha_stall:.1f}°") + print(f" Identified Cema (pre-stall): {params.get('Cema', 0):.4f} /rad") + print(f" Identified Cema_stall (post-stall): {params.get('Cema_stall', 0):.4f} /rad") + + # Theoretical post-stall CL at stall angle + alpha_s = np.deg2rad(alpha_stall) + CL_poststall_theory = 2 * np.sign(alpha_s) * np.sin(alpha_s)**2 * np.cos(alpha_s) + + print(f"\n At α = {alpha_stall:.1f}° (stall):") + print(f" - Theoretical CL (post-stall formula): {CL_poststall_theory:.4f}") + print(f" - Identified CL_poststall parameter: {CL_poststall:.4f}") + + if abs(CL_poststall) > 0.1: + # Note: CL_poststall parameter is weighted by sigma, so direct comparison is complex + print(f" ℹ Post-stall parameter identified (requires σ-weighting for comparison)") + + print("\n3. Basic Aerodynamic Coefficient Ranges") + print("-" * 79) + + # Typical ranges for fixed-wing aircraft + checks = [ + ("CL0", CL0, (-0.2, 0.3), "Zero-lift coefficient"), + ("CLa", CLa, (3.0, 7.0), "Lift curve slope (/rad)"), + ("CD0", CD0, (0.015, 0.08), "Parasitic drag"), + ("Cema", params.get('Cema', 0), (-2.0, -0.3), "Pitch moment slope (/rad)"), + ("Cemq", params.get('Cemq', 0), (-1000, -100), "Pitch damping"), + ("Cenb", params.get('Cenb', 0), (0.0, 0.01), "Directional stability"), + ] + + for name, value, (min_val, max_val), description in checks: + in_range = min_val <= value <= max_val + status = "✓" if in_range else "⚠" + print(f" {status} {name:12s} = {value:8.4f} (typical: [{min_val:.3f}, {max_val:.3f}]) - {description}") + if not in_range and abs(value) > 0.001: + if value < min_val: + print(f" → Value is lower than typical range") + else: + print(f" → Value is higher than typical range") + + print("\n4. Control Surface Effectiveness") + print("-" * 79) + + for ctrl_name in self.ctrl_surface_names: + CL_ctrl = params.get(f'CL_{ctrl_name}', 0) + Cem_ctrl = params.get(f'Cem_{ctrl_name}', 0) + + print(f" {ctrl_name.capitalize()}:") + print(f" - CL_{ctrl_name}: {CL_ctrl:.5f} /deg") + print(f" - Cem_{ctrl_name}: {Cem_ctrl:.5f} /deg") + + # Elevator should have strong pitch effect + if ctrl_name.lower() == 'elevator': + if abs(Cem_ctrl) > 0.001: + print(f" ✓ Elevator has pitch authority") + else: + print(f" ⚠ Elevator pitch authority seems low") + + # Rudder should have yaw effect + elif ctrl_name.lower() == 'rudder': + Cen_ctrl = params.get(f'Cen_{ctrl_name}', 0) + print(f" - Cen_{ctrl_name}: {Cen_ctrl:.5f} /deg") + if abs(Cen_ctrl) > 0.001: + print(f" ✓ Rudder has yaw authority") + else: + print(f" ⚠ Rudder yaw authority seems low") + + # Aileron should have roll effect + elif ctrl_name.lower() == 'aileron': + Cell_ctrl = params.get(f'Cell_{ctrl_name}', 0) + print(f" - Cell_{ctrl_name}: {Cell_ctrl:.5f} /deg") + if abs(Cell_ctrl) > 0.001: + print(f" ✓ Aileron has roll authority") + else: + print(f" ⚠ Aileron roll authority seems low") + + print("\n5. Coupling Effects") + print("-" * 79) + + # Check for significant coupling + coupling_checks = [ + ("Aileron → Pitch", params.get('Cem_aileron', 0)), + ("Rudder → Pitch", params.get('Cem_rudder', 0)), + ("Elevator → Roll", params.get('Cell_elevator', 0)), + ] + + for name, value in coupling_checks: + if abs(value) > 0.01: + print(f" ℹ {name}: {value:.5f} (significant coupling detected)") + else: + print(f" · {name}: {value:.5f} (minimal coupling)") + + print("\n" + "="*79) + print("Note: These checks compare identified parameters with typical values.") + print("Deviations may indicate: (1) unique aircraft characteristics,") + print("(2) insufficient data excitation, or (3) model structure issues.") + print("="*79 + "\n") diff --git a/resources/orange_plane.ulg b/resources/orange_plane.ulg new file mode 100644 index 00000000..cf7efaa9 Binary files /dev/null and b/resources/orange_plane.ulg differ