diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 7597c989b3..2124170c20 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -13,6 +13,7 @@ control_allocator start # # Start attitude controller. # +fw_indi_pos_control start fw_rate_control start fw_att_control start fw_mode_manager start diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 28c16f931f..e9d73a5b18 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -17,6 +17,7 @@ CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_DYN_SOAR_CONTROL=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_MODE_MANAGER=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index fdc23a889f..e2dc6eb24f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -206,6 +206,12 @@ set(msg_files SensorsStatusImu.msg SensorUwb.msg SensorAirflow.msg + soaring_controller_heartbeat.msg + soaring_controller_position_setpoint.msg + soaring_controller_position.msg + soaring_controller_status.msg + soaring_controller_wind.msg + soaring_estimator_shear.msg SystemPower.msg TakeoffStatus.msg TaskStackInfo.msg diff --git a/msg/soaring_controller_heartbeat.msg b/msg/soaring_controller_heartbeat.msg new file mode 100644 index 0000000000..1d699c277a --- /dev/null +++ b/msg/soaring_controller_heartbeat.msg @@ -0,0 +1,5 @@ +# SOARING CONTROLLER STATE, SERVES AS A HEARTBEAT IF CONTROLLER IS STILL PUBLISHING + +uint64 timestamp # time since system start (microseconds) + +uint64 heartbeat # time since system start (microseconds) of the last run of soaring controller diff --git a/msg/soaring_controller_position.msg b/msg/soaring_controller_position.msg new file mode 100644 index 0000000000..f77f6799b0 --- /dev/null +++ b/msg/soaring_controller_position.msg @@ -0,0 +1,8 @@ +# SOARING CONTROLLER POSITION IN ENU SOARING FRAME + +uint64 timestamp # time since system start (microseconds) + +float32[3] pos # POSITION VECTOR IN SOARING ENU FRAME +float32[3] vel # VELOCITY VECTOR IN SOARING ENU FRAME +float32[3] acc # ACCELERATION VECTOR IN SOARING ENU FRAME +float32[4] att # UNIT QUATERNION DESCRIBING BODY FRAME POSE TO ENU \ No newline at end of file diff --git a/msg/soaring_controller_position_setpoint.msg b/msg/soaring_controller_position_setpoint.msg new file mode 100644 index 0000000000..d756f9d8ec --- /dev/null +++ b/msg/soaring_controller_position_setpoint.msg @@ -0,0 +1,12 @@ +# SOARING CONTROLLER POSITION SETPOINT + +uint64 timestamp # time since system start (microseconds) + +float32[3] pos # position in ENU frame +float32[3] vel # velocity in ENU frame +float32[3] acc # acceleration in ENU frame +float32[3] f_command # force command inside controller +float32[3] m_command # moment command inside controller +float32[3] w_err # rotation vector to target pose + + diff --git a/msg/soaring_controller_status.msg b/msg/soaring_controller_status.msg new file mode 100644 index 0000000000..a9eee91e72 --- /dev/null +++ b/msg/soaring_controller_status.msg @@ -0,0 +1,6 @@ +# SOARING CONTROLLER STATE, SERVES AS FLAG IF CONTROLLER IS STILL PUBLISHING + +uint64 timestamp # time since system start (microseconds) + +bool soaring_controller_running # if true, the soaring controller is expected to publish +bool timeout_detected # if true, the soaring controller has crashed \ No newline at end of file diff --git a/msg/soaring_controller_wind.msg b/msg/soaring_controller_wind.msg new file mode 100644 index 0000000000..399ca9392b --- /dev/null +++ b/msg/soaring_controller_wind.msg @@ -0,0 +1,11 @@ +# SOARING CONTROLLER WIND ESTIMATE, USED FOR INDI CONTROL + +uint64 timestamp # time since system start (microseconds) + +float32[3] wind_estimate # WIND ESTIMATE IN ENU FRAME +float32[3] wind_estimate_filtered # LP-FILTERED WIND ESTIMATE IN ENU FRAME, USED BY INDI CONTROLLER +float32[3] position # position of the current estimate in the soaring frame +float32 airspeed # current airspeed + +bool valid # tell, if estimate is valid for shear estimator +bool lock_params # tell, if the shear estimator shall lock shear param values for soaring diff --git a/msg/soaring_estimator_shear.msg b/msg/soaring_estimator_shear.msg new file mode 100644 index 0000000000..53f94797a7 --- /dev/null +++ b/msg/soaring_estimator_shear.msg @@ -0,0 +1,30 @@ +# SOARING ESTIMATOR WIND ESTIMATE, USED FOR SELECTING THE CORRECT TRAJECTORY + +uint64 timestamp # time since system start (microseconds) + +float32 vx # maximum wind in x-direction +float32 vy # maximum wind in y-direction +float32 bx # wind offset in x-direction +float32 by # wind offset in y-direction +float32 h # vertical position of shear layer in soaring frame +float32 a # shear strength + +float32 sigma_vx # covariance of vx +float32 sigma_vy # covariance of vy +float32 sigma_bx # covariance of bx +float32 sigma_by # covariance of by +float32 sigma_h # covariance of h +float32 sigma_a # covariance of a + +float32 coeff_0 # offset vertical wind +float32 coeff_1 # linear coeff vertical wind +float32 coeff_2 # quadratic coeff vertical wind + +float32 v_max # discrete value of v_max (shear velocity), identifier for appropriate trajectrory +float32 alpha # discrete value of alpha (shear strength), identifier for appropriate trajectrory +float32 h_ref # discrete value of h_ref (shear location) +float32 psi # discrete value of shear heading +float32 aspd # airspeed identifier for appropriate trajectrory + +bool soaring_feasible # plausibility check +uint64 reset_counter # filter reset counter diff --git a/src/modules/fw_dyn_soar_control/CMakeLists.txt b/src/modules/fw_dyn_soar_control/CMakeLists.txt new file mode 100644 index 0000000000..c0f1c1edb9 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015-2017 PX4 Development Team. 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 PX4 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. +# +############################################################################ + +#add_subdirectory(launchdetection) + +px4_add_module( + MODULE modules__fw_dyn_soar_control + MAIN fw_dyn_soar_control + STACK_MAIN 4096 + SRCS + FixedwingPositionINDIControl.cpp + FixedwingPositionINDIControl.hpp + DEPENDS + ) diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp new file mode 100644 index 0000000000..18908f5632 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -0,0 +1,1632 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. 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 PX4 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. + * + ****************************************************************************/ + +#include "FixedwingPositionINDIControl.hpp" + +using namespace std; + +using math::constrain; +using math::max; +using math::min; +using math::radians; + +using matrix::Dcmf; +using matrix::Matrix; +using matrix::Euler; +using matrix::Quatf; +using matrix::AxisAnglef; +using matrix::Vector3f; +using matrix::Vector; +using matrix::wrap_pi; + + +FixedwingPositionINDIControl::FixedwingPositionINDIControl() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _actuators_0_pub(ORB_ID(actuator_controls_0)), + _attitude_sp_pub(ORB_ID(vehicle_attitude_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + // limit to 100 Hz + _vehicle_angular_velocity_sub.set_interval_ms(1000.f/_sample_frequency); + + + /* fetch initial parameter values */ + parameters_update(); +} + +FixedwingPositionINDIControl::~FixedwingPositionINDIControl() +{ + perf_free(_loop_perf); +} + +bool +FixedwingPositionINDIControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle position callback registration failed!"); + return false; + } + PX4_INFO("Starting FW_DYN_SOAR_CONTROLLER"); + char filename[] = "trajectory0.csv"; + _read_trajectory_coeffs_csv(filename); + + // initialize transformations + _R_ned_to_enu *= 0.f; + _R_ned_to_enu(0,1) = 1.f; + _R_ned_to_enu(1,0) = 1.f; + _R_ned_to_enu(2,2) = -1.f; + _R_ned_to_enu.renormalize(); + _R_enu_to_ned = _R_ned_to_enu; + + // initialize wind shear params + _shear_v_max = 8.f; + _shear_alpha = 0.5f; + _shear_h_ref = 0.f; + _shear_heading = M_PI_2_F; + _shear_aspd = 20.f; + + // init wind estimates + for (int i=0; i<3; i++) { + _wind_estimate(i) = 0.0f; + _wind_estimate_EKF(i) = 0.0f; + } + + // initialize in manual feedthrough + _switch_manual = true; + + // fix sitl mode on startup (no switch at runtime) + _switch_sitl = _param_switch_sitl.get(); + + // initialize transform to trajec frame + _compute_trajectory_transform(); + + return true; +} + +int +FixedwingPositionINDIControl::parameters_update() +{ + updateParams(); + + // INDI parameters + _K_x *= 0.f; + _K_v *= 0.f; + _K_a *= 0.f; + _K_q *= 0.f; + _K_w *= 0.f; + _K_x(0,0) = _param_lin_k_x.get(); + _K_x(1,1) = _param_lin_k_y.get(); + _K_x(2,2) = _param_lin_k_z.get(); + _K_v(0,0) = _param_lin_c_x.get()*2.f*sqrtf(_param_lin_k_x.get()); + _K_v(1,1) = _param_lin_c_y.get()*2.f*sqrtf(_param_lin_k_y.get()); + _K_v(2,2) = _param_lin_c_z.get()*2.f*sqrtf(_param_lin_k_z.get()); + _K_a(0,0) = _param_lin_ff_x.get(); + _K_a(1,1) = _param_lin_ff_y.get(); + _K_a(2,2) = _param_lin_ff_z.get(); + _K_q(0,0) = _param_rot_k_roll.get(); + _K_q(1,1) = _param_rot_k_pitch.get(); + _K_q(2,2) = _param_rot_ff_yaw.get(); // rudder is controlled via turn coordination, not INDI + _K_w(0,0) = _param_rot_c_roll.get()*2.f*sqrtf(_param_rot_k_roll.get()); + _K_w(1,1) = _param_rot_c_pitch.get()*2.f*sqrtf(_param_rot_k_pitch.get()); + _K_w(2,2) = _param_rot_p_yaw.get(); // rudder is controlled via turn coordination, not INDI + + // aircraft parameters + _inertia(0,0) = _param_fw_inertia_roll.get(); + _inertia(1,1) = _param_fw_inertia_pitch.get(); + _inertia(2,2) = _param_fw_inertia_yaw.get(); + _inertia(0,2) = _param_fw_inertia_rp.get(); + _inertia(2,0) = _param_fw_inertia_rp.get(); + _mass = _param_fw_mass.get(); + _area = _param_fw_wing_area.get(); + _rho = _param_rho.get(); + _C_L0 = _param_fw_c_l0.get(); + _C_L1 = _param_fw_c_l1.get(); + _C_D0 = _param_fw_c_d0.get(); + _C_D1 = _param_fw_c_d1.get(); + _C_D2 = _param_fw_c_d2.get(); + _C_B1 = _param_fw_c_b1.get(); + _aoa_offset = _param_aoa_offset.get(); + _stall_speed = _param_stall_speed.get(); + + // actuator gains + _k_ail = _param_k_act_roll.get(); + _k_ele = _param_k_act_pitch.get(); + _k_d_roll = _param_k_damping_roll.get(); + _k_d_pitch = _param_k_damping_pitch.get(); + + // trajectory origin + _origin_lat = _param_origin_lat.get(); + _origin_lon = _param_origin_lon.get(); + _origin_alt = _param_origin_alt.get(); + // check if local NED reference frame origin has changed: + // || (_local_pos.vxy_reset_counter != _pos_reset_counter + // initialize projection + map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon, + _local_pos.ref_timestamp); + // project the origin of the soaring ENU frame to the current NED frame + map_projection_project(&_global_local_proj_ref, _origin_lat, _origin_lon, &_origin_N, &_origin_E); + _origin_D = _local_pos.ref_alt - _origin_alt; + PX4_INFO("local reference frame updated"); + + _thrust_pos = _param_thrust.get(); + _thrust = _thrust_pos; + _switch_saturation = _param_switch_saturation.get(); + _switch_origin_hardcoded = _param_switch_origin_hardcoded.get(); + _switch_cl_soaring = _param_switch_cloop.get(); + if (_switch_sitl) { + // only use switch manual param in sitl mode + _switch_manual = _param_switch_manual.get(); + } + + _loiter = _param_loiter.get(); + + // only update shear heading and height with params, if desired + if (_switch_origin_hardcoded) { + _shear_heading = _param_shear_heading.get()/180.f * M_PI_F + M_PI_2_F; + _shear_h_ref = _param_shear_height.get(); + _select_loiter_trajectory(); + } + + + return PX4_OK; +} + +void +FixedwingPositionINDIControl::airspeed_poll() +{ + bool airspeed_valid = _airspeed_valid; + airspeed_validated_s airspeed_validated; + + if (_airspeed_validated_sub.update(&airspeed_validated)) { + + if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) + && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) + && (airspeed_validated.calibrated_airspeed_m_s > 0.0f)) { + + airspeed_valid = true; + + _airspeed_last_valid = airspeed_validated.timestamp; + _true_airspeed = airspeed_validated.true_airspeed_m_s; + _cal_airspeed = airspeed_validated.calibrated_airspeed_m_s; + } + + } else { + // no airspeed updates for one second + if (airspeed_valid && (hrt_elapsed_time(&_airspeed_last_valid) > 1_s)) { + airspeed_valid = false; + } + } + _airspeed_valid = airspeed_valid; +} + +void +FixedwingPositionINDIControl::airflow_aoa_poll() +{ + bool aoa_valid = _aoa_valid; + airflow_aoa_s aoa_validated; + + if (_airflow_aoa_sub.update(&aoa_validated)) { + + if (PX4_ISFINITE(aoa_validated.aoa_rad) + && (aoa_validated.valid)) { + + aoa_valid = true; + + _aoa_last_valid = aoa_validated.timestamp; + _aoa = aoa_validated.aoa_rad; + } + + } else { + // no aoa updates for one second + if (aoa_valid && (hrt_elapsed_time(&_aoa_last_valid) > 1_s)) { + aoa_valid = false; + } + } + _aoa_valid = aoa_valid; +} + +void +FixedwingPositionINDIControl::airflow_slip_poll() +{ + bool slip_valid = _slip_valid; + airflow_slip_s slip_validated; + + if (_airflow_slip_sub.update(&slip_validated)) { + + if (PX4_ISFINITE(slip_validated.slip_rad) + && (slip_validated.valid)) { + + slip_valid = true; + + _slip_last_valid = slip_validated.timestamp; + _slip = slip_validated.slip_rad; + } + + } else { + // no aoa updates for one second + if (slip_valid && (hrt_elapsed_time(&_slip_last_valid) > 1_s)) { + slip_valid = false; + } + } + _slip_valid = slip_valid; +} + +void +FixedwingPositionINDIControl::vehicle_status_poll() +{ + if(_vehicle_status_sub.update(&_vehicle_status)){ + //print_message(_vehicle_status); + } +} + +void +FixedwingPositionINDIControl::manual_control_setpoint_poll() +{ + if(_switch_manual){ + _manual_control_setpoint_sub.update(&_manual_control_setpoint); + _thrust = _manual_control_setpoint.z; + } + else { + _thrust = _thrust_pos; + } +} + +void +FixedwingPositionINDIControl::rc_channels_poll() +{ + if (_rc_channels_sub.update(&_rc_channels)) { + // use flaps channel to select manual feedthrough + if (!_switch_sitl) { + if (_rc_channels.channels[5]>=0.f){ + _switch_manual = true; + } + else{ + _switch_manual = false; + } + } + } +} + +void +FixedwingPositionINDIControl::vehicle_attitude_poll() +{ + if (_vehicle_attitude_sub.update(&_attitude)) { + // get rotation between NED frames + Dcmf R_ned_frd(Quatf(_attitude.q)); + // get rotation from FRD to ENU frame (change of basis) + Dcmf R_enu_frd(_R_ned_to_enu*R_ned_frd); + _att = Quatf(R_enu_frd); + } + if(hrt_absolute_time()-_attitude.timestamp > 20_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ + PX4_ERR("attitude sample is too old"); + } +} + +void +FixedwingPositionINDIControl::vehicle_angular_velocity_poll() +{ // + // no need to check if it was updated as the main loop is fired based on an update... + // + _omega = Vector3f(_angular_vel.xyz); + if(hrt_absolute_time()-_angular_vel.timestamp > 20_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ + PX4_ERR("angular velocity sample is too old"); + } +} + +void +FixedwingPositionINDIControl::vehicle_angular_acceleration_poll() +{ + if (_vehicle_angular_acceleration_sub.update(&_angular_accel)) { + _alpha = Vector3f(_angular_accel.xyz); + } + if(hrt_absolute_time()-_angular_accel.timestamp > 20_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ + PX4_ERR("angular acceleration sample is too old"); + } +} + +void +FixedwingPositionINDIControl::vehicle_local_position_poll() +{ + //vehicle_local_position_s pos; + if (_vehicle_local_position_sub.update(&_local_pos)){ + _pos = _R_ned_to_enu*Vector3f{_local_pos.x,_local_pos.y,_local_pos.z}; + _vel = _R_ned_to_enu*Vector3f{_local_pos.vx,_local_pos.vy,_local_pos.vz}; + // take accel from faster message, since 50Hz is too slow... + // transform to soaring frame + _pos = _pos - _R_ned_to_enu * Vector3f{_origin_N, _origin_E, _origin_D}; + } + if(hrt_absolute_time()-_local_pos.timestamp > 50_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ + PX4_ERR("local position sample is too old"); + } +} + +void +FixedwingPositionINDIControl::vehicle_acceleration_poll() +{ + //vehicle_local_position_s pos; + if (_vehicle_acceleration_sub.update(&_acceleration)){ + Dcmf R_ib(_att); + _acc = R_ib*Vector3f(_acceleration.xyz) - Vector3f{0.f,0.f,9.81f}; + } + if(hrt_absolute_time()-_acceleration.timestamp > 20_ms && _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD){ + PX4_ERR("linear acceleration sample is too old"); + } +} + +void +FixedwingPositionINDIControl::actuator_controls_poll() +{ + _actuator_controls_sub.update(&_actuators); +} + +void +FixedwingPositionINDIControl::soaring_controller_status_poll() +{ + if (_soaring_controller_status_sub.update(&_soaring_controller_status)){ + if (!_soaring_controller_status.soaring_controller_running){ + //PX4_INFO("Soaring controller turned off"); + } + if (_soaring_controller_status.timeout_detected){ + //PX4_INFO("Controller timeout detected"); + } + } +} + +void +FixedwingPositionINDIControl::soaring_estimator_shear_poll() +{ + if (!_switch_origin_hardcoded) { + if (_soaring_estimator_shear_sub.update(&_soaring_estimator_shear)){ + // update the shear estimate, only if we are flying in manual feedthrough for safety reasons + _shear_v_max = _soaring_estimator_shear.v_max; + _shear_alpha = _soaring_estimator_shear.alpha; + _shear_h_ref = _soaring_estimator_shear.h_ref; + _shear_heading = _soaring_estimator_shear.psi - M_PI_2_F; + _soaring_feasible = _soaring_estimator_shear.soaring_feasible; + // the initial speed of the target trajectory can safely be updated during soaring :) + _shear_aspd = _soaring_estimator_shear.aspd; + } + + } +} + +void +FixedwingPositionINDIControl::_compute_trajectory_transform() +{ + Eulerf e(0.f, 0.f, _shear_heading); + _R_enu_to_trajec = Dcmf(e); + _R_trajec_to_enu = _R_enu_to_trajec.transpose(); + _vec_enu_to_trajec = Vector3f{0.f,0.f,_shear_h_ref}; +} + +Vector3f +FixedwingPositionINDIControl::_compute_wind_estimate() +{ + Dcmf R_ib(_att); + Dcmf R_bi(R_ib.transpose()); + // compute expected AoA from g-forces: + Vector3f body_force = _mass*R_bi*(_acc + Vector3f{0.f,0.f,9.81f}); + + // approximate lift force, since implicit equation cannot be solved analytically: + // since alpha<<1, we approximate the lift force L = sin(alpha)*Fx - cos(alpha)*Fz + // as L = alpha*Fx - Fz + float Fx = cosf(_aoa_offset)*body_force(0) - sinf(_aoa_offset)*body_force(2); + float Fz = -cosf(_aoa_offset)*body_force(2) - sinf(_aoa_offset)*body_force(0); + float AoA_approx = (((2.f*Fz)/(_rho*_area*(fmaxf(_cal_airspeed*_cal_airspeed,_stall_speed*_stall_speed))+0.001f) - _C_L0)/_C_L1) / + (1 - ((2.f*Fx)/(_rho*_area*(fmaxf(_cal_airspeed*_cal_airspeed,_stall_speed*_stall_speed))+0.001f)/_C_L1)); + AoA_approx = constrain(AoA_approx,-0.2f,0.3f); + float speed = fmaxf(_cal_airspeed, _stall_speed); + float u_approx = _true_airspeed; + float v_approx = body_force(1)*_true_airspeed / (0.5f*_rho*powf(speed,2)*_area*_C_B1); + float w_approx = tanf(AoA_approx-_aoa_offset)*_true_airspeed; + Vector3f vel_air = R_ib*(Vector3f{u_approx, v_approx, w_approx}); + + // compute wind from wind triangle + Vector3f wind = _vel - vel_air; + //PX4_INFO("wind estimate: \t%.1f, \t%.1f, \t%.1f", (double)wind(0), (double)wind(1), (double)wind(2)); + return wind; +} + +Vector3f +FixedwingPositionINDIControl::_compute_wind_estimate_EKF() +{ + Dcmf R_ib(_att); + Dcmf R_bi(R_ib.transpose()); + // compute expected AoA from g-forces: + Vector3f body_force = _mass*R_bi*(_acc + Vector3f{0.f,0.f,9.81f}); + + // ***************** NEW COMPUTATION FROM MATLAB CALIBRATION ********************** + float speed = fmaxf(_cal_airspeed, _stall_speed); + float u_approx = _true_airspeed; + float v_approx = body_force(1)*_true_airspeed / (0.5f*_rho*powf(speed,2)*_area*_C_B1); + float w_approx = (-body_force(2)*_true_airspeed / (0.5f*_rho*powf(speed,2)*_area)-0.1949f)/3.5928f; + Vector3f vel_air = R_ib*(Vector3f{u_approx, v_approx, w_approx}); + + // compute wind from wind triangle + Vector3f wind = _vel - vel_air; + //PX4_INFO("wind estimate: \t%.1f, \t%.1f, \t%.1f", (double)wind(0), (double)wind(1), (double)wind(2)); + return wind; +} + +void +FixedwingPositionINDIControl::_set_wind_estimate(Vector3f wind) +{ + // apply some filtering + wind(0) = _lp_filter_wind[0].apply(wind(0)); + wind(1) = _lp_filter_wind[1].apply(wind(1)); + wind(2) = _lp_filter_wind[2].apply(wind(2)); + _wind_estimate = wind; + return; +} + +void +FixedwingPositionINDIControl::_set_wind_estimate_EKF(Vector3f wind) +{ + // apply some filtering + wind(0) = _lp_filter_wind_EKF[0].apply(wind(0)); + wind(1) = _lp_filter_wind_EKF[1].apply(wind(1)); + wind(2) = _lp_filter_wind_EKF[2].apply(wind(2)); + _wind_estimate_EKF = wind; + return; +} + + +void +FixedwingPositionINDIControl::_reverse(char* str, int len) +{ + // copied from: https://www.geeksforgeeks.org/convert-floating-point-number-string/ + int i = 0, j = len - 1, temp; + while (i < j) { + temp = str[i]; + str[i] = str[j]; + str[j] = temp; + i++; + j--; + } +} + +int +FixedwingPositionINDIControl::_int_to_str(int x, char str[], int d) +{ + // copied from: https://www.geeksforgeeks.org/convert-floating-point-number-string/ + int i = 0; + while (x) { + str[i++] = (x % 10) + '0'; + x = x / 10; + } + + // If number of digits required is more, then + // add 0s at the beginning + while (i < d) + str[i++] = '0'; + + _reverse(str, i); + str[i] = '\0'; + return i; +} + +void +FixedwingPositionINDIControl::_float_to_str(float n, char* res, int afterpoint) +{ + // copied from: https://www.geeksforgeeks.org/convert-floating-point-number-string/ + // Extract integer part + int ipart = (int)n; + + // Extract floating part + float fpart = n - (float)ipart; + + // convert integer part to string + int i = _int_to_str(ipart, res, 0); + + // check for display option after point + if (afterpoint != 0) { + res[i] = '.'; // add dot + + // Get the value of fraction part upto given no. + // of points after dot. The third parameter + // is needed to handle cases like 233.007 + fpart = fpart * (float)pow(10, afterpoint); + + _int_to_str((int)fpart, res + i + 1, afterpoint); + } +} + +void +FixedwingPositionINDIControl::_select_loiter_trajectory() +{ + + // select loiter trajectory for loiter test + char filename[16]; + switch (_loiter) + { + case 0: + strcpy(filename,"trajectory0.csv"); + break; + case 1: + strcpy(filename,"trajectory1.csv"); + break; + case 2: + strcpy(filename,"trajectory2.csv"); + break; + case 3: + strcpy(filename,"trajectory3.csv"); + break; + case 4: + strcpy(filename,"trajectory4.csv"); + break; + case 5: + strcpy(filename,"trajectory5.csv"); + break; + case 6: + strcpy(filename,"trajectory6.csv"); + break; + case 7: + strcpy(filename,"trajectory7.csv"); + break; + + default: + strcpy(filename,"trajectory0.csv"); + } + + /* + Also, we need to place the current trajectory, centered around zero height and assuming wind from the west, into the soaring frame. + Since the necessary computations for position, velocity and acceleration require the basis coefficients, which are not easy to transform, + we choose to transform our position in soaring frame into the "trajectory frame", compute position vector and it's derivatives from the basis coeffs + in the trajectory frame, and then transform these back to soaring frame for control purposes. + Therefore we define a new transform between the soaring frame and the trajectory frame. + */ + + _read_trajectory_coeffs_csv(filename); +} + +void +FixedwingPositionINDIControl::_select_soaring_trajectory() +{ + /* + We read a trajectory based on initial energy available at the beginning of the trajectory. + So the trajectory is selected based on two criteria, first the correct wind shear params (alpha and V_max) and the initial energy (potential + kinetic). + + The filename structure of the trajectories is the following: + trajec_____ + with + in {nominal, robust} + in [08,12] + in [020, 100] + in [E_min, E_max] + */ + + char file[40] = "robust/coeffs_robust"; + char v_str[3]; + char a_str[3]; + char e_str[3]; + // get the correct filename + _float_to_str(_shear_v_max, v_str, 0); + _float_to_str(_shear_alpha*100.f, a_str, 0); + _float_to_str(_shear_aspd, e_str, 0); + + strcat(file,"_"); + strcat(file,v_str); + strcat(file,"_"); + strcat(file,a_str); + strcat(file,"_"); + strcat(file,e_str); + strcat(file,".csv"); + PX4_INFO("filename: \t%.40s", file); + // get the basis coeffs from file + _read_trajectory_coeffs_csv(file); +} + +void +FixedwingPositionINDIControl::_read_trajectory_coeffs_csv(char *filename) +{ + + // ======================================================================= + bool error = false; + + //char home_dir[200] = "/home/marvin/Documents/master_thesis_ADS/PX4/Git/ethzasl_fw_px4/src/modules/fw_dyn_soar_control/trajectories/"; + char home_dir[200] = PX4_ROOTFSDIR"/fs/microsd/trajectories/"; + //PX4_ERR(home_dir); + strcat(home_dir,filename); + FILE* fp = fopen(home_dir, "r"); + + if (fp == nullptr) { + PX4_ERR("Can't open file"); + error = true; + } + else { + // Here we have taken size of + // array 1024 you can modify it + const uint buffersize = _num_basis_funs*32; + char buffer[buffersize]; + + int row = 0; + int column = 0; + + // loop over rows + while (fgets(buffer, + buffersize, fp)) { + column = 0; + + // Splitting the data + char* value = strtok(buffer, ","); + + // loop over columns + while (value) { + if (*value=='\0'||*value==' ') { + // simply skip extra characters + continue; + } + switch(row){ + case 0: + _basis_coeffs_x(column) = (float)atof(value); + break; + case 1: + _basis_coeffs_y(column) = (float)atof(value); + break; + case 2: + _basis_coeffs_z(column) = (float)atof(value); + break; + + default: + break; + } + //PX4_INFO("row: %d, col: %d, read value: %.3f", row, column, (double)atof(value)); + value = strtok(NULL, ","); + column++; + + } + row++; + } + int failure = fclose(fp); + if (failure==-1) { + PX4_ERR("Can't close file"); + } + } + // ======================================================================= + + + // go back to safety mode loiter circle in 30m height + if(error){ + // 100m radius circle trajec + _basis_coeffs_x(0) = 0.000038f; + _basis_coeffs_x(1) = 1812.140143f; + _basis_coeffs_x(2) = -6365.976106f; + _basis_coeffs_x(3) = 10773.875378f; + _basis_coeffs_x(4) = -9441.287977f; + _basis_coeffs_x(5) = 1439.744061f; + _basis_coeffs_x(6) = 6853.112823f; + _basis_coeffs_x(7) = -7433.361925f; + _basis_coeffs_x(8) = -72.566660f; + _basis_coeffs_x(9) = 7518.521784f; + _basis_coeffs_x(10) = -6807.858677f; + _basis_coeffs_x(11) = -1586.021605f; + _basis_coeffs_x(12) = 9599.405711f; + _basis_coeffs_x(13) = -10876.256865f; + _basis_coeffs_x(14) = 6406.017620f; + _basis_coeffs_x(15) = -1819.600542f; + + _basis_coeffs_y(0) = -59.999852f; + _basis_coeffs_y(1) = -2811.660383f; + _basis_coeffs_y(2) = 13178.399227f; + _basis_coeffs_y(3) = -30339.925641f; + _basis_coeffs_y(4) = 43145.286828f; + _basis_coeffs_y(5) = -37009.839292f; + _basis_coeffs_y(6) = 9438.328009f; + _basis_coeffs_y(7) = 23631.637452f; + _basis_coeffs_y(8) = -38371.559953f; + _basis_coeffs_y(9) = 23715.306334f; + _basis_coeffs_y(10) = 9316.038368f; + _basis_coeffs_y(11) = -36903.451639f; + _basis_coeffs_y(12) = 43082.749551f; + _basis_coeffs_y(13) = -30315.482005f; + _basis_coeffs_y(14) = 13172.915247f; + _basis_coeffs_y(15) = -2811.186858f; + + _basis_coeffs_z(0) = 30.0f; + _basis_coeffs_z(1) = 0.0f; + _basis_coeffs_z(2) = 0.0f; + _basis_coeffs_z(3) = 0.0f; + _basis_coeffs_z(4) = 0.0f; + _basis_coeffs_z(5) = 0.0f; + _basis_coeffs_z(6) = 0.0f; + _basis_coeffs_z(7) = 0.0f; + _basis_coeffs_z(8) = 0.0f; + _basis_coeffs_z(9) = 0.0f; + _basis_coeffs_z(10) = 0.0f; + _basis_coeffs_z(11) = 0.0f; + _basis_coeffs_z(12) = 0.0f; + _basis_coeffs_z(13) = 0.0f; + _basis_coeffs_z(14) = 0.0f; + _basis_coeffs_z(15) = 0.0f; + } + +} + +void +FixedwingPositionINDIControl::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // only run controller if pos, vel, acc changed + if (_vehicle_angular_velocity_sub.update(&_angular_vel)) + { + // only update parameters if they changed + bool params_updated = _parameter_update_sub.updated(); + + // check for parameter updates + if (params_updated) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + parameters_update(); + } + + //const float dt = math::constrain((pos.timestamp - _last_run) * 1e-6f, 0.002f, 0.04f); + //_last_run = _local_pos.timestamp; + + // check if local NED reference frame origin has changed: + // || (_local_pos.vxy_reset_counter != _pos_reset_counter + if (!map_projection_initialized(&_global_local_proj_ref) + || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp) + || (_local_pos.xy_reset_counter != _pos_reset_counter) + || (_local_pos.z_reset_counter != _alt_reset_counter)) { + // initialize projection + map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon, + _local_pos.ref_timestamp); + // project the origin of the soaring ENU frame to the current NED frame + map_projection_project(&_global_local_proj_ref, _origin_lat, _origin_lon, &_origin_N, &_origin_E); + _origin_D = _local_pos.ref_alt - _origin_alt; + PX4_INFO("local reference frame updated"); + } + // update reset counters + _pos_reset_counter = _local_pos.xy_reset_counter; + _alt_reset_counter = _local_pos.z_reset_counter; + + // run polls + vehicle_status_poll(); + airspeed_poll(); + airflow_aoa_poll(); + airflow_slip_poll(); + rc_channels_poll(); + manual_control_setpoint_poll(); + vehicle_local_position_poll(); + vehicle_attitude_poll(); + vehicle_acceleration_poll(); + vehicle_angular_velocity_poll(); + vehicle_angular_acceleration_poll(); + soaring_controller_status_poll(); + + // update the shear estimate, only target airspeed is updated in soaring mode + soaring_estimator_shear_poll(); + + // update transform from trajectory frame to ENU frame (soaring frame) + _compute_trajectory_transform(); + + // =============================== + // compute wind pseudo-measurement + // =============================== + Vector3f wind = _compute_wind_estimate(); + _set_wind_estimate(wind); + Vector3f wind_EKF = _compute_wind_estimate_EKF(); + _set_wind_estimate_EKF(wind_EKF); + + + // only run actuators poll, when our module is not publishing: + if (_vehicle_status.nav_state != vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + actuator_controls_poll(); + } + + // ================================= + // get reference point on trajectory + // ================================= + float t_ref = _get_closest_t(_pos); + + // ================================================================= + // possibly select a new trajectory, if we are finishing the old one + // ================================================================= + if (!_switch_origin_hardcoded && t_ref>=0.97f && (hrt_absolute_time()-_last_time_trajec)>1000000) { + _select_soaring_trajectory(); + _last_time_trajec = hrt_absolute_time(); + } + + // ============================ + // compute reference kinematics + // ============================ + // downscale velocity to match current one, + // terminal time is determined such that current velocity is met + Vector3f v_ref_ = _get_velocity_ref(t_ref, 1.0f); + float T = sqrtf((v_ref_*v_ref_)/(_vel*_vel+0.001f)); + Vector3f pos_ref = _get_position_ref(t_ref); // in inertial ENU + Vector3f vel_ref = _get_velocity_ref(t_ref,T); // in inertial ENU + Vector3f acc_ref = _get_acceleration_ref(t_ref,T); // gravity-corrected acceleration (ENU) + Quatf q = _get_attitude_ref(t_ref,T); + Vector3f omega_ref = _get_angular_velocity_ref(t_ref,T); // body angular velocity + Vector3f alpha_ref = _get_angular_acceleration_ref(t_ref,T); // body angular acceleration + + // ===================== + // compute control input + // ===================== + Vector3f ctrl = _compute_INDI_stage_1(pos_ref, vel_ref, acc_ref, omega_ref, alpha_ref); + Vector3f ctrl1 = _compute_INDI_stage_2(ctrl); + + // ============================ + // compute actuator deflections + // ============================ + Vector3f ctrl2 = _compute_actuator_deflections(ctrl1); + + // ================================= + // publish offboard control commands + // ================================= + offboard_control_mode_s ocm{}; + ocm.actuator = true; + ocm.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(ocm); + + // Publish actuator controls only once in OFFBOARD + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + + // ======================================== + // publish controller position in ENU frame + // ======================================== + _soaring_controller_position.timestamp = hrt_absolute_time(); + for (int i=0; i<3; i++){ + _soaring_controller_position.pos[i] = _pos(i); + _soaring_controller_position.vel[i] = _vel(i); + _soaring_controller_position.acc[i] = _acc(i); + } + _soaring_controller_position_pub.publish(_soaring_controller_position); + + // ==================================== + // publish controller position setpoint + // ==================================== + _soaring_controller_position_setpoint.timestamp = hrt_absolute_time(); + for (int i=0; i<3; i++){ + _soaring_controller_position_setpoint.pos[i] = pos_ref(i); + _soaring_controller_position_setpoint.vel[i] = vel_ref(i); + _soaring_controller_position_setpoint.acc[i] = acc_ref(i); + _soaring_controller_position_setpoint.f_command[i] = _f_command(i); + _soaring_controller_position_setpoint.m_command[i] = _m_command(i); + _soaring_controller_position_setpoint.w_err[i] = _w_err(i); + } + _soaring_controller_position_setpoint_pub.publish(_soaring_controller_position_setpoint); + + // ===================== + // publish control input + // ===================== + //_angular_accel_sp = {}; + _angular_accel_sp.timestamp = hrt_absolute_time(); + //_angular_accel_sp.timestamp_sample = hrt_absolute_time(); + _angular_accel_sp.xyz[0] = ctrl(0); + _angular_accel_sp.xyz[1] = ctrl(1); + _angular_accel_sp.xyz[2] = ctrl(2); + _angular_accel_sp_pub.publish(_angular_accel_sp); + + // ========================= + // publish attitude setpoint + // ========================= + //_attitude_sp = {}; + Quatf q_sp(_R_enu_to_ned*Dcmf(q)); + _attitude_sp.timestamp = hrt_absolute_time(); + _attitude_sp.q_d[0] = q_sp(0); + _attitude_sp.q_d[1] = q_sp(1); + _attitude_sp.q_d[2] = q_sp(2); + _attitude_sp.q_d[3] = q_sp(3); + _attitude_sp_pub.publish(_attitude_sp); + + // ====================== + // publish rates setpoint + // ====================== + //_angular_vel_sp = {}; + _angular_vel_sp.timestamp = hrt_absolute_time(); + _angular_vel_sp.roll = omega_ref(0); + _angular_vel_sp.pitch = omega_ref(1); + _angular_vel_sp.yaw = omega_ref(2); + _angular_vel_sp_pub.publish(_angular_vel_sp); + + // ========================= + // publish acutator controls + // ========================= + //_actuators = {}; + _actuators.timestamp = hrt_absolute_time(); + _actuators.timestamp_sample = hrt_absolute_time(); + _actuators.control[actuator_controls_s::INDEX_ROLL] = ctrl2(0); + _actuators.control[actuator_controls_s::INDEX_PITCH] = ctrl2(1); + _actuators.control[actuator_controls_s::INDEX_YAW] = ctrl2(2); + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _thrust; + _actuators_0_pub.publish(_actuators); + //print_message(_actuators); + + // ===================== + // publish wind estimate + // ===================== + //_soaring_controller_wind = {}; + _soaring_controller_wind.timestamp = hrt_absolute_time(); + _soaring_controller_wind.wind_estimate[0] = wind(0); + _soaring_controller_wind.wind_estimate[1] = wind(1); + _soaring_controller_wind.wind_estimate[2] = wind(2); + _soaring_controller_wind.wind_estimate_filtered[0] = _wind_estimate_EKF(0); + _soaring_controller_wind.wind_estimate_filtered[1] = _wind_estimate_EKF(1); + _soaring_controller_wind.wind_estimate_filtered[2] = _wind_estimate_EKF(2); + _soaring_controller_wind.position[0] = _pos(0); + _soaring_controller_wind.position[1] = _pos(1); + _soaring_controller_wind.position[2] = _pos(2); + _soaring_controller_wind.airspeed = _true_airspeed; + if (_switch_cl_soaring) { + // always update shear params in closed loop soaring mode + _soaring_controller_wind.lock_params = false; + } + else { + // only update in manual feedthrough in open loop soaring + _soaring_controller_wind.lock_params = !_switch_manual; + } + //Eulerf e(Quatf(_attitude.q)); + //float bank = e(0); + // only declare wind estimate valid for shear estimator, if we are close to the soaring center + if ((float)sqrtf(powf(_pos(0),2)+powf(_pos(1),2))<100.f) { + _soaring_controller_wind.valid = true; + } + else { + _soaring_controller_wind.valid = false; + } + _soaring_controller_wind_pub.publish(_soaring_controller_wind); + + + + if (_counter==100) { + _counter = 0; + //PX4_INFO("Feedthrough switch: \t%.2f", (double)(_rc_channels.channels[5])); + //PX4_INFO("frequency: \t%.3f", (double)(1000000*100)/(hrt_absolute_time()-_last_time)); + _last_time = hrt_absolute_time(); + } + else { + _counter += 1; + } + } + + // =========================== + // publish rate control status + // =========================== + rate_ctrl_status_s rate_ctrl_status{}; + rate_ctrl_status.timestamp = hrt_absolute_time(); + rate_ctrl_status.rollspeed_integ = 0.0f; + rate_ctrl_status.pitchspeed_integ = 0.0f; + rate_ctrl_status.yawspeed_integ = 0.0f; + _rate_ctrl_status_pub.publish(rate_ctrl_status); + + // ============================== + // publish soaring control status + // ============================== + //_soaring_controller_heartbeat_s _soaring_controller_heartbeat{}; + _soaring_controller_heartbeat.timestamp = hrt_absolute_time(); + _soaring_controller_heartbeat.heartbeat = hrt_absolute_time(); + _soaring_controller_heartbeat_pub.publish(_soaring_controller_heartbeat); + + // ==================== + // publish debug values + // ==================== + Dcmf R_ib(_att); + Dcmf R_bi(R_ib.transpose()); + Vector3f vel_body = R_bi*(_vel - _wind_estimate); + _slip = atan2f(vel_body(1), vel_body(0))*180.f/M_PI_2_F; + _debug_value.timestamp = hrt_absolute_time(); + _debug_value.value = _slip; + _debug_value_pub.publish(_debug_value); + + perf_end(_loop_perf); + } + +} + +Vector +FixedwingPositionINDIControl::_get_basis_funs(float t) +{ + Vector vec; + vec(0) = 1.0f; + float sigma = 1.0f/_num_basis_funs; + for(uint i=1; i<_num_basis_funs; i++){ + float fun1 = sinf(M_PI_F*t); + float fun2 = exp(-powf((t-float(i)/float(_num_basis_funs)),2)/sigma); + vec(i) = fun1*fun2; + } + return vec; +} + +Vector +FixedwingPositionINDIControl::_get_d_dt_basis_funs(float t) +{ + Vector vec; + vec(0) = 0.0f; + float sigma = 1.0f/_num_basis_funs; + for(uint i=1; i<_num_basis_funs; i++){ + float fun1 = sinf(M_PI_F*t); + float fun2 = exp(-powf((t-float(i)/_num_basis_funs),2)/sigma); + vec(i) = fun2*(M_PI_F*sigma*cosf(M_PI_F*t)-2*(t-float(i)/_num_basis_funs)*fun1)/sigma; + } + return vec; +} + +Vector +FixedwingPositionINDIControl::_get_d2_dt2_basis_funs(float t) +{ + Vector vec; + vec(0) = 0.0f; + float sigma = 1.0f/_num_basis_funs; + for(uint i=1; i<_num_basis_funs; i++){ + float fun1 = sinf(M_PI_F*t); + float fun2 = exp(-powf((t-float(i)/_num_basis_funs),2)/sigma); + vec(i) = fun2 * (fun1 * (4*powf((float(i)/_num_basis_funs-t),2) - \ + sigma*(powf(M_PI_F,2)*sigma + 2)) + 4*M_PI_F*sigma*(float(i)/_num_basis_funs-t)*cosf(M_PI_F*t))/(powf(sigma,2)); + + } + return vec; +} + +Vector3f +FixedwingPositionINDIControl::_get_position_ref(float t) +{ + Vector basis = _get_basis_funs(t); + float x = _basis_coeffs_x*basis; + float y = _basis_coeffs_y*basis; + float z = _basis_coeffs_z*basis; + return _R_trajec_to_enu*Vector3f{x, y, z} + _vec_enu_to_trajec; +} + +Vector3f +FixedwingPositionINDIControl::_get_velocity_ref(float t, float T) +{ + Vector basis = _get_d_dt_basis_funs(t); + float x = _basis_coeffs_x*basis; + float y = _basis_coeffs_y*basis; + float z = _basis_coeffs_z*basis; + return _R_trajec_to_enu*(Vector3f{x, y, z}/T); +} + +Vector3f +FixedwingPositionINDIControl::_get_acceleration_ref(float t, float T) +{ + Vector basis = _get_d2_dt2_basis_funs(t); + float x = _basis_coeffs_x*basis; + float y = _basis_coeffs_y*basis; + float z = _basis_coeffs_z*basis; + return _R_trajec_to_enu*(Vector3f{x, y, z}/powf(T,2)); +} + +Quatf +FixedwingPositionINDIControl::_get_attitude_ref(float t, float T) +{ + Vector3f vel = _get_velocity_ref(t,T); + Vector3f vel_air = vel - _wind_estimate; + Vector3f acc = _get_acceleration_ref(t,T); + // add gravity + acc(2) += 9.81f; + // compute required force + Vector3f f = _mass*acc; + // compute force component projected onto lift axis + Vector3f vel_normalized = vel_air.normalized(); + Vector3f f_lift = f - (f*vel_normalized)*vel_normalized; + Vector3f lift_normalized = f_lift.normalized(); + Vector3f wing_normalized = -vel_normalized.cross(lift_normalized); + // compute rotation matrix between ENU and FRD frame + Dcmf R_bi; + R_bi(0,0) = vel_normalized(0); + R_bi(0,1) = vel_normalized(1); + R_bi(0,2) = vel_normalized(2); + R_bi(1,0) = wing_normalized(0); + R_bi(1,1) = wing_normalized(1); + R_bi(1,2) = wing_normalized(2); + R_bi(2,0) = lift_normalized(0); + R_bi(2,1) = lift_normalized(1); + R_bi(2,2) = lift_normalized(2); + R_bi.renormalize(); + // compute actual air density to be used with true airspeed + float rho_corrected; + if (_cal_airspeed>=_stall_speed) { + rho_corrected = _rho*powf(_cal_airspeed/_true_airspeed, 2); + } + else { + rho_corrected = _rho; + } + // compute required AoA + Vector3f f_phi = R_bi*f_lift; + float AoA = ((2.f*f_phi(2))/(rho_corrected*_area*(vel_air*vel_air)+0.001f) - _C_L0)/_C_L1 - _aoa_offset; + // compute final rotation matrix + Eulerf e(0.f, AoA, 0.f); + Dcmf R_pitch(e); + Dcmf Rotation(R_pitch*R_bi); + // switch from FRD to ENU frame + Rotation(1,0) *= -1.f; + Rotation(1,1) *= -1.f; + Rotation(1,2) *= -1.f; + Rotation(2,0) *= -1.f; + Rotation(2,1) *= -1.f; + Rotation(2,2) *= -1.f; + // plausibility check + /* + float determinant = Rotation(0,0)*(Rotation(1,1)*Rotation(2,2)-Rotation(2,1)*Rotation(1,2)) - + Rotation(1,0)*(Rotation(0,1)*Rotation(2,2)-Rotation(2,1)*Rotation(0,2)) + + Rotation(2,0)*(Rotation(0,1)*Rotation(1,2)-Rotation(1,1)*Rotation(0,2)); + PX4_INFO("determinant: %.2f", (double)determinant); + PX4_INFO("length: %.2f", (double)(wing_normalized*wing_normalized)); + */ + + + + Quatf q(Rotation.transpose()); + return q; +} + +Vector3f +FixedwingPositionINDIControl::_get_angular_velocity_ref(float t, float T) +{ + float dt = 0.001; + float t_lower = fmaxf(0.f,t-dt); + float t_upper = fminf(t+dt,1.f); + Dcmf R_i0(_get_attitude_ref(t_lower, T)); + Dcmf R_i1(_get_attitude_ref(t_upper, T)); + Dcmf R_10 = R_i1.transpose()*R_i0; + AxisAnglef w_01(R_10); + return -w_01.axis()*w_01.angle()/(T*(t_upper-t_lower)); +} + +Vector3f +FixedwingPositionINDIControl::_get_angular_acceleration_ref(float t, float T) +{ + float dt = 0.001; + float t_lower = fmaxf(0.f,t-dt); + float t_upper = fminf(t+dt,1.f); + // compute roational velocity in inertial frame + Dcmf R_i0(_get_attitude_ref(t_lower, T)); + AxisAnglef w_0(R_i0*_get_angular_velocity_ref(t_lower, T)); + // compute roational velocity in inertial frame + Dcmf R_i1(_get_attitude_ref(t_upper, T)); + AxisAnglef w_1(R_i1*_get_angular_velocity_ref(t_upper, T)); + // compute gradient via finite differences + Vector3f dw_dt = (w_1.axis()*w_1.angle() - w_0.axis()*w_0.angle()) / (T*(t_upper-t_lower)); + // transform back to body frame + return R_i0.transpose()*dw_dt; +} + +float +FixedwingPositionINDIControl::_get_closest_t(Vector3f pos) +{ + // multi-stage computation of the closest point on the reference path + + const uint n_1 = 20; + Vector distances; + float t_ref; + // ======= + // STAGE 1 + // ======= + // STAGE 1: compute all distances + for(uint i=0; i distances_2; + float t_lower = fmod(t_1 - 1.0f/n_1,1.0f); + // STAGE 2: compute all distances + for(uint i=0; i<=n_2; i++){ + t_ref = fmod(t_lower + float(i)*2.f/float(n_1*n_2),1.0f); + Vector3f pos_ref = _get_position_ref(t_ref); + distances_2(i) = (pos_ref - pos)*(pos_ref - pos); + } + + // STAGE 2: get index of smallest distance + float t_2 = 0.f; + min_dist = distances_2(0); + for(uint i=1; i<=n_2; i++){ + if(distances_2(i)=_stall_speed) { + rho_corrected = _rho*powf(_cal_airspeed/_true_airspeed, 2); + } + else { + rho_corrected = _rho; + } + // compute required AoA + Vector3f f_phi = R_bi*f_lift; + float AoA = ((2.f*f_phi(2))/(rho_corrected*_area*(vel_air*vel_air)+0.001f) - _C_L0)/_C_L1 - _aoa_offset; + // compute final rotation matrix + Eulerf e(0.f, AoA, 0.f); + Dcmf R_pitch(e); + Dcmf Rotation(R_pitch*R_bi); + // switch from FRD to ENU frame + Rotation(1,0) *= -1.f; + Rotation(1,1) *= -1.f; + Rotation(1,2) *= -1.f; + Rotation(2,0) *= -1.f; + Rotation(2,1) *= -1.f; + Rotation(2,2) *= -1.f; + + Quatf q(Rotation.transpose()); + return q; +} + +Vector3f +FixedwingPositionINDIControl::_compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref) +{ + Dcmf R_ib(_att); + Dcmf R_bi(R_ib.transpose()); + // apply LP filter to acceleration & velocity + Vector3f acc_filtered; + acc_filtered(0) = _lp_filter_accel[0].apply(_acc(0)); + acc_filtered(1) = _lp_filter_accel[1].apply(_acc(1)); + acc_filtered(2) = _lp_filter_accel[2].apply(_acc(2)); + Vector3f omega_filtered; + omega_filtered(0) = _lp_filter_omega[0].apply(_omega(0)); + omega_filtered(1) = _lp_filter_omega[1].apply(_omega(1)); + omega_filtered(2) = _lp_filter_omega[2].apply(_omega(2)); + + // ========================================= + // apply PD control law on the body position + // ========================================= + Vector3f acc_command = R_ib*(_K_x*R_bi*(pos_ref-_pos) + _K_v*R_bi*(vel_ref-_vel) + _K_a*R_bi*(acc_ref-_acc)) + acc_ref; + + // ================================== + // compute expected aerodynamic force + // ================================== + Vector3f f_current; + Vector3f vel_body = R_bi*(_vel - _wind_estimate); + float AoA = atan2f(vel_body(2), vel_body(0)) + _aoa_offset; + float C_l = _C_L0 + _C_L1*AoA; + float C_d = _C_D0 + _C_D1*AoA + _C_D2*powf(AoA,2); + // compute actual air density + float rho_corrected; + if (_cal_airspeed>=_stall_speed) { + rho_corrected = _rho*powf(_cal_airspeed/_true_airspeed, 2); + } + else { + rho_corrected = _rho; + } + float factor = -0.5f*rho_corrected*_area*sqrtf(vel_body*vel_body); + Vector3f w_x = vel_body; + Vector3f w_z = w_x.cross(Vector3f{0.f,1.f,0.f}); + f_current = R_ib*(factor*(C_l*w_z + C_d*w_x)); + // apply LP filter to force + Vector3f f_current_filtered; + f_current_filtered(0) = _lp_filter_force[0].apply(f_current(0)); + f_current_filtered(1) = _lp_filter_force[1].apply(f_current(1)); + f_current_filtered(2) = _lp_filter_force[2].apply(f_current(2)); + + // ================================ + // get force command in world frame + // ================================ + Vector3f f_command = _mass*(acc_command - acc_filtered) + f_current_filtered; + + // ============================================================================================================ + // apply some filtering to the force command. This introduces some time delay, + // which is not desired for stability reasons, but it rejects some of the noise fed to the low-level controller + // ============================================================================================================ + f_command(0) = _lp_filter_ctrl0[0].apply(f_command(0)); + f_command(1) = _lp_filter_ctrl0[1].apply(f_command(1)); + f_command(2) = _lp_filter_ctrl0[2].apply(f_command(2)); + _f_command = f_command; + // limit maximum lift force by the maximum lift force, the aircraft can produce (assume max force at 15° aoa) + + // ==================================================================== + // saturate force command to avoid overly agressive maneuvers and stall + // ==================================================================== + if (_switch_saturation){ + float speed = vel_body*vel_body; + // compute maximum achievable force + float f_max; + if (speed>_stall_speed){ + f_max = -factor*sqrtf(vel_body*vel_body)*(_C_L0 + _C_L1*0.25f); // assume stall at 15° AoA + } + else { + f_max = -factor*_stall_speed*(_C_L0 + _C_L1*0.25f); // assume stall at 15° AoA + } + // compute current command + float f_now = sqrtf(f_command*f_command); + // saturate current command + if (f_now>f_max){ + f_command = f_max/f_now * f_command; + } + } + + // ========================================================================== + // get required attitude (assuming we can fly the target velocity), and error + // ========================================================================== + Dcmf R_ref(_get_attitude(vel_ref,f_command)); + // get attitude error + Dcmf R_ref_true(R_ref.transpose()*R_ib); + // get required rotation vector (in body frame) + AxisAnglef q_err(R_ref_true); + Vector3f w_err; + // project rotation angle to [-pi,pi] + if (q_err.angle()*q_err.angle()0.f){ + w_err = (2.f*M_PI_F-(float)fmod(q_err.angle(),2.f*M_PI_F))*q_err.axis(); + } + else{ + w_err = (-2.f*M_PI_F-(float)fmod(q_err.angle(),2.f*M_PI_F))*q_err.axis(); + } + } + + // ========================================= + // apply PD control law on the body attitude + // ========================================= + Vector3f rot_acc_command = _K_q*w_err + _K_w*(omega_ref-_omega) + alpha_ref; + + if (sqrtf(w_err*w_err)>M_PI_F){ + PX4_ERR("rotation angle larger than pi: \t%.2f, \t%.2f, \t%.2f", (double)sqrtf(w_err*w_err), (double)q_err.angle(), (double)(q_err.axis()*q_err.axis())); + } + + // ==================================== + // manual attitude setpoint feedthrough + // ==================================== + if (_switch_manual){ + // get an attitude setpoint from the current manual inputs + float roll_ref = 1.f * _manual_control_setpoint.y * 1.0f; + float pitch_ref = -1.f* _manual_control_setpoint.x * M_PI_4_F; + Eulerf E_current(Quatf(_attitude.q)); + float yaw_ref = E_current.psi(); + Dcmf R_ned_frd_ref(Eulerf(roll_ref, pitch_ref, yaw_ref)); + Dcmf R_enu_frd_ref(_R_ned_to_enu*R_ned_frd_ref); + Quatf att_ref(R_enu_frd_ref); + R_ref = Dcmf(att_ref); + + // get attitude error + R_ref_true = Dcmf(R_ref.transpose()*R_ib); + // get required rotation vector (in body frame) + q_err = AxisAnglef(R_ref_true); + // project rotation angle to [-pi,pi] + if (q_err.angle()*q_err.angle()0.f){ + w_err = (2.f*M_PI_F-(float)fmod(q_err.angle(),2.f*M_PI_F))*q_err.axis(); + } + else{ + w_err = (-2.f*M_PI_F-(float)fmod(q_err.angle(),2.f*M_PI_F))*q_err.axis(); + } + } + _w_err = w_err; + + // compute rot acc command + rot_acc_command = _K_q*w_err + _K_w*(Vector3f{0.f,0.f,0.f}-_omega); + + } + + // ============================================================== + // overwrite rudder rot_acc_command with turn coordination values + // ============================================================== + Vector3f vel_air = _vel - _wind_estimate; + Vector3f vel_normalized = vel_air.normalized(); + Vector3f acc_normalized = acc_filtered.normalized(); + // compute ideal angular velocity + Vector3f omega_turn_ref_normalized = vel_normalized.cross(acc_normalized); + Vector3f omega_turn_ref; + // constuct acc perpendicular to flight path + Vector3f acc_perp = acc_filtered - (acc_filtered*vel_normalized)*vel_normalized; + if (_airspeed_valid&&_cal_airspeed>_stall_speed) { + omega_turn_ref = sqrtf(acc_perp*acc_perp) / (_true_airspeed) * R_bi * omega_turn_ref_normalized.normalized(); + //PX4_INFO("yaw rate ref, yaw rate: \t%.2f\t%.2f", (double)(omega_turn_ref(2)), (double)(omega_filtered(2))); + } + else { + omega_turn_ref = sqrtf(acc_perp*acc_perp) / (_stall_speed) * R_bi * omega_turn_ref_normalized.normalized(); + //PX4_ERR("No valid airspeed message detected or airspeed too low"); + } + + // apply some smoothing since we don't want HF components in our rudder output + omega_turn_ref(2) = _lp_filter_rud.apply(omega_turn_ref(2)); + + // transform rate vector to body frame + float scaler = (_stall_speed*_stall_speed)/fmaxf(sqrtf(vel_body*vel_body)*vel_body(0), _stall_speed*_stall_speed); + // not really an accel command, rather a FF-P command + rot_acc_command(2) = _K_q(2,2)*omega_turn_ref(2)*scaler + _K_w(2,2)*(omega_turn_ref(2) - omega_filtered(2))* scaler*scaler; + + + return rot_acc_command; +} + +Vector3f +FixedwingPositionINDIControl::_compute_INDI_stage_2(Vector3f ctrl) +{ + // compute velocity in body frame + Dcmf R_ib(_att); + Vector3f vel_body = R_ib.transpose()*(_vel-_wind_estimate); + float q = fmaxf(0.5f*sqrtf(vel_body*vel_body)*vel_body(0), 0.5f*_stall_speed*_stall_speed); // dynamic pressure, saturates at stall speed + + // compute moments + Vector3f moment; + moment(0) = _k_ail*q*_actuators.control[actuator_controls_s::INDEX_ROLL] - _k_d_roll*q*_omega(0); + moment(1) = _k_ele*q*_actuators.control[actuator_controls_s::INDEX_PITCH] - _k_d_pitch*q*_omega(1); + moment(2) = 0.f; + + // introduce artificial time delay that is also present in acceleration + Vector3f moment_filtered; + moment_filtered(0) = _lp_filter_delay[0].apply(moment(0)); + moment_filtered(1) = _lp_filter_delay[1].apply(moment(1)); + moment_filtered(2) = _lp_filter_delay[2].apply(moment(2)); + + // No filter for alpha, since it is already filtered... + Vector3f alpha_filtered = _alpha; + Vector3f moment_command = _inertia * (ctrl - alpha_filtered) + moment_filtered; + _m_command = R_ib.transpose()*moment_command; + + // perform dynamic inversion + Vector3f deflection; + deflection(0) = (moment_command(0) + _k_d_roll*q*_omega(0))/fmaxf((_k_ail*q),0.0001f); + deflection(1) = (moment_command(1) + _k_d_pitch*q*_omega(1))/fmaxf((_k_ele*q),0.0001f); + + // overwrite rudder deflection with NDI turn coordination (no INDI) + deflection(2) = ctrl(2); + + return deflection; +} + + +Vector3f +FixedwingPositionINDIControl::_compute_actuator_deflections(Vector3f ctrl) +{ + // compute the normalized actuator deflection, including airspeed scaling + Vector3f deflection = ctrl; + + // limit actuator deflection + for(int i=0; i<3; i++){ + deflection(i) = constrain(deflection(i),-1.f,1.f); + } + /* + // add servo slew + float current_ail = _actuators.control[actuator_controls_s::INDEX_ROLL]; + float current_ele = _actuators.control[actuator_controls_s::INDEX_PITCH]; + float current_rud = _actuators.control[actuator_controls_s::INDEX_YAW]; + // + float max_rate = 0.5f/0.18f; // + float dt = 1.f/_sample_frequency; + // + deflection(0) = constrain(deflection(0),current_ail-dt*max_rate,current_ail+dt*max_rate); + deflection(1) = constrain(deflection(1),current_ele-dt*max_rate,current_ele+dt*max_rate); + deflection(2) = constrain(deflection(2),current_rud-dt*max_rate,current_rud+dt*max_rate); + */ + return deflection; +} + + + +int FixedwingPositionINDIControl::task_spawn(int argc, char *argv[]) +{ + FixedwingPositionINDIControl *instance = new FixedwingPositionINDIControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + + +int FixedwingPositionINDIControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int FixedwingPositionINDIControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +fw_dyn_soar_control is the fixed wing controller for dynamic soaring tasks. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("fw_dyn_soar_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int fw_dyn_soar_control_main(int argc, char *argv[]) +{ + return FixedwingPositionINDIControl::main(argc, argv); +} diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp new file mode 100644 index 0000000000..4beca7a009 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.hpp @@ -0,0 +1,434 @@ +/** + * Implementation of a generic incremental position controller + * using incremental nonlinear dynamic inversion and differential flatness + * for a fixed wing aircraft during dynamic soaring cycles. + * The controller directly outputs actuator deflections for ailerons, elevator and rudder. + * + * @author Marvin Harms + */ + +// use inclusion guards +#ifndef FIXEDWINGPOSITIONINDICONTROL_HPP_ +#define FIXEDWINGPOSITIONINDICONTROL_HPP_ + +#include + +#include +#include +#include + +#include +#include "fw_att_control/ecl_pitch_controller.h" +#include "fw_att_control/ecl_roll_controller.h" +#include "fw_att_control/ecl_wheel_controller.h" +#include "fw_att_control/ecl_yaw_controller.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + + +using namespace time_literals; + +using matrix::Dcmf; +using matrix::Eulerf; +using matrix::Quatf; +using matrix::Vector; +using matrix::Matrix3f; +using matrix::Vector3f; + + +class FixedwingPositionINDIControl final : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + FixedwingPositionINDIControl(); + ~FixedwingPositionINDIControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + orb_advert_t _mavlink_log_pub{nullptr}; + + // make the main task run, whenever a new body rate becomes available + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + // Subscriptions + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // vehicle status + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; // airspeed + uORB::Subscription _airflow_aoa_sub{ORB_ID(airflow_aoa)}; // angle of attack + uORB::Subscription _airflow_slip_sub{ORB_ID(airflow_slip)}; // angle of sideslip + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; // linear acceleration + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; // local NED position + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; // home position + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; // vehicle attitude + uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; // vehicle body accel + uORB::Subscription _soaring_controller_status_sub{ORB_ID(soaring_controller_status)}; // vehicle status flags + uORB::Subscription _soaring_estimator_shear_sub{ORB_ID(soaring_estimator_shear)}; // shear params for trajectory selection + uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)}; + + // Publishers + uORB::Publication _actuators_0_pub; + uORB::Publication _attitude_sp_pub; + uORB::Publication _angular_vel_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _angular_accel_sp_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; + uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; + uORB::Publication _soaring_controller_heartbeat_pub{ORB_ID(soaring_controller_status)}; + uORB::Publication _soaring_controller_position_setpoint_pub{ORB_ID(soaring_controller_position_setpoint)}; + uORB::Publication _soaring_controller_position_pub{ORB_ID(soaring_controller_position)}; + uORB::Publication _soaring_controller_wind_pub{ORB_ID(soaring_controller_wind)}; + uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; + uORB::Publication _debug_value_pub{ORB_ID(debug_value)}; + + // Message structs + vehicle_angular_acceleration_setpoint_s _angular_accel_sp {}; + actuator_controls_s _actuators {}; // actuator commands + manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data + rc_channels_s _rc_channels {}; ///< rc channels + vehicle_local_position_s _local_pos {}; ///< vehicle local position + vehicle_acceleration_s _acceleration {}; ///< vehicle acceleration + vehicle_attitude_s _attitude {}; ///< vehicle attitude + vehicle_attitude_setpoint_s _attitude_sp {}; ///< vehicle attitude setpoint + vehicle_angular_velocity_s _angular_vel {}; ///< vehicle angular velocity + vehicle_rates_setpoint_s _angular_vel_sp {}; ///< vehicle angular velocity setpoint + vehicle_angular_acceleration_s _angular_accel {}; ///< vehicle angular acceleration + home_position_s _home_pos {}; ///< home position + map_projection_reference_s _global_local_proj_ref{}; + vehicle_control_mode_s _control_mode {}; ///< control mode + offboard_control_mode_s _offboard_control_mode {}; ///< offboard control mode + vehicle_status_s _vehicle_status {}; ///< vehicle status + soaring_controller_status_s _soaring_controller_status {}; ///< soaring controller status + soaring_controller_heartbeat_s _soaring_controller_heartbeat{}; ///< soaring controller hrt + soaring_controller_position_setpoint_s _soaring_controller_position_setpoint{}; ///< soaring controller pos setpoint + soaring_controller_position_s _soaring_controller_position{}; ///< soaring controller pos + soaring_controller_wind_s _soaring_controller_wind{}; ///< soaring controller wind + soaring_estimator_shear_s _soaring_estimator_shear{}; ///< soaring estimator shear + debug_value_s _debug_value{}; // slip angle + + // parameter struct + DEFINE_PARAMETERS( + // aircraft params + (ParamFloat) _param_fw_inertia_roll, + (ParamFloat) _param_fw_inertia_pitch, + (ParamFloat) _param_fw_inertia_yaw, + (ParamFloat) _param_fw_inertia_rp, + (ParamFloat) _param_fw_mass, + (ParamFloat) _param_fw_wing_area, + (ParamFloat) _param_rho, + // aerodynamic params (INDI) + (ParamFloat) _param_fw_c_l0, + (ParamFloat) _param_fw_c_l1, + (ParamFloat) _param_fw_c_d0, + (ParamFloat) _param_fw_c_d1, + (ParamFloat) _param_fw_c_d2, + (ParamFloat) _param_fw_c_b1, + (ParamFloat) _param_aoa_offset, + (ParamFloat) _param_stall_speed, + // position PD control params + (ParamFloat) _param_lin_k_x, + (ParamFloat) _param_lin_k_y, + (ParamFloat) _param_lin_k_z, + (ParamFloat) _param_lin_c_x, + (ParamFloat) _param_lin_c_y, + (ParamFloat) _param_lin_c_z, + (ParamFloat) _param_lin_ff_x, + (ParamFloat) _param_lin_ff_y, + (ParamFloat) _param_lin_ff_z, + // attitude PD control params + (ParamFloat) _param_rot_k_roll, + (ParamFloat) _param_rot_k_pitch, + (ParamFloat) _param_rot_ff_yaw, + (ParamFloat) _param_rot_c_roll, + (ParamFloat) _param_rot_c_pitch, + (ParamFloat) _param_rot_p_yaw, + // low-level controller params (INDI) + (ParamFloat) _param_k_act_roll, + (ParamFloat) _param_k_act_pitch, + (ParamFloat) _param_k_damping_roll, + (ParamFloat) _param_k_damping_pitch, + // location params + (ParamFloat) _param_origin_lat, + (ParamFloat) _param_origin_lon, + (ParamFloat) _param_origin_alt, + // loiter params + (ParamInt) _param_loiter, + (ParamInt) _param_shear_heading, + (ParamFloat) _param_shear_height, + // thrust params + (ParamFloat) _param_thrust, + // force saturation + (ParamInt) _param_switch_saturation, + // hardcoded trajectory center + (ParamInt) _param_switch_origin_hardcoded, + // manual switch if manual feedthrough is used, only active in STIL mode + (ParamInt) _param_switch_manual, + // manual switch if manual feedthrough is used, REMOVE!!! + (ParamInt) _param_switch_cloop, + // manual switch if we are in SITL mode + (ParamInt) _param_switch_sitl + + ) + + + perf_counter_t _loop_perf; ///< loop performance counter + + // estimator reset counters + uint8_t _pos_reset_counter{0}; ///< captures the number of times the estimator has reset the horizontal position + uint8_t _alt_reset_counter{0}; ///< captures the number of times the estimator has reset the altitude state + + + // Update our local parameter cache. + int parameters_update(); + + // Update subscriptions + void wind_poll(); + void airspeed_poll(); + void airflow_aoa_poll(); + void airflow_slip_poll(); + + void vehicle_local_position_poll(); + void vehicle_acceleration_poll(); + void vehicle_attitude_poll(); + void vehicle_angular_velocity_poll(); + void vehicle_angular_acceleration_poll(); + void actuator_controls_poll(); + + void control_update(); + void manual_control_setpoint_poll(); + void rc_channels_poll(); + void vehicle_command_poll(); + void vehicle_control_mode_poll(); + void vehicle_status_poll(); + void soaring_controller_status_poll(); + void soaring_estimator_shear_poll(); + + // + void status_publish(); + + const static size_t _num_basis_funs = 16; // number of basis functions used for the trajectory approximation + + // controller methods + void _compute_trajectory_transform(); // compute the transform between trajectory frame and ENU frame (soaring frame) based on shear params + void _select_loiter_trajectory(); // select the correct loiter trajectory based on available energy + void _select_soaring_trajectory(); // select the correct loiter trajectory based on available energy + void _read_trajectory_coeffs_csv(char *filename); // read in the correct coefficients of the appropriate trajectory + float _get_closest_t(Vector3f pos); // get the normalized time, at which the reference path is closest to the current position + Vector3f _compute_wind_estimate(); // compute a wind estimate to be used only inside the controller + Vector3f _compute_wind_estimate_EKF(); // compute a wind estimate to be used only as a measurement for the shear estimator + void _set_wind_estimate(Vector3f wind); + void _set_wind_estimate_EKF(Vector3f wind); + Vector _get_basis_funs(float t=0); // compute the vector of basis functions at normalized time t in [0,1] + Vector _get_d_dt_basis_funs(float t=0); // compute the vector of basis function gradients at normalized time t in [0,1] + Vector _get_d2_dt2_basis_funs(float t=0); // compute the vector of basis function curvatures at normalized time t in [0,1] + void _load_basis_coefficients(); // load the coefficients of the current path approximation + Vector3f _get_position_ref(float t=0); // get the reference position on the current path, at normalized time t in [0,1] + Vector3f _get_velocity_ref(float t=0, float T=1); // get the reference velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T + Vector3f _get_acceleration_ref(float t=0, float T=1); // get the reference acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T + Quatf _get_attitude_ref(float t=0, float T=1); // get the reference attitude on the current path, at normalized time t in [0,1], with an intended cycle time of T + Vector3f _get_angular_velocity_ref(float t=0, float T=1); // get the reference angular velocity on the current path, at normalized time t in [0,1], with an intended cycle time of T + Vector3f _get_angular_acceleration_ref(float t=0, float T=1); // get the reference angular acceleration on the current path, at normalized time t in [0,1], with an intended cycle time of T + Quatf _get_attitude(Vector3f vel, Vector3f f); // get the attitude to produce force f while flying with velocity vel + Vector3f _compute_INDI_stage_1(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref, Vector3f omega_ref, Vector3f alpha_ref); + Vector3f _compute_INDI_stage_2(Vector3f ctrl); + Vector3f _compute_actuator_deflections(Vector3f ctrl); + + // helper methods + void _reverse(char* str, int len); // reverse a string of length 'len' + int _int_to_str(int x, char str[], int d); // convert an integer x into a string of length d + void _float_to_str(float n, char* res, int afterpoint); // convert float to string + + + // control variables + Vector _basis_coeffs_x = {}; // coefficients of the current path + Vector _basis_coeffs_y = {}; // coefficients of the current path + Vector _basis_coeffs_z = {}; // coefficients of the current path + Vector3f _alpha_sp; + Vector3f _wind_estimate; // wind estimate used internally in the controller + Vector3f _wind_estimate_EKF; // wind estimate only used in the wind estimator + Matrix3f _K_x; + Matrix3f _K_v; + Matrix3f _K_a; + Matrix3f _K_q; + Matrix3f _K_w; + Vector3f _pos; // current position + Vector3f _vel; // current velocity + Vector3f _acc; // current acceleration + Quatf _att; // attitude quaternion + Vector3f _omega; // angular rate vector + Vector3f _alpha; // angular acceleration vector + float _k_ail; + float _k_ele; + float _k_d_roll; + float _k_d_pitch; + hrt_abstime _last_run{0}; + + // controller frequency + const float _sample_frequency = 200.f; + // Low-Pass filters stage 1 + const float _cutoff_frequency_1 = 20.f; + math::LowPassFilter2p _lp_filter_accel[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // linear acceleration + math::LowPassFilter2p _lp_filter_force[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // force command + math::LowPassFilter2p _lp_filter_omega[3] {{_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}, {_sample_frequency, _cutoff_frequency_1}}; // body rates + // smoothing filter to reject HF noise in control output + const float _cutoff_frequency_smoothing = 20.f; // we want to attenuate noise at 30Hz with -10dB -> need cutoff frequency 5 times lower (6Hz) + math::LowPassFilter2p _lp_filter_ctrl0[3] {{_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}, {_sample_frequency, _cutoff_frequency_smoothing}}; // force command stage 1 + math::LowPassFilter2p _lp_filter_rud {_sample_frequency, 10}; // rudder command + // Low-Pass filters stage 2 + const float _cutoff_frequency_2 = 20.f; // MUST MATCH PARAM "IMU_DGYRO_CUTOFF" + math::LowPassFilter2p _lp_filter_delay[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}}; // filter to match acceleration processing delay + math::LowPassFilter2p _lp_filter_omega_2[3] {{_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}, {_sample_frequency, _cutoff_frequency_2}}; // body rates + // Low-Pass filter for wind estimate + const float _cutoff_frequency_wind = 1.f; + math::LowPassFilter2p _lp_filter_wind[3] {{_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}}; // wind_estimate inside controller + math::LowPassFilter2p _lp_filter_wind_EKF[3] {{_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}, {_sample_frequency, _cutoff_frequency_wind}}; // wind_estimate for EKF + uint _counter = 0; + hrt_abstime _last_time{0}; + + // parameter variables + Matrix3f _inertia {}; + float _mass; + float _area; + float _rho; + float _C_L0; + float _C_L1; + float _C_D0; + float _C_D1; + float _C_D2; + float _C_B1; + float _aoa_offset; + float _stall_speed; + // trajectory origin in WGS84 + float _origin_lat; + float _origin_lon; + float _origin_alt; + // trajectory origin in current NED local frame + float _origin_N; + float _origin_E; + float _origin_D; + // wind shear parameters + float _shear_v_max; + float _shear_alpha; + float _shear_energy; + float _shear_h_ref; + float _shear_heading; + float _shear_aspd; + // loiter circle + int _loiter; + // thrust + float _thrust; + float _thrust_pos; + // ================== + // controler switches + // ================== + // controller mode + bool _switch_manual; + // soaring mode + bool _switch_cl_soaring; + // force limit + bool _switch_saturation; + // use shear height from estimator + bool _switch_origin_hardcoded; + // + bool _switch_sitl; + // + bool _soaring_feasible; + + + bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists + hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts. + float _true_airspeed{0.0f}; + float _cal_airspeed{0.0f}; + + bool _aoa_valid{false}; ///< flag if a valid AoA estimate exists + hrt_abstime _aoa_last_valid{0}; ///< last time Aoa was received. Used to detect timeouts. + float _aoa{0.0f}; + + bool _slip_valid{false}; ///< flag if a valid AoA estimate exists + hrt_abstime _slip_last_valid{0}; ///< last time Aoa was received. Used to detect timeouts. + float _slip{0.0f}; + + // vectors defining the gridding for trajectory selection: initial velocities, wind speed and shear strength + const static size_t _gridsize = 11; + + + // helper variables + Dcmf _R_ned_to_enu; // rotation matrix from NED to ENU frame + Dcmf _R_enu_to_ned; // rotation matrix from ENU to NED frame + Dcmf _R_trajec_to_enu; // rotation matrix from trajectory frame to ENU frame + Dcmf _R_enu_to_trajec; // rotation matrix from ENU frame to trajectory frame + Vector3f _vec_enu_to_trajec; // 3D vector from ENU origin to trajectory frame origin (expressed in ENU) + Vector3f _zero_crossing_local_pos; // vector denoting the zero crossing of the trajectories in NED frame + Vector3f _f_command {}; + Vector3f _m_command {}; + Vector3f _w_err {}; + hrt_abstime _last_time_trajec{0}; + +}; + + + +#endif // FIXEDWINGPOSITIONINDICONTROL_HPP_ diff --git a/src/modules/fw_dyn_soar_control/Kconfig b/src/modules/fw_dyn_soar_control/Kconfig new file mode 100644 index 0000000000..29eb3a3e06 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_FW_DYN_SOAR_CONTROL + bool "fw_dyn_soar_control" + default n + ---help--- + Enable support for fw_dyn_soar_control + +menuconfig USER_FW_DYN_SOAR_CONTROL + bool "fw_dyn_soar_control running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_FW_DYN_SOAR_CONTROL + ---help--- + Put fw_dyn_soar_control in userspace memory diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c new file mode 100644 index 0000000000..4168d7ce67 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c @@ -0,0 +1,657 @@ +/** + * @file fw_dyn_soar_control_params.c + * + * Parameters defined by the INDI position controller + * + * @author Marvin Harms + */ + +/* + * Controller parameters, accessible via MAVLink + */ + +/** + * inertia around body x-axis + * + * This is the inertia of the aircraft, used for the INDI + * + * @unit kg + * @min 0.01 + * @max 10 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_INERTIA_ROLL, 0.197563f); + +/** + * inertia around body y-axis + * + * This is the inertia of the aircraft, used for the INDI + * + * @unit kg + * @min 0.01 + * @max 10 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_INERTIA_PITCH, 0.1458929f); + +/** + * inertia around body z-axis + * + * This is the inertia of the aircraft, used for the INDI + * + * @unit kg + * @min 0.01 + * @max 10 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_INERTIA_YAW, 0.1477f); + +/** + * inertia tensor term in body xz-axis (roll-yaw coupling) + * + * This is the inertia of the aircraft, used for the INDI + * + * @unit kg + * @min -0.5 + * @max 0 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_INERTIA_RP, -0.0f); + +/** + * total takeoff mass + * + * This is the mass of the aircraft, used for the INDI + * + * @unit kg + * @min 1.0 + * @max 2.0 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_MASS, 1.4f); + +/** + * total wing area used for lift and drag computation + * + * @unit m^2 + * @min 0.1 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_WING_AREA, 0.4f); + + +/** + * air density used for lift and drag computation + * + * @unit + * @min 0.5 + * @max 1.225 + * @decimal 3 + * @increment 0.001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_RHO, 1.223f); + +/** + * estimated lift coefficients used for lift and drag computation + * + * Used as L = C_l0 + C_l1*alpha, + * where alpha is the angle of attack. + * + * @unit + * @min -100 + * @max 100 + * @decimal 3 + * @increment 0.001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_C_L0, 0.356f); + +/** + * estimated lift coefficients used for lift and drag computation + * + * Used as L = C_l0 + C_l1*alpha, + * where alpha is the angle of attack. + * + * @unit + * @min -100 + * @max 100 + * @decimal 3 + * @increment 0.001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_C_L1, 2.354f); + + +/** + * estimated drag coefficients used for lift and drag computation + * + * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2, + * where alpha is the angle of attack. + * + * @unit + * @min -100 + * @max 100 + * @decimal 4 + * @increment 0.0001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_C_D0, 0.0288f); + +/** + * estimated drag coefficients used for lift and drag computation + * + * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2, + * where alpha is the angle of attack. + * + * @unit + * @min -100 + * @max 100 + * @decimal 4 + * @increment 0.0001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_C_D1, 0.3783f); + +/** + * estimated drag coefficients used for lift and drag computation + * + * Used as D = C_d0 + C_d1*alpha + C_d2*alpha**2, + * where alpha is the angle of attack. + * + * @unit + * @min -100 + * @max 100 + * @decimal 4 + * @increment 0.0001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_C_D2, 1.984f); + +/** + * estimated sideslip sensitivity coefficients used for wind estimation + * + * Used as F_y = 0.5 * DS_RHO * ASPD^2 * DS_C_B1, + * where alpha is the angle of attack. + * + * @unit + * @min -100 + * @max -0.01 + * @decimal 4 + * @increment 0.0001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_C_B1, -3.32f); + +/** + * offset angle between body frame (Pixhawk) and the wing chord + * + * Used to compute the AoA + * + * @unit rad + * @min 0 + * @max 0.1 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_AOA_OFFSET, 0.07f); + +/** + * stall speed of the aircraft + * + * @unit m/s + * @min 5 + * @max 10 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_STALL_SPEED, 9.0f); + + +// ======================================================== +// =================== CONTROL GAINS ====================== +// ======================================================== +/** + * control gain of position PD-controller (body x-direction) + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_LIN_K_X, 1.0f); + +/** + * control gain of position PD-controller (body y-direction) + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_LIN_K_Y, 1.0f); + +/** + * control gain of position PD-controller (body z-direction) + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_LIN_K_Z, 1.0f); + +/** + * normalized damping coefficient of position PD-controller (body x-direction) + * + * @unit + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_LIN_C_X, 1.0f); + +/** + * normalized damping coefficient of position PD-controller (body y-direction) + * + * @unit + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_LIN_C_Y, 1.0f); + +/** + * normalized damping coefficient of position PD-controller (body z-direction) + * +* @unit + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_LIN_C_Z, 1.0f); + +/** + * acceleration feedback gain of position PD-controller (body x-direction) + * + * @unit + * @min 0 + * @max 10 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_LIN_FF_X, 0.5f); + +/** + * acceleration feedback gain of position PD-controller (body y-direction) + * + * @unit + * @min 0 + * @max 10 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_LIN_FF_Y, 0.5f); + +/** + * acceleration feedback gain of position PD-controller (body z-direction) + * + * @unit + * @min 0 + * @max 10 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_LIN_FF_Z, 0.5f); + +/** + * control gain of attitude PD-controller (body roll-direction) + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_ROT_K_ROLL, 30.0f); + +/** + * control gain of attitude PD-controller (body pitch-direction) + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_ROT_K_PITCH, 30.0f); + +/** + * rudder turn coordination FF-gain + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_ROT_FF_YAW, 1.0f); + +/** + * normalized damping coefficient of attitude PD-controller (body roll-direction) + * + * @unit + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_ROT_C_ROLL, 1.0f); + +/** + * normalized damping coefficient of attitude PD-controller (body pitch-direction) + * + * @unit + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_ROT_C_PITCH, 1.0f); + +/** + * rudder turn coordination P-gain + * + * @unit + * @min 0 + * @max 100 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_ROT_P_YAW, 1.0f); + + +// ============================= +// low level INDI control params +// ============================= + +/** + * roll gain of K_ACT (actuator deflection gain) + * + * @unit + * @min 0 + * @max 100 + * @decimal 3 + * @increment 0.001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_K_ACT_ROLL, 0.25f); + +/** + * pitch gain of K_ACT (actuator deflection gain) + * + * @unit + * @min 0 + * @max 100 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_K_ACT_PITCH, 0.05f); + + +/** + * roll gain of K_ACT_DAMPING (actuator damping gain) + * + * @unit + * @min 0 + * @max 100 + * @decimal 3 + * @increment 0.001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_K_DAMP_ROLL, 0.04f); + +/** + * pitch gain of K_ACT_DAMPING (actuator damping gain) + * + * @unit + * @min 0 + * @max 100 + * @decimal 3 + * @increment 0.001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_K_DAMP_PITCH, 0.02f); + + +// =================================================== +// ============== trajectory center ================= +// =================================================== + +/** + * latitude of trajectory start point (WGS84) + * + * @unit + * @min -90 + * @max 90 + * @decimal 7 + * @increment 0.0000001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_ORIGIN_LAT, 47.3130000f); + +/** + * longitude of trajectory start point (WGS84) + * + * @unit + * @min -180 + * @max 180 + * @decimal 7 + * @increment 0.0000001 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_ORIGIN_LON, 8.8100000f); + +/** + * altitude of trajectory start point (WGS84) + * + * @unit + * @min 0 + * @max 2000 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_ORIGIN_ALT, 537.0f); + +// ====================================================== +// ============== loiter circle number ================= +// ====================================================== + +/** + * integer in {0,1,2,3,4,5} defining the loiter trajectory + * + * @unit + * @min 0 + * @max 7 + * @decimal 1 + * @increment 1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_INT32(DS_LOITER, 0); + +// ====================================================== +// ============ wind shear heading ====================== +// ====================================================== + +/** + * integer defining wind heading + * + * @unit deg + * @min -180 + * @max 180 + * @decimal 1 + * @increment 1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_INT32(DS_W_HEADING, 0); + +// ====================================================== +// ======= wind shear height in soaring frame =========== +// ====================================================== + +/** + * float defining wind shear vertical postition in soaring frame + * + * @unit + * @min 0 + * @max 100 + * @decimal 1 + * @increment 0.1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_W_HEIGHT, 100.f); + +// ============================================== +// ============== engine thrust ================= +// ============================================== +/** + * float in [0,1] corresponding to the engine thrust + * + * @unit + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_FLOAT(DS_THRUST, 0); + +// ====================================================== +// ============= controller force saturation ============ +// ====================================================== + +/** + * integer in {0,1} defining if the commanded force has an upper bound (saturates), 0=no saturation, 1=saturation + * + * @unit + * @min 0 + * @max 1 + * @decimal 1 + * @increment 1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_INT32(DS_SWITCH_SAT, 1); + + +// ====================================================== +// ============= hardcoded trajectory center ============ +// ====================================================== + +/** + * integer in {0,1} defining if the trajectory origin is taken from hardcoded params or shear estimate, 1=params, 0=estimate + * @unit + * @min 0 + * @max 1 + * @decimal 1 + * @increment 1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_INT32(DS_SWITCH_ORI_HC, 1); + +// ================================================= +// ============= loiter trajectory test ============ +// ================================================= + +/** + * integer in {0,1} defining if the loiter circle defined by param DS_LOITER shall be used, 0=soaring, 1=loiter + * @unit + * @min 0 + * @max 1 + * @decimal 1 + * @increment 1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_INT32(DS_SWITCH_LOITER, 1); + +// ==================================================== +// ============= manual feedthrough switch ============ +// ==================================================== + +/** + * integer in {0,1} defining if we are using manual feedthrough, only used in SITL mode + * @unit + * @min 0 + * @max 1 + * @decimal 1 + * @increment 1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_INT32(DS_SWITCH_MANUAL, 1); + +// ===================================================== +// ============= open loop / closed loop DS ============ +// ===================================================== + +/** + * integer in {0,1} defining if the shear parameters are changed by the estimator while soaring (closed loop). 0=fixed shear, 1=changing shear + * @unit + * @min 0 + * @max 1 + * @decimal 1 + * @increment 1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_INT32(DS_SWITCH_CLOOP, 0); + +// ===================================================== +// ============= open loop / closed loop DS ============ +// ===================================================== + +/** + * integer in {0,1} defining if we are running the controller in SITL. 0=hardware, 1=sitl + * @unit + * @min 0 + * @max 1 + * @decimal 1 + * @increment 1 + * @group FW DYN SOAR Control + */ +PARAM_DEFINE_INT32(DS_SWITCH_SITL, 0); \ No newline at end of file diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_100_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_100_18.csv new file mode 100644 index 0000000000..1d2c725ff3 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_100_18.csv @@ -0,0 +1,3 @@ +20.249834,-1498.036199,8991.961311,-24958.956623,41340.383016,-41631.426333,17050.379042,19263.586599,-39432.861865,25757.956616,11245.661203,-42344.771826,46970.231532,-29988.424357,10855.560938,-1677.368349 +-7.218114,5279.878939,-27707.912587,70515.279933,-109233.570552,102935.722143,-36476.002615,-51554.017800,94917.464988,-58786.945547,-27934.800286,100170.063042,-114150.638209,78214.058653,-32445.366144,6339.659956 +-3.934978,2391.325102,-14411.271810,42203.244020,-76716.253156,88945.359914,-51569.236117,-25572.454067,87611.185180,-77472.095071,-5984.618456,99770.259330,-135165.631325,101511.221087,-44631.608657,9162.447954 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_100_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_100_20.csv new file mode 100644 index 0000000000..6841d9ef2f --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_100_20.csv @@ -0,0 +1,3 @@ +30.389860,912.982507,-2859.936980,3349.275775,-415.369310,-3481.089396,3363.872340,1179.921845,-4478.256881,1837.357593,3498.540940,-4492.878780,-393.125759,5151.515013,-4941.188034,1720.463927 +-2.598681,6191.674693,-33961.873408,91771.100021,-153409.649369,160707.118624,-75518.823722,-63886.003518,154241.187907,-115027.482925,-29953.909268,169169.609712,-207234.688426,146197.933872,-61050.853957,11854.416042 +-5.175201,6337.029321,-35104.159570,93181.991417,-150770.497726,150093.399535,-61550.310528,-68258.178502,141363.130375,-95038.720689,-36247.135711,151579.287214,-176414.846548,120921.610558,-49646.077608,9643.082070 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_100_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_100_22.csv new file mode 100644 index 0000000000..6fbf2c8df2 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_100_22.csv @@ -0,0 +1,3 @@ +32.051371,1650.704144,-6598.531219,12390.119252,-13493.269501,7461.379871,1207.142418,-5970.998355,5109.589560,-2016.879398,-1051.211663,4872.887766,-9270.656434,10649.395680,-7172.952936,2187.636700 +-2.536588,5861.765277,-31409.143579,83398.251187,-137714.029102,143429.216873,-67935.630936,-55607.262617,137500.343143,-105281.108656,-23844.091823,152076.782007,-190543.222950,136636.797526,-57806.608731,11327.398362 +-5.904772,4632.654322,-26493.025191,72582.753211,-121329.073480,125497.496032,-56176.364936,-52937.280101,119529.487901,-84942.741316,-26808.719959,129359.716901,-154410.364036,107437.399311,-44588.163669,8729.336822 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_100_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_100_24.csv new file mode 100644 index 0000000000..de1c5a3f6f --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_100_24.csv @@ -0,0 +1,3 @@ +31.550461,1802.021465,-7280.950015,13535.491205,-13709.001555,4905.090872,6059.191197,-9051.239430,2193.646055,5653.911056,-5994.939291,187.142042,3940.145313,-3036.799806,467.894600,264.261964 +-1.408562,1530.681950,-7030.394025,17659.485566,-29768.713956,34377.591980,-22054.294715,-6577.428854,34071.279368,-35600.224927,3176.621270,41037.278387,-63095.706736,50947.639967,-23488.896320,4861.320938 +-6.656401,3614.186108,-20979.720257,57857.341097,-96439.348021,98059.213352,-40613.389558,-45559.747550,93161.401353,-60017.407117,-27238.757931,98854.244272,-108993.602048,70675.872603,-27268.100397,4939.680351 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_20_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_20_24.csv new file mode 100644 index 0000000000..4a05105e0d --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_20_24.csv @@ -0,0 +1,3 @@ +25.410335,12.369353,1959.341036,-8789.968366,18176.031111,-21228.034894,10779.468992,8250.834025,-20609.224850,14712.910998,5000.912523,-22268.932175,25004.319692,-15842.902093,5610.391090,-829.009529 +-3.716716,280.704489,-355.903224,-42.054028,411.441751,48.519658,-966.642939,1056.796821,306.389270,-1784.185307,1405.545188,941.197024,-3024.804943,2941.961264,-1386.139276,194.633758 +-12.998982,-54.523260,-107.431879,894.728680,-1811.407856,1537.414454,269.917037,-1799.743712,1168.710656,907.451000,-1775.945580,467.630285,1230.793798,-1536.221303,816.460944,-188.715147 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_20_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_20_26.csv new file mode 100644 index 0000000000..525005f66e --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_20_26.csv @@ -0,0 +1,3 @@ +26.364149,177.069561,1575.881647,-9015.105328,20284.748852,-24877.180293,13157.069666,9693.742113,-24630.776658,17017.249779,6929.891099,-26402.315476,27658.672872,-15960.791210,4816.324261,-481.249223 +-2.697646,1243.905991,-6235.286985,17263.905468,-30739.116331,35075.762820,-19491.224818,-11225.334388,34615.605572,-28943.673202,-4147.157668,38977.054051,-50259.872605,36240.469382,-15200.785366,2862.042425 +-14.014894,-1092.010145,5356.556949,-12997.656894,19726.458704,-18688.134723,7112.080636,8917.358306,-17613.793090,11582.620986,4857.978470,-19032.575481,21707.141219,-14570.913691,5928.978876,-1176.673154 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_20_28.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_20_28.csv new file mode 100644 index 0000000000..797abe0b62 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_20_28.csv @@ -0,0 +1,3 @@ +27.488500,656.714564,-711.172967,-3607.344213,12064.577564,-16596.522652,8958.423854,7053.652016,-16703.379958,9887.716036,6672.954466,-17214.521473,14463.030066,-5503.534726,-56.863972,586.594485 +-1.853833,2825.199512,-14819.713309,39749.522052,-66654.957686,70216.103407,-33192.369894,-27929.172351,67590.964130,-50210.497342,-13479.819481,74116.096587,-90020.883283,62825.979648,-25821.438511,4852.075439 +-15.520495,202.519559,-1992.254633,6620.692936,-11609.000166,11322.598007,-3304.062495,-6929.925323,10421.235612,-4187.407718,-5464.971653,10171.718394,-7863.904914,3026.319376,-146.513897,-237.651350 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_20_30.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_20_30.csv new file mode 100644 index 0000000000..4ccf8a90af --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_20_30.csv @@ -0,0 +1,3 @@ +28.743402,2078.362604,-7793.565123,13384.615828,-12830.619300,5492.366699,2331.149832,-5092.810031,3520.435807,-1488.401128,-400.152676,3956.617004,-8314.481482,9383.643006,-5997.567239,1730.742485 +-1.213005,2555.978690,-13236.079583,35779.247030,-61178.493015,66475.594832,-33888.467431,-23956.790239,64629.202523,-51159.544433,-10075.409300,71917.010022,-90613.598094,64688.363705,-27025.918149,5135.121115 +-17.411436,702.346154,-4990.389270,14886.524601,-24997.074775,24043.706420,-7220.267390,-14427.214360,22387.881697,-9673.656090,-11279.697705,22289.916422,-17857.848174,7289.262350,-761.324352,-358.570553 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_30_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_30_22.csv new file mode 100644 index 0000000000..c4b9d68c53 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_30_22.csv @@ -0,0 +1,3 @@ +25.807032,-2925.872531,17626.900070,-48527.605690,78596.032935,-75357.034383,25236.004181,41036.456675,-70062.204933,37296.741902,27673.478531,-72007.179628,70351.596401,-40377.817337,13192.714073,-1823.721759 +-4.269878,3512.386346,-18619.333231,49177.358335,-80294.515135,81470.119172,-35154.890428,-35584.523231,77472.889766,-53740.991735,-18680.370211,83663.230433,-98251.161352,67389.126785,-27495.736176,5196.375807 +-8.223582,1274.911396,-6466.041128,15689.522036,-23322.244799,21493.532705,-8013.896644,-9664.522910,19462.409749,-14105.767315,-3182.501226,21122.648162,-28453.328443,22442.344490,-10549.264290,2316.605428 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_30_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_30_24.csv new file mode 100644 index 0000000000..29ab2f3a5b --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_30_24.csv @@ -0,0 +1,3 @@ +28.265176,-979.386702,7419.686600,-22583.464563,37939.387710,-35793.651208,9323.655186,23112.091262,-32987.349099,11844.627678,18895.507035,-32027.986782,22354.058112,-6353.378068,-1374.824508,1159.236562 +-2.753809,5740.750259,-30974.191410,82232.589509,-134269.093565,135707.092980,-57790.468823,-59997.024412,128740.514055,-88400.008749,-31810.351026,138751.390069,-162076.229407,110706.908019,-44985.254781,8497.949168 +-10.288734,788.619135,-4015.272551,9464.238302,-13149.796146,10620.242441,-2203.465112,-6358.643341,9115.441480,-4803.765988,-3024.660611,9465.572049,-11175.374802,8141.654632,-3502.987926,673.165984 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_30_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_30_26.csv new file mode 100644 index 0000000000..e6c4beddc0 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_30_26.csv @@ -0,0 +1,3 @@ +30.325031,998.588079,-2506.097464,1112.679847,4050.362173,-7575.827035,3610.703835,4807.328728,-7912.810595,1423.062563,6908.855151,-7047.123610,-733.895950,6947.392420,-6118.106380,1990.418874 +-1.862138,5260.310326,-28362.138836,75966.525157,-125941.228823,130403.059750,-59433.171446,-53682.762909,124674.457655,-90703.860259,-26247.739993,136080.693972,-164317.109412,114677.998992,-47345.783502,9043.380773 +-12.328782,1149.268543,-6165.422014,15011.048121,-20807.425742,15269.068476,396.019698,-13600.991138,12819.784834,-209.559789,-10963.031285,11285.062493,-3827.872287,-2235.123927,2898.333874,-992.707506 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_30_28.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_30_28.csv new file mode 100644 index 0000000000..73ce2dee0a --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_30_28.csv @@ -0,0 +1,3 @@ +31.527635,1914.680208,-7066.786700,12001.498543,-11639.251141,5661.716637,833.889449,-3872.102019,4050.195339,-3333.621620,780.586620,4854.753625,-10852.776692,11992.859997,-7521.880502,2151.180312 +-1.186170,3479.031721,-18698.030760,51454.523473,-89011.366270,97857.359510,-51243.303570,-33721.655799,95275.494046,-77464.993449,-12838.707630,106615.132589,-136961.458686,99212.786606,-42070.122449,8177.263621 +-14.202558,-169.760541,386.222977,-846.701641,3119.384246,-7575.860335,9931.127792,-4154.570319,-8387.043291,16071.808162,-7926.249406,-12194.138559,27588.778783,-26471.170926,14053.983232,-3417.014125 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_40_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_40_20.csv new file mode 100644 index 0000000000..d41d7a6605 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_40_20.csv @@ -0,0 +1,3 @@ +23.841122,-2194.779263,12904.935717,-34666.854475,54673.688646,-50635.353984,15102.364305,29120.803253,-46437.225110,22974.724498,19662.679126,-47233.232100,44960.041833,-25267.742184,7979.427992,-1008.602929 +-5.541790,2437.156126,-12346.835276,31462.020082,-49779.670442,48984.591505,-19933.267591,-22260.621101,46139.632315,-31280.871902,-11476.000089,49587.080568,-58457.462709,40644.366832,-16922.470009,3256.705963 +-6.078312,4452.065484,-24199.711547,63335.871292,-101626.656720,101030.434432,-42179.328607,-44663.085381,95065.016826,-66090.072687,-21936.472305,102617.427709,-123300.181672,86973.857880,-36758.005156,7353.148051 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_40_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_40_22.csv new file mode 100644 index 0000000000..14422b73d3 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_40_22.csv @@ -0,0 +1,3 @@ +28.618106,-288.792161,3141.199165,-10182.309050,16315.417146,-12565.923060,-1804.904848,13936.301857,-10686.677337,-4628.945671,14412.868226,-7786.295636,-7077.609479,14394.202010,-10151.845969,2932.493559 +-3.146467,6224.002286,-33674.042838,89395.509712,-145919.391650,147545.429134,-63071.762176,-64892.129879,139981.750282,-96642.262001,-34043.616251,151037.881495,-177225.657573,121541.053753,-49595.003797,9419.304769 +-8.139910,1042.248379,-5428.501831,13729.525415,-21836.979841,22646.227162,-11832.542811,-6780.352416,21416.920927,-19818.471600,130.693847,24841.636573,-37042.455599,29967.380524,-13973.035278,2985.845469 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_40_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_40_24.csv new file mode 100644 index 0000000000..a5400a631f --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_40_24.csv @@ -0,0 +1,3 @@ +30.810604,1307.872770,-4685.931426,8016.638132,-8868.736924,7291.247921,-4460.229391,-212.227482,6632.103103,-10276.374662,4770.665287,9048.021536,-21033.757954,21529.664708,-12359.641022,3262.133326 +-2.003713,5926.719259,-32038.394880,85604.852385,-141394.314399,145789.327084,-65936.879985,-60406.008704,139213.824099,-100916.225923,-29562.362503,151847.930645,-183232.805959,127952.503579,-52916.094043,10148.154329 +-9.920525,1375.816163,-7255.885038,17902.439980,-26219.206645,22434.826259,-5206.533249,-13942.467627,20123.256080,-9092.569591,-9086.668740,20409.257860,-19272.018210,10906.073918,-3480.786190,437.460015 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_40_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_40_26.csv new file mode 100644 index 0000000000..f44107b9fe --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_40_26.csv @@ -0,0 +1,3 @@ +31.975666,1612.611465,-5710.058715,8983.152580,-7361.156073,1755.791235,2351.559612,-2104.864685,317.859148,-653.395208,1629.681386,644.939260,-5839.181279,8640.160027,-6266.314301,1950.947453 +-1.550916,5009.240516,-26964.424008,72614.247722,-121816.149633,128860.007481,-62472.103628,-49000.676378,124022.725423,-95624.567987,-21136.606762,137214.809484,-171792.694479,122820.585593,-51702.096671,10044.368612 +-11.512307,26.148037,-286.079982,260.064174,1800.601902,-6094.047095,8527.457143,-3801.084297,-6814.632040,13419.917602,-6774.440351,-9934.632608,22788.215433,-21918.884601,11642.657247,-2831.370660 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_50_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_50_20.csv new file mode 100644 index 0000000000..25e393e864 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_50_20.csv @@ -0,0 +1,3 @@ +26.197525,-437.694497,3176.392508,-8582.550287,11493.328808,-5761.250067,-5973.267536,11639.945217,-3699.985563,-9689.670713,12511.953326,-272.462659,-14745.197405,18589.305875,-11322.773440,3035.082897 +-4.312181,5382.750782,-28956.350536,76477.012326,-124302.299392,125306.448999,-53475.187432,-54956.732272,118708.057825,-82386.303956,-28234.161379,128172.934300,-151638.507587,104956.992453,-43302.356871,8327.542935 +-6.237767,3407.862003,-18668.402438,49496.114618,-81134.142058,83758.382323,-39258.050462,-32407.212901,79742.732300,-61609.230412,-12768.578095,88282.327808,-112834.619373,82498.944583,-35641.096688,7206.281828 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_50_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_50_22.csv new file mode 100644 index 0000000000..8e82cb990e --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_50_22.csv @@ -0,0 +1,3 @@ +30.257964,583.830210,-1208.136762,-2.526959,2307.048704,-1826.152951,-2722.543133,5796.497598,-1519.328038,-6933.762924,8772.887269,847.683336,-13639.685047,17503.530819,-11083.224620,3083.566374 +-2.528112,6221.602720,-33807.095242,90470.864195,-149454.640693,153985.111287,-69411.581584,-64092.936762,147001.161372,-106122.201244,-31627.909356,160149.763590,-192729.186294,134384.450096,-55553.298745,10670.421890 +-7.861770,2108.565894,-11258.246108,28746.526750,-44780.195243,42947.213456,-16524.082758,-20072.479055,39946.152469,-26746.123591,-9972.380017,42961.319178,-51085.084241,35820.355063,-14965.417848,2921.278175 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_50_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_50_24.csv new file mode 100644 index 0000000000..adf8a1ce76 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_50_24.csv @@ -0,0 +1,3 @@ +31.744656,1534.523358,-5660.106640,9645.556878,-9481.323315,4899.638607,124.608471,-2681.289259,3453.268987,-3692.985628,1667.287674,4202.151013,-10916.439306,12600.541302,-8088.121086,2347.044083 +-1.790821,5649.091225,-30477.524411,81634.880739,-135764.873445,141910.195977,-66938.534500,-55774.428590,136111.400298,-102746.145147,-25136.060368,149881.851485,-185513.231742,131780.189555,-55262.445023,10728.169679 +-9.274054,1126.681500,-6140.164082,15578.187652,-23326.557093,20201.799894,-4413.227212,-13265.477828,18310.881555,-6984.991725,-9777.115374,18115.716168,-14520.542549,6295.746276,-1029.613190,-144.927788 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_50_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_50_26.csv new file mode 100644 index 0000000000..d4cf2e90d3 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_50_26.csv @@ -0,0 +1,3 @@ +32.313506,1584.092324,-5605.223993,8686.890411,-6523.413622,-2.202002,4551.203533,-2989.327460,-1631.950770,3005.277452,-68.230017,-2288.821021,379.995442,3006.308255,-3509.060531,1350.213936 +-1.506337,5345.528013,-28375.292418,75104.162089,-123727.629750,128389.113566,-60241.407731,-50241.425284,122781.994923,-93542.027541,-21546.018222,135630.097151,-169909.998876,121905.256717,-51560.297189,10067.545975 +-10.460958,-1316.085987,6799.771231,-17445.271239,28278.564054,-29478.079693,14794.815126,9955.022320,-27932.830735,23885.694804,1863.570759,-31465.729600,44324.464288,-35103.544734,16533.867367,-3693.047661 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_60_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_60_20.csv new file mode 100644 index 0000000000..a59f92083d --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_60_20.csv @@ -0,0 +1,3 @@ +27.951832,638.328802,-2351.931860,4975.986695,-8596.067151,12018.262162,-11023.162232,1571.306001,12384.143733,-18278.506780,6593.108603,16287.247678,-32348.383393,30172.504130,-16062.609657,3987.108885 +-3.365734,6018.273497,-32517.973622,86211.183101,-140724.620243,142658.110939,-61742.681870,-61802.247860,135455.457804,-94888.078792,-31579.184826,146601.962661,-173914.026220,120363.800930,-49560.596992,9502.418884 +-6.104348,3201.275314,-17754.562836,47695.895661,-79329.702458,83416.393064,-40758.508779,-30635.945148,79769.551898,-63605.472743,-10946.521484,88831.098618,-115702.271403,85680.661552,-37444.718056,7658.547740 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_60_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_60_22.csv new file mode 100644 index 0000000000..5019261c8c --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_60_22.csv @@ -0,0 +1,3 @@ +31.001824,930.719531,-2793.108593,3141.476068,-661.183171,-1864.663995,764.308655,2442.430751,-2442.139019,-2166.699457,5355.493755,-1429.245593,-6839.577772,10965.631673,-7804.379965,2355.559021 +-2.219340,5936.165707,-32321.742502,86962.840562,-144877.866358,151271.084273,-70695.329596,-60394.820305,145028.774887,-107983.603836,-28250.329929,159080.962287,-194920.224629,137529.914036,-57399.015738,11118.445993 +-7.431729,2898.169478,-15788.839688,41074.656624,-64932.563599,62736.861890,-23724.908095,-30398.636526,58608.021645,-37210.526407,-16967.686410,62273.922682,-70282.054004,47062.879008,-18838.177823,3539.121855 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_60_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_60_24.csv new file mode 100644 index 0000000000..4a59a27057 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_60_24.csv @@ -0,0 +1,3 @@ +32.040952,1616.150391,-6006.612591,10103.834150,-9091.665921,2665.776624,3395.883844,-4060.733768,762.016598,1191.746895,-390.783941,298.603416,-3136.929357,5769.256275,-4802.103048,1634.597926 +-1.731759,5375.646507,-28720.205073,76389.339010,-126524.838131,132252.829871,-63093.298986,-50836.460308,126848.228539,-97650.463985,-21480.671839,140429.500638,-176591.235969,126919.183230,-53763.216414,10527.978609 +-8.566198,1112.969579,-6329.117843,16823.318957,-26605.949524,24911.270891,-7640.792073,-14419.877024,23218.405056,-11214.921311,-10412.068216,23593.670297,-21189.234632,10816.082774,-2832.909204,195.963399 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_70_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_70_20.csv new file mode 100644 index 0000000000..aaf60d643c --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_70_20.csv @@ -0,0 +1,3 @@ +29.031243,1024.647321,-4033.114979,8135.361831,-11329.985559,11613.567786,-7429.836583,-1456.944928,11074.724697,-13576.678971,3659.856096,13706.996434,-25848.280416,24274.757660,-13239.575621,3387.966126 +-2.869995,6307.197708,-34222.898443,91148.821264,-149598.974883,152769.155060,-67355.864507,-65020.079706,145410.184862,-103247.130664,-32732.918444,157819.533683,-188440.442327,130895.541062,-54031.350214,10383.858405 +-5.874280,3482.595948,-19408.919657,52253.155752,-86779.602070,90621.011001,-43171.320274,-34612.088869,86472.749651,-67050.484129,-13692.676843,95597.036137,-122197.691754,89465.504669,-38797.665513,7896.161794 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_70_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_70_22.csv new file mode 100644 index 0000000000..c381e3b9e3 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_70_22.csv @@ -0,0 +1,3 @@ +31.364619,1212.655810,-4140.134031,5989.654081,-3741.079063,-1040.246094,3086.315995,-540.557386,-2345.872467,1007.627429,2465.798756,-2341.000707,-2489.858931,6474.019714,-5471.225438,1825.025631 +-2.085064,5342.386691,-29058.805335,78508.767204,-131865.271205,139602.464184,-67736.448951,-53113.804084,134433.995467,-103508.997051,-23081.781604,148606.763393,-185796.096666,132816.415605,-56006.731815,10940.088391 +-6.979470,3531.315531,-19481.141875,51319.072579,-82101.614578,80259.965487,-30994.103031,-38573.364768,75284.207028,-47868.363905,-21961.493090,79934.241557,-89537.598855,59416.260125,-23569.998180,4397.467051 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_70_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_70_24.csv new file mode 100644 index 0000000000..eef97afeee --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_70_24.csv @@ -0,0 +1,3 @@ +32.118330,1644.395389,-6254.807567,10873.889952,-10278.101757,3446.898034,3808.518260,-5268.521377,1341.247037,2017.006948,-1677.936611,477.703238,-1874.368294,4159.726362,-3880.770230,1411.729386 +-1.799531,5298.851513,-28031.782953,73822.225340,-121146.249448,125568.067255,-59252.700865,-48553.198324,120116.072837,-92517.590338,-20070.157422,133100.603297,-168171.510185,121441.114109,-51685.750957,10163.900045 +-7.948920,1165.252117,-6907.991430,19221.711094,-32063.063965,32330.929643,-12724.446375,-15983.772200,30791.208957,-18345.762287,-10642.076057,32259.043449,-33020.319993,19673.032543,-6721.993394,998.677553 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_80_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_80_18.csv new file mode 100644 index 0000000000..1ce1c79e20 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_80_18.csv @@ -0,0 +1,3 @@ +18.957378,-2142.429781,13031.460439,-37132.714349,63917.152425,-68221.643247,32874.254900,26378.039108,-65510.622479,49567.820526,12099.520876,-72095.453763,88791.645495,-62475.499863,25722.781741,-4881.104252 +-7.271660,1230.781638,-5139.998814,10339.745576,-11638.325252,5789.985878,3089.137797,-6940.535159,3366.473968,2062.320239,-3598.192612,2118.637907,-1494.153688,2090.527628,-1728.767106,498.555412 +-4.250695,1145.198970,-6696.814130,19147.050318,-34175.335883,38988.235400,-21961.874824,-11949.438784,38371.594238,-32947.115639,-3594.386974,43405.612014,-57564.387475,42600.989204,-18463.475468,3729.467912 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_80_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_80_20.csv new file mode 100644 index 0000000000..a6e662349d --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_80_20.csv @@ -0,0 +1,3 @@ +29.634646,874.371880,-2924.947771,4392.655364,-3802.310823,2203.644208,-1516.694047,945.856883,1621.237997,-5037.255479,4257.793478,2910.650092,-11412.712613,13644.807785,-8673.929571,2473.914105 +-2.624312,6195.977834,-33737.320255,90322.525995,-149256.129733,153900.699485,-69566.349436,-63842.519699,146951.923111,-106371.067267,-31316.979263,160138.916176,-193162.809545,135024.310391,-56004.325081,10808.417754 +-5.626598,4419.272666,-24466.289492,65143.740175,-106310.816029,107816.701255,-47221.328386,-45627.352725,102055.064601,-73304.532897,-21632.295075,110946.856485,-135117.034158,95825.423792,-40531.065915,8083.223359 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_80_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_80_22.csv new file mode 100644 index 0000000000..9e0781bc4f --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_80_22.csv @@ -0,0 +1,3 @@ +31.596820,1491.996250,-5598.607060,9502.469992,-8591.297693,2494.458195,3211.437315,-3694.993149,561.108482,987.222006,25.640706,150.838015,-3462.676894,6277.742491,-5122.538723,1717.779398 +-2.124735,4997.617817,-27063.772146,73055.518237,-122967.334431,130980.693732,-64818.203195,-48381.322124,126393.156404,-99332.249036,-19814.871788,140442.571604,-177948.522841,128324.002277,-54486.257170,10700.553053 +-6.569346,4356.732743,-24252.920685,64500.521465,-104215.385706,103048.585056,-40905.184137,-48600.836606,96987.980148,-62644.487211,-27517.529599,103210.932228,-116304.060894,77446.890145,-30830.849244,5782.926212 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_80_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_80_24.csv new file mode 100644 index 0000000000..9a6e49eb2c --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_80_24.csv @@ -0,0 +1,3 @@ +32.237271,1576.979840,-6011.105218,10537.182412,-10168.827661,3713.062074,3489.757595,-5306.308416,1661.733853,1842.791653,-1826.392065,715.856460,-1979.616956,4169.964558,-3893.728187,1424.641377 +-1.875380,5974.190884,-31519.799452,82324.293177,-133499.684063,136087.475561,-61770.108925,-54872.448189,129448.464695,-97116.471394,-23796.214851,142650.336726,-178069.672198,127804.236988,-54197.153437,10639.750686 +-7.428003,730.853985,-4892.614938,15085.233982,-27716.928753,31175.421498,-15762.115417,-12146.514696,30612.353263,-22182.982912,-7113.943754,33191.149366,-38025.284272,24959.950814,-9545.961325,1664.630000 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_90_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_90_18.csv new file mode 100644 index 0000000000..f358bfdac8 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_90_18.csv @@ -0,0 +1,3 @@ +19.582381,-1944.597644,11704.912267,-32905.095208,55666.529915,-57953.486980,26135.611847,24346.629133,-55387.829992,39369.638042,12757.567607,-60308.681675,70879.985327,-47846.952730,18738.520936,-3318.620647 +-7.210948,3484.292663,-17786.198177,44325.090196,-67302.646391,62005.807840,-20790.055633,-31771.140653,56582.739056,-34555.398716,-16626.018559,59510.043449,-68754.265121,48175.015263,-20542.262738,4113.275553 +-4.090756,1078.592263,-6853.831501,21247.065828,-41011.193251,50798.298513,-32791.791165,-11375.156667,50962.408894,-48860.037271,-187.417001,59257.424304,-83720.105154,64126.940269,-28504.128333,5880.916978 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_90_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_90_20.csv new file mode 100644 index 0000000000..f5b644d2c2 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_90_20.csv @@ -0,0 +1,3 @@ +30.046116,796.992567,-2314.903793,2220.454766,773.119258,-3785.342421,2524.157949,2194.054117,-4465.822804,805.804648,4331.776158,-4152.042288,-1615.416982,6274.791566,-5455.785712,1820.631724 +-2.498103,5950.357153,-32513.296291,87545.026280,-145769.831516,151925.342550,-70537.031442,-61208.952051,145567.295506,-107588.765697,-29085.258272,159336.522346,-194347.581963,136788.859812,-57032.763200,11055.250318 +-5.387076,5458.089371,-30190.809038,80107.506752,-129773.642153,129715.074618,-54095.133492,-57923.523629,122305.139097,-83741.048054,-29870.725816,131639.878808,-155230.532238,107515.679598,-44564.229048,8732.065402 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_90_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_90_22.csv new file mode 100644 index 0000000000..cbe5272bc4 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_90_22.csv @@ -0,0 +1,3 @@ +31.823995,1724.762800,-6899.334458,12917.520571,-13923.760460,7411.117717,1717.695803,-6435.202319,5004.251814,-1360.782522,-1618.005310,4654.184094,-8218.944316,9425.938085,-6437.751007,1991.297469 +-2.307275,5251.485843,-28273.224797,75757.169185,-126545.892420,133748.478898,-65368.644097,-50000.938124,128784.096000,-100724.491328,-20520.930509,143042.022303,-181105.569681,130623.859030,-55484.617786,10901.523977 +-6.218630,4764.538869,-26802.648562,72111.374802,-118055.054424,118769.901432,-49421.422435,-53823.559668,112312.081428,-75210.054182,-29449.411514,120254.236410,-138378.704667,93647.618983,-37870.025482,7223.902442 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_90_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_90_24.csv new file mode 100644 index 0000000000..7db7c82b5c --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_10_90_24.csv @@ -0,0 +1,3 @@ +32.373059,1112.776820,-3583.384825,4502.467941,-1120.374610,-4442.300108,6007.899471,-880.018083,-5808.227076,6086.192488,778.614495,-7145.339477,6410.937974,-1170.787572,-1878.360323,1072.512212 +-1.881201,6854.955461,-36237.630023,94329.376769,-151873.475174,152922.239142,-67075.012540,-63981.943668,144823.592371,-105758.002193,-29158.346943,158634.391627,-195203.481989,138944.386296,-58597.097271,11470.536911 +-6.977581,-14.400007,-1253.041679,6981.692594,-17717.429546,25588.417962,-18498.644453,-4716.944510,26568.663948,-25459.917227,-680.811151,30582.088256,-41320.032399,30354.625255,-12902.245552,2525.805041 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_100_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_100_18.csv new file mode 100644 index 0000000000..c2b418a9c1 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_100_18.csv @@ -0,0 +1,3 @@ +22.520164,-1811.124454,11047.564051,-31078.856822,52208.044994,-53692.790893,23588.424060,22928.235432,-50924.457178,35937.061955,11728.633205,-55383.831413,65491.259210,-44534.360334,17507.142754,-3082.442677 +-5.468960,5753.281237,-31125.716910,81971.394700,-132003.413167,130785.412020,-53117.719638,-59595.284508,122593.437229,-82570.475078,-30663.882956,131196.284423,-154737.749547,107965.356424,-45293.980900,8934.326936 +-4.071809,3941.881452,-22391.523294,61684.168694,-105361.499053,114244.520883,-59176.523188,-39060.518027,110325.468350,-91010.165273,-12745.982558,123628.665961,-163110.350583,121569.230670,-53487.801764,11045.494527 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_100_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_100_20.csv new file mode 100644 index 0000000000..7b53851398 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_100_20.csv @@ -0,0 +1,3 @@ +31.894480,704.302861,-1650.323389,181.114795,4284.804143,-7143.414631,3259.352761,4726.296999,-7740.742400,1408.397582,6993.837431,-7270.085292,-896.282956,7811.733728,-7088.483823,2376.967479 +-2.458160,9352.278411,-51702.738405,139446.245712,-231286.255467,238644.109332,-107277.616701,-100023.725978,227883.955953,-163177.929551,-50456.574214,247666.483368,-295887.225995,205188.714228,-84495.899747,16242.390607 +-5.236569,7036.413796,-38919.471734,103221.194390,-167046.694647,166697.036561,-69293.688071,-74489.295537,156941.485080,-107533.126718,-38064.492526,168904.531569,-199833.982898,138945.303175,-57856.113430,11396.448043 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_100_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_100_22.csv new file mode 100644 index 0000000000..85dce8a707 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_100_22.csv @@ -0,0 +1,3 @@ +33.277677,1283.544707,-4069.219315,4425.981255,1415.709806,-9728.663876,10528.086946,-74.610764,-11806.972395,11414.549293,1723.083448,-13901.436372,13818.681313,-5341.750559,-738.295349,998.677180 +-1.700823,5867.892823,-31934.775528,86390.818547,-145711.534661,155720.225688,-77801.497702,-56690.889130,150433.501225,-119342.504238,-22582.878831,167574.003155,-213491.597002,154459.315006,-65753.632202,12957.253670 +-5.873377,5452.957877,-30867.873073,83638.682498,-138072.817151,140559.954500,-60604.777739,-61302.633030,133063.881726,-92324.425168,-31529.495548,143238.741258,-169622.512264,117828.596087,-49010.431298,9646.259405 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_20_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_20_26.csv new file mode 100644 index 0000000000..7642ef8115 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_20_26.csv @@ -0,0 +1,3 @@ +26.858760,-434.443303,4800.482829,-16959.521527,31758.356920,-34173.881003,14342.720848,16577.699462,-32769.993134,19112.137370,12080.635018,-34081.032376,32924.202451,-17755.570415,4895.961784,-375.664523 +-2.723993,2720.803509,-14296.331585,38236.327851,-63749.085291,66566.734471,-30770.807737,-27175.745144,63902.250648,-46589.300871,-13529.730307,69777.769952,-83845.351861,58124.379941,-23777.224729,4453.363811 +-14.455692,-1335.706870,6854.576649,-17287.531514,27050.526239,-26255.638215,10310.908590,12393.231476,-24809.415810,16228.159909,6953.984690,-26633.765119,30086.914470,-20043.637608,8091.953105,-1589.091494 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_20_28.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_20_28.csv new file mode 100644 index 0000000000..79ffd1c03a --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_20_28.csv @@ -0,0 +1,3 @@ +28.181865,249.836154,1295.756263,-8080.546419,17512.238816,-19428.174699,7076.301707,11316.736924,-18608.042301,7489.148773,10436.823339,-18146.253050,11751.767902,-1818.194899,-2226.777613,1130.603497 +-1.757510,4609.852730,-24556.201479,65016.922841,-106181.127795,107430.746282,-45827.369923,-47477.674747,101996.839278,-70026.782106,-25293.364764,109987.170146,-128212.085127,87259.498380,-35235.726260,6568.643636 +-15.964545,7.467719,-654.911308,2319.778755,-3269.809312,1251.909774,2654.393606,-3941.055571,348.849030,4351.766758,-4282.560934,-1133.211771,6581.334724,-7341.104609,4253.444452,-1121.194589 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_20_30.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_20_30.csv new file mode 100644 index 0000000000..7006dfabf4 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_20_30.csv @@ -0,0 +1,3 @@ +29.567700,1865.036949,-6787.807747,11431.698529,-11303.509841,6440.922536,-1015.354910,-2624.677863,5177.554165,-6210.696674,2369.741660,6849.449777,-15366.855759,15968.108629,-9323.300777,2492.576312 +-1.040541,2946.947725,-15690.292889,43096.409876,-74346.187369,81051.872174,-41142.507220,-29625.500516,78804.919856,-61572.158177,-13109.806317,87304.023396,-108808.798515,77089.165399,-32021.603931,6072.662537 +-17.836569,92.385342,-1497.414704,5317.610570,-9009.441875,7498.041622,215.657320,-7463.494290,6581.402206,1566.062062,-7688.179813,5165.651799,2458.686529,-6772.856395,5074.165896,-1512.041784 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_30_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_30_22.csv new file mode 100644 index 0000000000..d93aced1f6 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_30_22.csv @@ -0,0 +1,3 @@ +26.390744,-2362.866410,14234.543664,-38687.131652,61077.022666,-55759.262457,14786.609539,34419.140449,-50985.699248,21712.517766,25209.973678,-50766.486213,42884.773678,-20209.575730,4386.943338,-0.828732 +-4.231894,5712.113545,-30636.197509,80470.717531,-129594.961837,128556.556034,-52101.411012,-59311.883104,121227.045053,-80286.771794,-32468.432413,129714.867945,-148848.582934,100620.379206,-40628.115666,7652.001334 +-8.647257,714.757880,-3457.831489,8134.454947,-12216.503616,12371.761072,-6866.992177,-2790.191271,11483.535570,-12340.115496,2096.481887,13848.351726,-23823.925591,20960.897693,-10417.794624,2346.506002 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_30_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_30_24.csv new file mode 100644 index 0000000000..70ced2876d --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_30_24.csv @@ -0,0 +1,3 @@ +29.451271,440.117958,-605.785372,-721.528759,1440.985626,2072.068807,-7881.908289,7431.707784,3153.742344,-14253.089213,11164.058079,7214.589481,-24975.123803,26900.498108,-15383.301878,3968.691215 +-2.594677,7050.985034,-38088.366052,100689.363504,-163251.716510,163266.060630,-67554.612778,-74047.589816,154324.894914,-103715.829237,-40074.214507,165640.126419,-191344.614132,129722.106853,-52373.497680,9839.300441 +-10.742429,340.165488,-1545.261204,3060.484230,-3299.405059,1804.716913,8.990599,-847.717386,1149.990634,-1468.698829,823.824469,1683.605434,-4254.491883,4296.970437,-2145.000126,424.594389 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_30_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_30_26.csv new file mode 100644 index 0000000000..b6e9b46624 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_30_26.csv @@ -0,0 +1,3 @@ +31.479088,2051.557654,-8479.521989,17532.892421,-23764.966977,21923.800376,-10551.792544,-6681.911514,20478.754265,-19967.810634,1575.400218,24089.672876,-39051.858523,34154.596223,-17653.429778,4313.663748 +-1.651955,6026.531620,-32767.707429,88146.352660,-146462.352614,151713.032123,-68896.507464,-62881.180071,145041.747197,-104789.629224,-31286.047692,158017.004257,-189711.023347,131788.962656,-54168.392711,10306.635333 +-12.734173,747.836787,-3848.402992,8611.306044,-10010.129494,3922.993369,5755.588995,-9170.359494,2014.724744,7972.803966,-9087.394361,-515.906521,11087.495932,-13176.704725,7722.420024,-2006.935217 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_30_28.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_30_28.csv new file mode 100644 index 0000000000..3b8fdad419 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_30_28.csv @@ -0,0 +1,3 @@ +32.916094,2791.116480,-11752.909409,24107.370470,-30822.701332,24422.031448,-6642.289612,-12526.174399,21626.289005,-15099.785474,-3683.690787,23659.743799,-32988.901798,27483.272426,-14111.380146,3501.396152 +-1.081277,3522.495769,-19187.418598,53645.309597,-94403.900918,105831.799562,-57450.259971,-34623.200253,103666.705235,-86355.969715,-12287.599090,116671.120484,-151440.757039,110156.397652,-46770.132874,9088.613687 +-14.566224,-639.353764,3023.123394,-7946.245456,14826.918093,-19643.773641,15537.709509,514.860327,-19770.855492,24891.253804,-6234.641470,-24717.868049,43947.940118,-38784.992729,19593.815162,-4597.859528 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_40_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_40_20.csv new file mode 100644 index 0000000000..fd6eb0ca77 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_40_20.csv @@ -0,0 +1,3 @@ +24.773492,-1264.695209,7587.757276,-20034.101951,29995.252976,-24706.054955,2918.028229,18862.026292,-21707.111780,4470.389864,15110.633467,-20290.412453,11322.736811,-819.902773,-2731.995112,1237.800469 +-5.511904,3964.801529,-20902.527320,54475.512923,-87619.773205,87558.000731,-36919.055238,-38544.107336,82668.385977,-57504.959973,-19256.454577,89281.660183,-106684.222452,74727.202371,-31253.202576,6081.055472 +-6.614628,3958.344894,-21527.145950,56593.659538,-91757.257167,93172.776018,-41760.367097,-38053.208328,88314.550294,-65664.290179,-16524.013637,96960.935475,-121071.558038,87221.488445,-37256.901272,7465.784879 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_40_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_40_22.csv new file mode 100644 index 0000000000..11bc377c1d --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_40_22.csv @@ -0,0 +1,3 @@ +29.841135,1371.745857,-6086.855568,14451.924219,-23833.338865,27773.304027,-18719.089132,-4093.774446,27412.976321,-30575.232929,4934.440400,33092.274281,-54989.366628,47512.349031,-23958.467531,5684.456484 +-2.964036,7728.604291,-41956.775350,111204.823180,-180751.979500,181426.763445,-75898.975317,-81439.186860,171687.202569,-116460.400494,-43606.062437,184608.466878,-214435.005390,145975.381082,-59160.171042,11167.307176 +-8.543289,1749.370137,-9246.812592,23557.178927,-37104.515347,36883.379243,-16432.775921,-14610.721101,34675.463419,-26942.821029,-5082.185147,38615.590326,-50571.498858,37716.961721,-16487.873597,3331.459711 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_40_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_40_24.csv new file mode 100644 index 0000000000..4cca8a2777 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_40_24.csv @@ -0,0 +1,3 @@ +32.160869,2191.479660,-9458.724309,20447.455743,-28697.937442,26811.237799,-12362.487808,-9092.316231,24926.759588,-22668.338348,277.663463,28638.286964,-44310.072866,37963.363478,-19418.465171,4723.002623 +-1.859483,7358.832923,-40105.721767,107437.460678,-177438.152256,182475.294048,-81659.014832,-76651.557175,174104.763324,-124706.990857,-38450.375181,189406.806522,-226549.023873,157101.808323,-64545.084252,12311.798579 +-10.256590,1405.744655,-7178.104567,16956.856943,-23221.904511,17318.495092,-504.032972,-14293.484522,14777.393075,-2080.197940,-10911.588114,13677.295260,-7455.913464,714.172835,1511.772591,-690.762715 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_40_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_40_26.csv new file mode 100644 index 0000000000..9b18beb2c0 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_40_26.csv @@ -0,0 +1,3 @@ +33.352905,2417.788822,-9991.532638,19980.183101,-24681.737899,18573.718659,-4243.485479,-9938.941078,16025.247514,-11100.310233,-2386.264378,17414.644136,-25619.337609,22571.361701,-12260.115710,3198.546374 +-1.380778,5476.216510,-29835.669193,81189.169342,-137565.157003,147054.885284,-72640.254647,-54857.721926,142023.431645,-110509.299351,-23526.957263,157417.189335,-197397.907753,140994.501316,-59211.961438,11470.407813 +-11.754313,-677.906363,3673.656283,-10428.721602,19480.171375,-24353.988333,16963.796372,3419.760290,-24137.475640,26546.363040,-3794.237453,-28946.703224,46894.857531,-39576.925381,19369.227446,-4433.651118 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_50_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_50_20.csv new file mode 100644 index 0000000000..ca58a5179a --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_50_20.csv @@ -0,0 +1,3 @@ +27.660583,1988.451694,-10129.551297,26396.019684,-44468.217317,49024.092007,-27298.463665,-14448.185330,47632.373853,-42683.727963,-1995.431388,54202.695185,-76613.712040,60451.961543,-28484.873877,6411.405274 +-3.894782,6911.988271,-37333.916161,98443.944295,-159247.524166,159158.743729,-66204.256101,-71506.170268,150317.798801,-102189.788993,-37671.365405,161669.794494,-188988.379221,129641.711271,-53028.677074,10114.312850 +-6.616320,4367.998914,-24088.525848,64173.560530,-105419.214112,108650.328933,-50237.535144,-43073.072876,103514.090092,-78325.513693,-18415.271285,114115.087158,-143051.524020,102966.293131,-43835.469600,8740.310388 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_50_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_50_22.csv new file mode 100644 index 0000000000..0ad5ae7ed0 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_50_22.csv @@ -0,0 +1,3 @@ +31.668587,1795.200598,-7770.499283,17079.626500,-24814.727365,24572.045644,-12945.558457,-6741.863178,23153.262371,-22914.812955,1991.824129,27049.754483,-43796.558877,38252.245825,-19781.555735,4843.187949 +-2.281542,8075.159241,-44203.389959,118407.882093,-195106.934747,199713.266101,-88112.034803,-85216.710870,190223.940058,-134502.519122,-43560.218583,206292.674453,-244949.153587,169125.086473,-69309.435608,13219.988866 +-8.145047,2696.068051,-14332.670965,36321.797191,-55794.599745,52004.377157,-17812.816251,-26753.779936,48037.074905,-28850.661403,-15241.374715,50679.390361,-55946.345934,36893.683404,-14510.235185,2655.657664 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_50_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_50_24.csv new file mode 100644 index 0000000000..fe1f0814d4 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_50_24.csv @@ -0,0 +1,3 @@ +33.095976,2152.118291,-8881.755325,17751.571206,-21973.379351,16738.927075,-4299.582060,-8310.200116,14388.013059,-10919.114624,-1069.093949,15844.362600,-24922.232859,22749.322552,-12617.267502,3329.413601 +-1.629301,6984.736243,-38146.290727,102836.227371,-171631.368374,179581.004249,-84350.652121,-71284.704488,172311.543150,-128810.440733,-33195.123641,189271.329181,-232165.944474,163704.178487,-68171.348375,13160.821925 +-9.465573,1207.103526,-6365.911728,15489.522852,-21766.752423,16520.631023,-248.592411,-14336.848681,14291.935234,-754.603439,-12029.877956,12745.288860,-4192.816314,-2897.294148,3544.887251,-1186.993664 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_60_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_60_20.csv new file mode 100644 index 0000000000..0dfab1e0bc --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_60_20.csv @@ -0,0 +1,3 @@ +29.535354,1990.789675,-9611.914868,23689.011726,-37984.300226,40224.977052,-21547.832032,-12153.302911,38597.878487,-34908.842791,-846.172275,43999.328886,-64117.264158,52084.114215,-25314.316062,5878.657243 +-3.018719,8237.316078,-44852.238234,119038.853335,-193792.652711,195080.814063,-82425.180460,-86673.371989,184760.416590,-126530.880033,-45771.753526,199024.999272,-232698.983942,159277.487579,-64921.251600,12343.393826 +-6.387072,3872.411459,-21421.379848,57225.800079,-94259.625809,97477.710808,-45477.206924,-38151.871562,92834.776181,-70914.953769,-15732.902324,102427.701335,-129632.304199,94160.493392,-40490.566693,8161.518697 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_60_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_60_22.csv new file mode 100644 index 0000000000..b9e2fdc300 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_60_22.csv @@ -0,0 +1,3 @@ +32.416195,1664.417162,-6682.586866,13086.999481,-16238.804669,13163.468616,-5059.079769,-4584.252374,11541.821541,-11469.122878,1832.660716,13478.359483,-24586.507337,23634.967132,-13356.031761,3536.474783 +-1.969092,7918.506253,-43586.322535,117660.895553,-195838.479688,203360.087317,-93116.513000,-83440.877776,194594.514858,-141711.038804,-40944.196152,212339.959086,-256226.501404,178774.853863,-73891.594226,14207.855408 +-7.608015,3365.666296,-18129.232647,46498.368984,-72090.482053,67474.383528,-22755.175910,-35517.804074,62445.429029,-36025.467924,-21442.815150,65283.009273,-69480.750893,44280.852728,-16849.071396,2990.151175 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_60_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_60_24.csv new file mode 100644 index 0000000000..1a9db88d3e --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_60_24.csv @@ -0,0 +1,3 @@ +33.215169,1918.728026,-7576.090530,14033.650467,-15157.855228,8539.840378,901.341859,-6438.676318,6180.442737,-3017.235031,-1082.711648,6260.352706,-11281.137514,12227.384270,-7899.724458,2345.460074 +-1.602410,6196.486859,-33650.456257,90637.517917,-151749.615571,160153.955527,-77387.462552,-61103.616268,154108.755297,-118624.064353,-26467.224332,170480.157713,-213199.040922,152339.447259,-64143.843601,12499.454209 +-8.662570,1542.642106,-8450.407603,21634.359133,-32694.038740,28447.920640,-6025.063788,-19087.067337,25792.116172,-9101.201770,-14506.375832,25151.744389,-18980.132282,7375.128886,-744.816890,-330.717893 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_70_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_70_18.csv new file mode 100644 index 0000000000..aa94320be0 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_70_18.csv @@ -0,0 +1,3 @@ +19.365294,-2095.545193,12992.367455,-37467.667008,65124.303637,-70421.563197,35300.578276,25407.464257,-67481.301403,53716.162580,9491.333807,-74885.706094,96742.562399,-70965.793749,30557.567376,-6105.052372 +-5.583529,-2486.727618,15256.869231,-43173.947058,73908.593900,-78341.598347,36872.854698,31784.537550,-75723.048351,54958.401592,16993.619680,-82913.887366,97087.985081,-65023.217137,25530.304408,-4725.430282 +-4.624419,2033.915685,-11388.386977,30975.695527,-52403.626425,56455.140312,-29035.429889,-19497.573995,54570.088323,-44823.940879,-6557.671939,61205.294352,-80336.897382,59525.776427,-25977.801599,5306.367769 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_70_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_70_20.csv new file mode 100644 index 0000000000..cc2b5c3150 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_70_20.csv @@ -0,0 +1,3 @@ +30.503881,1637.998679,-7267.999690,16378.248790,-24223.472854,24081.439332,-12359.939876,-7130.949283,22646.884895,-21518.618415,1042.236825,26085.752419,-41081.765839,35497.556767,-18318.082781,4501.993109 +-2.571367,8697.612353,-47588.673985,126925.880254,-207796.398876,210723.488448,-90714.763504,-92073.086149,200061.627326,-138828.581520,-48035.845962,216060.950625,-254203.822609,174650.692703,-71395.106346,13618.449711 +-6.087855,4200.955011,-23038.088867,60786.801336,-98367.993355,98956.181772,-42778.775678,-42207.440561,93450.796283,-66974.818386,-19827.254609,101662.537188,-124045.408718,88122.512060,-37287.120290,7422.819137 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_70_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_70_22.csv new file mode 100644 index 0000000000..5257592a0d --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_70_22.csv @@ -0,0 +1,3 @@ +32.669731,1464.640323,-5418.195010,9167.366035,-8704.684489,3949.747897,687.515681,-2263.097535,2319.875738,-3005.414089,2240.864417,2895.811859,-10111.899780,12756.547030,-8582.235791,2559.256552 +-1.856224,7259.600381,-40068.198408,108820.895395,-182726.755646,192249.916662,-91039.232026,-75869.293774,184744.217173,-138314.909205,-35519.521132,202813.237921,-248586.080019,175207.222366,-73010.102773,14138.451462 +-7.072842,4082.259966,-22267.625644,57891.573803,-91085.034900,86842.232631,-30928.317428,-44268.990274,80792.822117,-48238.362354,-26346.450654,84843.567516,-91799.492134,59301.103604,-22928.121380,4163.244189 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_70_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_70_24.csv new file mode 100644 index 0000000000..de97d633d4 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_70_24.csv @@ -0,0 +1,3 @@ +33.206614,1853.327255,-7244.657524,13071.338938,-13167.761045,5668.764047,3326.456526,-6441.130986,3162.926365,741.584811,-1871.627411,2439.283272,-4941.604887,6964.482860,-5420.429639,1808.832444 +-1.604248,5730.800286,-30858.243925,82656.553652,-137988.359441,145702.695463,-71070.131302,-54579.121498,140230.285429,-109580.670268,-22410.380140,155754.869823,-197151.151646,142131.688018,-60311.313360,11830.168397 +-7.994372,2076.524949,-11708.916682,31135.178498,-49541.510260,46982.992611,-15364.347524,-25996.175738,43723.566810,-22829.733941,-17684.599938,44781.672320,-43283.996237,24696.949282,-8108.054201,1153.333265 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_80_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_80_18.csv new file mode 100644 index 0000000000..7eb6e77b8b --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_80_18.csv @@ -0,0 +1,3 @@ +20.524085,-2384.106779,14334.064346,-40208.886730,67851.511272,-70551.417413,32067.724115,28987.895685,-67104.685871,48863.207647,13932.199634,-73287.353094,88806.778148,-61915.793992,25259.444872,-4730.313075 +-5.542702,1204.595984,-5391.281348,12181.374045,-16636.284882,13152.549297,-2315.375343,-8095.136310,10814.526678,-5457.890606,-3201.902999,10667.724450,-14147.136122,12189.494755,-6419.637380,1500.040893 +-4.434680,3483.180113,-19892.713623,55038.374000,-94311.913589,102443.264457,-52966.826967,-35393.614569,99147.858125,-81063.481820,-12451.808957,111017.493542,-144649.481616,106480.578384,-46188.857561,9386.334375 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_80_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_80_20.csv new file mode 100644 index 0000000000..9c8dcf2a94 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_80_20.csv @@ -0,0 +1,3 @@ +31.095676,1259.940785,-4928.388381,9470.135848,-11799.203255,10143.058201,-5019.926187,-2231.953954,9028.611048,-10735.422885,3213.629875,10995.449259,-22257.016109,22164.661712,-12755.069239,3415.397633 +-2.347638,8846.783859,-48615.068766,130297.200740,-214541.717146,219226.621137,-96199.882411,-94086.196643,208653.699271,-146804.477482,-48387.557818,225950.297513,-267659.993646,184677.248038,-75750.288862,14499.495494 +-5.778510,5365.687135,-29405.391198,77306.277174,-124087.882410,122793.285592,-50158.609340,-55612.433236,115416.348491,-78336.501816,-28652.117521,124115.053152,-146151.462575,101240.198342,-41964.350729,8213.778006 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_80_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_80_22.csv new file mode 100644 index 0000000000..a977170faa --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_80_22.csv @@ -0,0 +1,3 @@ +32.810843,1375.563071,-4834.722141,7261.430230,-4821.861813,-1095.720968,4120.642254,-1251.301197,-2832.641833,2006.411530,2249.645821,-3116.454886,-1708.361710,6405.151884,-5797.098406,1991.878370 +-1.851467,6773.136016,-37371.563432,101740.892017,-171627.601637,181984.590539,-88005.438525,-69906.367693,175328.437031,-133745.199262,-31466.981144,193312.155238,-239600.670353,170109.178367,-71303.372631,13876.453012 +-6.618097,4996.330215,-27500.930286,72209.783004,-114886.850514,111166.013744,-41417.786426,-54919.243522,103818.621659,-64107.655830,-31914.924294,109570.014652,-120901.914292,79426.732484,-31288.087687,5815.727935 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_80_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_80_24.csv new file mode 100644 index 0000000000..11030f5062 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_80_24.csv @@ -0,0 +1,3 @@ +33.255618,1701.464062,-6447.746607,11003.506907,-9754.166709,1971.192604,5372.232929,-5363.806344,-449.823926,4023.157333,-1714.903796,-1767.275802,900.157622,2533.091470,-3475.086435,1414.773242 +-1.553495,6163.359174,-32962.081282,87383.445202,-144086.029837,149857.676526,-70887.898824,-58045.452082,143519.224266,-110111.249580,-24546.196196,158815.379963,-199638.013728,143556.265212,-60876.251876,11948.782786 +-7.433983,2354.334624,-13702.236763,37753.425454,-62667.458991,63134.035772,-25242.735416,-30351.698923,59782.006675,-37095.076748,-18769.239958,62917.328031,-67736.839907,43054.092373,-16231.798493,2845.048827 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_90_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_90_18.csv new file mode 100644 index 0000000000..8191010b78 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_90_18.csv @@ -0,0 +1,3 @@ +21.514746,-1675.125871,10746.532882,-31438.946684,54789.432380,-58932.264009,28957.683829,21867.781711,-56293.822759,44008.486102,8707.502139,-62325.230163,79402.905758,-57415.569501,24187.395267,-4668.209959 +-3.910112,-4241.466210,23573.644364,-60773.415309,93591.390837,-85887.706636,26461.486785,48047.486393,-79318.439727,41902.993253,31474.227937,-82171.957140,81034.801182,-47113.529080,15951.442219,-2527.791017 +-4.323854,6695.030912,-37378.929779,100415.462678,-165683.567119,170861.633741,-78402.081698,-68716.204329,162742.180793,-121438.156621,-30657.234353,178577.312295,-221558.515627,158630.017478,-67513.185218,13539.603054 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_90_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_90_20.csv new file mode 100644 index 0000000000..2d07486853 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_90_20.csv @@ -0,0 +1,3 @@ +31.517201,961.052003,-3143.302629,4352.202314,-2830.953757,359.618797,-160.045751,1513.796884,-484.724619,-3590.911681,5105.710992,560.704438,-9716.014506,13539.372702,-9262.998089,2756.143658 +-2.331790,9089.464607,-50144.649052,134949.160762,-223241.414167,229506.870703,-102217.014757,-97103.554359,218878.574608,-155633.172470,-49387.760301,237523.411268,-282790.235247,195700.337895,-80457.491676,15438.569240 +-5.495639,6247.525896,-34355.740577,90557.948458,-145564.199455,143990.025341,-58442.744397,-65747.440248,135294.093696,-90972.134096,-34438.444145,145149.921278,-169728.476697,116958.773058,-48284.330115,9425.969372 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_90_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_90_22.csv new file mode 100644 index 0000000000..82e492fcbe --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_90_22.csv @@ -0,0 +1,3 @@ +32.994469,1354.579587,-4590.685623,6196.733993,-2242.256209,-4892.452743,7084.825055,-838.215964,-6839.614645,6276.798664,1977.934109,-7942.782874,5227.419594,1185.034709,-3562.939488,1556.224013 +-1.821043,6342.218791,-34864.389241,94869.515285,-160387.313083,171053.191660,-84258.901607,-63947.849094,165118.598564,-128390.770818,-27361.936486,182929.426635,-229572.596630,164346.572603,-69348.270295,13568.534906 +-6.228475,5494.544653,-30537.724211,81057.024034,-130608.071875,128618.988676,-50551.697763,-60927.879222,120657.369066,-77768.939883,-34044.579077,128217.635942,-145288.711502,97601.284095,-39345.342570,7504.582282 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_90_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_90_24.csv new file mode 100644 index 0000000000..ffd2f0c3b1 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_11_90_24.csv @@ -0,0 +1,3 @@ +33.435960,1242.401519,-3962.967895,4594.081826,304.082284,-7759.163303,9235.736110,-945.695840,-9547.109424,10289.866079,450.937755,-11663.435529,12705.986959,-5667.580234,-103.956956,768.103096 +-1.336397,6866.741970,-36418.959669,95299.472361,-154600.523127,157446.677375,-71119.787313,-63931.711665,149720.130428,-111650.241109,-28183.809414,164727.685613,-204750.413797,146545.575345,-62030.939198,12175.933958 +-6.939453,1623.556153,-10318.636245,30834.408278,-55508.188352,61576.593042,-30934.342031,-23531.853361,59858.460044,-44588.576919,-12106.782791,65112.779851,-78088.546924,54013.042978,-22147.962880,4257.630037 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_100_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_100_18.csv new file mode 100644 index 0000000000..961607c788 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_100_18.csv @@ -0,0 +1,3 @@ +24.755681,-3422.558154,20160.321190,-55625.123088,92327.980112,-93937.031702,40252.468918,41213.142321,-88996.876037,61420.476761,21825.044932,-96295.708153,112268.277236,-75787.490084,29874.350167,-5367.886884 +-4.961508,10033.058983,-55494.866523,148299.009937,-241628.020281,241936.619098,-99953.838107,-109503.905178,227759.623648,-153400.382066,-57871.955863,243812.285105,-284686.617848,196078.467857,-81029.159813,15782.954402 +-4.128535,4532.874809,-25745.113112,70876.346207,-120911.130404,130856.819632,-67543.630959,-44879.120077,126179.256077,-104009.637785,-14484.769866,141281.639761,-186818.019281,139669.139518,-61693.678613,12798.731167 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_100_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_100_20.csv new file mode 100644 index 0000000000..4c22062e9b --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_100_20.csv @@ -0,0 +1,3 @@ +33.199367,138.158211,1760.713681,-9485.356597,20775.355572,-24394.374222,11022.993950,12043.617253,-24216.889279,12975.017872,10915.744792,-25108.453420,19912.090947,-6202.218691,-1558.928735,1372.621625 +-1.577474,11255.921476,-62528.511615,168920.215318,-280064.453770,288192.218751,-128206.255817,-122292.181709,274821.844107,-194699.491129,-62707.548151,297855.283959,-353627.428607,244271.805008,-100332.330615,19273.711817 +-5.232276,7150.642176,-39352.614462,103874.534052,-167384.012470,166428.279800,-68963.735603,-74209.056392,156402.730841,-107735.024304,-37109.268357,168521.885142,-200975.805149,140877.548496,-59181.720048,11767.244977 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_100_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_100_22.csv new file mode 100644 index 0000000000..b5ec5abff0 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_100_22.csv @@ -0,0 +1,3 @@ +34.614122,815.961926,-1154.434655,-4074.926602,16237.852320,-25421.655410,17512.513391,6849.609562,-26845.886359,21434.443242,5859.638022,-29910.962362,31662.834439,-16932.920460,3694.762609,214.658082 +-1.000321,6174.378368,-34366.376569,94953.230525,-163294.900698,177742.781111,-91230.156970,-63020.847394,172563.267754,-138133.950006,-25160.082116,192299.226737,-245013.260217,176986.057501,-75213.242200,14812.891679 +-5.868994,7923.296287,-43743.274861,115295.056066,-184359.193460,179939.705168,-69452.166823,-85858.131895,167973.226972,-108048.062601,-46935.644757,178340.547884,-203862.795949,138663.454881,-56837.053190,11072.305399 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_20_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_20_24.csv new file mode 100644 index 0000000000..3e69f72d3a --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_20_24.csv @@ -0,0 +1,3 @@ +26.274174,-35.445711,2200.712032,-9326.770362,18819.996632,-21546.216785,10512.810672,8819.557083,-20853.522932,14326.962931,5608.658825,-22395.307092,24418.707230,-14990.069709,5043.782779,-666.267695 +-3.887569,1356.038781,-6326.155274,15809.273891,-25140.634269,25249.443255,-10950.421146,-10801.263549,23996.177237,-17160.751928,-5224.492736,26183.279819,-31634.374975,22113.432181,-9085.547229,1654.322481 +-14.356673,-1060.265957,5394.145878,-13303.180729,20072.052768,-18434.734968,6154.889826,9634.740953,-17111.374829,10177.720206,5697.768311,-18213.579723,19545.275662,-12351.844267,4641.202624,-827.468340 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_20_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_20_26.csv new file mode 100644 index 0000000000..8fa2916525 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_20_26.csv @@ -0,0 +1,3 @@ +27.410096,-864.285016,6851.412485,-21350.203651,36748.146830,-36147.112391,11676.125630,20850.574429,-33741.113702,15615.747592,15988.201620,-33852.530525,28490.635303,-12774.633535,2180.330829,272.043093 +-2.746678,4712.801399,-25015.160393,65633.794977,-105879.897961,105340.913431,-42990.142655,-48381.577403,99488.890364,-66121.043877,-26557.658142,106612.071962,-122220.655684,82308.447444,-32988.048560,6118.951586 +-15.005835,-1012.800223,5256.805590,-13504.215719,21625.140329,-21602.912973,9071.909514,9734.213287,-20623.915646,13950.178465,5458.040914,-22239.632938,25334.921307,-16952.568064,6903.638306,-1380.370579 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_20_28.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_20_28.csv new file mode 100644 index 0000000000..09603aaf00 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_20_28.csv @@ -0,0 +1,3 @@ +29.005850,427.426465,186.089235,-4673.942632,11034.625029,-11612.898773,2299.065563,9293.363978,-10876.537185,413.581510,10162.276317,-9338.953291,-537.312907,7669.927208,-6552.450655,2061.728899 +-1.782697,5200.508968,-27990.038293,74567.066622,-122329.028981,124270.446343,-53428.076689,-54571.296413,118079.500037,-81413.303327,-28997.516048,127400.007067,-148760.795702,101333.834739,-40945.284151,7648.211115 +-16.396999,-464.621154,1997.704671,-4791.457255,8314.348255,-10349.969774,7481.169088,1261.140147,-10576.402396,11782.470755,-1580.977744,-12840.379938,20366.966244,-16939.807851,8298.183321,-1939.156604 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_30_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_30_22.csv new file mode 100644 index 0000000000..f666307f23 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_30_22.csv @@ -0,0 +1,3 @@ +27.199352,-528.204958,3742.912985,-9913.718831,12926.475648,-5922.651791,-7504.426957,13319.469550,-3471.154708,-11894.353797,14344.176890,580.992388,-18064.283657,22061.615330,-13194.527033,3479.924962 +-4.196332,6414.445410,-34331.142537,89717.826767,-143543.239787,141199.844034,-56098.750517,-66003.233228,132695.304964,-87052.861005,-36067.095787,141765.953390,-162470.023307,109917.141395,-44440.873480,8378.601074 +-9.109570,526.734930,-2571.028565,6380.634656,-10789.260478,13213.847387,-10068.506209,-318.991193,13079.161800,-17059.936486,4785.488011,16921.105034,-30749.222302,26910.139672,-13070.615768,2854.649973 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_30_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_30_24.csv new file mode 100644 index 0000000000..97ab3814cf --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_30_24.csv @@ -0,0 +1,3 @@ +30.646976,1629.862294,-7271.645262,17283.430815,-28379.407108,32772.027370,-21683.535397,-5316.738197,32329.791906,-35385.160596,5099.833300,38893.137320,-63616.893259,54407.003012,-27147.867828,6367.308854 +-2.509790,8236.757472,-44581.595066,117709.854480,-190329.008283,189517.832578,-77458.608951,-86867.278355,178884.002288,-119113.233369,-47447.488617,191706.992134,-220276.824798,148691.477782,-59754.583674,11170.294083 +-11.190316,367.201161,-1384.446493,1729.583280,547.172565,-4320.433214,5145.642118,-642.555399,-5209.186240,5981.348142,-444.066664,-6026.133303,8185.338547,-6011.059744,2779.439015,-671.650393 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_30_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_30_26.csv new file mode 100644 index 0000000000..4d15bd1775 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_30_26.csv @@ -0,0 +1,3 @@ +32.809066,3240.065573,-14954.674327,34529.289194,-51059.348227,48938.507235,-21532.596158,-19026.382905,45878.447078,-37011.289024,-4910.666911,51266.024445,-70842.710220,56196.122555,-26908.129049,6178.439775 +-1.486918,6656.518783,-36426.201342,98405.872747,-164067.847629,170476.922192,-77804.446418,-70416.547315,163149.411429,-118021.767947,-35177.672485,177791.946851,-213213.900997,147826.185392,-60584.733237,11486.999247 +-13.133450,224.259873,-859.414770,446.876891,3604.085436,-10192.345703,12257.909428,-3545.498049,-11346.773013,18007.582976,-6739.049702,-15097.390065,29562.523585,-26767.242315,13720.587194,-3265.909871 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_40_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_40_20.csv new file mode 100644 index 0000000000..34ef5788eb --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_40_20.csv @@ -0,0 +1,3 @@ +25.847112,1268.765892,-6585.935117,17988.370166,-32188.857279,37924.281861,-23399.158623,-9131.109727,37467.223653,-35759.631374,305.078839,43154.292098,-62872.062624,50350.761154,-24005.750364,5463.657034 +-5.248830,5789.509204,-30779.352192,79984.114434,-127495.795971,125365.305340,-50409.433698,-57521.790063,117652.347947,-79008.142823,-29896.843367,126262.546501,-148030.973316,102282.587307,-42241.197865,8131.152591 +-7.070734,3522.246467,-19306.602620,51369.522539,-84774.893353,88491.737638,-42638.620903,-33173.687605,84725.615938,-66720.264267,-12715.641616,94419.206750,-121121.946564,88254.275469,-37812.916887,7552.424638 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_40_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_40_22.csv new file mode 100644 index 0000000000..92a3f0f137 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_40_22.csv @@ -0,0 +1,3 @@ +31.251162,3979.202387,-20299.845125,51522.329716,-82518.943564,84284.616798,-39541.936485,-32201.066944,80108.176049,-62913.347712,-11373.408630,88557.535207,-116232.473656,88032.389405,-40223.341699,8819.630022 +-2.677196,8361.488940,-45412.215200,120299.150008,-195366.326607,195881.193269,-81772.217381,-88029.340139,185284.123496,-125632.747569,-47077.464863,199273.882539,-231498.676929,157545.851862,-63768.807657,12003.674834 +-8.906908,1999.265732,-10742.703913,27882.758792,-44795.398472,45449.198232,-20854.533660,-17820.053812,43195.699032,-33357.300467,-6972.370084,48101.193085,-61375.873242,44549.435211,-18910.878149,3702.185243 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_40_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_40_24.csv new file mode 100644 index 0000000000..0255f40040 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_40_24.csv @@ -0,0 +1,3 @@ +33.657829,3228.483519,-15022.389800,34816.013307,-51368.866400,48767.214351,-20820.338034,-19505.942978,45399.166679,-36006.086972,-5219.633139,50393.369850,-69623.211799,55602.900978,-26927.324454,6267.943668 +-1.725483,8835.043467,-48401.383614,129820.531178,-214234.982933,219672.356724,-97268.866171,-93472.035539,209412.561369,-148328.702066,-47872.758595,227284.469828,-269682.873639,185809.989314,-75857.318020,14384.049893 +-10.565018,1096.939398,-5179.080600,10791.209048,-11576.004549,3349.681312,8074.312560,-10839.177121,1029.709437,10830.230874,-10797.172349,-2128.682079,15149.217020,-16972.574434,9615.689302,-2430.161385 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_40_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_40_26.csv new file mode 100644 index 0000000000..2a82db6077 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_40_26.csv @@ -0,0 +1,3 @@ +34.736972,3190.507394,-14166.176198,30884.910182,-42194.566929,36052.387076,-11669.923752,-17502.218930,32472.945473,-22768.531678,-5928.191215,35199.999832,-47384.794872,38258.279146,-19111.730673,4636.122243 +-1.283728,6111.624715,-33582.409578,91860.820653,-156182.202705,167262.580808,-82540.357913,-62777.931804,161687.083101,-125013.762539,-27727.514343,178925.657581,-222813.541638,158175.337995,-66003.174280,12701.669168 +-11.962387,-1213.184184,6900.515987,-19760.880870,36052.018868,-42918.673237,26997.991268,9488.788715,-42227.590715,41639.813289,-1753.981409,-49236.227713,73503.482824,-59242.249431,27956.442140,-6199.345248 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_50_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_50_20.csv new file mode 100644 index 0000000000..1b416eca9b --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_50_20.csv @@ -0,0 +1,3 @@ +29.192685,4132.924878,-21683.922637,56174.809607,-91014.014900,93151.547185,-42911.540392,-36870.273902,88496.681983,-67307.376497,-14866.803738,97032.815581,-123985.575539,92105.303145,-41416.530532,8965.927180 +-3.549593,8119.780731,-44017.302204,116195.801847,-187943.014291,187592.319287,-77622.084238,-84738.331969,177095.274712,-119770.022575,-45020.185247,190331.558121,-221576.612446,151367.147741,-61596.424017,11675.661644 +-6.952903,4943.960229,-27293.766654,72695.763028,-119165.867291,122160.520122,-55416.996110,-49733.755806,116302.209123,-86139.501717,-22604.284758,127647.492733,-157358.222673,111813.440234,-47033.786136,9267.605554 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_50_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_50_22.csv new file mode 100644 index 0000000000..33d21cc756 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_50_22.csv @@ -0,0 +1,3 @@ +33.127170,3169.216022,-15208.971727,36413.394628,-55433.036751,54238.962979,-24254.947815,-21019.363921,50839.645231,-40603.648828,-5839.290014,56371.664140,-77275.981053,61158.747813,-29341.409848,6770.047285 +-2.074269,9913.516687,-54456.881306,145785.047894,-239498.821224,243698.194214,-105552.064870,-106057.653131,231655.448115,-161074.162948,-55559.851382,250349.481150,-294152.543721,201506.315421,-81981.508038,15534.274009 +-8.382507,2565.395022,-13432.841653,33425.950184,-50146.597697,45026.891716,-13314.857811,-25286.710570,41193.107028,-22067.088783,-15609.334846,42771.024221,-43965.365321,27097.060931,-9825.696932,1606.803756 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_50_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_50_24.csv new file mode 100644 index 0000000000..71dd1512a6 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_50_24.csv @@ -0,0 +1,3 @@ +34.377514,2604.716670,-11353.832025,24311.396536,-32753.847754,27914.601568,-9612.848219,-12524.998963,24993.101319,-19268.189302,-2546.549426,27544.563856,-40302.209943,34412.753870,-17941.006059,4490.725853 +-1.563054,8480.334139,-46729.542021,126437.630989,-211134.296882,220266.643974,-102038.453265,-89257.564595,211162.371544,-155141.817706,-43356.984393,230977.019677,-279685.992384,195285.988413,-80604.453797,15443.320886 +-9.624542,918.845731,-4363.978269,8975.208864,-8926.305293,562.632990,9926.218291,-10573.237174,-1677.595135,14233.520777,-11665.069052,-5658.065190,21435.858976,-22418.269583,12239.114971,-3000.426534 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_60_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_60_18.csv new file mode 100644 index 0000000000..4a11c9a0e4 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_60_18.csv @@ -0,0 +1,3 @@ +18.644236,3664.504512,-18910.052428,47406.616989,-72593.958812,66763.877241,-20033.423313,-39186.042631,62879.857281,-30162.531932,-29180.255671,64565.368311,-56004.553026,26322.123288,-5677.399794,87.588777 +-2.528995,-8753.763501,49977.705155,-135403.600651,223932.134891,-230332.098007,104320.591397,94309.464496,-219226.453624,161175.904142,43986.049206,-240143.875640,293742.966807,-207302.854519,86747.190627,-17122.532582 +-5.040884,4198.062971,-22594.610444,58180.491072,-91437.599749,88711.911899,-35315.315132,-40289.857682,82743.051959,-56817.301938,-19341.604525,89256.093630,-107792.673876,76747.333267,-32858.481661,6681.145476 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_60_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_60_20.csv new file mode 100644 index 0000000000..4b5a052d53 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_60_20.csv @@ -0,0 +1,3 @@ +31.015223,3191.701810,-15995.098483,39927.512319,-63031.923630,63597.356105,-29477.302102,-24266.761430,60077.060904,-47632.528848,-7678.339675,66386.687195,-89048.555505,69060.950608,-32472.203295,7353.602653 +-2.749667,10094.743211,-55142.234218,146327.105123,-237740.436599,238317.631355,-99347.807818,-107266.716910,225369.973139,-152550.506696,-57478.575458,242229.400637,-281172.210471,191355.958492,-77552.453741,14664.329118 +-6.632093,3748.718132,-20429.641854,53678.980244,-86758.297948,87547.573753,-38514.884985,-36526.478115,82855.945285,-60594.089407,-16504.099721,90691.187986,-111925.083344,79936.369223,-33848.234402,6711.623517 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_60_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_60_22.csv new file mode 100644 index 0000000000..c38d3bcbe2 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_60_22.csv @@ -0,0 +1,3 @@ +33.767761,2168.945564,-9430.523610,20360.321407,-28155.801972,25452.673326,-10802.058834,-9341.211171,23192.237493,-20466.293068,43.465958,26262.829076,-41188.595841,36150.576264,-19056.760171,4781.246148 +-1.820943,10343.316962,-57178.300488,154089.207862,-255095.617455,262238.905477,-116540.356892,-111345.763462,250113.241130,-177218.564545,-57153.788391,271295.371892,-321825.603517,221834.240449,-90736.778907,17298.314630 +-7.745505,3063.351919,-16103.153297,40103.657176,-59852.391675,52763.059804,-13906.968400,-31507.901211,47878.371689,-23011.273164,-20555.184696,48745.120468,-46997.409063,27319.161432,-9319.222521,1417.719545 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_60_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_60_24.csv new file mode 100644 index 0000000000..9347284af2 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_60_24.csv @@ -0,0 +1,3 @@ +34.322345,1974.509717,-7859.726512,14782.535894,-16520.209360,10352.811615,-654.394736,-6258.097283,7956.892883,-5657.228825,-63.883746,8592.085986,-16088.430887,16800.917252,-10348.701715,2948.328479 +-1.498902,7530.912981,-41512.827629,112782.973071,-189688.895781,200236.416550,-95753.195172,-78022.807746,192671.666628,-145588.948619,-35881.459729,212066.537227,-261216.385436,184531.683190,-76928.736265,14873.922504 +-8.749728,1948.586653,-10252.757606,25054.270588,-35546.027684,27536.306709,-1363.704606,-22761.999051,23777.220654,-2794.039174,-18289.439524,21487.443121,-9940.714843,-980.316955,3406.238588,-1261.576523 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_70_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_70_18.csv new file mode 100644 index 0000000000..7efe21e7a0 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_70_18.csv @@ -0,0 +1,3 @@ +20.529011,-2747.286472,16503.238571,-46424.876165,78809.421551,-82781.724925,38719.704896,32870.236898,-78993.216906,59015.177606,15034.380333,-86759.651861,106787.564015,-75272.190191,31048.092778,-5895.405692 +-5.607306,1214.575284,-5374.230778,11960.280690,-16027.211132,12382.213187,-2096.027144,-7365.839765,9875.688149,-5554.816525,-2002.129364,9838.624845,-14877.937441,13872.786710,-7639.636286,1844.329884 +-4.770300,4122.903474,-23546.348184,65044.365547,-111049.122790,119790.595231,-60763.312927,-42735.353329,115766.538389,-92805.403940,-16361.574980,129053.822804,-165758.962463,120799.326433,-51945.171114,10472.814992 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_70_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_70_20.csv new file mode 100644 index 0000000000..34e48cda99 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_70_20.csv @@ -0,0 +1,3 @@ +31.984713,2023.850304,-9245.948133,21312.564982,-31847.269237,31453.221485,-15406.889885,-10254.610086,29438.879941,-26560.309243,-20.671764,33408.679971,-50905.556368,43319.596637,-22138.971310,5401.616975 +-2.242528,11240.984791,-61686.608417,164297.838963,-267913.003057,269715.864719,-113570.598293,-120432.584209,255416.108801,-173927.203471,-64318.952075,274790.523789,-319711.804554,217874.631676,-88420.586333,16765.178563 +-6.246984,4302.236674,-23202.715460,60036.850413,-94863.932638,92282.048598,-36341.301257,-42826.803890,86315.433687,-57647.028866,-22113.143125,92683.886892,-108704.546114,75181.051678,-31084.752511,6049.779819 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_70_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_70_22.csv new file mode 100644 index 0000000000..4e457c679b --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_70_22.csv @@ -0,0 +1,3 @@ +33.837819,1414.548251,-5166.913745,8681.560100,-8454.579047,4682.896989,-979.494037,-1111.584697,3261.564348,-5618.915739,3903.090837,4479.506060,-14456.164558,17173.479755,-11019.908350,3170.384451 +-1.617083,9879.717385,-54794.883790,148343.476177,-247030.816018,256052.886572,-116213.508606,-106362.771365,244851.690599,-176375.073799,-53403.347586,266461.242398,-319001.920318,221282.141331,-91026.533691,17456.013097 +-7.141911,3955.934207,-21171.020979,53832.902235,-82403.727150,75445.768684,-23315.277802,-41754.052175,69210.296484,-37369.510075,-25963.343451,71534.742536,-73638.036802,45737.183334,-17004.537292,2950.238742 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_70_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_70_24.csv new file mode 100644 index 0000000000..f88bfd72a1 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_70_24.csv @@ -0,0 +1,3 @@ +34.179720,1545.444045,-5471.597457,8224.120467,-5235.766940,-2023.805845,5853.792296,-2040.279567,-4091.668439,4217.042009,1459.855043,-4880.648438,1408.167143,3926.137545,-4735.507831,1794.132477 +-1.437931,7250.586351,-39918.743423,108405.971178,-182405.100638,192865.194784,-92768.619982,-74505.902188,185658.229193,-141200.365108,-33666.847639,204626.120116,-253293.775722,179646.568587,-75197.870042,14605.037542 +-8.040029,3760.356633,-20495.379730,52720.810321,-80910.639347,73090.993970,-20116.092108,-43593.934138,66709.651592,-31289.330175,-29673.348657,67273.558935,-62635.360227,34867.200483,-11271.682312,1602.226859 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_80_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_80_18.csv new file mode 100644 index 0000000000..515230af6f --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_80_18.csv @@ -0,0 +1,3 @@ +22.288150,-1216.112159,8055.134713,-24063.992409,42718.695275,-46944.427514,24062.582842,16619.770270,-45352.729852,36274.916207,6664.106496,-50745.050133,64300.821029,-45587.723780,18491.417665,-3347.272585 +-5.391914,4390.326152,-23406.359046,61120.354919,-97892.289845,96803.193856,-39721.037590,-43177.364130,90440.090009,-62578.521368,-20576.581588,97233.777673,-118129.976741,84707.020778,-36491.849667,7360.829552 +-4.540043,5468.669072,-31045.272013,85168.685854,-144195.215784,153826.117460,-76161.712393,-56639.537021,148074.291196,-116628.159422,-22612.502601,164298.247143,-209472.474862,152317.314049,-65546.030523,13255.384802 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_80_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_80_20.csv new file mode 100644 index 0000000000..c1a46af82f --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_80_20.csv @@ -0,0 +1,3 @@ +32.413556,1350.230262,-5328.289035,10381.893774,-13219.320747,11735.356865,-6107.687318,-2379.985235,10532.239135,-12603.376498,3772.495044,12791.407905,-25816.013823,25660.946034,-14742.100742,3939.931269 +-1.904795,11436.781524,-62969.113389,168299.232038,-275512.830426,278751.031436,-118822.754691,-123167.021845,264392.260197,-181518.582985,-65331.027142,284840.568458,-332710.033817,227341.450386,-92503.068042,17597.147289 +-5.861866,5088.035984,-27595.127016,71753.861246,-113809.899704,110988.169306,-43748.606974,-51602.183738,103827.932453,-69055.058939,-26891.726690,111286.394758,-130074.656459,89801.122165,-37147.437745,7255.848973 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_80_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_80_22.csv new file mode 100644 index 0000000000..850dd35919 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_80_22.csv @@ -0,0 +1,3 @@ +33.895367,938.974468,-2432.582976,1053.357511,4671.995875,-9491.178251,6070.600337,4188.998010,-10447.173796,4954.378216,6264.850784,-10620.977330,4353.002309,3736.945875,-5318.591138,2029.191279 +-1.472308,9113.356083,-50615.525451,137519.274034,-230223.841884,240553.365805,-111504.398675,-97584.107864,230618.090814,-169070.995698,-47654.566084,251907.213935,-304679.426914,212827.389984,-88071.843864,16980.892549 +-6.648367,5041.708590,-27268.016598,70188.519030,-109050.260526,102077.365615,-34301.738975,-53750.965052,94195.263135,-54323.331478,-32107.298892,98287.149334,-105276.101680,67852.542292,-26330.157338,4827.260240 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_80_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_80_24.csv new file mode 100644 index 0000000000..9605666aaa --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_80_24.csv @@ -0,0 +1,3 @@ +34.141674,1436.872510,-4812.134181,6189.251387,-1225.241278,-7182.004046,9426.993052,-1143.215515,-9339.907587,9575.862612,1195.672110,-11103.095715,10502.702443,-3161.794670,-1539.958935,1125.171333 +-1.344374,7050.331411,-38690.594959,104816.703260,-176087.399762,186096.844829,-89712.854143,-71497.698413,179097.043513,-136906.418517,-31716.659634,197645.908548,-245818.940802,175039.978143,-73562.001209,14345.951919 +-7.459472,5173.356957,-28673.207858,75355.744521,-119031.647594,112704.110521,-37899.550383,-60299.513402,104411.623719,-58014.039471,-38155.777104,107962.685571,-111211.694063,68771.856848,-25459.033280,4423.356464 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_90_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_90_18.csv new file mode 100644 index 0000000000..95fdc61c47 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_90_18.csv @@ -0,0 +1,3 @@ +23.715784,-2262.238156,13808.968470,-39071.815131,66245.518216,-69030.271919,31304.664160,28627.848946,-65827.087578,47419.367795,14428.175855,-71899.720996,85655.435617,-58466.042395,23103.058533,-4114.439323 +-5.150416,8473.185971,-46519.080736,123650.783670,-200598.919190,200104.971036,-82216.436307,-90755.883762,188145.903989,-126763.251285,-47558.503715,201443.498845,-235953.977675,163139.056762,-67706.454640,13233.041578 +-4.323923,5087.221946,-28871.031013,79273.441812,-134543.533880,144254.925908,-72529.461585,-51757.257889,138909.779219,-111334.159529,-19171.329564,154620.050669,-200079.052231,147251.814332,-64130.721528,13129.210912 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_90_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_90_20.csv new file mode 100644 index 0000000000..01b5eae0a5 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_90_20.csv @@ -0,0 +1,3 @@ +32.717729,746.286895,-1889.189442,978.691784,2478.809189,-4468.970458,1085.001603,4544.186218,-4912.146474,-1763.650426,7387.193504,-3881.960181,-6139.384492,12104.339176,-9164.537172,2855.179294 +-1.773344,11584.756004,-63969.070808,171495.789247,-281705.622591,286248.577336,-123295.075197,-125345.752396,271888.912609,-187942.317195,-66125.546040,293261.939242,-343614.179791,235264.666656,-95907.337900,18288.215017 +-5.531466,5856.008232,-32001.241360,83868.898569,-134153.826070,132223.326047,-53622.315663,-60030.185579,123997.571097,-84193.702149,-30521.553384,133330.719941,-157752.147031,109928.362076,-45900.852837,9061.420475 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_90_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_90_22.csv new file mode 100644 index 0000000000..0ed5d8944d --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_12_90_22.csv @@ -0,0 +1,3 @@ +34.112345,615.551005,-424.956080,-4923.885188,15573.070861,-21985.246434,12975.654010,8252.454920,-22754.040041,15111.021065,7873.610025,-24403.368387,21935.588571,-8878.567071,-5.478560,984.205571 +-1.308467,8166.061206,-45424.107324,124060.807887,-209358.600195,221420.837995,-105892.527817,-86516.406564,213096.614454,-160416.017065,-40281.000286,234134.213160,-287566.189161,202906.378411,-84652.171798,16432.570483 +-6.231543,6035.260492,-32923.365892,85592.279491,-134614.041685,128318.303268,-45993.279414,-64648.228462,118947.432720,-72322.767105,-37011.150874,125112.471608,-138566.173171,91998.247341,-36855.156674,7009.953881 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_100_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_100_20.csv new file mode 100644 index 0000000000..19597918f5 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_100_20.csv @@ -0,0 +1,3 @@ +27.658666,-516.811248,4602.804218,-15344.772765,28086.216006,-29907.462686,12408.523038,14657.344073,-28771.086946,16667.119869,10799.999325,-30030.551642,28675.563420,-15003.510948,3719.217656,-103.077518 +-3.316146,2882.807916,-14864.454433,38832.724751,-63819.934174,66833.940250,-32578.672695,-24754.838302,64243.051776,-50906.058749,-9439.092743,71643.450979,-92089.725542,67287.667811,-28939.837647,5727.411065 +-4.902192,868.847118,-5093.367006,14754.289545,-27055.547153,32402.379651,-20623.799547,-6853.326945,31870.648670,-31865.239294,1832.962222,37295.852289,-56386.299783,45698.108646,-21463.815920,4671.743547 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_100_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_100_22.csv new file mode 100644 index 0000000000..a9a2095c02 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_100_22.csv @@ -0,0 +1,3 @@ +29.379887,439.211137,-135.319839,-4252.854888,12851.008107,-18532.403055,12293.126405,4860.899359,-19052.625923,15985.320042,2975.418656,-21429.361189,25056.056529,-15624.894853,5037.107586,-537.464294 +-2.182514,2446.600297,-12047.755114,30447.670626,-48841.150521,50445.112570,-24761.084108,-17867.389042,48220.514751,-39945.607742,-5103.606388,54505.362968,-73202.493902,55220.097097,-24370.804086,4906.231871 +-5.803756,840.805287,-5137.795364,14994.366254,-26780.224853,30005.163070,-15932.125022,-10287.272046,29234.798023,-23586.517787,-4060.565835,32413.388009,-41524.835484,30289.392293,-13079.351850,2650.213483 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_100_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_100_24.csv new file mode 100644 index 0000000000..5159b97e3f --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_100_24.csv @@ -0,0 +1,3 @@ +29.875843,175.084354,1376.973628,-8187.345611,18837.979337,-23961.855438,14163.415136,7331.688990,-23772.195448,19447.099244,3588.919152,-26556.228310,32340.449142,-21619.214210,7964.846220,-1205.411862 +-1.347320,3314.780986,-16656.191345,42004.461899,-65885.074403,64667.043426,-27153.103524,-27665.663938,60553.950859,-44091.818097,-11747.984290,66307.111231,-83207.542500,60564.621235,-26142.072586,5191.587831 +-6.646225,-1649.348529,8372.412565,-20223.212347,29248.941949,-24752.385635,5657.701337,15205.537424,-21934.711883,10344.817199,9209.620874,-22279.149529,22495.256806,-14117.332381,5473.115616,-1037.126383 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_20_28.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_20_28.csv new file mode 100644 index 0000000000..be8139f4c4 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_20_28.csv @@ -0,0 +1,3 @@ +26.200611,1848.118100,-7059.241725,12328.246135,-11716.954193,4027.102283,4300.415570,-6249.066687,1938.536689,2398.065539,-2817.302030,1276.315770,-1183.104461,2132.694554,-2003.006570,734.264104 +-1.923428,953.550986,-4117.008853,10550.933303,-18306.270717,21067.809763,-12332.100380,-5967.573487,20979.257832,-18718.615158,-1422.528115,24131.664090,-32483.054185,23987.985920,-10182.205701,1888.340903 +-14.716461,58.268162,-1222.708081,4658.247310,-8597.002368,8596.928922,-2569.726061,-5249.154800,7892.128827,-3087.219479,-4206.296310,7612.516155,-5741.380145,2115.391215,-33.673781,-196.950653 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_20_30.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_20_30.csv new file mode 100644 index 0000000000..0def01817b --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_20_30.csv @@ -0,0 +1,3 @@ +26.861429,1934.843845,-7220.751786,12142.792827,-10769.692266,2736.455280,4804.225597,-5441.103474,703.543442,2707.785693,-1942.743066,200.408727,-832.167040,2446.184546,-2372.914581,858.852883 +-1.284031,1045.854195,-4630.771263,12261.941166,-21996.471776,26154.499001,-16092.730302,-6661.455771,26225.509240,-24249.206725,-1023.595455,30400.311567,-41735.866530,31177.031890,-13372.140181,2528.989531 +-16.608055,-464.147129,998.049900,238.077644,-3444.139030,5334.798382,-2609.753422,-2879.981831,5368.326914,-1966.891216,-3423.189180,5035.162937,-2070.563598,-1244.963359,1845.935972,-688.672234 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_20_32.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_20_32.csv new file mode 100644 index 0000000000..b5d082f580 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_20_32.csv @@ -0,0 +1,3 @@ +27.532230,2302.816919,-8961.649092,16151.736821,-16483.557962,7723.458178,3328.900659,-8203.674553,5313.717748,107.905094,-3590.036910,5069.746109,-5985.492565,5729.312493,-3643.286242,1096.819359 +-0.813213,1272.228443,-5650.401750,14699.363532,-25809.297670,30091.471063,-18123.554848,-7908.166197,29997.223707,-27623.544898,-1170.471898,34771.282889,-47949.462514,36002.787420,-15525.728035,2960.764654 +-18.702952,-778.879597,2082.569469,-1399.715403,-2048.919755,4501.096886,-1957.149894,-3305.441579,4679.257581,77.504929,-5023.277217,3722.131110,2362.383180,-6176.957729,4724.328259,-1433.806110 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_20_34.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_20_34.csv new file mode 100644 index 0000000000..a6776d4f2c --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_20_34.csv @@ -0,0 +1,3 @@ +28.370210,3658.665435,-16218.014432,34751.478355,-45388.428794,34842.762766,-5815.246379,-22549.570199,30440.431423,-14236.368092,-12381.704915,31234.970540,-33584.857297,23244.965447,-10366.565862,2332.122986 +-0.426324,-289.765280,3224.452263,-8898.824735,11758.868282,-5809.528627,-5630.929098,10855.346323,-3341.350132,-8527.259106,10493.858480,134.146982,-11615.711246,13113.621198,-6809.269142,1364.787005 +-20.788428,-2615.393631,11086.926832,-22612.030149,28322.643196,-21493.676445,4734.818950,11968.613863,-18748.641892,12107.538978,4018.779814,-20376.784020,27611.108540,-22608.128901,11363.692456,-2744.508340 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_30_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_30_24.csv new file mode 100644 index 0000000000..65608454c1 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_30_24.csv @@ -0,0 +1,3 @@ +26.645719,-379.731542,4644.756942,-17167.568353,33415.002857,-37664.802693,17835.016185,16223.697703,-36615.185421,24029.347494,11046.376767,-38909.542107,40764.708320,-24096.704045,7795.188856,-981.623627 +-2.809545,1541.294818,-7623.565601,20212.932012,-34377.343889,37606.429988,-19790.034174,-12753.609604,36655.899909,-30210.824734,-4519.908983,41203.511586,-53556.937042,39129.654062,-16698.462804,3221.840514 +-9.397636,453.635228,-2201.582474,4651.306267,-5145.271458,2087.810675,2177.694806,-3451.073293,859.703733,2099.858121,-2106.181685,44.807904,1185.370764,-821.324106,221.264105,-26.814860 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_30_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_30_26.csv new file mode 100644 index 0000000000..b1339b2ff8 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_30_26.csv @@ -0,0 +1,3 @@ +28.054581,770.821932,-1201.496838,-2828.810099,11874.288885,-17910.639591,11228.066590,5968.690232,-18376.424357,13083.186332,5273.422079,-19658.730142,19385.163269,-9788.355132,1963.607836,163.734883 +-1.896579,2291.380534,-11720.035312,31229.002846,-52692.419207,56650.777277,-28627.368766,-20367.327594,54844.773532,-43793.445620,-7979.451170,61176.003889,-78209.567354,56627.709510,-24032.747772,4649.738263 +-11.450847,1288.062535,-7292.951968,19000.797510,-29053.848191,25828.749653,-6563.500458,-15841.583732,23353.476719,-10405.150393,-10821.946751,23412.337561,-21340.546980,11568.149271,-3508.447314,410.582539 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_30_28.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_30_28.csv new file mode 100644 index 0000000000..7fdf7e579b --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_30_28.csv @@ -0,0 +1,3 @@ +29.065240,1406.682178,-4329.174394,4553.218686,1355.406035,-9151.069425,9488.710920,163.159853,-10501.162807,10002.780908,1226.599736,-11843.989044,12690.667331,-6360.254295,940.960057,303.515690 +-1.323648,2493.544569,-12662.110922,33595.848216,-56631.342114,61067.314016,-31324.308796,-21312.879027,59143.855612,-48219.362341,-7603.246953,66344.759713,-86187.002806,63080.154779,-26998.228604,5263.020668 +-13.472740,1038.934699,-6562.048685,18284.988149,-29086.372658,26212.177266,-5988.332293,-17419.843467,23866.632463,-8260.036713,-13750.299400,23144.592377,-16550.804461,5400.429628,293.465790,-595.827421 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_30_30.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_30_30.csv new file mode 100644 index 0000000000..c3df87d70a --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_30_30.csv @@ -0,0 +1,3 @@ +29.751439,1883.118230,-6728.652535,10407.652786,-7365.695357,-1379.324112,7355.929478,-4498.340589,-3311.653836,6616.206320,-2040.109678,-4526.615041,6044.823659,-2837.489823,-113.963428,439.033130 +-0.848688,2386.725502,-11963.891298,31727.301019,-53748.445786,58453.225002,-30593.329321,-19725.453793,56694.796305,-47129.425291,-6391.403165,63877.207647,-84146.556872,62147.185371,-26775.427808,5236.615722 +-15.270636,57.170755,-2060.517469,8350.791012,-15630.313012,15092.005284,-2968.508719,-11412.888762,13940.893430,-2098.350211,-11143.405565,12543.259954,-3350.562192,-4628.905800,4971.607006,-1642.722728 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_40_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_40_22.csv new file mode 100644 index 0000000000..6599c9a9b9 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_40_22.csv @@ -0,0 +1,3 @@ +26.320652,-1984.839159,12859.630550,-37168.540299,62555.195380,-62399.709404,23158.032100,32141.716120,-58731.367307,33207.248003,21749.856006,-60903.904341,60645.355808,-35085.070189,11428.360870,-1540.105434 +-3.335228,2255.404345,-11474.642075,29935.609623,-49100.858119,50959.841953,-23911.872262,-20043.088842,48848.517704,-37008.895565,-8816.028873,53858.960911,-67102.772499,47983.227021,-20247.482089,3912.377156 +-7.229553,1032.831077,-5069.350790,11749.878664,-16449.916225,13986.841340,-4228.887074,-6917.717433,12250.189698,-8466.523093,-2094.828999,13226.909126,-18298.622358,14921.558892,-7219.584586,1616.988046 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_40_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_40_24.csv new file mode 100644 index 0000000000..61ca37a4ee --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_40_24.csv @@ -0,0 +1,3 @@ +28.425954,-280.871514,4204.992185,-16111.237886,31530.730335,-35109.975447,15637.109326,16372.276610,-33982.140732,20425.881220,12124.718807,-35494.383081,34468.714485,-18466.956871,4916.830290,-295.348832 +-2.196546,3022.596829,-15760.577422,41789.353753,-69461.640754,73031.540543,-35154.490703,-27894.753740,70187.265245,-54122.103058,-11844.839183,77664.833698,-97615.348050,70108.083142,-29662.081985,5767.913242 +-9.183410,1360.457574,-7195.015717,17754.596676,-25937.837312,22079.668911,-5045.056709,-13662.795233,19617.906723,-8986.471633,-8520.814841,19797.410157,-19487.273871,11892.850007,-4363.949515,731.659273 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_40_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_40_26.csv new file mode 100644 index 0000000000..cc805ece25 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_40_26.csv @@ -0,0 +1,3 @@ +29.466167,711.972066,-949.212060,-3276.790929,12189.563609,-17749.314450,10757.591328,6163.457734,-18050.022308,12658.037807,5284.556063,-19293.522245,19023.491721,-9597.654654,1869.310037,200.530295 +-1.642874,3334.473805,-17300.704433,45634.908430,-75497.857983,79068.397976,-37963.189116,-30095.867442,75843.889048,-58831.932842,-12348.855708,84089.387926,-106483.769412,76933.193729,-32712.683470,6389.461782 +-10.952804,904.758509,-5292.063488,13917.762791,-21009.369543,17745.218160,-2842.601004,-12798.949845,15801.732510,-4309.369580,-10040.428580,15037.544644,-9760.141873,2431.662092,773.175600,-532.719917 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_40_28.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_40_28.csv new file mode 100644 index 0000000000..ef81870bf5 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_40_28.csv @@ -0,0 +1,3 @@ +29.985995,999.049084,-2227.377880,-653.475468,9253.168629,-16585.302573,12386.805738,3439.718985,-17335.514048,14987.040820,2458.042988,-19364.727360,22411.156604,-13620.342808,4136.329035,-347.994879 +-1.264060,3563.116055,-18359.729704,48065.689652,-78852.029643,81738.654677,-38467.911360,-31701.356569,78068.256873,-60026.890518,-13012.676117,86400.280800,-109396.416853,79191.766568,-33757.453981,6607.377708 +-12.471673,-5.059757,-841.395155,3251.166586,-4991.256242,2489.804259,3466.419982,-6399.643960,1644.113368,6422.703823,-7914.921761,-536.571889,11029.184123,-13675.653430,8253.787963,-2177.641059 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_50_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_50_20.csv new file mode 100644 index 0000000000..97d214ad2e --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_50_20.csv @@ -0,0 +1,3 @@ +23.441162,-277.276226,2705.650139,-8963.517227,15844.734795,-15908.803870,5461.236807,8791.698916,-14812.751730,7336.849593,6457.251832,-14958.930966,13453.389546,-6728.872954,1513.927372,30.068056 +-4.623675,721.149651,-2651.752317,5317.283489,-7020.700612,6223.787293,-2580.777472,-2420.731323,5879.856293,-4954.803439,-435.718431,6792.627276,-9663.581276,7687.645033,-3541.507353,680.555998 +-5.288155,2526.990511,-13782.215174,36407.019688,-59450.892254,60966.984127,-27884.928270,-24493.558166,58000.210669,-43451.716034,-10573.574304,63588.705230,-79724.199444,57814.502868,-24974.826095,5086.746724 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_50_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_50_22.csv new file mode 100644 index 0000000000..196bbcb515 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_50_22.csv @@ -0,0 +1,3 @@ +27.632770,-1250.667866,8895.447487,-26862.325996,46139.922856,-46265.335093,16668.054677,24723.948504,-43569.235736,23056.187232,17851.260500,-44661.681389,41673.005833,-21952.601400,5920.752555,-429.222468 +-2.777090,2918.349655,-15180.063277,40036.154854,-66166.387876,69182.568667,-33009.152773,-26650.804721,66408.584482,-51001.591477,-11356.422689,73427.761589,-92214.130764,66278.092710,-28100.638065,5483.576888 +-7.162397,862.771269,-4252.733961,9912.526249,-14033.481296,12261.500176,-4212.961163,-5540.162351,10867.533214,-8192.701042,-1269.679997,11988.662435,-17179.588625,14127.449349,-6807.106010,1506.332036 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_50_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_50_24.csv new file mode 100644 index 0000000000..876d03ab52 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_50_24.csv @@ -0,0 +1,3 @@ +29.268577,167.449817,1663.983275,-9312.617162,20638.636358,-24698.598590,12141.671727,10706.550785,-24294.312254,15251.270928,8338.477718,-25579.783724,24673.319090,-12651.655069,2827.880958,67.968096 +-2.054045,3310.553188,-17233.564066,45464.811867,-75209.623411,78855.359587,-38069.686023,-29759.900149,75689.209802,-59074.560795,-11983.661810,84043.521476,-106861.866321,77447.043088,-33042.756760,6485.427735 +-8.808944,1182.670040,-6497.420717,16654.262319,-25366.662126,22834.826426,-6487.095397,-13209.812146,20810.705271,-10459.700195,-8519.204243,21274.613219,-20981.125252,12472.772818,-4346.690836,670.354771 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_50_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_50_26.csv new file mode 100644 index 0000000000..1e88d7379c --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_50_26.csv @@ -0,0 +1,3 @@ +29.911019,726.943395,-1105.699718,-2796.431812,11565.483479,-17701.840094,11750.969245,4937.560536,-18174.765585,14465.018647,3661.841514,-20042.249323,22081.638472,-12813.401386,3572.787531,-190.916451 +-1.682222,4099.682587,-21314.701963,55613.685711,-90407.761250,92405.624125,-42018.176551,-37260.345950,87876.166074,-65842.024954,-16176.416637,96712.603052,-120758.583671,86745.030653,-36815.504807,7206.809170 +-10.206269,359.139697,-2453.224924,6946.949925,-10811.116986,9035.709313,-828.471636,-7446.483639,8104.876135,-734.505417,-6804.209628,7306.245106,-1918.889011,-2586.903465,2769.378234,-916.785346 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_60_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_60_20.csv new file mode 100644 index 0000000000..db1d8078c2 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_60_20.csv @@ -0,0 +1,3 @@ +24.753463,-916.538273,6164.428724,-17702.722603,28765.091887,-26629.883344,6953.520960,16796.968220,-24374.275816,9608.133066,12890.495249,-23974.144593,18769.883597,-7512.956389,680.934337,426.102221 +-4.145624,1746.501979,-8526.321196,21575.209770,-34677.162158,35691.557982,-16955.486855,-13562.124364,34202.667017,-26815.169329,-5201.959750,38062.240641,-48924.737141,35885.675307,-15510.494272,3054.949357 +-5.361179,1660.778642,-8931.223903,23450.625744,-38570.124654,40736.509758,-20794.762766,-13657.129776,38954.405614,-33128.452666,-3117.302067,44024.835593,-60638.118466,46769.084009,-21176.934799,4470.401323 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_60_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_60_22.csv new file mode 100644 index 0000000000..9604a113c5 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_60_22.csv @@ -0,0 +1,3 @@ +28.472301,-440.801684,4583.622650,-15999.072308,29784.200294,-31795.489936,12922.859963,15961.786523,-30480.056807,17001.369561,12047.314527,-31509.619796,29209.972461,-14705.575222,3342.348428,17.117490 +-2.567262,3051.985280,-15873.744105,41861.816164,-69300.318515,72833.408271,-35401.084918,-27266.972553,70008.223887,-54897.125750,-10877.476559,77809.325844,-99143.765091,71963.638072,-30760.974906,6051.776589 +-6.937641,1315.230166,-7099.097168,18365.775743,-29176.145533,28978.265927,-12530.405530,-12050.006918,27188.058179,-20173.257204,-4868.384958,29781.345185,-37943.077993,28032.642919,-12303.707937,2525.921759 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_60_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_60_24.csv new file mode 100644 index 0000000000..79fb6b7d70 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_60_24.csv @@ -0,0 +1,3 @@ +29.614552,406.435630,309.226750,-5755.996170,15195.269547,-20009.581156,11342.582298,7308.337326,-20092.532522,14173.393392,5605.942820,-21668.703648,22259.870120,-12093.174730,2983.188482,-26.182526 +-2.034302,3610.233519,-18692.558567,48760.255475,-79539.024833,82005.346541,-38326.505900,-31963.701346,78263.717779,-60095.326624,-13101.079545,86639.240755,-109717.925573,79510.180304,-33978.588405,6688.528129 +-8.308190,821.357387,-4744.473432,12747.692118,-20440.704292,19747.245563,-7146.080109,-10080.770142,18488.962226,-10876.354146,-6314.442218,19389.316833,-20347.724807,12602.478421,-4543.047864,727.463786 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_60_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_60_26.csv new file mode 100644 index 0000000000..d3c4924589 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_60_26.csv @@ -0,0 +1,3 @@ +30.007431,304.557307,1092.977262,-8290.744409,19948.146782,-25562.321510,14658.989831,8645.108100,-25411.115027,19350.390422,5379.909730,-27858.927020,31591.581490,-19635.476391,6500.947657,-783.321967 +-1.696103,4793.509872,-25077.962478,65264.144435,-105241.764927,105999.345520,-46237.038194,-44692.432376,100243.590425,-72666.058980,-20565.974985,109482.907442,-134398.175862,95647.265615,-40368.180690,7888.587985 +-9.455116,-300.386242,1183.204326,-2567.236529,4184.357421,-5208.696189,4131.396807,-75.389242,-5089.400064,6991.841293,-2388.126864,-6481.208419,12892.153615,-12120.483704,6453.718826,-1589.133392 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_70_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_70_20.csv new file mode 100644 index 0000000000..05672f4818 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_70_20.csv @@ -0,0 +1,3 @@ +25.896189,-1076.892011,7226.990007,-20893.636516,34375.323377,-32517.867654,9408.030152,19652.377502,-30062.710071,12989.406318,14949.536527,-29949.119115,24607.444463,-10696.170669,1589.166607,341.475075 +-3.667217,2339.922241,-11882.399385,30798.435966,-50308.621975,52316.412700,-25049.240105,-19880.889339,50218.080870,-39126.112811,-7990.376146,55760.517586,-70959.337840,51563.716317,-22093.532964,4344.719823 +-5.306299,754.215339,-4067.388918,11038.441928,-19526.006188,23370.960818,-15721.011581,-3564.925759,22944.053322,-25251.063146,3766.475849,27709.464010,-45251.795530,38176.616874,-18396.400065,4068.813793 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_70_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_70_22.csv new file mode 100644 index 0000000000..ac59205ba2 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_70_22.csv @@ -0,0 +1,3 @@ +28.893764,-31.617144,2387.533961,-10482.905690,21635.654962,-24963.027384,11767.147301,11163.002022,-24435.634218,15123.452349,8516.708709,-25741.958272,24875.450985,-12870.240004,2965.061319,30.799219 +-2.509450,3055.073692,-15786.411512,41379.822902,-68186.903464,71499.860245,-34858.270250,-26478.794368,68674.418630,-54412.581890,-10060.233841,76563.702312,-98484.150878,71987.933281,-30956.079738,6121.767433 +-6.662196,1321.865441,-7331.553082,19509.655280,-31888.814074,32624.204278,-14832.019155,-13133.180864,30945.537537,-23191.034574,-5571.920897,33932.152231,-42711.358169,31081.416843,-13430.225843,2718.300465 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_70_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_70_24.csv new file mode 100644 index 0000000000..4cbb5ae961 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_70_24.csv @@ -0,0 +1,3 @@ +29.764366,483.203717,-177.969361,-4378.256485,12994.231747,-18116.276929,11171.540443,5687.914315,-18385.758194,14143.398908,4066.135567,-20231.834763,22097.394682,-12797.584222,3572.984686,-193.625552 +-2.008366,3893.322473,-20098.684550,51987.304593,-83806.130731,85030.714997,-38331.239841,-34379.528377,80689.144634,-60602.901041,-14577.360624,88912.049672,-111660.854769,80670.275996,-34447.294796,6785.713482 +-7.810505,472.652400,-3001.456901,8702.181532,-14994.877037,15758.217455,-7017.578430,-6922.739965,15185.567776,-10186.823642,-4207.716307,16292.158538,-17988.568981,11473.458001,-4225.101156,689.810387 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_70_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_70_26.csv new file mode 100644 index 0000000000..8efcc2022a --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_70_26.csv @@ -0,0 +1,3 @@ +30.063368,-138.884079,3446.092007,-14256.327018,29117.283862,-34117.369703,17634.182691,12949.285599,-33271.565713,24233.216573,7672.441100,-36190.163219,41131.244295,-26173.272052,9202.167789,-1312.906095 +-1.716121,5331.622669,-28071.082807,73136.359020,-117662.646407,117730.906744,-50178.656937,-50909.816397,111036.837738,-78776.926642,-24319.791815,120643.995527,-146275.913067,103317.369586,-43383.914564,8457.629336 +-8.837328,-770.519627,3796.880053,-9429.154576,14972.620972,-15329.229053,7423.593979,5425.077715,-14410.341239,12075.713455,1077.207963,-16085.241217,22759.731061,-18230.126819,8723.607578,-1986.202673 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_80_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_80_20.csv new file mode 100644 index 0000000000..e36f21c9df --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_80_20.csv @@ -0,0 +1,3 @@ +26.723738,-732.009032,5510.217684,-16882.185561,28880.746015,-28367.468931,9159.126858,16409.895783,-26568.049619,12250.158406,12705.893786,-26724.883881,22241.677209,-9582.962251,1235.800332,407.512186 +-3.414058,2658.075070,-13659.400458,35635.365971,-58452.766514,60957.707438,-29292.482475,-23087.085150,58529.683463,-45657.184389,-9283.116118,65002.020773,-82698.385761,60044.768954,-25702.805069,5060.346171 +-5.181992,464.774400,-2725.745987,8224.709022,-16337.214549,21983.806198,-17118.508195,-1063.473832,22124.304529,-26942.066988,5960.809438,27466.847304,-47220.634396,40608.792233,-19785.059185,4410.781422 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_80_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_80_22.csv new file mode 100644 index 0000000000..9d3d47f761 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_80_22.csv @@ -0,0 +1,3 @@ +29.144419,141.726542,1483.871218,-8340.416375,18811.905107,-23218.738842,12430.233070,8953.318545,-23123.577943,16072.406390,6525.301421,-24934.413534,25918.301625,-14533.135529,3966.784677,-216.427086 +-2.466565,2907.317270,-14852.326095,38604.146552,-63257.781313,66203.973458,-32485.805963,-24092.335123,63552.037571,-51123.364872,-8493.107888,71166.565748,-92746.209881,68429.334989,-29652.334151,5897.746847 +-6.368347,1454.002899,-8214.650860,22247.766852,-36920.803470,38207.456480,-17489.529365,-15524.237582,36407.614925,-26826.526552,-7163.857065,39731.197585,-48908.703117,34914.394942,-14829.187857,2958.661100 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_80_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_80_24.csv new file mode 100644 index 0000000000..81b86a46d3 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_80_24.csv @@ -0,0 +1,3 @@ +29.845718,501.229722,-343.314394,-3807.273192,11951.329794,-17119.099014,11040.802379,4797.503939,-17427.711280,14208.611897,3069.092223,-19456.741896,22315.373069,-13601.485369,4160.791115,-356.172918 +-1.920012,4085.870107,-21062.227356,54190.979195,-86643.074359,86837.613001,-37958.503198,-36191.227552,82018.255475,-60358.578404,-15816.670193,89971.066694,-112062.314769,80687.375470,-34413.008426,6779.571253 +-7.372246,142.130147,-1333.770028,4781.399400,-9609.131662,11636.129680,-6613.519707,-3919.235530,11701.730742,-9111.758577,-2286.975920,12918.738333,-15045.983140,9826.649575,-3656.323165,597.065980 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_80_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_80_26.csv new file mode 100644 index 0000000000..aa2ade62ed --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_80_26.csv @@ -0,0 +1,3 @@ +30.160559,-541.119673,5567.180692,-19582.698121,37172.462045,-41380.942794,19767.243293,17061.247383,-39903.974086,27718.491136,10244.023287,-43033.672420,48087.058179,-30432.764843,10757.794257,-1577.709277 +-1.936410,6076.961802,-32271.726996,84400.636163,-135933.416140,135771.131468,-57219.396641,-59563.361750,127968.087292,-89490.792549,-29313.405557,138558.273723,-166235.922175,116513.115440,-48619.060541,9441.770215 +-8.353145,-884.806032,4451.772252,-11115.570413,17412.550075,-17153.489948,7318.140892,7144.197865,-15902.146558,11904.637854,2506.425685,-17278.242210,22903.127013,-17743.360556,8327.524220,-1876.520046 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_90_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_90_20.csv new file mode 100644 index 0000000000..9ded9f98df --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_90_20.csv @@ -0,0 +1,3 @@ +27.266259,-653.319176,5251.635270,-16722.372891,29614.206031,-30420.549541,11441.562268,16026.586351,-28933.774947,15381.717166,12045.197943,-29752.961965,27028.576745,-13314.966995,2865.195844,84.414565 +-3.338313,2829.183699,-14594.846846,38146.471919,-62674.586561,65519.966119,-31714.046032,-24548.911850,62946.493055,-49469.107448,-9641.504718,70042.763185,-89528.641788,65183.682686,-27958.926752,5519.262061 +-5.047650,714.348752,-4129.527373,11942.589935,-22176.994300,27323.692680,-18527.845819,-4400.195421,27017.674304,-28949.967355,3484.602996,32255.429608,-51204.745110,42546.991480,-20315.515742,4472.099092 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_90_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_90_22.csv new file mode 100644 index 0000000000..370bc55723 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_90_22.csv @@ -0,0 +1,3 @@ +29.289201,317.914135,532.929258,-5963.345490,15385.061621,-20566.459196,12388.359540,6625.167947,-20844.867425,16033.607618,4549.818995,-22976.816426,25387.196377,-15058.973734,4503.048506,-377.366836 +-2.360521,2680.808011,-13474.226815,34608.389736,-56239.365716,58620.635919,-28893.777068,-20957.738859,56202.190683,-45954.238722,-6693.223965,63246.827712,-83675.586440,62409.081224,-27284.473943,5459.869556 +-6.077201,1329.718861,-7688.497202,21301.948500,-36148.809959,38293.051387,-18282.059712,-14982.701313,36750.667239,-27605.181917,-6869.278524,40216.326612,-49687.220092,35456.582113,-15040.567188,2998.788013 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_90_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_90_24.csv new file mode 100644 index 0000000000..5561cdf044 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_8_90_24.csv @@ -0,0 +1,3 @@ +29.883400,463.714242,-184.306318,-4124.638436,12357.941118,-17534.452276,11418.546717,4628.871190,-17771.421099,15033.093817,2496.467519,-20015.827942,23996.773912,-15401.131840,5176.093409,-610.016030 +-1.701726,3891.027233,-19920.392945,50927.199258,-80859.642720,80347.217217,-34467.492752,-33982.653136,75612.187482,-55184.903429,-14838.702893,82795.624619,-103128.170180,74428.236833,-31849.642932,6291.989952 +-6.994129,-321.281080,1099.864905,-1255.873054,-656.023155,3794.485042,-4525.148973,649.943510,4654.317605,-5513.751716,464.961195,5681.027333,-7571.392318,5100.155503,-1848.306669,270.613818 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_100_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_100_18.csv new file mode 100644 index 0000000000..96c8987c3a --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_100_18.csv @@ -0,0 +1,3 @@ +18.676729,-1264.177476,8116.847364,-24061.720179,42886.725709,-47589.120748,24826.477742,16600.540851,-46226.015870,37099.544229,6747.015719,-51592.268390,65355.900818,-46587.899101,19254.502774,-3633.111097 +-7.457167,1162.208054,-4647.924372,8693.677268,-8392.383846,1880.797559,5346.863162,-5750.333414,-446.539436,5275.005073,-3246.121607,-2061.102427,4150.607308,-2268.245753,288.170900,57.727950 +-3.839777,-193.060660,667.970431,-173.750676,-3506.213151,9456.022052,-11156.265911,2838.653923,10851.638126,-16211.416440,5085.366420,14456.271493,-25866.553868,21884.335900,-10319.707106,2210.206336 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_100_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_100_20.csv new file mode 100644 index 0000000000..76412f38d4 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_100_20.csv @@ -0,0 +1,3 @@ +28.928685,-186.637228,3098.236567,-12174.178846,24308.154720,-27760.063708,13159.624838,12220.761217,-27204.073082,17266.502823,9029.853761,-28848.466143,28641.517825,-15418.046164,3931.875977,-126.583229 +-2.609178,3287.110162,-17439.129932,46704.953395,-78501.911002,83956.928637,-42217.135535,-30225.018884,81163.283002,-64902.757603,-11628.951147,90540.583188,-116193.680598,84603.893200,-36245.335935,7159.482418 +-5.037719,3432.420766,-19107.123406,51152.747531,-83922.529454,85560.785392,-37803.103861,-35975.084536,81051.155049,-58382.843976,-16989.005546,88009.655540,-107457.987685,76514.755469,-32566.494304,6550.517939 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_100_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_100_22.csv new file mode 100644 index 0000000000..24ac4bb394 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_100_22.csv @@ -0,0 +1,3 @@ +30.399471,1176.072577,-3938.223027,5074.398583,-890.833347,-6539.638794,9123.288717,-2169.925026,-8241.940022,10521.535970,-1323.729842,-10353.736759,13743.441028,-8490.938420,2247.768937,-0.952420 +-1.802933,1772.533867,-8582.238856,22230.876190,-37729.615957,42698.009145,-25570.269330,-10456.807458,41998.553368,-40586.967992,713.937560,49363.644091,-72004.753124,56568.083960,-25613.204527,5247.630181 +-5.827714,3082.456700,-17556.586106,47783.338499,-79099.774420,80512.957506,-34250.759488,-35996.498916,76449.537770,-51543.869224,-19892.599658,81874.651837,-94059.298575,63482.363816,-25612.129927,4880.497262 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_100_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_100_24.csv new file mode 100644 index 0000000000..60e6b5e254 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_100_24.csv @@ -0,0 +1,3 @@ +30.786183,1161.053278,-3926.731899,5291.505225,-1655.174657,-5470.150334,8743.796452,-3081.147949,-7003.319277,10559.084777,-2720.161173,-9319.315078,14391.674653,-10169.346498,3485.267198,-356.741035 +-1.272997,2117.494052,-9874.934794,23858.841727,-36918.361200,37265.808299,-18347.809755,-12470.021883,35320.081791,-30933.626200,-1767.568916,40618.883152,-57737.671919,45300.466134,-20620.448091,4235.259183 +-6.654478,204.427001,-2251.245482,8863.222199,-19065.399451,24343.446767,-14815.056945,-7358.765714,24683.952437,-20121.765636,-3952.898566,27347.257056,-32957.946988,22297.463102,-8783.011698,1595.040824 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_20_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_20_26.csv new file mode 100644 index 0000000000..584e4b6153 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_20_26.csv @@ -0,0 +1,3 @@ +25.974225,1015.666849,-2992.467204,2773.935673,2039.617442,-8094.423278,8106.063731,98.365509,-9166.161738,9216.338919,498.165715,-10615.761612,12445.657525,-7217.596314,1871.489270,-31.505475 +-2.667512,657.642137,-2703.179997,7126.245592,-13054.167039,15963.254751,-10211.854834,-3716.454947,16125.761231,-15286.845377,-309.286635,18808.822502,-26126.875922,19595.890370,-8396.180016,1556.805655 +-13.625184,-120.421424,28.954670,833.933145,-1806.383934,1304.564759,791.393426,-2115.225471,785.349461,1732.270799,-2175.319452,-106.903583,2517.363355,-2816.805459,1575.732004,-405.290954 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_20_28.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_20_28.csv new file mode 100644 index 0000000000..1e5b60704f --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_20_28.csv @@ -0,0 +1,3 @@ +26.845994,1532.828899,-5347.527684,7927.033992,-4920.964331,-2215.407178,6151.263152,-2605.161038,-3871.831235,5213.655676,-214.853261,-4666.078225,4182.633921,-609.555548,-1283.240946,680.883210 +-1.932284,1447.352729,-7079.009940,19023.266995,-33059.114750,37041.734091,-20217.670085,-12035.056223,36416.953830,-30459.942713,-4269.677422,41098.809945,-53306.261822,38633.460956,-16270.099757,3071.315735 +-15.112305,136.395798,-1728.061100,6229.111347,-11580.468084,12137.595124,-4595.358769,-6396.621283,11456.643237,-5968.119679,-4802.725520,11606.251561,-10543.850904,5330.347125,-1266.903315,16.125103 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_20_30.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_20_30.csv new file mode 100644 index 0000000000..0bccd989ea --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_20_30.csv @@ -0,0 +1,3 @@ +27.792905,2021.530950,-7517.229524,12577.430478,-11112.760514,2947.781730,4535.974841,-5164.951350,866.420854,1924.021934,-1241.693161,610.931281,-2489.051221,4343.862506,-3500.519908,1155.821379 +-1.245996,2063.863171,-10201.559333,26848.559804,-45215.884678,48782.398841,-24880.107766,-17393.686632,47420.209160,-37999.396564,-6915.558569,53027.151995,-67534.053858,48555.926556,-20372.612701,3855.474532 +-16.998014,461.102045,-3753.319495,11842.222898,-20473.995993,20098.373206,-6315.823903,-11837.607749,18750.282418,-8285.433354,-9288.825451,18674.476565,-15088.909658,6236.479938,-688.352162,-297.204294 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_20_32.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_20_32.csv new file mode 100644 index 0000000000..0f330baefd --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_20_32.csv @@ -0,0 +1,3 @@ +28.579172,2283.482545,-8722.319853,15336.161599,-15124.008725,6615.610572,3262.190562,-7061.365286,4346.308035,-178.365987,-2404.450851,4329.658883,-6439.935854,6821.624140,-4432.119534,1324.506138 +-0.699544,523.671259,-1893.434088,6011.052712,-14337.188265,22277.640784,-18986.599466,-528.662633,23771.095050,-28027.184102,4366.005851,29499.109726,-46091.806285,36570.012514,-16307.617363,3185.579570 +-19.087099,-1048.680768,3557.095117,-5204.947644,3875.622497,-1146.093628,225.023129,-758.564331,-496.669624,3721.139319,-3971.441599,-1879.101799,9457.445307,-11480.622932,7123.389991,-1952.449125 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_30_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_30_22.csv new file mode 100644 index 0000000000..25fd294c80 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_30_22.csv @@ -0,0 +1,3 @@ +25.309470,-1669.387761,10981.783261,-31992.151644,54253.073175,-54816.788101,21398.922465,27019.614456,-51753.520331,30959.865372,17517.894713,-54218.939851,56265.757763,-34045.772939,11814.672909,-1783.766136 +-4.172404,945.413138,-4304.679424,11089.680910,-18898.630562,21209.005447,-11956.810515,-6362.736319,20932.088712,-18333.067282,-1625.666770,23919.073847,-32178.756384,23973.532012,-10367.085230,1997.059932 +-7.761773,325.158012,-1278.902809,2131.975079,-1775.495006,542.794064,78.948539,268.816740,-84.290390,-1378.888860,2130.162695,253.027120,-4393.635793,6061.707737,-3891.521886,1044.038135 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_30_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_30_24.csv new file mode 100644 index 0000000000..6f2705229a --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_30_24.csv @@ -0,0 +1,3 @@ +27.394173,-1012.379744,7880.021550,-24787.220976,43615.968522,-44580.409767,16690.506875,23383.485655,-42194.382238,22689.673477,17094.503257,-43324.985122,40373.112903,-21078.193522,5544.459979,-353.649566 +-2.781509,3390.878560,-18022.834180,48159.591320,-79984.597154,83170.452404,-38281.251649,-33926.743855,79662.542118,-58291.821765,-16497.059695,87022.993062,-105354.281066,73671.890616,-30477.671582,5814.761714 +-9.833501,425.597547,-2045.511778,4314.213565,-4888.955681,2401.143987,1241.339052,-2719.805472,1331.388186,600.361920,-1142.413862,981.438260,-1230.196513,1419.663279,-850.965197,193.842050 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_30_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_30_26.csv new file mode 100644 index 0000000000..24322d0346 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_30_26.csv @@ -0,0 +1,3 @@ +29.115379,458.495513,453.224373,-6778.400911,17070.010385,-21061.016675,9888.288799,10124.415688,-20744.479982,11143.042675,9127.416644,-21097.781644,17067.376101,-6144.276673,-321.129382,763.162034 +-1.823944,4102.634417,-21788.915536,58018.120638,-95997.487205,99475.601624,-45630.483819,-40556.243750,95147.813038,-69839.979828,-19406.659968,104075.555662,-126558.263119,88813.867414,-36845.555242,7052.157426 +-11.887467,1081.016987,-5889.135829,14572.168534,-20670.310790,15982.326379,-991.656600,-12724.029036,13634.521612,-2347.836142,-9590.156051,12587.247788,-7479.996189,1515.365572,829.516236,-479.185161 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_30_28.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_30_28.csv new file mode 100644 index 0000000000..11b2d04323 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_30_28.csv @@ -0,0 +1,3 @@ +30.239613,1418.729106,-4391.048553,4925.358369,25.420525,-6500.453760,6640.413552,774.973092,-7583.206175,5678.082691,2637.715800,-7975.279989,5542.901827,-91.384251,-2174.199108,1020.750144 +-1.221783,3196.790651,-16833.671371,45511.722782,-77448.102154,83723.067671,-42596.192118,-29884.745488,81101.691509,-64915.308135,-11692.825302,90470.835024,-115681.933116,83729.228960,-35528.754260,6905.900448 +-13.841844,478.935186,-3309.762247,9248.228342,-13747.008051,9983.904603,1766.286838,-11122.727407,8327.315030,3517.745974,-10963.770767,6105.495323,4695.688810,-9896.938726,6877.503646,-1941.932744 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_40_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_40_20.csv new file mode 100644 index 0000000000..72b78afded --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_40_20.csv @@ -0,0 +1,3 @@ +23.257673,-381.733192,3193.427698,-10038.584913,17355.698594,-17407.115127,6408.136806,8914.114224,-16181.835339,9189.781800,5799.688242,-16747.750174,17076.878901,-10159.425760,3326.934259,-407.381286 +-5.356647,423.375723,-857.919646,120.304581,2240.171202,-4182.922423,3063.287974,967.329957,-4260.339779,3520.730819,689.838040,-4584.100159,5281.016901,-3353.933163,1254.732177,-290.285425 +-5.584373,2081.945448,-11236.087304,29530.740746,-48283.861470,49968.070085,-23646.950580,-19122.330855,47607.740148,-37008.562706,-7241.349035,52488.789130,-67938.373927,50644.348414,-22520.913838,4731.115833 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_40_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_40_22.csv new file mode 100644 index 0000000000..83b9e20c6c --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_40_22.csv @@ -0,0 +1,3 @@ +27.373727,-2048.249506,13038.274876,-36956.646286,60622.629672,-57978.884885,18209.176062,33342.271668,-53869.717042,25824.141756,24294.934990,-54453.309634,48478.751089,-24313.613449,6048.979007,-300.420345 +-3.350148,4478.641612,-23976.280960,63593.967426,-104206.116720,106299.647317,-46719.677148,-45418.526029,101177.371948,-71604.233473,-23012.237751,109746.544150,-130720.312637,90614.089049,-37317.480899,7132.608078 +-7.708399,1041.474974,-5258.747242,12732.276214,-19072.305285,18175.431229,-7916.207827,-6804.823974,16645.310710,-14021.021464,-871.923097,18813.057455,-27585.664071,22535.728794,-10705.698740,2337.760525 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_40_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_40_24.csv new file mode 100644 index 0000000000..a521674ee7 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_40_24.csv @@ -0,0 +1,3 @@ +29.612105,316.103658,842.697582,-6896.623267,15888.972604,-18356.146419,7274.269871,10272.180575,-17860.185723,7647.382490,9775.116979,-17643.033666,11494.499271,-1524.875817,-2553.668727,1272.743884 +-2.213415,4444.167135,-23773.265520,63500.371013,-105400.712656,109821.277816,-51250.257191,-43800.726150,105220.951562,-78545.831177,-20234.625531,115530.321706,-142027.010234,100459.905818,-41989.452024,8113.996856 +-9.573734,1708.555888,-9117.139153,22833.423272,-34075.905245,29985.502766,-7929.771628,-17739.671631,27091.655737,-13302.559249,-11225.974643,27643.130144,-27404.649168,16513.021489,-5886.899957,944.135034 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_40_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_40_26.csv new file mode 100644 index 0000000000..3c9261586d --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_40_26.csv @@ -0,0 +1,3 @@ +30.685786,1030.375195,-2622.487033,1054.741350,5154.304396,-10464.550216,7245.675260,3473.848232,-11110.098601,7101.910731,4395.725773,-11594.867015,8814.162140,-1813.954844,-1706.495758,987.767639 +-1.628668,4483.069978,-23728.857317,62978.658534,-104178.829742,108539.990919,-51129.563342,-42493.843549,103963.785378,-78949.561414,-18603.059744,114691.032758,-142997.088345,102186.766597,-43065.426047,8373.107061 +-11.242232,511.563825,-2974.899323,7400.214830,-9820.465343,5743.251186,3105.463115,-8406.056649,4317.388223,4766.950104,-8384.901406,2350.881392,6647.101150,-9723.596972,6143.608041,-1657.610118 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_50_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_50_20.csv new file mode 100644 index 0000000000..28f6f85870 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_50_20.csv @@ -0,0 +1,3 @@ +24.702384,-1336.658710,8268.393071,-22600.499341,35372.496721,-31416.110194,6904.850018,20890.583531,-28362.598764,9980.380606,15941.561225,-27580.477755,20681.195169,-7731.341381,370.185782,563.546351 +-4.649381,2834.291324,-14709.562469,38322.051635,-62142.432949,63175.532682,-28035.898209,-26448.481918,60092.211441,-43511.463451,-12552.327977,65492.735612,-79816.701232,56524.019797,-23801.310615,4635.231267 +-5.787691,3317.431113,-18068.594520,47535.471401,-77076.134034,78211.538334,-34939.472017,-32068.477275,74064.713312,-54879.627855,-13946.337803,81138.838295,-101368.514599,73256.908240,-31465.417242,6353.304461 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_50_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_50_22.csv new file mode 100644 index 0000000000..7fe3e627a3 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_50_22.csv @@ -0,0 +1,3 @@ +28.920999,-195.482698,3044.837733,-11207.652155,20428.069102,-20014.579897,5059.636504,13647.462539,-18655.018405,5205.107346,12491.856893,-17657.825895,8937.844586,1275.188657,-4012.723106,1603.580894 +-2.719360,4538.350397,-24315.036283,64786.890881,-107097.304997,110964.201262,-51089.347775,-44943.928928,106147.412744,-78384.167031,-21160.638507,116249.715893,-142091.917348,100214.186290,-41840.557344,8092.905448 +-7.530397,1171.930440,-6204.827976,15805.615420,-24912.637644,24898.843342,-11398.018366,-9420.908530,23349.754336,-18811.716415,-2621.025346,26116.071352,-35499.489827,27339.890356,-12353.227964,2584.558331 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_50_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_50_24.csv new file mode 100644 index 0000000000..214a38cd05 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_50_24.csv @@ -0,0 +1,3 @@ +30.494315,729.904860,-1336.558543,-1574.624518,8382.625979,-12681.529423,7261.279220,5301.086438,-13051.877816,7511.913473,5726.195453,-13504.059645,10071.738621,-2159.829345,-1774.610749,1044.445339 +-2.051683,4433.898481,-23597.672050,62867.656617,-104424.801964,109442.827075,-52319.208831,-42102.959866,105048.253335,-80672.519724,-18019.385377,116163.646419,-145684.082900,104497.544763,-44192.798985,8631.548840 +-9.071596,1319.152348,-7240.888131,18588.402841,-28372.955924,25527.196546,-7048.034031,-15146.831815,23353.143600,-11124.856285,-10285.839896,23709.075378,-22165.863668,12259.474445,-3811.057835,470.829012 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_50_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_50_26.csv new file mode 100644 index 0000000000..cef5a3e1b2 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_50_26.csv @@ -0,0 +1,3 @@ +31.060801,990.621626,-2469.365263,686.454456,5975.407553,-11993.238143,9088.141713,2739.254785,-12760.897545,10242.814985,2880.364370,-14090.005945,14258.838192,-6833.396015,790.455029,434.519028 +-1.646246,5021.450893,-26415.112621,69271.794465,-112927.259640,115627.672691,-52648.938254,-46687.870845,110074.561956,-82247.401470,-20609.234190,121079.752519,-150508.052132,107659.714678,-45503.658813,8888.213823 +-10.330287,-513.343117,2460.125333,-6259.899734,10779.410758,-12721.045881,8431.636338,2051.774774,-12407.911875,13592.117065,-2079.028561,-14841.293719,24586.782830,-21183.850218,10600.609674,-2485.182339 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_60_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_60_20.csv new file mode 100644 index 0000000000..cc6edd8648 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_60_20.csv @@ -0,0 +1,3 @@ +26.388609,-795.506744,5479.885334,-15606.991182,24467.510621,-20694.451029,2254.106191,16529.776745,-18351.095097,2333.006293,14431.516801,-16574.407259,6141.356897,3416.828985,-4777.993351,1698.445351 +-3.781158,3927.661619,-20862.268982,55076.367008,-90165.463822,92389.296848,-41528.473348,-38325.742275,88086.018784,-64027.546663,-18365.742728,96115.647178,-116783.100476,82248.332122,-34385.625091,6666.926700 +-5.768275,2153.378713,-11829.354172,31658.634091,-52884.064272,56520.126502,-29186.899420,-18837.461798,54221.372312,-45983.871965,-4655.554731,61249.473152,-83562.370148,63837.670946,-28618.944480,5981.696855 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_60_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_60_22.csv new file mode 100644 index 0000000000..26fbaf7485 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_60_22.csv @@ -0,0 +1,3 @@ +29.680791,244.739081,855.880562,-6212.613938,14013.893342,-16033.292133,6250.654526,9080.803680,-15631.787470,6581.063213,8715.923141,-15490.706507,9826.153893,-867.466927,-2653.697439,1268.026594 +-2.474120,4374.818396,-23403.540121,62489.046517,-103871.771918,108778.904872,-51703.393780,-42293.914157,104412.407798,-79464.981425,-18631.985093,115158.988968,-143441.310523,102442.553988,-43204.512130,8430.150340 +-7.209450,1943.969465,-10643.341134,27919.465682,-44806.781926,44591.496558,-18770.155474,-19492.212753,41982.828207,-29526.608493,-9378.086926,45494.769837,-55029.915234,38910.967780,-16381.271775,3232.592910 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_60_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_60_24.csv new file mode 100644 index 0000000000..96a7cdb2cb --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_60_24.csv @@ -0,0 +1,3 @@ +30.800922,969.580129,-2637.206380,1658.213017,3820.442081,-9374.257116,7614.055675,1964.753185,-10303.877969,8106.215929,2757.599271,-11366.729662,10410.125997,-3799.414247,-615.503193,738.806205 +-1.976464,4466.825240,-23526.114283,61995.415216,-101925.537978,105842.298421,-50028.500307,-40919.432435,101280.170930,-77931.589294,-17002.483652,112146.159313,-141568.435179,102187.446519,-43487.247938,8543.701345 +-8.452686,961.007951,-5541.940723,14925.709549,-24007.494313,23177.074421,-8158.986249,-12243.556118,21773.857451,-12135.387400,-8200.907527,22626.250721,-22512.613947,13081.212328,-4304.430433,589.785195 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_60_26.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_60_26.csv new file mode 100644 index 0000000000..2f8a29da2e --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_60_26.csv @@ -0,0 +1,3 @@ +31.138258,588.155569,-380.181476,-4529.944508,13960.592970,-19576.123606,12066.021511,6117.080499,-19782.085684,15264.164743,4296.956265,-21790.053514,23920.095594,-13870.915581,3829.732866,-179.670196 +-1.670907,5804.430833,-30539.096440,79490.387111,-127963.011335,128503.351065,-55623.158909,-54625.410209,121469.611807,-87486.682229,-25500.154913,132539.051253,-161911.203414,114775.127936,-48258.573626,9406.379230 +-9.504587,-1283.957197,6550.932791,-16474.482369,25908.082662,-25710.978331,11312.032518,10328.950692,-23988.414179,18459.152863,3447.075897,-26354.987276,35094.669420,-27046.781042,12549.945706,-2783.778862 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_70_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_70_20.csv new file mode 100644 index 0000000000..b20aec6f79 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_70_20.csv @@ -0,0 +1,3 @@ +27.513054,-399.645888,3601.618471,-11487.341341,19335.178498,-17544.343953,3045.712762,13201.151860,-15994.698378,2948.031994,12018.819803,-14745.394926,5797.599904,3043.086405,-4520.143750,1651.782477 +-3.323174,4241.784209,-22625.773100,59941.337642,-98516.776519,101482.684493,-46232.940328,-41499.516115,96932.100419,-71189.754273,-19601.925502,106033.903076,-129449.530794,91362.073086,-38223.881994,7414.648999 +-5.627266,1808.186559,-10094.779197,27531.634663,-47023.916350,51699.062354,-28285.195071,-15621.665435,49912.561283,-44297.495245,-2434.396808,56916.264257,-79860.374246,62044.010039,-28193.707685,5963.855552 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_70_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_70_22.csv new file mode 100644 index 0000000000..f2a0f163ca --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_70_22.csv @@ -0,0 +1,3 @@ +30.080721,611.110931,-1009.594651,-1848.480774,8242.575758,-12250.895741,7100.508986,4957.939614,-12658.390560,7616.525049,5233.798866,-13263.582275,10430.534144,-2790.697595,-1361.500970,935.132992 +-2.338594,3997.482168,-21256.740162,56719.676508,-94618.240424,99997.301332,-48925.337292,-37292.203913,96263.971763,-75450.205459,-15120.519076,106937.966057,-135788.829774,98220.089871,-41845.970221,8231.079124 +-6.839408,2304.315911,-12805.011252,34053.378263,-55256.529316,55357.558057,-23207.133993,-24650.433533,52284.799532,-35835.650978,-12794.599010,56304.181484,-66249.575489,45728.257447,-18828.321759,3644.003262 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_70_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_70_24.csv new file mode 100644 index 0000000000..de75ba8c6e --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_70_24.csv @@ -0,0 +1,3 @@ +30.935652,1126.005868,-3556.263255,4138.796380,-53.340523,-6002.441161,7075.150596,-566.243555,-7271.037708,7488.054062,615.212795,-8613.231670,9010.460167,-3780.062754,-289.080573,618.826695 +-1.957286,4687.393285,-24512.771604,63922.835400,-103809.785474,106232.024982,-48786.011150,-42205.977107,101139.941011,-76727.649838,-17749.842414,111701.787000,-140560.422418,101482.177053,-43257.383289,8517.815460 +-7.890941,814.069294,-4980.795309,14194.999544,-24224.784701,25167.242049,-10789.945787,-11567.869584,24211.064010,-15531.856216,-7410.131180,25738.577226,-27464.586157,16955.199156,-6025.557629,944.494652 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_80_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_80_20.csv new file mode 100644 index 0000000000..592ff40f82 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_80_20.csv @@ -0,0 +1,3 @@ +28.229934,-75.521423,2114.256849,-8439.225793,16087.221194,-16569.477493,5155.921036,10390.459260,-15754.957999,5622.397097,9520.867917,-15362.808744,9152.154785,-369.575568,-2794.631595,1271.817203 +-3.038612,4342.016220,-23226.670514,61751.351512,-101963.059276,105740.571614,-49019.946324,-42387.732297,101210.198808,-75402.176617,-19518.173006,111066.356096,-136673.858806,96930.803318,-40702.871390,7923.373199 +-5.435851,2364.579213,-13219.530914,35831.450184,-60209.550670,64141.202183,-32254.645472,-22603.857071,61445.360505,-50179.595527,-7196.374149,68630.854583,-91111.107489,68534.477541,-30439.859866,6333.371600 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_80_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_80_22.csv new file mode 100644 index 0000000000..a595e721e0 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_80_22.csv @@ -0,0 +1,3 @@ +30.309479,949.000056,-2767.527257,2389.095838,2364.981554,-7936.836980,7256.483809,1041.464525,-9053.937575,7689.908913,2051.542172,-10192.129357,9553.773212,-3492.791947,-627.249374,717.409213 +-2.236517,3491.709718,-18339.318192,48717.287989,-81383.736445,86747.968630,-43755.454815,-30773.212832,83738.485962,-67886.630671,-10987.378183,93832.350159,-121943.206452,89559.789494,-38615.686008,7662.748674 +-6.476809,2883.542650,-16194.908965,43505.984774,-71205.604406,71795.318238,-30212.123430,-32143.358185,67985.775217,-46086.512285,-17314.912398,73005.427189,-84656.472322,57620.711026,-23406.372102,4477.117198 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_80_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_80_24.csv new file mode 100644 index 0000000000..df965b9249 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_80_24.csv @@ -0,0 +1,3 @@ +31.057825,1174.177486,-3963.319882,5546.862201,-2786.480855,-2885.575351,5566.759943,-1891.404974,-4240.554788,5488.170031,-255.867728,-5445.790978,5582.350692,-1587.096023,-1141.766468,780.353244 +-2.101292,5340.704375,-27974.649765,72607.081485,-116871.487162,117912.283933,-52165.361073,-48778.182501,111711.892452,-82393.205039,-21652.977148,122622.296501,-152072.464446,108892.018125,-46158.929814,9060.339883 +-7.428808,747.162033,-4878.548619,14746.328087,-26667.299546,29609.365723,-14719.827718,-11709.431641,29022.901722,-20904.520029,-6875.964761,31510.350817,-35947.685153,23467.552845,-8902.902726,1534.754108 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_90_18.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_90_18.csv new file mode 100644 index 0000000000..003239d5ef --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_90_18.csv @@ -0,0 +1,3 @@ +18.432907,-105.786769,1698.226642,-7090.241513,15736.814858,-21286.255978,15235.546335,3160.103884,-21505.397610,22594.300854,-1813.904834,-25668.734492,38406.567887,-30099.738900,13337.501336,-2659.867947 +-7.474005,188.883840,931.708217,-6648.276550,17407.601005,-25113.691582,17912.785198,5061.117727,-26225.270724,24410.377965,1754.992104,-30253.659877,38819.476018,-26890.739159,10677.461274,-2003.117850 +-3.964340,-564.217502,2810.112628,-6144.280612,6772.088539,-1775.126523,-5187.689965,6420.930032,85.236617,-6906.636477,5824.861191,2274.547025,-9009.423358,8880.490432,-4432.385121,965.541075 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_90_20.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_90_20.csv new file mode 100644 index 0000000000..69c056f457 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_90_20.csv @@ -0,0 +1,3 @@ +28.653423,-56.394907,2213.352825,-9324.008225,18717.443360,-20831.914450,8758.710800,10567.653990,-20269.207747,10781.646333,8843.313506,-20856.559269,17557.735553,-7008.907130,206.329169,645.110263 +-2.834311,3991.174112,-21320.864673,56876.049042,-94552.159987,99176.042599,-47399.327739,-38288.240900,95277.661111,-72866.043860,-16674.904961,105179.888980,-131428.193382,94118.414229,-39816.664222,7795.086836 +-5.234945,2997.534298,-16735.083784,45071.815806,-74725.813976,77620.397523,-36280.305758,-30455.576798,73898.376024,-56158.234475,-12706.461695,81190.683489,-102663.786736,74836.888059,-32444.767693,6622.831911 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_90_22.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_90_22.csv new file mode 100644 index 0000000000..37b040ba20 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_90_22.csv @@ -0,0 +1,3 @@ +30.449264,1284.014532,-4579.384685,6980.522887,-4490.686709,-2092.650676,6156.593568,-2974.562818,-3836.801552,5959.080907,-926.538668,-5163.826480,5730.587469,-1898.529616,-912.752265,707.520595 +-2.171240,2995.643601,-15456.169550,40726.045741,-67978.019062,73020.764240,-37993.565787,-24443.639012,70666.657342,-59430.535610,-7185.140540,79966.796737,-106653.989486,79646.848045,-34784.562430,6964.286332 +-6.150660,3557.216640,-20105.030313,54334.303834,-89375.580356,90441.490604,-38125.161460,-40631.040822,85758.638664,-57748.776668,-22337.701094,91930.505381,-105706.349568,71360.289428,-28756.216037,5462.344690 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_90_24.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_90_24.csv new file mode 100644 index 0000000000..79d1b4c647 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/coeffs_robust_9_90_24.csv @@ -0,0 +1,3 @@ +31.204926,871.976056,-2523.680231,2365.345188,1238.906756,-5469.152388,5087.656032,754.598381,-6331.804619,5136.831840,1888.089689,-7238.566627,5759.045702,-704.002341,-1902.048539,1008.386733 +-2.458544,6633.475107,-35101.310978,91342.160816,-146698.351230,146785.693938,-62943.528956,-63017.760146,138688.516588,-99088.270139,-29934.916316,151127.265935,-183508.437802,129497.697188,-54257.530355,10566.148554 +-7.036408,412.701948,-3332.575667,11637.544791,-23632.924833,29372.472477,-17791.765378,-8554.331179,29592.240753,-24994.016298,-3714.222381,33188.790752,-41725.847345,29281.059898,-11945.035372,2241.715121 diff --git a/src/modules/fw_dyn_soar_control/trajectories/robust/min_aspd_matrix.csv b/src/modules/fw_dyn_soar_control/trajectories/robust/min_aspd_matrix.csv new file mode 100644 index 0000000000..0cf6e64cc9 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/robust/min_aspd_matrix.csv @@ -0,0 +1,5 @@ +28.000000,24.000000,22.000000,20.000000,20.000000,20.000000,20.000000,20.000000,20.000000 +26.000000,22.000000,20.000000,20.000000,20.000000,20.000000,20.000000,18.000000,18.000000 +24.000000,22.000000,20.000000,20.000000,20.000000,20.000000,18.000000,18.000000,18.000000 +26.000000,22.000000,20.000000,20.000000,20.000000,18.000000,18.000000,18.000000,18.000000 +24.000000,22.000000,20.000000,20.000000,18.000000,18.000000,18.000000,18.000000,18.000000 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv new file mode 100644 index 0000000000..f43db57906 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv @@ -0,0 +1,3 @@ +0.000064,3020.233571,-10609.960177,17956.458964,-15735.479961,2399.573434,11421.854705,-12388.936542,-120.944433,12530.869640,-11346.431128,-2643.369342,15999.009519,-18127.094775,10676.696033,-3032.667571 +-99.999753,-4686.100638,21963.998712,-50566.542735,71908.811380,-61683.065487,15730.546681,39386.062419,-63952.599921,39525.510557,15526.730614,-61505.752731,71804.582585,-50525.803342,21954.858744,-4685.311430 +0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv new file mode 100644 index 0000000000..0083d14f4a --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv @@ -0,0 +1,3 @@ +0.000038,1812.140143,-6365.976106,10773.875378,-9441.287977,1439.744061,6853.112823,-7433.361925,-72.566660,7518.521784,-6807.858677,-1586.021605,9599.405711,-10876.256865,6406.017620,-1819.600542 +-59.999852,-2811.660383,13178.399227,-30339.925641,43145.286828,-37009.839292,9438.328009,23631.637452,-38371.559953,23715.306334,9316.038368,-36903.451639,43082.749551,-30315.482005,13172.915247,-2811.186858 +0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv new file mode 100644 index 0000000000..c33415e34c --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv @@ -0,0 +1,3 @@ +0.000038,1812.140143,-6365.976106,10773.875378,-9441.287977,1439.744061,6853.112823,-7433.361925,-72.566660,7518.521784,-6807.858677,-1586.021605,9599.405711,-10876.256865,6406.017620,-1819.600542 +-59.999852,-2811.660383,13178.399227,-30339.925641,43145.286828,-37009.839292,9438.328009,23631.637452,-38371.559953,23715.306334,9316.038368,-36903.451639,43082.749551,-30315.482005,13172.915247,-2811.186858 +20.000000,-0.002651,-0.002417,-0.056259,0.064816,-0.087643,0.012642,0.019224,0.006944,0.012035,0.030712,-0.080564,0.137899,-0.038476,0.010115,-0.002374 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv new file mode 100644 index 0000000000..82cde96a85 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv @@ -0,0 +1,3 @@ +0.000064,3020.233571,-10609.960177,17956.458964,-15735.479961,2399.573434,11421.854705,-12388.936542,-120.944433,12530.869640,-11346.431128,-2643.369342,15999.009519,-18127.094775,10676.696033,-3032.667571 +-99.999753,-4686.100638,21963.998712,-50566.542735,71908.811380,-61683.065487,15730.546681,39386.062419,-63952.599921,39525.510557,15526.730614,-61505.752731,71804.582585,-50525.803342,21954.858744,-4685.311430 +20.000000,-0.002651,-0.002417,-0.056259,0.064816,-0.087643,0.012642,0.019224,0.006944,0.012035,0.030712,-0.080564,0.137899,-0.038476,0.010115,-0.002374 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv new file mode 100644 index 0000000000..71a8ec8126 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv @@ -0,0 +1,3 @@ +0.000038,1812.140143,-6365.976106,10773.875378,-9441.287977,1439.744061,6853.112823,-7433.361925,-72.566660,7518.521784,-6807.858677,-1586.021605,9599.405711,-10876.256865,6406.017620,-1819.600542 +-59.999852,-2811.660383,13178.399227,-30339.925641,43145.286828,-37009.839292,9438.328009,23631.637452,-38371.559953,23715.306334,9316.038368,-36903.451639,43082.749551,-30315.482005,13172.915247,-2811.186858 +-0.000019,-906.070071,3182.988053,-5386.937689,4720.643988,-719.872030,-3426.556411,3716.680963,36.283330,-3759.260892,3403.929338,793.010803,-4799.702856,5438.128432,-3203.008810,909.800271 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv new file mode 100644 index 0000000000..964e032f5d --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv @@ -0,0 +1,3 @@ +0.000025,1208.093428,-4243.984071,7182.583586,-6294.191984,959.829374,4568.741882,-4955.574617,-48.377773,5012.347856,-4538.572451,-1057.347737,6399.603807,-7250.837910,4270.678413,-1213.067028 +-39.999901,-1874.440255,8785.599485,-20226.617094,28763.524552,-24673.226195,6292.218672,15754.424968,-25581.039968,15810.204223,6210.692245,-24602.301093,28721.833034,-20210.321337,8781.943498,-1874.124572 +-0.000008,-362.428029,1273.195221,-2154.775076,1888.257595,-287.948812,-1370.622565,1486.672385,14.513332,-1503.704357,1361.571735,317.204321,-1919.881142,2175.251373,-1281.203524,363.920108 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv new file mode 100644 index 0000000000..da5d2bb0b4 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv @@ -0,0 +1,3 @@ +0.000016,755.058393,-2652.490044,4489.114741,-3933.869990,599.893359,2855.463676,-3097.234136,-30.236108,3132.717410,-2836.607782,-660.842335,3999.752380,-4531.773694,2669.174008,-758.166893 +-24.999938,-1171.525159,5490.999678,-12641.635684,17977.202845,-15420.766372,3932.636670,9846.515605,-15988.149980,9881.377639,3881.682653,-15376.438183,17951.145646,-12631.450836,5488.714686,-1171.327857 +-0.000005,-226.517518,795.747013,-1346.734422,1180.160997,-179.968008,-856.639103,929.170241,9.070832,-939.815223,850.982335,198.252701,-1199.925714,1359.532108,-800.752202,227.450068 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv new file mode 100644 index 0000000000..4135794c29 --- /dev/null +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv @@ -0,0 +1,3 @@ +25.647286,1322.946130,-6288.219118,15695.694585,-26100.811348,29190.042729,-17320.882010,-7206.280655,28417.502641,-27570.615375,1092.280900,32845.587042,-49815.421890,41317.083747,-20391.608674,4792.959996 +-4.022804,5578.254991,-29867.374126,78424.519794,-126648.076540,126732.911714,-53339.402193,-56007.887034,119673.494918,-82844.941058,-28429.087152,129249.441369,-153381.353797,106507.489754,-44042.229641,8473.887609 +-7.320205,2064.611816,-11998.518948,33890.176305,-59497.114479,66640.989601,-36741.103291,-20637.068567,64946.871726,-56132.549319,-5332.110902,73702.059578,-99372.156096,74574.323597,-32725.856186,6681.314060