diff --git a/Simulation/6DOF_RK4/LookUp/IGRF13coeffs.csv b/Simulation/6DOF_RK4/Configurations/LookUp/IGRF13coeffs.csv similarity index 100% rename from Simulation/6DOF_RK4/LookUp/IGRF13coeffs.csv rename to Simulation/6DOF_RK4/Configurations/LookUp/IGRF13coeffs.csv diff --git a/Simulation/6DOF_RK4/LookUp/RASAero.csv b/Simulation/6DOF_RK4/Configurations/LookUp/RASAero.csv similarity index 100% rename from Simulation/6DOF_RK4/LookUp/RASAero.csv rename to Simulation/6DOF_RK4/Configurations/LookUp/RASAero.csv diff --git a/Simulation/6DOF_RK4/LookUp/flight_computer_20221029.csv b/Simulation/6DOF_RK4/Configurations/LookUp/flight_computer_20221029.csv similarity index 100% rename from Simulation/6DOF_RK4/LookUp/flight_computer_20221029.csv rename to Simulation/6DOF_RK4/Configurations/LookUp/flight_computer_20221029.csv diff --git a/Simulation/6DOF_RK4/LookUp/j244.csv b/Simulation/6DOF_RK4/Configurations/LookUp/j244.csv similarity index 100% rename from Simulation/6DOF_RK4/LookUp/j244.csv rename to Simulation/6DOF_RK4/Configurations/LookUp/j244.csv diff --git a/Simulation/6DOF_RK4/LookUp/m2500.csv b/Simulation/6DOF_RK4/Configurations/LookUp/m2500.csv similarity index 100% rename from Simulation/6DOF_RK4/LookUp/m2500.csv rename to Simulation/6DOF_RK4/Configurations/LookUp/m2500.csv diff --git a/Simulation/6DOF_RK4/LookUp/n2540.csv b/Simulation/6DOF_RK4/Configurations/LookUp/n2540.csv similarity index 100% rename from Simulation/6DOF_RK4/LookUp/n2540.csv rename to Simulation/6DOF_RK4/Configurations/LookUp/n2540.csv diff --git a/Simulation/6DOF_RK4/LookUp/n5800.csv b/Simulation/6DOF_RK4/Configurations/LookUp/n5800.csv similarity index 100% rename from Simulation/6DOF_RK4/LookUp/n5800.csv rename to Simulation/6DOF_RK4/Configurations/LookUp/n5800.csv diff --git a/Simulation/6DOF_RK4/properties/README.md b/Simulation/6DOF_RK4/Configurations/README.md similarity index 100% rename from Simulation/6DOF_RK4/properties/README.md rename to Simulation/6DOF_RK4/Configurations/README.md diff --git a/Simulation/6DOF_RK4/properties/configs/legacy_properties.yaml b/Simulation/6DOF_RK4/Configurations/configs/legacy_properties.yaml similarity index 86% rename from Simulation/6DOF_RK4/properties/configs/legacy_properties.yaml rename to Simulation/6DOF_RK4/Configurations/configs/legacy_properties.yaml index 45df8fd..eb82398 100644 --- a/Simulation/6DOF_RK4/properties/configs/legacy_properties.yaml +++ b/Simulation/6DOF_RK4/Configurations/configs/legacy_properties.yaml @@ -31,5 +31,5 @@ sensors: bno: error: !eval 2.5/3 meta: - output_file: '../output/simulated_6dof.csv' - monte_carlo_output_folder: '../output/monte_carlo' \ No newline at end of file + output_file: '../../output/simulated_6dof.csv' + monte_carlo_output_folder: '../../output/monte_carlo' \ No newline at end of file diff --git a/Simulation/6DOF_RK4/properties/configs/staging_rocket.yaml b/Simulation/6DOF_RK4/Configurations/configs/staging_rocket.yaml similarity index 85% rename from Simulation/6DOF_RK4/properties/configs/staging_rocket.yaml rename to Simulation/6DOF_RK4/Configurations/configs/staging_rocket.yaml index 2268321..65aced0 100644 --- a/Simulation/6DOF_RK4/properties/configs/staging_rocket.yaml +++ b/Simulation/6DOF_RK4/Configurations/configs/staging_rocket.yaml @@ -6,7 +6,7 @@ rocket: <<: *simulated_stage rocket_body: <<: *simulated_rocket_body - rasaero_lookup_file: "../lookup/RASAero.csv" + rasaero_lookup_file: "../../Configurations/LookUp/RASAero.csv" C_d: 0.5 C_d_s: 1.2 length: 6.68 @@ -34,7 +34,7 @@ rocket: <<: *simulated_stage rocket_body: <<: *simulated_rocket_body - rasaero_lookup_file: "../lookup/RASAero.csv" + rasaero_lookup_file: "../../Configurations/LookUp/RASAero.csv" C_d: 0.5 C_d_s: 1.2 length: 3.34 @@ -60,7 +60,7 @@ rocket: max_ext_spd: 0.001 desired_apogee: 4572 meta: - output_file: '../output/simulated_6dof.csv' - monte_carlo_output_folder: '../output/monte_carlo' + output_file: '../../Output/simulated_6dof.csv' + monte_carlo_output_folder: '../../Output/monte_carlo' diff --git a/Simulation/6DOF_RK4/properties/data_loader.py b/Simulation/6DOF_RK4/Configurations/data_loader.py similarity index 94% rename from Simulation/6DOF_RK4/properties/data_loader.py rename to Simulation/6DOF_RK4/Configurations/data_loader.py index 25d5900..bcc2900 100644 --- a/Simulation/6DOF_RK4/properties/data_loader.py +++ b/Simulation/6DOF_RK4/Configurations/data_loader.py @@ -1,8 +1,10 @@ import yaml import numpy import os -from properties.yaml_datatypes import get_loader -import properties.properties as prop +import sys + +from yaml_datatypes import get_loader +import properties as prop config = None @@ -46,12 +48,12 @@ def load_config(config_path): raw_yaml = "" # Load all data types and YAML structures - with open(os.path.join(os.path.dirname(__file__), "../properties/typedef.yaml")) as typedef: + with open(os.path.join(os.path.dirname(__file__), "typedef.yaml")) as typedef: # Add yaml type definitions to current loaded file raw_yaml += typedef.read() # Load all ./templates - template_directory = os.path.join(os.path.dirname(__file__), "../properties/templates") + template_directory = os.path.join(os.path.dirname(__file__), "templates") for template_filename in os.listdir(template_directory): template = os.path.join(template_directory, template_filename) # Check if template is a yaml file or not (All ./templates must be .yaml files.) diff --git a/Simulation/6DOF_RK4/properties/properties.py b/Simulation/6DOF_RK4/Configurations/properties.py similarity index 76% rename from Simulation/6DOF_RK4/properties/properties.py rename to Simulation/6DOF_RK4/Configurations/properties.py index 188ed78..8dd5e86 100644 --- a/Simulation/6DOF_RK4/properties/properties.py +++ b/Simulation/6DOF_RK4/Configurations/properties.py @@ -2,7 +2,7 @@ import numpy as np # Config file -sim_config = "../properties/configs/staging_rocket.yaml" +sim_config = "configs/staging_rocket.yaml" ### Calculated constants # Gravitational const diff --git a/Simulation/6DOF_RK4/properties/templates/motors.yaml b/Simulation/6DOF_RK4/Configurations/templates/motors.yaml similarity index 68% rename from Simulation/6DOF_RK4/properties/templates/motors.yaml rename to Simulation/6DOF_RK4/Configurations/templates/motors.yaml index 84e6dce..9b2a04f 100644 --- a/Simulation/6DOF_RK4/properties/templates/motors.yaml +++ b/Simulation/6DOF_RK4/Configurations/templates/motors.yaml @@ -3,7 +3,7 @@ define: &motor_M2500 impulse: 9671.0 motor_mass: 8.064 delay: 60 - motor_lookup_file: "../lookup/m2500.csv" + motor_lookup_file: "../../Configurations/LookUp/m2500.csv" cm: !numpy/array [0.3755, 0., 0.] define: &motor_N5800 @@ -11,7 +11,7 @@ define: &motor_N5800 impulse: 20145.7 motor_mass: 14.826 delay: 60 - motor_lookup_file: "../lookup/n5800.csv" + motor_lookup_file: "../../Configurations/LookUp/n5800.csv" cm: !numpy/array [0.3755, 0., 0.] define: &motor_N2450 @@ -19,7 +19,7 @@ define: &motor_N2450 impulse: 17907.0 motor_mass: 10.7 delay: 60 - motor_lookup_file: "../lookup/n2540.csv" + motor_lookup_file: "../../Configurations/LookUp/n2540.csv" cm: !numpy/array [0.3755, 0., 0.] define: &motor_J244 @@ -27,5 +27,5 @@ define: &motor_J244 impulse: 849 motor_mass: 0.911 delay: 60 - motor_lookup_file: "../lookup/j244.csv" + motor_lookup_file: "../../Configurations/LookUp/j244.csv" cm: !numpy/array [0.3755, 0., 0.] \ No newline at end of file diff --git a/Simulation/6DOF_RK4/properties/typedef.yaml b/Simulation/6DOF_RK4/Configurations/typedef.yaml similarity index 100% rename from Simulation/6DOF_RK4/properties/typedef.yaml rename to Simulation/6DOF_RK4/Configurations/typedef.yaml diff --git a/Simulation/6DOF_RK4/properties/yaml_datatypes.py b/Simulation/6DOF_RK4/Configurations/yaml_datatypes.py similarity index 100% rename from Simulation/6DOF_RK4/properties/yaml_datatypes.py rename to Simulation/6DOF_RK4/Configurations/yaml_datatypes.py diff --git a/Simulation/6DOF_RK4/dynamics/controller.py b/Simulation/6DOF_RK4/Core/dynamics/controller.py similarity index 100% rename from Simulation/6DOF_RK4/dynamics/controller.py rename to Simulation/6DOF_RK4/Core/dynamics/controller.py diff --git a/Simulation/6DOF_RK4/dynamics/forces.py b/Simulation/6DOF_RK4/Core/dynamics/forces.py similarity index 98% rename from Simulation/6DOF_RK4/dynamics/forces.py rename to Simulation/6DOF_RK4/Core/dynamics/forces.py index 9b227ac..d73fab5 100644 --- a/Simulation/6DOF_RK4/dynamics/forces.py +++ b/Simulation/6DOF_RK4/Core/dynamics/forces.py @@ -5,14 +5,15 @@ - aerodynamic - wind (comes from atmosphere class) ''' -import dynamics.motor as motor -import environment.atmosphere as atmosphere -import properties.properties as prop import numpy as np import util.vectors as vct import pandas as pd import os -import random +import sys + +sys.path.insert(0, os.path.abspath( + os.path.join(os.path.dirname(__file__), '..','..','Configurations'))) +import properties as prop # Define Objects diff --git a/Simulation/6DOF_RK4/dynamics/motor.py b/Simulation/6DOF_RK4/Core/dynamics/motor.py similarity index 100% rename from Simulation/6DOF_RK4/dynamics/motor.py rename to Simulation/6DOF_RK4/Core/dynamics/motor.py diff --git a/Simulation/6DOF_RK4/simulation/simulator.py b/Simulation/6DOF_RK4/Core/dynamics/propagator.py similarity index 97% rename from Simulation/6DOF_RK4/simulation/simulator.py rename to Simulation/6DOF_RK4/Core/dynamics/propagator.py index 007a917..8e56278 100644 --- a/Simulation/6DOF_RK4/simulation/simulator.py +++ b/Simulation/6DOF_RK4/Core/dynamics/propagator.py @@ -1,10 +1,6 @@ import numpy as np -import scipy import random -import matplotlib.pyplot as plt -# import dynamics.forces as forces -import util.vectors as vct -# import properties.properties as prop + import dynamics.rocket as rocket_model import environment.atmosphere as atm_model diff --git a/Simulation/6DOF_RK4/dynamics/rocket.py b/Simulation/6DOF_RK4/Core/dynamics/rocket.py similarity index 99% rename from Simulation/6DOF_RK4/dynamics/rocket.py rename to Simulation/6DOF_RK4/Core/dynamics/rocket.py index 8804b8e..27f1e6f 100644 --- a/Simulation/6DOF_RK4/dynamics/rocket.py +++ b/Simulation/6DOF_RK4/Core/dynamics/rocket.py @@ -264,7 +264,7 @@ def add_to_dict(self, x, baro_alt, accel, bno_ang_pos, gyro, kalman_filter, kf_c self.sim_dict["motor_mass"].append(motor_mass) # Converts the data saved in this sim into csv - def to_csv(self): + def to_list(self): #Output record = [] for point in range(len(self.sim_dict["time"])): @@ -303,8 +303,8 @@ def to_csv(self): record.append(cur_point) return record - - + + # if __name__ == '__main__': # rocket = Rocket(motor_mass=5,rocket_dry_mass=2) # print(rocket.rocket_total_mass) diff --git a/Simulation/6DOF_RK4/dynamics/sensors.py b/Simulation/6DOF_RK4/Core/dynamics/sensors.py similarity index 93% rename from Simulation/6DOF_RK4/dynamics/sensors.py rename to Simulation/6DOF_RK4/Core/dynamics/sensors.py index 148c612..591b36b 100644 --- a/Simulation/6DOF_RK4/dynamics/sensors.py +++ b/Simulation/6DOF_RK4/Core/dynamics/sensors.py @@ -1,10 +1,16 @@ import numpy as np import util.vectors as vct import random -import properties.properties as prop -import properties.data_loader as dataloader +import os +import sys + import environment.atmosphere as atm +sys.path.insert(0, os.path.abspath( + os.path.join(os.path.dirname(__file__), '..','..','Configurations'))) +import properties as prop +import data_loader as dataloader + def get_accelerometer_data(x_state, sensor_config): """Returns the accelerometer data in the body frame of the rocket. diff --git a/Simulation/6DOF_RK4/environment/atmosphere.py b/Simulation/6DOF_RK4/Core/environment/atmosphere.py similarity index 99% rename from Simulation/6DOF_RK4/environment/atmosphere.py rename to Simulation/6DOF_RK4/Core/environment/atmosphere.py index a2e74e6..9d3ed1c 100644 --- a/Simulation/6DOF_RK4/environment/atmosphere.py +++ b/Simulation/6DOF_RK4/Core/environment/atmosphere.py @@ -293,7 +293,7 @@ def get_wind_vector(self, tStamp)->np.ndarray: current_wind_direction_ = np.zeros(3) if self.enable_direction_variance_: if((tStamp - self.last_direction_variance_update_) >= self.direction_variance_update_rate_): - generated_direction_variance = np.random.normal(self.nominal_wind_direction_, 3) + generated_direction_variance = np.random.normal(self.nominal_wind_direction_, self.wind_direction_variance_stddev_) generated_direction_variance = generated_direction_variance / np.linalg.norm(generated_direction_variance) self.last_direction_variance_update_ = tStamp diff --git a/Simulation/6DOF_RK4/environment/magnetic_model.py b/Simulation/6DOF_RK4/Core/environment/magnetic_model.py similarity index 100% rename from Simulation/6DOF_RK4/environment/magnetic_model.py rename to Simulation/6DOF_RK4/Core/environment/magnetic_model.py diff --git a/Simulation/6DOF_RK4/estimation/apogee_estimator.py b/Simulation/6DOF_RK4/Core/estimation/apogee_estimator.py similarity index 98% rename from Simulation/6DOF_RK4/estimation/apogee_estimator.py rename to Simulation/6DOF_RK4/Core/estimation/apogee_estimator.py index 6d2e3b1..d19ad45 100644 --- a/Simulation/6DOF_RK4/estimation/apogee_estimator.py +++ b/Simulation/6DOF_RK4/Core/estimation/apogee_estimator.py @@ -1,14 +1,16 @@ import numpy as np -import scipy import matplotlib.pyplot as plt import os +import sys import pandas as pd import math -import dynamics.forces as forces -import util.vectors as vct -import properties.properties as prop import dynamics.rocket as rocket_model + +sys.path.insert(0, os.path.abspath( + os.path.join(os.path.dirname(__file__), '..','..','Configurations'))) + +import properties as prop class Apogee: '''Apogee Estimator Class diff --git a/Simulation/6DOF_RK4/estimation/ekf.py b/Simulation/6DOF_RK4/Core/estimation/ekf.py similarity index 100% rename from Simulation/6DOF_RK4/estimation/ekf.py rename to Simulation/6DOF_RK4/Core/estimation/ekf.py diff --git a/Simulation/6DOF_RK4/estimation/r_ekf.py b/Simulation/6DOF_RK4/Core/estimation/r_ekf.py similarity index 100% rename from Simulation/6DOF_RK4/estimation/r_ekf.py rename to Simulation/6DOF_RK4/Core/estimation/r_ekf.py diff --git a/Simulation/6DOF_RK4/simulation/pysim.py b/Simulation/6DOF_RK4/Core/simulation.py similarity index 53% rename from Simulation/6DOF_RK4/simulation/pysim.py rename to Simulation/6DOF_RK4/Core/simulation.py index abddce2..705e84b 100644 --- a/Simulation/6DOF_RK4/simulation/pysim.py +++ b/Simulation/6DOF_RK4/Core/simulation.py @@ -1,66 +1,28 @@ -# ______ _____ _ __________ ___________ -# | ___ \ / ___(_) / ___| _ \ _ | ___| -# | |_/ / _\ `--. _ _ __ ___ / /___| | | | | | | |_ -# | __/ | | |`--. \ | '_ ` _ \ | ___ \ | | | | | | _| -# | | | |_| /\__/ / | | | | | | | \_/ | |/ /\ \_/ / | -# \_| \__, \____/|_|_| |_| |_| \_____/___/ \___/\_| -# __/ | -# |___/ - -# A 6DOF RK-4 Based simulation that uses RASAero aerodynamic data and known motor thrust data to simulate motion of the rocket -# with simulated sensor data as well as a implementation of the Extended Kalman Filter and active drag PID controller for the ISS -# Spaceshot entry for the 2023 IREC competition. This simulation was used to quantify the effects of the airbrakes, test -# different system design methodlogies, and provide preliminary tuning for the EKF and controller prior to implementation in -# SILSIM and flight software. -# -# 2022-2023 Guidance, Navigation, and Control Main Contributors # -# Sub-Team Lead: Parth Shrotri (2024) -# Colin Kinsey (2024) -# Evan Yu (2025) -# Rithvik Bhogavilli (2025) -# Kabir Cheema (2025) -# Freya Bansal (2025) -# Ishaan Bansal (2025) -# Ethan Pereira (2026) - import numpy as np -import matplotlib.pyplot as plt -import os -import sys -import shutil - -sys.path.insert(0, os.path.abspath( - os.path.join(os.path.dirname(__file__), '..'))) import estimation.ekf as ekf import estimation.r_ekf as r_ekf -import properties.properties as prop -import properties.data_loader as dataloader -import simulator as sim_class import dynamics.sensors as sensors -import time import estimation.apogee_estimator as apg import dynamics.rocket as rocket_model import environment.atmosphere as atmosphere -# Load desired config file -config = dataloader.config - -# Runs simulation for the specific component of the rocket class Simulation: # dt can be dynamic in the future, so we need to - def __init__(self, rocket, motor, dt, x0, time_stamp=0, stages=[]): + def __init__(self, sim, rocket, atm, dt, x0, kf_dt, time_stamp=0, stages=[]): + self.sim = sim self.rocket = rocket self.stages = stages - self.motor = motor + self.motor = rocket.motor + self.atm = atm self.dt = dt self.x = x0.copy() self.baro = 0 self.time_stamp = time_stamp self.sensor_config = self.rocket.stage_config['sensors'] - self.init_kalman_filters() + self.init_gnc(kf_dt) - def init_kalman_filters(self): + def init_gnc(self, dt): # TODO: Init with previous rocket data self.kalman_filter = ekf.KalmanFilter(dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # TODO: init this data with the previous rocket becuase of staging @@ -82,7 +44,7 @@ def init_kalman_filters(self): pitch = -1 * (np.arctan2(-az,-ay) + np.pi/2) yaw = np.arctan2(-ax,-ay) + np.pi/2 self.r_kalman_filter = r_ekf.KalmanFilter_R(dt, 0.0, 0.0, 0.0, pitch, 0.0, 0.0, yaw, 0.0, 0.0) - self.apogee_estimator = apg.Apogee(self.kalman_filter.get_state(), 0.1, 0.01, 3, 30, atm, self.rocket.stage_config) + self.apogee_estimator = apg.Apogee(self.kalman_filter.get_state(), 0.1, 0.01, 3, 30, self.atm, self.rocket.stage_config) def update_kalman(self, baro_alt, accel, gyro, bno_ang_pos): self.kalman_filter.priori() @@ -102,17 +64,18 @@ def idle_stage(self): self.kalman_filter.reset_lateral_pos() current_state, current_covariance, current_state_r = self.get_kalman_state() - self.rocket.add_to_dict(self.x, baro_alt, accel, bno_ang_pos, gyro, current_state, current_covariance, current_state_r, 0, current_state[0], self.rocket.get_rocket_dry_mass(), self.rocket.get_total_motor_mass(self.time_stamp), 0, dt) + self.rocket.add_to_dict(self.x, baro_alt, accel, bno_ang_pos, gyro, current_state, current_covariance, current_state_r, 0, current_state[0], self.rocket.get_rocket_dry_mass(), self.rocket.get_total_motor_mass(self.time_stamp), 0, self.dt) self.time_step() - def execute_stage(self): + def execute_stage(self, print=False): # Run the stages stage_separation_delay = 1 self.rocket.get_motor().ignite(self.time_stamp) ignition_time = self.time_stamp start = True - print(f"Staged at {self.time_stamp}") + if print: + print(f"Staged at {self.time_stamp}") while self.time_stamp < ignition_time + self.rocket.get_motor().get_burn_time() + stage_separation_delay: # Get sensor data baro_alt, accel, gyro, bno_ang_pos = self.get_sensor_data() @@ -124,9 +87,9 @@ def execute_stage(self): self.rocket.set_motor_mass(self.time_stamp) is_staging = start and self.rocket.current_stage != -1 - self.x, alpha = sim.RK4(self.x, dt, self.time_stamp, is_staging, 0) + self.x, alpha = self.sim.RK4(self.x, self.dt, self.time_stamp, is_staging, 0) - self.rocket.add_to_dict(self.x, baro_alt, accel, bno_ang_pos, gyro, current_state, current_covariance, current_state_r, alpha, apogee_est, self.rocket.get_rocket_dry_mass(), self.rocket.get_total_motor_mass(self.time_stamp), 0, dt) + self.rocket.add_to_dict(self.x, baro_alt, accel, bno_ang_pos, gyro, current_state, current_covariance, current_state_r, alpha, apogee_est, self.rocket.get_rocket_dry_mass(), self.rocket.get_total_motor_mass(self.time_stamp), 0, self.dt) self.time_step() if start: start = False @@ -159,56 +122,22 @@ def coast(self): apogee_est = self.apogee_estimator.predict_apogee(current_state[0:3]) - self.x, alpha = sim.RK4(self.x, dt, self.time_stamp, 0) + self.x, alpha = self.sim.RK4(self.x, self.dt, self.time_stamp, 0) - self.rocket.add_to_dict(self.x, baro_alt, accel, bno_ang_pos, gyro, current_state, current_covariance, current_state_r, alpha, apogee_est, self.rocket.get_rocket_dry_mass(), self.rocket.get_total_motor_mass(self.time_stamp), 0, dt) + self.rocket.add_to_dict(self.x, baro_alt, accel, bno_ang_pos, gyro, current_state, current_covariance, current_state_r, alpha, apogee_est, self.rocket.get_rocket_dry_mass(), self.rocket.get_total_motor_mass(self.time_stamp), 0, self.dt) self.time_step() - -def simulator(x0, rocket, motor, dt) -> None: - '''Method which handles running the simulation and logging sim data to dict - - Args: - x0 (np.array): state vector initialized to 0s [6x3] - x: y: z: - [[pos, pos, pos], - [vel, vel, vel], - [accel, accel, accel], - [ang_pos, ang_pos, ang_pos], - [ang_vel, ang_vel, ang_vel], - [ang_accel, ang_accel, ang_accel]] - dt (float): time step between each iteration in simulation - ''' - simulator = Simulation(rocket, motor, dt, x0, stages=rocket.stages) - simulator.idle_stage() - t_start = time.time() - simulator.run_stages() - simulator.coast() - t_end = time.time() - t_start - print(f"Runtime: {t_end:.2f} seconds") - - -if __name__ == '__main__': - x0 = np.zeros((6, 3)) - x0[3] = [0, 0.05, 0] - dt = 0.01 - - atm = atmosphere.Atmosphere(enable_direction_variance=True, enable_magnitude_variance=True) +def gen_sim_objs(config, atmo_args): + atm = atmosphere.Atmosphere(wind_direction_variance_stddev=atmo_args.get("wind_direction_stddev", 0), + wind_magnitude_variance_stddev=atmo_args.get("wind_magnitude_stddev", 0), + enable_direction_variance=atmo_args.get("enable_direction_variance", False), + enable_magnitude_variance=atmo_args.get("enable_magnitude_variance", False), + nominal_wind_magnitude=atmo_args.get("nominal_wind_magnitude", 0.0), + nominal_wind_direction=atmo_args.get("nominal_wind_direction", np.array([0, 0, 0]))) stages = [] for stage in config['rocket']['stages'][1:]: stages.append(rocket_model.Rocket(stage, atm=atm)) rocket = rocket_model.Rocket(config['rocket']['stages'][0], atm=atm, stages=stages) - motor = rocket.motor - sim = sim_class.Simulator(atm=atm, rocket=rocket) - - simulator(x0, rocket, motor, dt) - - print("Writing to file...") - - record = rocket.to_csv() - output_file = os.path.join(os.path.dirname(__file__), config["meta"]["output_file"]) - with open(output_file, 'w') as f: - f.write("time,pos_x,pos_y,pos_z,vel_x,vel_y,vel_z,accel_x,accel_y,accel_z,ang_pos_x,ang_pos_y,ang_pos_z,ang_vel_x,ang_vel_y,ang_vel_z,ang_accel_x,ang_accel_y,ang_accel_z,alpha,rocket_total_mass,motor_mass,flap_ext,baro_alt,imu_accel_x,imu_accel_y,imu_accel_z,imu_ang_pos_x,imu_ang_pos_y,imu_ang_pos_z,imu_gyro_x,imu_gyro_y,imu_gyro_z,apogee_estimate,kalman_pos_x,kalman_vel_x,kalman_accel_x,kalman_pos_y,kalman_vel_y,kalman_accel_y,kalman_pos_z,kalman_vel_z,kalman_accel_z,pos_cov_x,vel_cov_x,accel_cov_x,pos_cov_y,vel_cov_y,accel_cov_y,pos_cov_z,vel_cov_z,accel_cov_z,kalman_rpos_x,kalman_rvel_x,kalman_raccel_x,kalman_rpos_y,kalman_rvel_y,kalman_raccel_y,kalman_rpos_z,kalman_rvel_z,kalman_raccel_z\n") - for point in record: - f.write(f"{','.join(point)}\n") + return atm, rocket + \ No newline at end of file diff --git a/Simulation/6DOF_RK4/util/coordinate_transformation.py b/Simulation/6DOF_RK4/Core/util/coordinate_transformation.py similarity index 100% rename from Simulation/6DOF_RK4/util/coordinate_transformation.py rename to Simulation/6DOF_RK4/Core/util/coordinate_transformation.py diff --git a/Simulation/6DOF_RK4/Core/util/data_process.py b/Simulation/6DOF_RK4/Core/util/data_process.py new file mode 100644 index 0000000..5258fd4 --- /dev/null +++ b/Simulation/6DOF_RK4/Core/util/data_process.py @@ -0,0 +1,61 @@ +import numpy as np +import pandas as pd +import os +import sys + +sys.path.insert(0, os.path.abspath( + os.path.join(os.path.dirname(__file__), '..','..','Configurations'))) + +from data_loader import config + +def load_record(file_path): + sim_dict = {} + sensor_dict = {} + kalman_dict = {} + output_file = os.path.join(os.path.dirname(__file__), config["meta"]["output_file"]) + file_data = pd.read_csv(output_file) + sim_dict["time"] = file_data["time"].values + sensor_dict["time"] = file_data["time"].values + kalman_dict["time"] = file_data["time"].values + + for attr in ["pos", "vel", "accel", "ang_pos", "ang_vel", "ang_accel"]: + sim_dict[attr] = np.array( + list(zip(file_data[f"{attr}_x"].values, file_data[f"{attr}_y"].values, file_data[f"{attr}_z"].values))) + for attr in ["kalman_pos", "kalman_vel", "kalman_accel", "pos_cov", "vel_cov", "accel_cov"]: + kalman_dict[attr] = np.array( + list(zip(file_data[f"{attr}_x"].values, file_data[f"{attr}_y"].values, file_data[f"{attr}_z"].values))) + for attr in ["kalman_rpos", "kalman_rvel", "kalman_raccel"]: + kalman_dict[attr] = np.array( + list(zip(file_data[f"{attr}_x"].values, file_data[f"{attr}_y"].values, file_data[f"{attr}_z"].values))) + sim_dict["alpha"] = np.array(list(zip(file_data["alpha"].values))) + sim_dict["rocket_total_mass"] = np.array(list(zip(file_data["rocket_total_mass"].values))) + sim_dict["motor_mass"] = np.array(list(zip(file_data["motor_mass"].values))) + sim_dict["flap_ext"] = np.array(list(zip(file_data["flap_ext"].values))) + sensor_dict["baro_alt"] = np.array(list(zip(file_data["baro_alt"].values))) + sensor_dict["imu_accel_x"] = np.array(list(zip(file_data["imu_accel_x"].values))) + sensor_dict["imu_accel_y"] = np.array(list(zip(file_data["imu_accel_y"].values))) + sensor_dict["imu_accel_z"] = np.array(list(zip(file_data["imu_accel_z"].values))) + sensor_dict["imu_ang_pos_x"] = np.array(list(zip(file_data["imu_ang_pos_x"].values))) + sensor_dict["imu_ang_pos_y"] = np.array(list(zip(file_data["imu_ang_pos_y"].values))) + sensor_dict["imu_ang_pos_z"] = np.array(list(zip(file_data["imu_ang_pos_z"].values))) + sensor_dict["imu_gyro_x"] = np.array(list(zip(file_data["imu_gyro_x"].values))) + sensor_dict["imu_gyro_y"] = np.array(list(zip(file_data["imu_gyro_y"].values))) + sensor_dict["imu_gyro_z"] = np.array(list(zip(file_data["imu_gyro_z"].values))) + sensor_dict["apogee_estimate"] = np.array(list(zip(file_data["apogee_estimate"].values))) + + return sim_dict, sensor_dict, kalman_dict + +def save_record(record, output_file, target_size=None): + print("Writing to file...") + if target_size is not None: + if len(record) > target_size: + record = record[:target_size] + elif len(record) < target_size: + while len(record) < target_size: + record = np.append(record, np.array([record[-1]]), axis=0) + + with open(output_file, 'w') as f: + f.write("time,pos_x,pos_y,pos_z,vel_x,vel_y,vel_z,accel_x,accel_y,accel_z,ang_pos_x,ang_pos_y,ang_pos_z,ang_vel_x,ang_vel_y,ang_vel_z,ang_accel_x,ang_accel_y,ang_accel_z,alpha,rocket_total_mass,motor_mass,flap_ext,baro_alt,imu_accel_x,imu_accel_y,imu_accel_z,imu_ang_pos_x,imu_ang_pos_y,imu_ang_pos_z,imu_gyro_x,imu_gyro_y,imu_gyro_z,apogee_estimate,kalman_pos_x,kalman_vel_x,kalman_accel_x,kalman_pos_y,kalman_vel_y,kalman_accel_y,kalman_pos_z,kalman_vel_z,kalman_accel_z,pos_cov_x,vel_cov_x,accel_cov_x,pos_cov_y,vel_cov_y,accel_cov_y,pos_cov_z,vel_cov_z,accel_cov_z,kalman_rpos_x,kalman_rvel_x,kalman_raccel_x,kalman_rpos_y,kalman_rvel_y,kalman_raccel_y,kalman_rpos_z,kalman_rvel_z,kalman_raccel_z\n") + for point in record: + f.write(f"{','.join(point)}\n") + \ No newline at end of file diff --git a/Simulation/6DOF_RK4/util/random_noise.py b/Simulation/6DOF_RK4/Core/util/random_noise.py similarity index 100% rename from Simulation/6DOF_RK4/util/random_noise.py rename to Simulation/6DOF_RK4/Core/util/random_noise.py diff --git a/Simulation/6DOF_RK4/util/vectors.py b/Simulation/6DOF_RK4/Core/util/vectors.py similarity index 100% rename from Simulation/6DOF_RK4/util/vectors.py rename to Simulation/6DOF_RK4/Core/util/vectors.py diff --git a/Simulation/6DOF_RK4/plotter/analysis.py b/Simulation/6DOF_RK4/Executables/analysis/analysis.py similarity index 86% rename from Simulation/6DOF_RK4/plotter/analysis.py rename to Simulation/6DOF_RK4/Executables/analysis/analysis.py index aff0fa5..ec54e12 100644 --- a/Simulation/6DOF_RK4/plotter/analysis.py +++ b/Simulation/6DOF_RK4/Executables/analysis/analysis.py @@ -6,10 +6,14 @@ import sys sys.path.insert(0, os.path.abspath( - os.path.join(os.path.dirname(__file__), '..'))) + os.path.join(os.path.dirname(__file__), '../../Configurations'))) -import properties.properties as prop -import properties.data_loader as dataloader +import properties as prop +import data_loader as dataloader + +sys.path.insert(0, os.path.abspath( + os.path.join(os.path.dirname(__file__), '../../Core'))) +import util.data_process as data_process config = dataloader.config @@ -173,39 +177,7 @@ def rocket_mass_plot(sim_dict, sensor_dict=0, kalman_dict=0): plt.show(block=False) if __name__ == "__main__": - sim_dict = {} - sensor_dict = {} - kalman_dict = {} - output_file = os.path.join(os.path.dirname(__file__), config["meta"]["output_file"]) - file_data = pd.read_csv(output_file) - sim_dict["time"] = file_data["time"].values - sensor_dict["time"] = file_data["time"].values - kalman_dict["time"] = file_data["time"].values - - for attr in ["pos", "vel", "accel", "ang_pos", "ang_vel", "ang_accel"]: - sim_dict[attr] = np.array( - list(zip(file_data[f"{attr}_x"].values, file_data[f"{attr}_y"].values, file_data[f"{attr}_z"].values))) - for attr in ["kalman_pos", "kalman_vel", "kalman_accel", "pos_cov", "vel_cov", "accel_cov"]: - kalman_dict[attr] = np.array( - list(zip(file_data[f"{attr}_x"].values, file_data[f"{attr}_y"].values, file_data[f"{attr}_z"].values))) - for attr in ["kalman_rpos", "kalman_rvel", "kalman_raccel"]: - kalman_dict[attr] = np.array( - list(zip(file_data[f"{attr}_x"].values, file_data[f"{attr}_y"].values, file_data[f"{attr}_z"].values))) - sim_dict["alpha"] = np.array(list(zip(file_data["alpha"].values))) - sim_dict["rocket_total_mass"] = np.array(list(zip(file_data["rocket_total_mass"].values))) - sim_dict["motor_mass"] = np.array(list(zip(file_data["motor_mass"].values))) - sim_dict["flap_ext"] = np.array(list(zip(file_data["flap_ext"].values))) - sensor_dict["baro_alt"] = np.array(list(zip(file_data["baro_alt"].values))) - sensor_dict["imu_accel_x"] = np.array(list(zip(file_data["imu_accel_x"].values))) - sensor_dict["imu_accel_y"] = np.array(list(zip(file_data["imu_accel_y"].values))) - sensor_dict["imu_accel_z"] = np.array(list(zip(file_data["imu_accel_z"].values))) - sensor_dict["imu_ang_pos_x"] = np.array(list(zip(file_data["imu_ang_pos_x"].values))) - sensor_dict["imu_ang_pos_y"] = np.array(list(zip(file_data["imu_ang_pos_y"].values))) - sensor_dict["imu_ang_pos_z"] = np.array(list(zip(file_data["imu_ang_pos_z"].values))) - sensor_dict["imu_gyro_x"] = np.array(list(zip(file_data["imu_gyro_x"].values))) - sensor_dict["imu_gyro_y"] = np.array(list(zip(file_data["imu_gyro_y"].values))) - sensor_dict["imu_gyro_z"] = np.array(list(zip(file_data["imu_gyro_z"].values))) - sensor_dict["apogee_estimate"] = np.array(list(zip(file_data["apogee_estimate"].values))) + sim_dict, sensor_dict, kalman_dict = data_process.load_record(os.path.join(os.path.dirname(__file__), config["meta"]["output_file"])) rocket_mass_plot(sim_dict=sim_dict, sensor_dict=sensor_dict, kalman_dict=kalman_dict) plotter(sim_dict=sim_dict, sensor_dict=sensor_dict, kalman_dict=kalman_dict) diff --git a/Simulation/6DOF_RK4/plotter/ekf_tester.py b/Simulation/6DOF_RK4/Executables/analysis/ekf_tester.py similarity index 96% rename from Simulation/6DOF_RK4/plotter/ekf_tester.py rename to Simulation/6DOF_RK4/Executables/analysis/ekf_tester.py index c0d1988..6b8904e 100644 --- a/Simulation/6DOF_RK4/plotter/ekf_tester.py +++ b/Simulation/6DOF_RK4/Executables/analysis/ekf_tester.py @@ -1,7 +1,7 @@ import os import sys -sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) +sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '../..'))) import estimation.ekf as kf import pandas as pandas diff --git a/Simulation/6DOF_RK4/plotter/monte_carlo_plotter.py b/Simulation/6DOF_RK4/Executables/analysis/monte_carlo_plotter.py similarity index 76% rename from Simulation/6DOF_RK4/plotter/monte_carlo_plotter.py rename to Simulation/6DOF_RK4/Executables/analysis/monte_carlo_plotter.py index 9e19acb..d7c1a6e 100644 --- a/Simulation/6DOF_RK4/plotter/monte_carlo_plotter.py +++ b/Simulation/6DOF_RK4/Executables/analysis/monte_carlo_plotter.py @@ -1,71 +1,80 @@ import matplotlib.pyplot as plt import numpy as np +import pandas as pd import os import sys plt.rcParams.update({'font.size': 14}) sys.path.insert(0, os.path.abspath( - os.path.join(os.path.dirname(__file__), '..'))) + os.path.join(os.path.dirname(__file__), '..','..','Configurations'))) -import properties.properties as prop -import properties.data_loader as dataloader +import data_loader as dataloader config = dataloader.config # Indices of each state variable in the output array # kf and rkf values are arrays containing position, velocity and acceleration in each axis -indices = {"x":0, - "y":1, - "z":2, - "vx":3, - "vy":4, - "vz":5, - "ax":6, - "ay":7, - "az":8, - "roll":9, - "pitch":10, - "yaw":11, - "ang_vel_roll":12, - "ang_vel_pitch":13, - "ang_vel_yaw":14, - "ang_accel_roll":15, - "ang_accel_pitch":16, - "ang_accel_yaw":17, - "time":18, - "flap_ext":19, - "alpha":20, - "rocket_total_mass":21, - "motor_mass":22, +indices = {"time":0, + "pos_x":1, + "pos_y":2, + "pos_z":3, + "vel_x":4, + "vel_y":5, + "vel_z":6, + "accel_x":7, + "accel_y":8, + "accel_z":9, + "ang_pos_x":10, + "ang_pos_y":11, + "ang_pos_z":12, + "ang_vel_x":13, + "ang_vel_y":14, + "ang_vel_z":15, + "ang_accel_x":16, + "ang_accel_y":17, + "ang_accel_z":18, + "alpha":19, + "rocket_total_mass":20, + "motor_mass":21, + "flap_ext":22, "baro_alt":23, - "accel_x":24, - "accel_y":25, - "accel_z":26, - "imu_x":27, - "imu_y":28, - "imu_z":29, - "gyro_x":30, - "gyro_y":31, - "gyro_z":32, + "imu_accel_x":24, + "imu_accel_y":25, + "imu_accel_z":26, + "imu_ang_pos_x":27, + "imu_ang_pos_y":28, + "imu_ang_pos_z":29, + "imu_gyro_x":30, + "imu_gyro_y":31, + "imu_gyro_z":32, "apogee_estimate":33, - "kf_x":34, - "kf_vx":35, - "kf_ax":36, - "kf_y":37, - "kf_vy":38, - "kf_ay":39, - "kf_z":40, - "kf_vz":41, - "kf_az":42, - "rkf_r":43, - "rkf_vr":44, - "rkf_ar":45, - "rkf_p":46, - "rkf_vp":47, - "rkf_ap":48, - "rkf_y":49, - "rkf_vy":50, - "rkf_ay":51} + "kalman_pos_x":34, + "kalman_vel_x":35, + "kalman_accel_x":36, + "kalman_pos_y":37, + "kalman_vel_y":38, + "kalman_accel_y":39, + "kalman_pos_z":40, + "kalman_vel_z":41, + "kalman_accel_z":42, + "pos_cov_x":43, + "vel_cov_x":44, + "accel_cov_x":45, + "pos_cov_y":46, + "vel_cov_y":47, + "accel_cov_y":48, + "pos_cov_z":49, + "vel_cov_z":50, + "accel_cov_z":51, + "kalman_rpos_x":52, + "kalman_rvel_x":53, + "kalman_raccel_x":54, + "kalman_rpos_y":55, + "kalman_rvel_y":56, + "kalman_raccel_y":57, + "kalman_rpos_z":58, + "kalman_rvel_z":59, + "kalman_raccel_z":60} default_output_folder = 'Output' def rmse(predictions, targets): @@ -82,6 +91,7 @@ def plotState(state:str, nominal_data:np.ndarray, monte_carlo_data:np.ndarray, f # Get the time stamps from the nominal data times = nominal_data[:, indices["time"]] + # Plot the Monte Carlo data for point in monte_carlo_data: ax.plot(times[:len(point[indices[state]])], point[indices[state]], '0.8') @@ -185,7 +195,7 @@ def plotTrajectory3D(states:list, nominal_data:np.ndarray, monte_carlo_data:np.n monte_carlo_states[:,i,:] = monte_carlo_data[:, indices[state], :] # Plot nominal data - cbar = fig.colorbar(ax.scatter(nominal_states[0,:], nominal_states[1,:], nominal_states[2,:], c=color_data, cmap=cmap, label="Nominal", marker='.', linewidth=1, location='bottom'), pad=0.1) + cbar = fig.colorbar(ax.scatter(nominal_states[0,:], nominal_states[1,:], nominal_states[2,:], c=color_data, cmap=cmap, label="Nominal", marker='.', linewidth=1), pad=0.1) cbar.set_label('Velocity (m/s)') # Generate uncertainty cone based on monte carlo data # Every 10th point, generate points on a circle with radius 3 times the standard deviation of the monte carlo data that is orthogonal to the nominal trajectory @@ -227,25 +237,25 @@ def plotTrajectory3D(states:list, nominal_data:np.ndarray, monte_carlo_data:np.n if __name__ == "__main__": folder_path = os.path.join(os.path.dirname(__file__), config["meta"]["monte_carlo_output_folder"]) - nominal_data = np.load(os.path.join(folder_path, 'SimData/nominal.npy')) - monte_carlo_data = np.load(os.path.join(folder_path, 'SimData/sim_data_0.npy')) + nominal_data = pd.read_csv(os.path.join(folder_path, 'SimData/nominal.csv')).to_numpy() + monte_carlo_data = pd.read_csv(os.path.join(folder_path, 'SimData/sim_data_0.csv')).to_numpy() for path in os.listdir(os.path.join(folder_path, 'SimData')): if path.startswith("sim_data") and not path.endswith("_0.npy"): - monte_carlo_data = np.dstack((monte_carlo_data, np.array(np.load(os.path.join(folder_path, 'SimData', path))))) + monte_carlo_data = np.dstack((monte_carlo_data, np.array(pd.read_csv(os.path.join(folder_path, 'SimData', path))))) # Reshape the data to make it easier to work with (Different samples on first axis, different states on second axis, timesteps on third axis) monte_carlo_data = np.transpose(monte_carlo_data, (2, 1, 0)) - + # Define the output folder as the 'figures' folder in the monte carlo output folder output_folder = os.path.join(folder_path, 'figures') - fig, ax = plotState("x", nominal_data, monte_carlo_data, output_folder=output_folder) - plotState("kf_x", nominal_data, monte_carlo_data, output_folder=output_folder) - plotRMSE("x", "kf_x", ["accel_x", "baro_alt"], nominal_data, monte_carlo_data, output_folder=output_folder) - plotStateAndEstimate("x", "kf_x", nominal_data, monte_carlo_data, output_folder=output_folder) + fig, ax = plotState("pos_x", nominal_data, monte_carlo_data, output_folder=output_folder) + plotState("kalman_pos_x", nominal_data, monte_carlo_data, output_folder=output_folder) + plotRMSE("pos_x", "kalman_pos_x", ["accel_x", "baro_alt"], nominal_data, monte_carlo_data, output_folder=output_folder) + plotStateAndEstimate("pos_x", "kalman_pos_x", nominal_data, monte_carlo_data, output_folder=output_folder) # Plot 3D trajectory - velocity = np.sqrt(nominal_data[:, indices["vx"]]**2 + nominal_data[:, indices["vy"]]**2 + nominal_data[:, indices["vz"]]**2) - plotTrajectory3D(["z", "y", "x"], nominal_data, monte_carlo_data, velocity, "cool", output_folder=output_folder) + velocity = np.sqrt(nominal_data[:, indices["vel_x"]]**2 + nominal_data[:, indices["vel_y"]]**2 + nominal_data[:, indices["vel_z"]]**2) + plotTrajectory3D(["pos_z", "pos_y", "pos_x"], nominal_data, monte_carlo_data, velocity, "cool", output_folder=output_folder) plt.show() \ No newline at end of file diff --git a/Simulation/6DOF_RK4/Executables/analysis/monte_carlo_plotter_plotly.py b/Simulation/6DOF_RK4/Executables/analysis/monte_carlo_plotter_plotly.py new file mode 100644 index 0000000..3d81c13 --- /dev/null +++ b/Simulation/6DOF_RK4/Executables/analysis/monte_carlo_plotter_plotly.py @@ -0,0 +1,156 @@ +import numpy as np +import pandas as pd +import os +import sys +import plotly.graph_objects as go +from plotly.graph_objs import Mesh3d + +sys.path.insert(0, os.path.abspath( + os.path.join(os.path.dirname(__file__), '..','..','Configurations'))) + +import data_loader as dataloader +config = dataloader.config + +# Indices of each state variable in the output array +# kf and rkf values are arrays containing position, velocity and acceleration in each axis +indices = {"time":0, + "pos_x":1, + "pos_y":2, + "pos_z":3, + "vel_x":4, + "vel_y":5, + "vel_z":6, + "accel_x":7, + "accel_y":8, + "accel_z":9, + "ang_pos_x":10, + "ang_pos_y":11, + "ang_pos_z":12, + "ang_vel_x":13, + "ang_vel_y":14, + "ang_vel_z":15, + "ang_accel_x":16, + "ang_accel_y":17, + "ang_accel_z":18, + "alpha":19, + "rocket_total_mass":20, + "motor_mass":21, + "flap_ext":22, + "baro_alt":23, + "imu_accel_x":24, + "imu_accel_y":25, + "imu_accel_z":26, + "imu_ang_pos_x":27, + "imu_ang_pos_y":28, + "imu_ang_pos_z":29, + "imu_gyro_x":30, + "imu_gyro_y":31, + "imu_gyro_z":32, + "apogee_estimate":33, + "kalman_pos_x":34, + "kalman_vel_x":35, + "kalman_accel_x":36, + "kalman_pos_y":37, + "kalman_vel_y":38, + "kalman_accel_y":39, + "kalman_pos_z":40, + "kalman_vel_z":41, + "kalman_accel_z":42, + "pos_cov_x":43, + "vel_cov_x":44, + "accel_cov_x":45, + "pos_cov_y":46, + "vel_cov_y":47, + "accel_cov_y":48, + "pos_cov_z":49, + "vel_cov_z":50, + "accel_cov_z":51, + "kalman_rpos_x":52, + "kalman_rvel_x":53, + "kalman_raccel_x":54, + "kalman_rpos_y":55, + "kalman_rvel_y":56, + "kalman_raccel_y":57, + "kalman_rpos_z":58, + "kalman_rvel_z":59, + "kalman_raccel_z":60} + +default_output_folder = 'Output' +def rmse(predictions, targets): + return np.sqrt(((predictions - targets) ** 2).mean()) + +def std_dev(data:np.ndarray): + return np.std(data) + +def plotTrajectory3D(states:list, nominal_data:np.ndarray, monte_carlo_data:np.ndarray, color_data:np.ndarray=None, cmap:str=None, ax=None, output_folder=None): + fig = go.Figure() + # Make sure the data is 3D + if len(states) != 3: + raise ValueError("Can only plot 3D trajectories for 3 states") + + # Create arrays for the data + nominal_states = np.zeros((len(states), len(nominal_data))) + monte_carlo_states = np.zeros((len(monte_carlo_data), len(states), len(monte_carlo_data[0][0]))) + + # Generate the data from the nominal and monte carlo data + for i, state in enumerate(states): + nominal_states[i,:] = nominal_data[:, indices[state]] + monte_carlo_states[:,i,:] = monte_carlo_data[:, indices[state], :] + fig.add_trace(go.Scatter3d(x=nominal_states[0,:], y=nominal_states[1,:], z=nominal_states[2,:], mode='lines', name='Nominal', line=dict(color=velocity, width=4))) + for i in range(len(monte_carlo_data)): + fig.add_trace(go.Scatter3d(x=monte_carlo_states[i,0,:], y=monte_carlo_states[i,1,:], z=monte_carlo_states[i,2,:], mode='lines', name=f'Monte Carlo {i}', line=dict(color=velocity, width=4))) + + std_devs = np.zeros(int(len(monte_carlo_states[0][0])/10)) + for i in range(0, len(monte_carlo_states[0][0]), 10): + # Generate points on a circle + for j in range(len(states)): + std_devs[int(i/10)] += (3*std_dev(monte_carlo_states[:,j,i]))**2 + std_devs[int(i/10)] = np.sqrt(std_devs[int(i/10)]) + + # Iterate over each timestep in the standard deviation matrix + for i in range(len(std_devs)): + phi = np.linspace(0, 2*np.pi) + theta = np.linspace(-np.pi/2, np.pi/2) + phi, theta=np.meshgrid(phi, theta) + + xs = ((std_devs[0]*np.cos(theta) * np.sin(phi)) + nominal_states[0,i*10]) + ys = (std_devs[1]*np.cos(theta) * np.cos(phi) + nominal_states[1,i*10]) + zs = (std_devs[2]*np.sin(theta) + nominal_states[2,i*10]) + fig.add_surface(x=xs, y=ys, z=zs, showscale=False) + + fig.update_layout( + title="Monte Python Trajectories", + scene = dict( + xaxis_title='X [m]', + yaxis_title='Y [m]', + zaxis_title='Z [m]'), + font=dict( + family="Courier New, monospace", + size=18, + color="RebeccaPurple" + ) + ) + fig.show() + +if __name__ == "__main__": + folder_path = os.path.join(os.path.dirname(__file__), config["meta"]["monte_carlo_output_folder"]) + nominal_data = pd.read_csv(os.path.join(folder_path, 'SimData/nominal.csv')).to_numpy() + monte_carlo_data = pd.read_csv(os.path.join(folder_path, 'SimData/sim_data_0.csv')).to_numpy() + + for path in os.listdir(os.path.join(folder_path, 'SimData')): + if path.startswith("sim_data") and not path.endswith("_0.csv"): + monte_carlo_data = np.dstack((monte_carlo_data, np.array(pd.read_csv(os.path.join(folder_path, 'SimData', path))))) + + # Reshape the data to make it easier to work with (Different samples on first axis, different states on second axis, timesteps on third axis) + monte_carlo_data = np.transpose(monte_carlo_data, (2, 1, 0)) + + # Define the output folder as the 'figures' folder in the monte carlo output folder + output_folder = os.path.join(folder_path, 'figures') + # fig, ax = plotState("pos_x", nominal_data, monte_carlo_data, output_folder=output_folder) + # plotState("kalman_pos_x", nominal_data, monte_carlo_data, output_folder=output_folder) + # plotRMSE("pos_x", "kalman_pos_x", ["accel_x", "baro_alt"], nominal_data, monte_carlo_data, output_folder=output_folder) + # plotStateAndEstimate("pos_x", "kalman_pos_x", nominal_data, monte_carlo_data, output_folder=output_folder) + + # Plot 3D trajectory + velocity = np.sqrt(nominal_data[:, indices["vel_x"]]**2 + nominal_data[:, indices["vel_y"]]**2 + nominal_data[:, indices["vel_z"]]**2) + plotTrajectory3D(["pos_z", "pos_y", "pos_x"], nominal_data, monte_carlo_data, velocity, "cool", output_folder=output_folder) \ No newline at end of file diff --git a/Simulation/6DOF_RK4/plotter/rasaero_plots.py b/Simulation/6DOF_RK4/Executables/analysis/rasaero_plots.py similarity index 100% rename from Simulation/6DOF_RK4/plotter/rasaero_plots.py rename to Simulation/6DOF_RK4/Executables/analysis/rasaero_plots.py diff --git a/Simulation/6DOF_RK4/Executables/simulations/monte_python.py b/Simulation/6DOF_RK4/Executables/simulations/monte_python.py new file mode 100644 index 0000000..dac6a3d --- /dev/null +++ b/Simulation/6DOF_RK4/Executables/simulations/monte_python.py @@ -0,0 +1,81 @@ +import numpy as np +import os +import sys +import shutil +import time + +sys.path.insert(0, os.path.abspath( + os.path.join(os.path.dirname(__file__), '..','..','Core'))) + +import simulation as simulation +import dynamics.rocket as rocket_model +import dynamics.propagator as propagtor +import environment.atmosphere as atmosphere +import util.data_process as data_process + +import properties as prop +import data_loader as dataloader +import pysim as pysim + +def monte(dt, kf_dt, samples, output_folder, target_size, clear_contents=True, **kwargs) -> None: + '''Method which handles running the simulation and logging sim data to dict + + Args: + x0 (np.array): state vector initialized to 0s [6x3] + x: y: z: + [[pos, pos, pos], + [vel, vel, vel], + [accel, accel, accel], + [ang_pos, ang_pos, ang_pos], + [ang_vel, ang_vel, ang_vel], + [ang_accel, ang_accel, ang_accel]] + dt (float): time step between each iteration in simulation + ''' + atm, rocket = simulation.gen_sim_objs(dataloader.config, kwargs) + sim = propagtor.Simulator(atm=atm, rocket=rocket) + if clear_contents and os.path.exists(output_folder): + shutil.rmtree(output_folder) + if not os.path.exists(output_folder): + os.makedirs(output_folder) + os.mkdir(f"{output_folder}/figures") + os.mkdir(f"{output_folder}/SimData") + + print("Running Monte Python") + t_start = time.time() + # Nominal run + print("Calculating nominal trajectory") + record = pysim.pysim(np.zeros((6,3)), dt, kf_dt) + data_process.save_record(record, os.path.join(output_folder, "SimData", "nominal.csv"), target_size=target_size) + + # Monte Carlo runs + for i in range(samples): + print(f"Running sample {i+1} of {samples}") + atm, rocket = simulation.gen_sim_objs(dataloader.config, kwargs) + sim = propagtor.Simulator(atm=atm, rocket=rocket) + x0 = np.zeros((6,3)) + x0[3:6] = np.random.normal(kwargs.get("launch_angle_mean", 0), kwargs.get("launch_angle_stddev",0), (3,)) + pysim.simulator(x0, sim, rocket, atm, dt, kf_dt) + record = rocket.to_list() + data_process.save_record(record, os.path.join(output_folder, "SimData", f"sim_data_{i}.csv"), target_size=target_size) + t_end = time.time() - t_start + print(f"Monte Python Runtime: {t_end:.2f} seconds") + +config = dataloader.config +dt = 0.1 +kf_dt = 0.1 + +target_size = 880 +samples = 10 + +output_folder = os.path.join(os.path.dirname(__file__), config["meta"]["monte_carlo_output_folder"]) + +sim_params = {"wind_direction_stddev": 0, + "wind_magnitude_stddev": 10, + "enable_direction_variance": True, + "enable_magnitude_variance": True, + "nominal_wind_magnitude": 0.0, + "nominal_wind_direction": np.array([0, 1.0, 0]), + "launch_angle_mean": .00, + "launch_angle_stddev": 0.00} + +monte(dt, kf_dt, samples, output_folder, target_size, **sim_params) \ No newline at end of file diff --git a/Simulation/6DOF_RK4/Executables/simulations/pysim.py b/Simulation/6DOF_RK4/Executables/simulations/pysim.py new file mode 100644 index 0000000..b56a314 --- /dev/null +++ b/Simulation/6DOF_RK4/Executables/simulations/pysim.py @@ -0,0 +1,86 @@ +# ______ _____ _ __________ ___________ +# | ___ \ / ___(_) / ___| _ \ _ | ___| +# | |_/ / _\ `--. _ _ __ ___ / /___| | | | | | | |_ +# | __/ | | |`--. \ | '_ ` _ \ | ___ \ | | | | | | _| +# | | | |_| /\__/ / | | | | | | | \_/ | |/ /\ \_/ / | +# \_| \__, \____/|_|_| |_| |_| \_____/___/ \___/\_| +# __/ | +# |___/ + +# A 6DOF RK-4 Based simulation that uses RASAero aerodynamic data and known motor thrust data to simulate motion of the rocket +# with simulated sensor data as well as a implementation of the Extended Kalman Filter and active drag PID controller for the ISS +# Spaceshot entry for the 2023 IREC competition. This simulation was used to quantify the effects of the airbrakes, test +# different system design methodlogies, and provide preliminary tuning for the EKF and controller prior to implementation in +# SILSIM and flight software. +# +# 2022-2023 Guidance, Navigation, and Control Main Contributors # +# Sub-Team Lead: Parth Shrotri (2024) +# Colin Kinsey (2024) +# Evan Yu (2025) +# Rithvik Bhogavilli (2025) +# Kabir Cheema (2025) +# Freya Bansal (2025) +# Ishaan Bansal (2025) +# Ethan Pereira (2026) + +import numpy as np +import os +import sys +import time + +sys.path.insert(0, os.path.abspath( + os.path.join(os.path.dirname(__file__), '..','..','Core'))) + +import simulation as simulation +import dynamics.rocket as rocket_model +import dynamics.propagator as propagtor +import environment.atmosphere as atmosphere +import util.data_process as data_process + +import data_loader as dataloader + +def simulator(x0, sim, rocket, atm, dt, kf_dt) -> None: + '''Method which handles running the simulation and logging sim data to dict + + Args: + x0 (np.array): state vector initialized to 0s [6x3] + x: y: z: + [[pos, pos, pos], + [vel, vel, vel], + [accel, accel, accel], + [ang_pos, ang_pos, ang_pos], + [ang_vel, ang_vel, ang_vel], + [ang_accel, ang_accel, ang_accel]] + dt (float): time step between each iteration in simulation + ''' + simulator = simulation.Simulation(sim, rocket, atm, dt, x0, kf_dt, stages=rocket.stages) + simulator.idle_stage() + t_start = time.time() + simulator.run_stages() + simulator.coast() + t_end = time.time() - t_start + print(f"Runtime: {t_end:.2f} seconds") + +def pysim(x0, dt=0.01, kf_dt=0.01, config=dataloader.config): + # Load desired config file + + atm = atmosphere.Atmosphere() + + stages = [] + for stage in config['rocket']['stages'][1:]: + stages.append(rocket_model.Rocket(stage, atm=atm)) + rocket = rocket_model.Rocket(config['rocket']['stages'][0], atm=atm, stages=stages) + + sim = propagtor.Simulator(atm=atm, rocket=rocket) + + simulator(x0, sim, rocket, atm, dt, kf_dt) + return rocket.to_list() + +if __name__ == "__main__": + config = dataloader.config + x0 = np.zeros((6, 3)) + x0[3] = [0, 0.05, 0] + record = pysim(x0) + print("Simulation complete") + output_file = os.path.join(os.path.dirname(__file__), config["meta"]["output_file"]) + data_process.save_record(record, output_file) \ No newline at end of file diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/nominal.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/nominal.npy deleted file mode 100644 index 8060038..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/nominal.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_0.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_0.npy deleted file mode 100644 index 195cdac..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_0.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_1.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_1.npy deleted file mode 100644 index a59ceeb..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_1.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_10.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_10.npy deleted file mode 100644 index ff33923..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_10.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_11.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_11.npy deleted file mode 100644 index 3f3036b..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_11.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_12.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_12.npy deleted file mode 100644 index 51f63c2..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_12.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_13.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_13.npy deleted file mode 100644 index b93265d..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_13.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_14.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_14.npy deleted file mode 100644 index f732354..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_14.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_15.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_15.npy deleted file mode 100644 index f28f67a..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_15.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_16.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_16.npy deleted file mode 100644 index 240228b..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_16.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_17.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_17.npy deleted file mode 100644 index ff53212..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_17.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_18.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_18.npy deleted file mode 100644 index 29b9651..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_18.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_19.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_19.npy deleted file mode 100644 index 37b8411..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_19.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_2.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_2.npy deleted file mode 100644 index d2e05e6..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_2.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_20.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_20.npy deleted file mode 100644 index 0922598..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_20.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_21.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_21.npy deleted file mode 100644 index da3ed7b..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_21.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_22.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_22.npy deleted file mode 100644 index 3346b06..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_22.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_23.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_23.npy deleted file mode 100644 index 4eb57b7..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_23.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_24.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_24.npy deleted file mode 100644 index af43bd0..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_24.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_25.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_25.npy deleted file mode 100644 index a6b8437..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_25.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_26.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_26.npy deleted file mode 100644 index 3f241e5..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_26.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_27.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_27.npy deleted file mode 100644 index 7c5f4e6..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_27.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_28.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_28.npy deleted file mode 100644 index 6355f1c..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_28.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_29.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_29.npy deleted file mode 100644 index 2944f0c..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_29.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_3.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_3.npy deleted file mode 100644 index b499af1..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_3.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_4.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_4.npy deleted file mode 100644 index 6d98461..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_4.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_5.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_5.npy deleted file mode 100644 index 408bc96..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_5.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_6.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_6.npy deleted file mode 100644 index ddc9845..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_6.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_7.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_7.npy deleted file mode 100644 index 0174f19..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_7.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_8.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_8.npy deleted file mode 100644 index 2373ff3..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_8.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_9.npy b/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_9.npy deleted file mode 100644 index a950acf..0000000 Binary files a/Simulation/6DOF_RK4/Output/monte_carlo/SimData/sim_data_9.npy and /dev/null differ diff --git a/Simulation/6DOF_RK4/simulation/monte_python.py b/Simulation/6DOF_RK4/simulation/monte_python.py deleted file mode 100644 index 993ae47..0000000 --- a/Simulation/6DOF_RK4/simulation/monte_python.py +++ /dev/null @@ -1,348 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import os -import sys -import shutil - -sys.path.insert(0, os.path.abspath( - os.path.join(os.path.dirname(__file__), '..'))) - -import estimation.ekf as ekf -import estimation.r_ekf as r_ekf -import properties.properties as prop -import properties.data_loader as dataloader -import simulator as sim_class -# import plotter.plotSIM as plotter -import dynamics.sensors as sensors -import time -import estimation.apogee_estimator as apg -import dynamics.rocket as rocket_model -import environment.atmosphere as atmosphere -import dynamics.controller as contr - -config = dataloader.config - -def append_to_array(array, x, time_stamp, baro_alt, accel, bno_ang_pos, gyro, kalman_filter, kalman_filter_r, alpha, apogee_estimation, rocket_total_mass, motor_mass, flap_ext): - '''Append data to array - - Args: - array (np.array): array to append to - x (np.array): state vector of rocket - time_stamp (float): current time stamp - baro_alt (float): barometric altitude - accel (np.array): accelerometer data - bno_ang_pos (np.array): bno055 angular position - gyro (np.array): gyroscope data - kalman_filter (np.ndarray): Linear kalman filter data - kalman_filter_r (np.ndarray): Rotational kalman filter data - alpha (float): angle of attack - apogee_estimation (float): current apogee estimate - rocket_total_mass (float): current rocket mass - motor_mass (float): current motor mass - flap_ext (float): current flap extension - - Returns: - (np.array): Array with new datapoint appended - ''' - - datapoint = np.array([]) - # Simulation true values - datapoint = np.append(datapoint, x[0]) - datapoint = np.append(datapoint, x[1]) - datapoint = np.append(datapoint, x[2]) - datapoint = np.append(datapoint, x[3]) - datapoint = np.append(datapoint, x[4]) - datapoint = np.append(datapoint, x[5]) - datapoint = np.append(datapoint, time_stamp) - datapoint = np.append(datapoint, flap_ext) - datapoint = np.append(datapoint, alpha) - datapoint = np.append(datapoint, rocket_total_mass) - datapoint = np.append(datapoint, motor_mass) - - # Sensor values - datapoint = np.append(datapoint, baro_alt) - datapoint = np.append(datapoint, accel[0]) - datapoint = np.append(datapoint, accel[1]) - datapoint = np.append(datapoint, accel[2]) - datapoint = np.append(datapoint, bno_ang_pos[0]) - datapoint = np.append(datapoint, bno_ang_pos[1]) - datapoint = np.append(datapoint, bno_ang_pos[2]) - datapoint = np.append(datapoint, gyro[0]) - datapoint = np.append(datapoint, gyro[1]) - datapoint = np.append(datapoint, gyro[2]) - datapoint = np.append(datapoint, apogee_estimation) - - # Linear Kalman filter values - datapoint = np.append(datapoint, kalman_filter[0:3]) - datapoint = np.append(datapoint, kalman_filter[3:6]) - datapoint = np.append(datapoint, kalman_filter[6:9]) - - # Rotational Kalman filter values - datapoint = np.append(datapoint, kalman_filter_r[0:3]) - datapoint = np.append(datapoint, kalman_filter_r[3:6]) - datapoint = np.append(datapoint, kalman_filter_r[6:9]) - - return np.vstack((array, datapoint)) if len(array) != 0 else np.array([datapoint]) - - -def simulator(x0, dt, sample_number, run_folder, target_size:int, nominal:bool=False, wind_direction_variance_mean:float=0, - wind_direction_variance_stddev:float=0.01, - wind_magnitude_variance_mean:float=0, - wind_magnitude_variance_stddev:float=0.5, - enable_direction_variance:bool=None, - enable_magnitude_variance:bool=None, - nominal_wind_direction:np.ndarray=np.array([-1,0,0]), - nominal_wind_magnitude:float=0): - '''Runs the simulation with the given x0 and wind variance parameters - - Args: - x0 (np.ndarray): initial state vector - dt (float): time step between each iteration in simulation - wind_direction_variance_mean (float, optional): mean of wind direction variance. Defaults to 0. - wind_direction_variance_stddev (float, optional): standard deviation of wind direction variance. Defaults to 0.01. - wind_magnitude_variance_mean (float, optional): mean of wind magnitude variance. Defaults to 0. - wind_magnitude_variance_stddev (float, optional): standard deviation of wind magnitude variance. Defaults to 0.5. - nominal_wind_direction (np.ndarray, optional): nominal wind direction. Defaults to np.array([-1,0,0]). - nominal_wind_magnitude (float, optional): nominal wind magnitude. Defaults to 0. - ''' - t_start = time.time() - enable_direction_variance = not nominal if enable_direction_variance is None else enable_direction_variance - enable_magnitude_variance = not nominal if enable_magnitude_variance is None else enable_magnitude_variance - atm = atmosphere.Atmosphere(wind_direction_variance_mean=wind_direction_variance_mean, - wind_direction_variance_stddev=wind_direction_variance_stddev, - wind_magnitude_variance_mean=wind_magnitude_variance_mean, - wind_magnitude_variance_stddev=wind_magnitude_variance_stddev, - enable_direction_variance=enable_direction_variance, - enable_magnitude_variance=enable_magnitude_variance, - nominal_wind_direction=nominal_wind_direction, - nominal_wind_magnitude=nominal_wind_magnitude) - - # rocket = rocket_model.Rocket(atm=atm) - rocket = rocket_model.Rocket(config['rocket']['stages'][0], atm=atm) - motor = rocket.motor - sim = sim_class.Simulator(atm=atm, rocket=rocket) - - # Array to store the data from this simulation. Each row is a datapoint. This replaces the 3 dictionaries used in main.py - # for the performance improvement offered by .npy files over .csv files. - sim_data = np.array([]) - - # Get sensor config data - sensor_config = rocket.stage_config['sensors'] - - x = x0.copy() - # Random variations to initial state - if not nominal: - x[0,0] *= np.random.uniform(0.9, 1.1) - x[0,1] *= np.random.uniform(0.9, 1.1) - x[0,2] *= np.random.uniform(0.9, 1.1) - - kalman_filter = ekf.KalmanFilter( - dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - accel = sensors.get_accelerometer_data(x, sensor_config) - x_data = [] - y_data = [] - z_data = [] - for i in range(10): - reading = sensors.get_accelerometer_data(x, sensor_config) - x_data.append(reading[0]) - y_data.append(reading[1]) - z_data.append(reading[2]) - - ax = sum(x_data)/len(x_data) - ay = sum(y_data)/len(y_data) - az = sum(z_data)/len(z_data) - - pitch = -1 * (np.arctan2(-az,-ay) + np.pi/2) - yaw = np.arctan2(-ax,-ay) + np.pi/2 - r_kalman_filter = r_ekf.KalmanFilter_R( - dt, 0.0, 0.0, 0.0, pitch, 0.0, 0.0, yaw, 0.0, 0.0) - # r_kalman_filter = r_ekf.KalmanFilter_R( - # dt, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - time_stamp = 0 - - # Use an n value (last parameter) that is divisible by 3 to make computations easier - apogee_estimator = apg.Apogee(kalman_filter.get_state(), 0.1, 0.01, 3, 30, atm, rocket.stage_config) - Kp, Ki, Kd = 0.0002, 0, 0 - controller = contr.Controller(Kp, Ki, Kd, dt, config['desired_apogee'], rocket.stage_config) - - # Idle stage - while time_stamp < rocket.delay: - time_stamp += dt - baro_alt = sensors.get_barometer_data(x, sensor_config) - accel = sensors.get_accelerometer_data(x, sensor_config) - gyro = sensors.get_gyro_data(x, sensor_config) - bno_ang_pos = sensors.get_bno_orientation(x, sensor_config) - - kalman_filter.priori(np.array([0.0, 0.0, 0.0, 0.0])) - kalman_filter.update(bno_ang_pos, baro_alt, - accel[0], accel[1], accel[2]) - - r_kalman_filter.priori() - r_kalman_filter.update(*gyro, *accel) - - kalman_filter.reset_lateral_pos() - current_state = kalman_filter.get_state() - current_state_r = r_kalman_filter.get_state() - x = np.round(x, 3) - time_stamp = np.round(time_stamp, 3) - baro_alt = np.round(baro_alt, 3) - accel = np.round(accel, 3) - bno_ang_pos = np.round(bno_ang_pos, 3) - gyro = np.round(gyro, 3) - current_state = np.round(current_state, 3) - current_state_r = np.round(current_state_r, 3) - rocket.rocket_total_mass = np.round(rocket.rocket_total_mass, 3) - rocket.motor_mass = np.round(rocket.motor_mass, 3) - sim_data = append_to_array(sim_data, x, time_stamp, baro_alt, accel, bno_ang_pos, gyro, current_state, current_state_r, 0, current_state[0], rocket.rocket_total_mass, rocket.motor_mass, 0) - - - # print("Ignition") - - # # while x[1][prop.vertical] > prop.apogee_thresh and x[0][prop.vertical] > prop.start_thresh: - start = True - - while x[1, 0] >= 0 or start: - if start: - start = False - # print("Timestamp: ", time_stamp) - # Get sensor data - baro_alt = sensors.get_barometer_data(x, sensor_config) - accel = sensors.get_accelerometer_data(x, sensor_config) - gyro = sensors.get_gyro_data(x, sensor_config) - bno_ang_pos = sensors.get_bno_orientation(x, sensor_config) - - # Kalman Filter stuff goes here - kalman_filter.priori(np.array([0.0, 0.0, 0.0, 0.0])) - kalman_filter.update(bno_ang_pos, baro_alt, - accel[0], accel[1], accel[2]) - - r_kalman_filter.priori() - r_kalman_filter.update(*gyro, *accel) - - current_state = kalman_filter.get_state() - current_state_r = r_kalman_filter.get_state() - - apogee_est = apogee_estimator.predict_apogee(current_state[0:3]) - - flap_ext = controller.get_flap_extension(time_stamp > rocket.stage_config["motor"]["delay"] and np.linalg.norm(motor.get_thrust(time_stamp)) <= 0, apogee_est) - - # flap_ext will be passed by kalman filter - rocket.set_motor_mass(time_stamp) - # rocket.motor_mass = motor.get_mass(time_stamp) - - x, alpha = sim.RK4(x, dt, time_stamp, flap_ext, density_noise=True) - time_stamp += dt - - x = np.round(x, 3) - time_stamp = np.round(time_stamp, 3) - baro_alt = np.round(baro_alt, 3) - accel = np.round(accel, 3) - bno_ang_pos = np.round(bno_ang_pos, 3) - gyro = np.round(gyro, 3) - current_state = np.round(current_state, 3) - current_state_r = np.round(current_state_r, 3) - alpha = np.round(alpha, 3) - apogee_est = np.round(apogee_est, 3) - rocket.rocket_total_mass = np.round(rocket.rocket_total_mass, 3) - rocket.motor_mass = np.round(rocket.motor_mass, 3) - flap_ext = np.round(flap_ext, 3) - - sim_data = append_to_array(sim_data, x, time_stamp, baro_alt, accel, bno_ang_pos, gyro, current_state, current_state_r, alpha, apogee_est, rocket.rocket_total_mass, rocket.motor_mass, flap_ext) - - # Post processing to make sure all datasets are the same size - if len(sim_data) > target_size: - sim_data = sim_data[:target_size] - elif len(sim_data) < target_size: - while len(sim_data) < target_size: - sim_data = np.append(sim_data, np.array([sim_data[-1]]), axis=0) - - print(sim_data.shape) - filename = 'nominal.npy' if nominal else f'sim_data_{sample_number}.npy' - np.save(f"{run_folder}/SimData/{filename}", sim_data) - t_end = time.time() - t_start - print(f"Time: {t_end:.2f}") - - -def run(x0, dt, samples:int, run_folder:str, target_size:int, clear_contents:bool=False, - wind_direction_variance_mean:float=0.0, - wind_direction_variance_stddev:float=0.01, - wind_magnitude_variance_mean:float=0.0, - wind_magnitude_variance_stddev:float=0.5, - enable_direction_variance:bool=None, - enable_magnitude_variance:bool=None, - nominal_wind_direction:np.ndarray=np.array([-1.0, 0.0, 0.0]), - nominal_wind_magnitude:float=0.0): - if clear_contents and os.path.exists(run_folder): - shutil.rmtree(run_folder) - if not os.path.exists(run_folder): - os.mkdir(run_folder) - os.mkdir(f"{run_folder}/figures") - os.mkdir(f"{run_folder}/SimData") - - # Calculate nominal trajectory - print("Calculating nominal trajectory") - simulator(x0, dt, 0, run_folder, target_size, nominal=True, - nominal_wind_direction=nominal_wind_direction, - nominal_wind_magnitude=nominal_wind_magnitude) - print("Running Monte Carlo simulations") - for i in range(samples): - print(f"Running sample {i+1} of {samples}") - simulator(x0, dt, i, run_folder, target_size, wind_direction_variance_mean=wind_direction_variance_mean, - wind_direction_variance_stddev=wind_direction_variance_stddev, - wind_magnitude_variance_mean=wind_magnitude_variance_mean, - wind_magnitude_variance_stddev=wind_magnitude_variance_stddev, - enable_direction_variance=enable_direction_variance, - enable_magnitude_variance=enable_magnitude_variance, - nominal_wind_direction=nominal_wind_direction, - nominal_wind_magnitude=nominal_wind_magnitude) - - print("Done") - -if __name__ == "__main__": - '''Main function for running the simulator - - Set the initial conditions for the simulation in x0 using the following scheme: - x: y: z: - [[pos, pos, pos], - [vel, vel, vel], - [accel, accel, accel], - [ang_pos, ang_pos, ang_pos], - [ang_vel, ang_vel, ang_vel], - [ang_accel, ang_accel, ang_accel]] - - The time step is set by dt, and the number of samples is set by samples. The timestep is set to 0.1s instead of the default - standard 0.01s used in main.py to speed up the simulation for testing. For higher fidelity simulations, use 0.01s. Similarly, - the number of samples is set to 30 instead of something higher (100-1000) to speed up the simulation. Future work will include - adding multiprocessing to speed up the simulation when using larger sample sizes. - - Target size is the number of data points to be saved for each sample. This ensures that each - sample has the same number of data points. - - The output folder is set by output_folder. This is where the data will be saved. There is a folder - saved in properties that can be used by default, but you can also change it if you want to save multiple runs with - different parameters. - - The nominal trajectory is defined as the trajectory that the rocket would take if there were no wind. This is useful for quantifying - trajectory uncertainty, but not as useful for quantifying sensor noise. To analyze sensor noise, disable the direction and magnitude - variance by setting enable_direction_variance and enable_magnitude_variance to False. - ''' - # Set up the rocket - x0 = np.zeros((6,3)) - x0[3] = np.array([0.0, 0.05, 0.01]) - dt = 0.1 - target_size = 880 - samples = 30 - # Calculate nominal trajectory - output_folder = os.path.join(os.path.dirname(__file__), config["meta"]["monte_carlo_output_folder"]) - run(x0, - dt, - samples, - output_folder, - target_size, - clear_contents=True, - wind_direction_variance_stddev=0.2, - wind_magnitude_variance_stddev=3.0, - enable_direction_variance=True, - enable_magnitude_variance=True, - nominal_wind_magnitude=2, - nominal_wind_direction=np.array([0,1,0])) \ No newline at end of file