├── .gitignore ├── LICENSE.md ├── README.md ├── basilisk_env ├── __init__.py ├── envs │ ├── __init__.py │ ├── leoPowerAttitudeEnvironment.py │ └── opNavEnvironment.py └── simulators │ ├── __init__.py │ ├── dynamics │ ├── __init__.py │ └── effectorPrimatives │ │ ├── __init__.py │ │ └── actuatorPrimatives.py │ ├── initial_conditions │ ├── __init__.py │ ├── leo_orbit.py │ └── sc_attitudes.py │ ├── leoPowerAttitudeSimulator.py │ ├── opNavSimulator.py │ ├── opNav_models │ ├── BSK_OpNavDynamics.py │ ├── BSK_OpNavFsw.py │ ├── BSK_masters.py │ ├── OpNav_Plotting.py │ └── opnav_boi-v1k.zip │ └── scenario_OpNavOD.py ├── envSetup.sh └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | */__pycache__/* 3 | basilisk_env.egg-info -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | Copyright 2019, Autonomous Vehicle Systems Laboratory 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 4 | 5 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 6 | 7 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # basilisk_env: A Basilisk-based Astrodynamics Reinforcement Learning Problems Package 2 | 3 | basilisk_env is a package of reference planning and decision-making problems built on both the AVS Basilisk simulation 4 | engine (https://hanspeterschaub.info/bskMain.html) and using the OpenAI gym API (https://gym.openai.com/) 5 | ## Getting Started 6 | 7 | These instructions will give you the information needed to install and use gym_orbit. 8 | 9 | ### Prerequsites 10 | gym_orbit requires the following Python packages: 11 | 12 | - Numerics and plotting: numpy, scipy, matplotlib, pandas 13 | - Spacecraft Simulation: Basilisk 14 | - Machine Learning + Agent Libraries: tensorflow, keras, gym, stable-baselines 15 | 16 | ### Installation 17 | 18 | To install the package, run: 19 | 20 | ``` 21 | pip install -e . 22 | ``` 23 | 24 | while in the base directory. Test the installation by opening a Python terminal and calling 25 | 26 | ``` 27 | import gym 28 | import basilisk_env 29 | ``` 30 | 31 | Optical Navigation scenarios are run with the Basilisk Vizard visualization suite. This can be downloaded from (http://hanspeterschaub.info/basilisk/Vizard/VizardDownload.html). 32 | 33 | To use Vizard with basilisk-env, you must set up environment variables corresponding to the vizard application directory and address/port ('viz_app','viz_address', and 'viz_port', respectively). For example: 34 | 35 | ``` 36 | $ export viz_app = '/mnt/c/Users/aharr/OneDrive/dev/Vizard/vizard20201211L/Vizard.exe' 37 | $ export viz_port = '5556' 38 | $ export viz_address = '172.20.128.1' 39 | ``` 40 | 41 | ## Repository Structure 42 | 43 | `basilisk_env` is broken into two principal components: `/envs`, which contains environment wrappers for Basilisk, 44 | and `simulators`, which contains Basilisk simulation definitions for various scenarios. 45 | 46 | Within `simulators`, core simulation routines representing interchangable dynamics and flight software stacks 47 | are stored in `simulators/dynamics` and `simulators/fsw` respectively. 48 | 49 | 50 | ### Contributions 51 | 52 | 53 | 54 | ## Authors 55 | Maintainer: Mr. Andrew T. Harris (andrew.t.harris@colorado.edu) 56 | 57 | Mr. Thibaud Teil (Thibaud.Teil@colorado.edu) 58 | 59 | ## License 60 | 61 | This project is licensed under the MIT license - see the [LICENSE.md](LICENSE.md) file for details. 62 | 63 | ## Acknowledgements 64 | 65 | [The Autonomous Vehicle Systems Laboratory](http://hanspeterschaub.info/main.html) 66 | -------------------------------------------------------------------------------- /basilisk_env/__init__.py: -------------------------------------------------------------------------------- 1 | import logging 2 | from gym.envs.registration import register 3 | 4 | logger = logging.getLogger(__name__) 5 | 6 | register( 7 | id='leo_power_att_env-v0', 8 | entry_point='basilisk_env.envs:leoPowerAttEnv' 9 | ) 10 | 11 | # register( 12 | # id='opnav_env-v0', 13 | # entry_point='basilisk_env.envs:opNavEnv' 14 | # ) -------------------------------------------------------------------------------- /basilisk_env/envs/__init__.py: -------------------------------------------------------------------------------- 1 | from basilisk_env.envs.leoPowerAttitudeEnvironment import leoPowerAttEnv 2 | from basilisk_env.envs.opNavEnvironment import opNavEnv -------------------------------------------------------------------------------- /basilisk_env/envs/leoPowerAttitudeEnvironment.py: -------------------------------------------------------------------------------- 1 | # 3rd party modules 2 | import gym 3 | import numpy as np 4 | import scipy as sci 5 | from scipy.linalg import expm 6 | from gym import spaces 7 | import copy 8 | 9 | from basilisk_env.simulators import leoPowerAttitudeSimulator 10 | from Basilisk.utilities import macros as mc 11 | from Basilisk.utilities import orbitalMotion as om 12 | 13 | 14 | class leoPowerAttEnv(gym.Env): 15 | """ 16 | Simple attitude/orbit control problem. The spacecraft must decide when to point at the ground (which generates a 17 | reward) versus pointing at the sun (which increases the sim duration). 18 | """ 19 | 20 | def __init__(self): 21 | self.__version__ = "0.1.0" 22 | print("Basilisk Attitude Mode Management Sim - Version {}".format(self.__version__)) 23 | 24 | # General variables defining the environment 25 | self.max_length =int(3*180) # Specify the maximum number of planning intervals 26 | 27 | # Tell the environment that it doesn't have a sim attribute... 28 | self.simulator_init = 0 29 | self.simulator = None 30 | self.simulator_backup = None 31 | self.reward_total = 0 32 | 33 | # Set some attributes for the simulator; parameterized such that they can be varied in-sim 34 | self.mass = 330.0 # kg 35 | self.powerDraw = -5. # W 36 | self.wheel_limit = 3000*mc.RPM # 3000 RPM in radians/s 37 | self.power_max = 20.0 # W/Hr 38 | 39 | # Set up options, constants for this environment 40 | self.step_duration = 180. # Set step duration equal to 1 minute (180min ~ 2 orbits) 41 | self.reward_mult = 1./self.max_length # Normalize reward to episode duration; '1' represents 100% ground observation time 42 | self.failure_penalty = 1 # Default is 50. 43 | low = -1e16 44 | high = 1e16 45 | self.observation_space = spaces.Box(low, high,shape=(5,1)) 46 | self.obs = np.zeros([5,]) 47 | 48 | ## Action Space description 49 | # 0 - earth pointing (mission objective) 50 | # 1 - sun pointing (power objective) 51 | # 2 - desaturation (required for long-term pointing) 52 | 53 | self.action_space = spaces.Discrete(3) 54 | 55 | # Store what the agent tried 56 | self.curr_episode = -1 57 | self.action_episode_memory = [] 58 | self.curr_step = 0 59 | self.episode_over = False 60 | 61 | def _seed(self): 62 | np.random.seed() 63 | return 64 | 65 | def step(self, action): 66 | """ 67 | The agent takes a step in the environment. 68 | Parameters 69 | ---------- 70 | action : int 71 | Returns 72 | ------- 73 | ob, reward, episode_over, info : tuple 74 | ob (object) : 75 | an environment-specific object representing your observation of 76 | the environment. 77 | reward (float) : 78 | amount of reward achieved by the previous action. The scale 79 | varies between environments, but the goal is always to increase 80 | your total reward. 81 | episode_over (bool) : 82 | whether it's time to reset the environment again. Most (but not 83 | all) tasks are divided up into well-defined episodes, and done 84 | being True indicates the episode has terminated. (For example, 85 | perhaps the pole tipped too far, or you lost your last life.) 86 | info (dict) : 87 | diagnostic information useful for debugging. It can sometimes 88 | be useful for learning (for example, it might contain the raw 89 | probabilities behind the environment's last state change). 90 | However, official evaluations of your agent are not allowed to 91 | use this for learning. 92 | """ 93 | 94 | if self.simulator_init == 0: 95 | self.simulator = leoPowerAttitudeSimulator.LEOPowerAttitudeSimulator(.1, 1.0, self.step_duration, mass=self.mass, powerDraw = self.powerDraw) 96 | self.simulator_init = 1 97 | 98 | if self.curr_step >= self.max_length: 99 | self.episode_over = True 100 | 101 | prev_ob = self._get_state() 102 | self._take_action(action) 103 | 104 | reward = self._get_reward() 105 | self.reward_total += reward 106 | ob = self._get_state() 107 | ob[2] = ob[2] / self.wheel_limit # Normalize reaction wheel speed to fraction of limit 108 | ob[3] = ob[3] / self.power_max # Normalize current power to fraction of total power 109 | # If the wheel speeds get too large, end the episode. 110 | if ob[2] > 1: 111 | self.episode_over = True 112 | reward -= self.failure_penalty 113 | self.reward_total -= self.failure_penalty 114 | print("Died from wheel explosion. RPMs were norm:"+str(ob[2]*self.wheel_limit)+", limit is "+str(self.wheel_limit)+", body rate was "+str(ob[1])+"action taken was "+str(action)+", env step"+str(self.curr_step)) 115 | print("Prior state was RPM:"+str(prev_ob[2]*self.wheel_limit)+" . body rate was:"+str(prev_ob[1])) 116 | 117 | 118 | # If we run out of power, end the episode. 119 | if ob[3] == 0: 120 | self.episode_over = True 121 | reward -= self.failure_penalty 122 | self.reward_total -= self.failure_penalty 123 | print("Ran out of power. Battery level was at at:"+str(prev_ob[3])+", env step"+str(self.curr_step-1)) 124 | 125 | if self.sim_over: 126 | self.episode_over = True 127 | print("Orbit decayed - no penalty, but this one is over.") 128 | 129 | 130 | if self.episode_over: 131 | info = {'episode':{ 132 | 'r': self.reward_total, 133 | 'l': self.curr_step}, 134 | 'full_states': self.debug_states, 135 | 'obs': ob 136 | } 137 | self.simulator.close_gracefully() # Stop spice from blowing up 138 | else: 139 | info={ 140 | 'full_states': self.debug_states, 141 | 'obs': ob 142 | } 143 | 144 | self.curr_step += 1 145 | return ob, reward, self.episode_over, info 146 | 147 | def _take_action(self, action): 148 | ''' 149 | Interfaces with the simulator to 150 | :param action: 151 | :return: 152 | ''' 153 | 154 | # print(self.curr_episode) 155 | 156 | self.action_episode_memory[self.curr_episode].append(action) 157 | 158 | # Let the simulator handle action management: 159 | self.obs, self.debug_states, self.sim_over = self.simulator.run_sim(action) 160 | 161 | def _get_reward(self): 162 | """ 163 | Reward is based on time spent with the inertial attitude pointed towards the ground within a given tolerance. 164 | 165 | """ 166 | reward = 0 167 | 168 | if self.action_episode_memory[self.curr_episode][-1] == 0: 169 | reward = np.linalg.norm(self.reward_mult / (1. + self.obs[0]**2.0)) 170 | return reward 171 | 172 | def reset(self): 173 | """ 174 | Reset the state of the environment and returns an initial observation. 175 | Returns 176 | ------- 177 | observation (object): the initial observation of the space. 178 | """ 179 | 180 | self.action_episode_memory.append([]) 181 | self.episode_over = False 182 | self.curr_step = 0 183 | self.reward_total = 0 184 | del(self.simulator) # Force delete the sim to make sure nothing funky happens under the hood 185 | self.simulator = leoPowerAttitudeSimulator.LEOPowerAttitudeSimulator(.1, 1.0, self.step_duration) 186 | self.simulator_init = 1 187 | self.seed() 188 | ob = copy.deepcopy(self.simulator.obs) 189 | ob[2] = ob[2] / self.wheel_limit # Normalize reaction wheel speed to fraction of limit 190 | ob[3] = ob[3] / self.power_max # Normalize current power to fraction of total power 191 | return ob 192 | 193 | def _render(self, mode='human', close=False): 194 | return 195 | 196 | def _get_state(self): 197 | """Get the observation. 198 | WIP: Work out which error representation to give the algo.""" 199 | 200 | return self.simulator.obs 201 | 202 | def reset_init(self): 203 | self.action_episode_memory.append([]) 204 | self.episode_over = False 205 | self.curr_step = 0 206 | self.reward_total = 0 207 | 208 | initial_conditions = self.simulator.initial_conditions 209 | del(self.simulator) 210 | self.simulator = leoPowerAttitudeSimulator.LEOPowerAttitudeSimulator(.1, 1.0, self.step_duration, initial_conditions) 211 | 212 | self.simulator_init = 1 213 | ob = copy.deepcopy(self.simulator.obs) 214 | ob[2] = ob[2] / self.wheel_limit # Normalize reaction wheel speed to fraction of limit 215 | ob[3] = ob[3] / self.power_max # Normalize current power to fraction of total power 216 | return ob 217 | 218 | if __name__=="__main__": 219 | env = gym.make('leo_power_att_env-v0') 220 | hist_list = [] 221 | # Loop through the env twice 222 | for ind in range(0,2): 223 | hist = np.zeros([5,2*env.max_length]) 224 | env.reset() 225 | env.seed(seed=12345) 226 | for step in range(0,env.max_length): 227 | ob, reward, ep_over, info = env.step(0) 228 | hist[:,step] = ob[:,0] 229 | if ep_over: 230 | break 231 | hist_list.append(hist) 232 | 233 | from matplotlib import pyplot as plt 234 | for count,hist in enumerate(hist_list): 235 | plt.figure() 236 | plt.plot(range(0,env.max_length*2),hist[0,:],label='attitude norm') 237 | plt.plot(range(0,env.max_length*2),hist[1,:],label='rate norm') 238 | plt.plot(range(0,env.max_length*2),hist[2,:],label='Battery Level') 239 | plt.plot(range(0,env.max_length*2),hist[3,:],label='wheel norm') 240 | plt.plot(range(0,env.max_length*2),hist[4,:],label='Eclipse ind') 241 | plt.grid() 242 | plt.legend() 243 | plt.title(f'History of run {count}') 244 | plt.show() -------------------------------------------------------------------------------- /basilisk_env/envs/opNavEnvironment.py: -------------------------------------------------------------------------------- 1 | # 3rd party modules 2 | import gym 3 | import numpy as np 4 | import subprocess 5 | import os 6 | from gym import spaces 7 | 8 | from basilisk_env.simulators import opNavSimulator 9 | 10 | 11 | 12 | class opNavEnv(gym.Env): 13 | """ 14 | OpNav scenario. The spacecraft must decide when to point at the ground (which generates a 15 | reward) versus pointing at the sun (which increases the sim duration). 16 | """ 17 | 18 | def __init__(self): 19 | self.__version__ = "0.0.2" 20 | print("Basilisk OpNav Mode Management Sim - Version {}".format(self.__version__)) 21 | 22 | # General variables defining the environment 23 | self.max_length =int(40) # Specify the maximum number of planning intervals 24 | 25 | # Tell the environment that it doesn't have a sim attribute... 26 | self.sim_init = 0 27 | self.simulator = None 28 | self.reward_total = 0 29 | 30 | # Set up options, constants for this environment 31 | self.step_duration = 50. # Set step duration equal to 60 minute 32 | self.reward_mult = 1. 33 | low = -1e16 34 | high = 1e16 35 | self.observation_space = spaces.Box(low, high,shape=(4,1)) 36 | self.obs = np.zeros([4,]) 37 | self.debug_states = np.zeros([12,]) 38 | ## Action Space description 39 | # 0 - earth pointing (mission objective) 40 | # 1 - sun pointing (power objective) 41 | # 2 - desaturation (required for long-term pointing) 42 | 43 | self.action_space = spaces.Discrete(2) 44 | 45 | # Store what the agent tried 46 | self.curr_episode = -1 47 | self.action_episode_memory = [] 48 | self.curr_step = 0 49 | self.episode_over = False 50 | 51 | def _seed(self): 52 | np.random.seed() 53 | return 54 | 55 | def step(self, action): 56 | """ 57 | The agent takes a step in the environment. 58 | Parameters 59 | ---------- 60 | action : int 61 | Returns 62 | ------- 63 | ob, reward, episode_over, info : tuple 64 | ob (object) : 65 | an environment-specific object representing your observation of 66 | the environment. 67 | reward (float) : 68 | amount of reward achieved by the previous action. The scale 69 | varies between environments, but the goal is always to increase 70 | your total reward. 71 | episode_over (bool) : 72 | whether it's time to reset the environment again. Most (but not 73 | all) tasks are divided up into well-defined episodes, and done 74 | being True indicates the episode has terminated. (For example, 75 | perhaps the pole tipped too far, or you lost your last life.) 76 | info (dict) : 77 | diagnostic information useful for debugging. It can sometimes 78 | be useful for learning (for example, it might contain the raw 79 | probabilities behind the environment's last state change). 80 | However, official evaluations of your agent are not allowed to 81 | use this for learning. 82 | """ 83 | 84 | if self.sim_init == 0: 85 | 86 | self.simulator = opNavSimulator.scenario_OpNav(1., 1., self.step_duration) 87 | self.sim_init = 1 88 | 89 | 90 | 91 | if self.curr_step%10 == 0: 92 | print("At step ", self.curr_step, " of ", self.max_length) 93 | 94 | if self.curr_step >= self.max_length: 95 | self.episode_over = True 96 | 97 | prev_ob = self._get_state() 98 | self._take_action(action) 99 | 100 | reward = self._get_reward() 101 | self.reward_total += reward 102 | ob = self._get_state() 103 | 104 | if self.sim_over: 105 | self.episode_over = True 106 | print("End of episode") 107 | 108 | 109 | if self.episode_over: 110 | info = {'episode':{ 111 | 'r': self.reward_total, 112 | 'l': self.curr_step}, 113 | 'full_states': self.debug_states, 114 | 'obs': ob 115 | } 116 | self.simulator.close_gracefully() # Stop spice from blowing up 117 | self.sim_init = 0 118 | else: 119 | info={ 120 | 'full_states': self.debug_states, 121 | 'obs': ob 122 | } 123 | 124 | self.curr_step += 1 125 | return ob, reward, self.episode_over, info 126 | 127 | def _take_action(self, action): 128 | ''' 129 | Interfaces with the simulator to 130 | :param action: 131 | :return: 132 | ''' 133 | 134 | self.action_episode_memory[self.curr_episode].append(action) 135 | 136 | # Let the simulator handle action management: 137 | self.obs, self.debug_states, self.sim_over = self.simulator.run_sim(action) 138 | 139 | def _get_reward(self): 140 | """ 141 | Reward is based on time spent with the inertial attitude pointed towards the ground within a given tolerance. 142 | 143 | """ 144 | reward = 0 145 | real = np.array([self.debug_states[3],self.debug_states[4], self.debug_states[5]]) 146 | nav = np.array([self.debug_states[0],self.debug_states[1], self.debug_states[2]]) 147 | nav -= real 148 | nav *= 1./np.linalg.norm(real) 149 | 150 | if self.action_episode_memory[self.curr_episode][-1] == 1: 151 | reward = np.linalg.norm(self.reward_mult / (1. + np.linalg.norm(nav)**2.0)) 152 | return reward 153 | 154 | def reset(self): 155 | """ 156 | Reset the state of the environment and returns an initial observation. 157 | Returns 158 | ------- 159 | observation (object): the initial observation of the space. 160 | """ 161 | 162 | self.action_episode_memory.append([]) 163 | self.episode_over = False 164 | self.curr_step = 0 165 | self.reward_total = 0 166 | self.simulator = opNavSimulator.scenario_OpNav(1., 1., self.step_duration) 167 | self.sim_init=1 168 | 169 | return self.simulator.obs 170 | 171 | def _render(self, mode='human', close=False): 172 | return 173 | 174 | def _get_state(self): 175 | """Get the observation. 176 | WIP: Work out which error representation to give the algo.""" 177 | 178 | return self.simulator.obs -------------------------------------------------------------------------------- /basilisk_env/simulators/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/atharris/basilisk_env/8fa610520859eb020ad226e9dedb38fb1c1ad678/basilisk_env/simulators/__init__.py -------------------------------------------------------------------------------- /basilisk_env/simulators/dynamics/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/atharris/basilisk_env/8fa610520859eb020ad226e9dedb38fb1c1ad678/basilisk_env/simulators/dynamics/__init__.py -------------------------------------------------------------------------------- /basilisk_env/simulators/dynamics/effectorPrimatives/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/atharris/basilisk_env/8fa610520859eb020ad226e9dedb38fb1c1ad678/basilisk_env/simulators/dynamics/effectorPrimatives/__init__.py -------------------------------------------------------------------------------- /basilisk_env/simulators/dynamics/effectorPrimatives/actuatorPrimatives.py: -------------------------------------------------------------------------------- 1 | from Basilisk.simulation import thrusterDynamicEffector, reactionWheelStateEffector 2 | from Basilisk.utilities import simIncludeThruster, simIncludeRW 3 | from Basilisk.utilities import fswSetupThrusters, fswSetupRW 4 | from Basilisk.utilities import macros as mc 5 | from numpy.random import uniform 6 | 7 | def balancedHR16Triad(useRandom = False, randomBounds = (-400,400)): 8 | """ 9 | Creates a set of eight ADCS thrusters using MOOG Monarc-1 attributes. 10 | Returns a set of thrusters and thrusterFac instance to add thrusters to a spacecraft. 11 | @return thrusterSet: thruster dynamic effector instance 12 | @return thrusterFac: factory containing defined thrusters 13 | """ 14 | rwFactory = simIncludeRW.rwFactory() 15 | varRWModel = rwFactory.BalancedWheels 16 | if useRandom: 17 | 18 | wheelSpeeds = uniform(randomBounds[0],randomBounds[1],3) 19 | 20 | RW1 = rwFactory.create('Honeywell_HR16' 21 | , [1, 0, 0] 22 | , maxMomentum=50. 23 | , Omega=wheelSpeeds[0] # RPM 24 | , RWModel=varRWModel 25 | ) 26 | RW2 = rwFactory.create('Honeywell_HR16' 27 | , [0, 1, 0] 28 | , maxMomentum=50. 29 | , Omega=wheelSpeeds[1] # RPM 30 | , RWModel=varRWModel 31 | ) 32 | RW3 = rwFactory.create('Honeywell_HR16' 33 | , [0, 0, 1] 34 | , maxMomentum=50. 35 | , Omega=wheelSpeeds[2] # RPM 36 | , RWModel=varRWModel 37 | ) 38 | else: 39 | RW1 = rwFactory.create('Honeywell_HR16' 40 | , [1, 0, 0] 41 | , maxMomentum=50. 42 | , Omega=500. # RPM 43 | , RWModel=varRWModel 44 | ) 45 | RW2 = rwFactory.create('Honeywell_HR16' 46 | , [0, 1, 0] 47 | , maxMomentum=50. 48 | , Omega=500. # RPM 49 | 50 | , RWModel=varRWModel 51 | ) 52 | RW3 = rwFactory.create('Honeywell_HR16' 53 | , [0, 0, 1] 54 | , maxMomentum=50. 55 | , Omega=500. # RPM 56 | , rWB_B=[0.5, 0.5, 0.5] # meters 57 | , RWModel=varRWModel 58 | ) 59 | wheelSpeeds = [500,500,500] 60 | numRW = rwFactory.getNumOfDevices() 61 | rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector() 62 | 63 | return rwStateEffector, rwFactory, wheelSpeeds*mc.RPM 64 | 65 | 66 | def idealMonarc1Octet(): 67 | """ 68 | Creates a set of eight ADCS thrusters using MOOG Monarc-1 attributes. 69 | Returns a set of thrusters and thrusterFac instance to add thrusters to a spacecraft. 70 | @return thrusterSet: thruster dynamic effector instance 71 | @return thrusterFac: factory containing defined thrusters 72 | """ 73 | location = [ 74 | [ 75 | 3.874945160902288e-2, 76 | -1.206182747348013, 77 | 0.85245 78 | ], 79 | [ 80 | 3.874945160902288e-2, 81 | -1.206182747348013, 82 | -0.85245 83 | ], 84 | [ 85 | -3.8749451609022656e-2, 86 | -1.206182747348013, 87 | 0.85245 88 | ], 89 | [ 90 | -3.8749451609022656e-2, 91 | -1.206182747348013, 92 | -0.85245 93 | ], 94 | [ 95 | -3.874945160902288e-2, 96 | 1.206182747348013, 97 | 0.85245 98 | ], 99 | [ 100 | -3.874945160902288e-2, 101 | 1.206182747348013, 102 | -0.85245 103 | ], 104 | [ 105 | 3.8749451609022656e-2, 106 | 1.206182747348013, 107 | 0.85245 108 | ], 109 | [ 110 | 3.8749451609022656e-2, 111 | 1.206182747348013, 112 | -0.85245 113 | ] 114 | ] 115 | 116 | direction = [ 117 | [ 118 | -0.7071067811865476, 119 | 0.7071067811865475, 120 | 0.0 121 | ], 122 | [ 123 | -0.7071067811865476, 124 | 0.7071067811865475, 125 | 0.0 126 | ], 127 | [ 128 | 0.7071067811865475, 129 | 0.7071067811865476, 130 | 0.0 131 | ], 132 | [ 133 | 0.7071067811865475, 134 | 0.7071067811865476, 135 | 0.0 136 | ], 137 | [ 138 | 0.7071067811865476, 139 | -0.7071067811865475, 140 | 0.0 141 | ], 142 | [ 143 | 0.7071067811865476, 144 | -0.7071067811865475, 145 | 0.0 146 | ], 147 | [ 148 | -0.7071067811865475, 149 | -0.7071067811865476, 150 | 0.0 151 | ], 152 | [ 153 | -0.7071067811865475, 154 | -0.7071067811865476, 155 | 0.0 156 | ] 157 | ] 158 | thrusterSet = thrusterDynamicEffector.ThrusterDynamicEffector() 159 | thFactory = simIncludeThruster.thrusterFactory() 160 | for pos_B, dir_B in zip(location, direction): 161 | thFactory.create('MOOG_Monarc_1', pos_B, dir_B) 162 | return thrusterSet, thFactory -------------------------------------------------------------------------------- /basilisk_env/simulators/initial_conditions/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/atharris/basilisk_env/8fa610520859eb020ad226e9dedb38fb1c1ad678/basilisk_env/simulators/initial_conditions/__init__.py -------------------------------------------------------------------------------- /basilisk_env/simulators/initial_conditions/leo_orbit.py: -------------------------------------------------------------------------------- 1 | from Basilisk.utilities import orbitalMotion 2 | from Basilisk.utilities import macros as mc 3 | 4 | from numpy.random import uniform 5 | 6 | def inclined_circular_300km(): 7 | """ 8 | Returns an inclined, circular LEO orbit. 9 | :return: 10 | """ 11 | 12 | mu = 0.3986004415E+15 13 | oe = orbitalMotion.ClassicElements() 14 | oe.a = 6371 * 1000.0 + 300. * 1000 15 | oe.e = 0.0 16 | oe.i = 45.0 * mc.D2R 17 | 18 | oe.Omega = 0.0 * mc.D2R 19 | oe.omega = 0.0 * mc.D2R 20 | oe.f = 0.0 * mc.D2R 21 | rN, vN = orbitalMotion.elem2rv(mu, oe) 22 | 23 | return oe, rN, vN 24 | 25 | def sampled_400km(): 26 | """ 27 | Returns an elliptical, prograde LEO orbit with an SMA of 400km. 28 | :return: 29 | """ 30 | mu = 0.3986004415E+15 31 | oe = orbitalMotion.ClassicElements() 32 | oe.a = 6371 * 1000.0 + 500. * 1000 33 | oe.e = uniform(0,0.05, 1) 34 | oe.i = uniform(-90*mc.D2R, 90*mc.D2R,1) 35 | oe.Omega = uniform(0*mc.D2R, 360*mc.D2R,1) 36 | oe.omega = uniform(0*mc.D2R, 360*mc.D2R,1) 37 | oe.f = uniform(0*mc.D2R, 360*mc.D2R,1) 38 | rN, vN = orbitalMotion.elem2rv(mu, oe) 39 | 40 | return oe, rN, vN -------------------------------------------------------------------------------- /basilisk_env/simulators/initial_conditions/sc_attitudes.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def random_tumble(maxSpinRate = 0.001): 4 | """ 5 | Simulates a spacecraft in a random tumble with uniformly sampled initial conditions. 6 | :return: sigma_bn 7 | :return: omega_bn 8 | """ 9 | 10 | sigma_bn = np.random.uniform(0,1.0,[3,]) 11 | omega_bn = np.random.uniform(-maxSpinRate, maxSpinRate,[3,]) 12 | 13 | return sigma_bn, omega_bn 14 | 15 | def static_inertial(): 16 | """ 17 | Simulates a spacecraft in a sidereal stare fixed to the inertial origin. 18 | :return: 19 | """ 20 | 21 | sigma_bn = np.zeros([3,]) 22 | omega_bn = np.zeros([3,]) 23 | 24 | return sigma_bn, omega_bn -------------------------------------------------------------------------------- /basilisk_env/simulators/leoPowerAttitudeSimulator.py: -------------------------------------------------------------------------------- 1 | # 3rd party modules 2 | import numpy as np 3 | 4 | # Basilisk modules 5 | from Basilisk.utilities import SimulationBaseClass 6 | from Basilisk.utilities import macros as mc 7 | from Basilisk.utilities import unitTestSupport 8 | from Basilisk.utilities import orbitalMotion 9 | from Basilisk.simulation import sim_model 10 | 11 | from Basilisk.simulation import (spacecraftPlus, gravityEffector, extForceTorque, simple_nav, spice_interface, 12 | eclipse, imu_sensor, exponentialAtmosphere, facetDragDynamicEffector, planetEphemeris, reactionWheelStateEffector, 13 | thrusterDynamicEffector) 14 | from Basilisk.simulation import simMessages 15 | from Basilisk.utilities import simIncludeRW, simIncludeGravBody, simIncludeThruster 16 | from Basilisk.utilities import RigidBodyKinematics as rbk 17 | from Basilisk.utilities import simulationArchTypes 18 | from Basilisk.simulation import simpleBattery, simplePowerSink, simpleSolarPanel 19 | from Basilisk import __path__ 20 | bskPath = __path__[0] 21 | 22 | from Basilisk.fswAlgorithms import inertial3D, hillPoint, celestialTwoBodyPoint 23 | from Basilisk.fswAlgorithms import attTrackingError 24 | from Basilisk.fswAlgorithms import MRP_Feedback, rwMotorTorque 25 | from Basilisk.fswAlgorithms import fswMessages 26 | from Basilisk.fswAlgorithms import inertialUKF 27 | from Basilisk.fswAlgorithms import thrMomentumManagement, thrMomentumDumping, thrForceMapping, thrFiringSchmitt 28 | 29 | from basilisk_env.simulators.dynamics.effectorPrimatives import actuatorPrimatives as ap 30 | from basilisk_env.simulators.initial_conditions import leo_orbit, sc_attitudes 31 | from numpy.random import uniform 32 | 33 | class LEOPowerAttitudeSimulator(SimulationBaseClass.SimBaseClass): 34 | ''' 35 | Simulates a spacecraft in LEO with atmospheric drag and J2. 36 | 37 | Dynamics Components 38 | - Forces: J2, Atmospheric Drag w/ COM offset 39 | - Environment: Exponential density model; eclipse 40 | - Actuators: ExternalForceTorque 41 | - Sensors: SimpleNav 42 | - Systems: SimpleBattery, SimpleSink, SimpleSolarPanel 43 | 44 | FSW Components: 45 | - MRP_Feedback controller 46 | - inertial3d (sun pointing), hillPoint (nadir pointing) 47 | - [WIP, not implemented] inertialEKF attitude filter 48 | 49 | Action Space (discrete, 0 or 1): 50 | 0 - orients the spacecraft towards the sun. 51 | 1 - orients the spacecraft towards Earth. 52 | [WIP, not implemented] 2 - Runs inertialEKF to better estimate the spacecraft state. 53 | 54 | Observation Space: 55 | eclipseIndicator - float [0,1] - indicates the flux mitigator due to eclipse. 56 | storedCharge - float [0,batCapacity] - indicates the s/c battery charge level in W-s. 57 | |sigma_RB| - float [0,1] - norm of the spacecraft error MRP with respect to the last reference frame specified. 58 | |diag(filter_covar)| - float - norm of the diagonal of the EKF covariance matrix at that time. 59 | 60 | Reward Function: 61 | r = 1/(1+ | sigma_RB|) if action=1 62 | Intended to provide a rich reward in action 1 when the spacecraft is pointed towards the earth, decaying as sigma^2 63 | as the pointing error increases. 64 | 65 | :return: 66 | ''' 67 | def __init__(self, dynRate, fswRate, step_duration, initial_conditions=None): 68 | ''' 69 | Creates the simulation, but does not initialize the initial conditions. 70 | ''' 71 | self.dynRate = dynRate 72 | self.fswRate = fswRate 73 | self.step_duration = step_duration 74 | 75 | SimulationBaseClass.SimBaseClass.__init__(self) 76 | self.TotalSim.terminateSimulation() 77 | 78 | self.simTime = 0.0 79 | 80 | # If no initial conditions are defined yet, set ICs 81 | if initial_conditions == None: 82 | self.initial_conditions=self.set_ICs() 83 | # If ICs were passed through, use the ones that were passed through 84 | else: 85 | self.initial_conditions = initial_conditions 86 | 87 | # Specify some simulation parameters 88 | self.mass = self.initial_conditions.get("mass") #kg 89 | self.powerDraw = self.initial_conditions.get("powerDraw") #W 90 | 91 | self.DynModels = [] 92 | self.FSWModels = [] 93 | 94 | # Initialize the dynamics+fsw task groups, modules 95 | 96 | self.DynamicsProcessName = 'DynamicsProcess' #Create simulation process name 97 | self.dynProc = self.CreateNewProcess(self.DynamicsProcessName) #Create process 98 | self.dynTaskName = 'DynTask' 99 | self.spiceTaskName = 'SpiceTask' 100 | self.envTaskName = 'EnvTask' 101 | self.dynTask = self.dynProc.addTask(self.CreateNewTask(self.dynTaskName, mc.sec2nano(self.dynRate))) 102 | self.spiceTask = self.dynProc.addTask(self.CreateNewTask(self.spiceTaskName, mc.sec2nano(self.step_duration))) 103 | self.envTask = self.dynProc.addTask(self.CreateNewTask(self.envTaskName, mc.sec2nano(self.dynRate))) 104 | 105 | self.obs = np.zeros([5,1]) 106 | self.sim_states = np.zeros([11,1]) 107 | 108 | self.set_dynamics() 109 | self.set_fsw() 110 | 111 | self.set_logging() 112 | self.previousPointingGoal = "sunPointTask" 113 | 114 | self.modeRequest = None 115 | self.InitializeSimulationAndDiscover() 116 | 117 | return 118 | 119 | def set_ICs(self): 120 | # Sample orbital parameters 121 | oe, rN,vN = leo_orbit.sampled_400km() 122 | 123 | # Sample attitude and rates 124 | sigma_init, omega_init = sc_attitudes.random_tumble(maxSpinRate=0.00001) 125 | 126 | # Dict of initial conditions 127 | initial_conditions = { 128 | # Mass 129 | "mass": 330, # kg 130 | 131 | # Orbital parameters 132 | "oe": oe, 133 | "rN": rN, 134 | "vN": vN, 135 | 136 | # Spacecraft dimensions 137 | "width": 1.38, 138 | "depth": 1.04, 139 | "height": 1.58, 140 | 141 | # Attitude and rate initialization 142 | "sigma_init": sigma_init, 143 | "omega_init": omega_init, 144 | 145 | # Atmospheric density 146 | "planetRadius": orbitalMotion.REQ_EARTH * 1000., 147 | "baseDensity": 1.22, #kg/m^3 148 | "scaleHeight": 8e3, #m 149 | 150 | # Disturbance Torque 151 | "disturbance_magnitude": 2e-4, 152 | "disturbance_vector": np.random.standard_normal(3), 153 | 154 | # Reaction Wheel speeds 155 | "wheelSpeeds": uniform(-800,800,3), # RPM 156 | 157 | # Solar Panel Parameters 158 | "nHat_B": np.array([0,-1,0]), 159 | "panelArea": 0.2*0.3, 160 | "panelEfficiency": 0.20, 161 | 162 | # Power Sink Parameters 163 | "powerDraw": -5.0, # W 164 | 165 | # Battery Parameters 166 | "storageCapacity": 20.0 * 3600., 167 | "storedCharge_Init": np.random.uniform(8.*3600., 20.*3600., 1)[0], 168 | 169 | # Sun pointing FSW config 170 | "sigma_R0N": [1,0,0], 171 | 172 | # RW motor torque and thruster force mapping FSW config 173 | "controlAxes_B": [1, 0, 0, 174 | 0, 1, 0, 175 | 0, 0, 1], 176 | 177 | # Attitude controller FSW config 178 | "K": 7, 179 | "Ki": -1.0, # Note: make value negative to turn off integral feedback 180 | "P": 35, 181 | 182 | # Momentum dumping config 183 | "hs_min": 4., # Nms 184 | 185 | # Thruster force mapping FSW module 186 | "thrForceSign": 1, 187 | 188 | # Thruster momentum dumping FSW config 189 | "maxCounterValue": 4, 190 | "thrMinFireTime": 0.002 # Seconds 191 | } 192 | 193 | return initial_conditions 194 | 195 | def set_dynamics(self): 196 | ''' 197 | Sets up the dynamics modules for the sim. This simulator runs: 198 | scObject (spacecraft dynamics simulation) 199 | SpiceObject 200 | EclipseObject (simulates eclipse for simpleSolarPanel) 201 | extForceTorque (attitude actuation) 202 | simpleNav (attitude determination/sensing) 203 | simpleSolarPanel (attitude-dependent power generation) 204 | simpleBattery (power storage) 205 | simplePowerNode (constant power draw) 206 | 207 | By default, parameters are set to those for a 6U cubesat. 208 | :return: 209 | ''' 210 | sc_number=0 211 | 212 | # Spacecraft, Planet Setup 213 | self.scObject = spacecraftPlus.SpacecraftPlus() 214 | self.scObject.ModelTag = 'spacecraft' 215 | 216 | # clear prior gravitational body and SPICE setup definitions 217 | self.gravFactory = simIncludeGravBody.gravBodyFactory() 218 | # setup Spice interface for some solar system bodies 219 | timeInitString = '2021 MAY 04 07:47:48.965 (UTC)' 220 | self.gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/' 221 | , timeInitString 222 | , spicePlanetNames=["sun", "earth"] 223 | ) 224 | 225 | self.gravFactory.spiceObject.zeroBase="earth" # Make sure that the Earth is the zero base 226 | 227 | self.gravFactory.createSun() 228 | planet = self.gravFactory.createEarth() 229 | planet.isCentralBody = True # ensure this is the central gravitational body 230 | mu = planet.mu 231 | # attach gravity model to spaceCraftPlus 232 | self.scObject.gravField.gravBodies = spacecraftPlus.GravBodyVector(list(self.gravFactory.gravBodies.values())) 233 | 234 | oe = self.initial_conditions.get("oe") 235 | rN = self.initial_conditions.get("rN") 236 | vN = self.initial_conditions.get("vN") 237 | 238 | n = np.sqrt(mu / oe.a / oe.a / oe.a) 239 | P = 2. * np.pi / n 240 | 241 | width = self.initial_conditions.get("width") 242 | depth = self.initial_conditions.get("depth") 243 | height = self.initial_conditions.get("height") 244 | 245 | I = [1./12.*self.mass*(width**2. + depth**2.), 0., 0., 246 | 0., 1./12.*self.mass*(depth**2. + height**2.), 0., 247 | 0., 0.,1./12.*self.mass*(width**2. + height**2.)] 248 | 249 | self.scObject.hub.mHub = self.mass # kg 250 | self.scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) 251 | 252 | self.scObject.hub.r_CN_NInit = unitTestSupport.np2EigenVectorXd(rN) 253 | self.scObject.hub.v_CN_NInit = unitTestSupport.np2EigenVectorXd(vN) 254 | 255 | sigma_init = self.initial_conditions.get("sigma_init") 256 | omega_init = self.initial_conditions.get("omega_init") 257 | 258 | self.scObject.hub.sigma_BNInit = sigma_init # sigma_BN_B 259 | self.scObject.hub.omega_BN_BInit = omega_init 260 | 261 | self.sim_states[0:3,0] = np.asarray(self.scObject.hub.sigma_BNInit).flatten() 262 | self.sim_states[3:6,0] = np.asarray(self.scObject.hub.r_CN_NInit).flatten() 263 | self.sim_states[6:9,0] = np.asarray(self.scObject.hub.v_CN_NInit).flatten() 264 | 265 | self.densityModel = exponentialAtmosphere.ExponentialAtmosphere() 266 | self.densityModel.ModelTag = "expDensity" 267 | self.densityModel.addSpacecraftToModel(self.scObject.scStateOutMsgName) 268 | self.densityModel.planetRadius = self.initial_conditions.get("planetRadius") 269 | self.densityModel.baseDensity = self.initial_conditions.get("baseDensity") 270 | self.densityModel.scaleHeight = self.initial_conditions.get("scaleHeight") 271 | 272 | self.dragEffector = facetDragDynamicEffector.FacetDragDynamicEffector() 273 | # Set up the goemetry of a 6U cubesat 274 | self.dragEffector.addFacet(0.2*0.3, 2.2, [1,0,0], [0.05, 0.0, 0]) 275 | self.dragEffector.addFacet(0.2*0.3, 2.2, [-1,0,0], [0.05, 0.0, 0]) 276 | self.dragEffector.addFacet(0.1*0.2, 2.2, [0,1,0], [0, 0.15, 0]) 277 | self.dragEffector.addFacet(0.1*0.2, 2.2, [0,-1,0], [0, -0.15, 0]) 278 | self.dragEffector.addFacet(0.1*0.3, 2.2, [0,0,1], [0,0, 0.1]) 279 | self.dragEffector.addFacet(0.1*0.3, 2.2, [0,0,-1], [0, 0, -0.1]) 280 | self.dragEffector.addFacet(1.*2., 2.2, [0,1,0],[0,2.,0]) 281 | self.dragEffector.addFacet(1.*2., 2.2, [0,-1,0],[0,2.,0]) 282 | 283 | self.dragEffector.atmoDensInMsgName = self.densityModel.envOutMsgNames[-1] 284 | self.scObject.addDynamicEffector(self.dragEffector) 285 | 286 | self.eclipseObject = eclipse.Eclipse() 287 | self.eclipseObject.addPositionMsgName(self.scObject.scStateOutMsgName) 288 | self.eclipseObject.addPlanetName('earth') 289 | 290 | # Disturbance Torque Setup 291 | disturbance_magnitude = self.initial_conditions.get("disturbance_magnitude") 292 | disturbance_vector = self.initial_conditions.get("disturbance_vector") 293 | unit_disturbance = disturbance_vector/np.linalg.norm(disturbance_vector) 294 | self.extForceTorqueObject = extForceTorque.ExtForceTorque() 295 | self.extForceTorqueObject.extTorquePntB_B = disturbance_magnitude * disturbance_vector 296 | self.extForceTorqueObject.ModelTag = 'DisturbanceTorque' 297 | self.extForceTorqueObject.cmdForceInertialInMsgName = 'disturbance_torque' 298 | self.scObject.addDynamicEffector(self.extForceTorqueObject) 299 | 300 | # Add reaction wheels to the spacecraft 301 | self.rwStateEffector, rwFactory, initWheelSpeeds = ap.balancedHR16Triad(useRandom=True,randomBounds=(-800,800)) 302 | # Change the wheel speeds 303 | rwFactory.rwList["RW1"].Omega = self.initial_conditions.get("wheelSpeeds")[0]*mc.RPM #rad/s 304 | rwFactory.rwList["RW2"].Omega = self.initial_conditions.get("wheelSpeeds")[1]*mc.RPM #rad/s 305 | rwFactory.rwList["RW3"].Omega = self.initial_conditions.get("wheelSpeeds")[2]*mc.RPM #rad/s 306 | initWheelSpeeds = self.initial_conditions.get("wheelSpeeds") 307 | self.rwStateEffector.InputCmds = "rwTorqueCommand" 308 | rwFactory.addToSpacecraft("ReactionWheels", self.rwStateEffector, self.scObject) 309 | self.rwConfigMsgName = "rwConfig" 310 | unitTestSupport.setMessage(self.TotalSim, self.DynamicsProcessName, self.rwConfigMsgName, rwFactory.getConfigMessage(), msgStrName="RWArrayConfigFswMsg") 311 | 312 | # Add thrusters to the spacecraft 313 | self.thrusterSet, thrFactory = ap.idealMonarc1Octet() 314 | self.thrusterSet.InputCmds = "rwDesatTimeOnCmd" 315 | thrModelTag = "ACSThrusterDynamics" 316 | self.thrusterConfigMsgName = "thrusterConfig" 317 | unitTestSupport.setMessage(self.TotalSim, self.DynamicsProcessName, self.thrusterConfigMsgName, thrFactory.getConfigMessage(), msgStrName="THRArrayConfigFswMsg") 318 | thrFactory.addToSpacecraft(thrModelTag, self.thrusterSet, self.scObject) 319 | 320 | # Add simpleNav as a mock estimator to the spacecraft 321 | self.simpleNavObject = simple_nav.SimpleNav() 322 | self.simpleNavObject.ModelTag = 'SimpleNav' 323 | self.simpleNavObject.inputStateName = self.scObject.scStateOutMsgName 324 | 325 | # Power Setup 326 | self.solarPanel = simpleSolarPanel.SimpleSolarPanel() 327 | self.solarPanel.ModelTag = 'solarPanel' + str(sc_number) 328 | self.solarPanel.stateInMsgName = self.scObject.scStateOutMsgName 329 | self.solarPanel.sunEclipseInMsgName = 'eclipse_data_'+str(sc_number) 330 | self.solarPanel.sunInMsgName = 'sun_planet_data' 331 | self.solarPanel.setPanelParameters(unitTestSupport.np2EigenVectorXd(self.initial_conditions.get("nHat_B")), self.initial_conditions.get("panelArea"), self.initial_conditions.get("panelEfficiency")) 332 | self.solarPanel.nodePowerOutMsgName = "panelPowerMsg" + str(sc_number) 333 | 334 | self.powerSink = simplePowerSink.SimplePowerSink() 335 | self.powerSink.ModelTag = "powerSink" + str(sc_number) 336 | self.powerSink.nodePowerOut = self.powerDraw # Watts 337 | self.powerSink.nodePowerOutMsgName = "powerSinkMsg" + str(sc_number) 338 | 339 | self.powerMonitor = simpleBattery.SimpleBattery() 340 | self.powerMonitor.ModelTag = "powerMonitor" 341 | self.powerMonitor.batPowerOutMsgName = "powerMonitorMsg" 342 | self.powerMonitor.storageCapacity = self.initial_conditions.get("storageCapacity") 343 | self.powerMonitor.storedCharge_Init = self.initial_conditions.get("storedCharge_Init") 344 | self.powerMonitor.addPowerNodeToModel(self.solarPanel.nodePowerOutMsgName) 345 | self.powerMonitor.addPowerNodeToModel(self.powerSink.nodePowerOutMsgName) 346 | 347 | self.sim_states[9,0] = self.powerMonitor.storedCharge_Init 348 | self.obs[0,0] = np.linalg.norm(self.scObject.hub.sigma_BNInit) 349 | self.obs[1,0] = np.linalg.norm(self.scObject.hub.omega_BN_BInit) 350 | self.obs[2,0] = np.linalg.norm(initWheelSpeeds) 351 | self.obs[3,0] = self.powerMonitor.storedCharge_Init/3600.0 352 | 353 | 354 | 355 | # Add all the models to the dynamics task 356 | self.AddModelToTask(self.dynTaskName, self.scObject) 357 | self.AddModelToTask(self.spiceTaskName, self.gravFactory.spiceObject) 358 | self.AddModelToTask(self.dynTaskName, self.densityModel) 359 | self.AddModelToTask(self.dynTaskName, self.dragEffector) 360 | self.AddModelToTask(self.dynTaskName, self.simpleNavObject) 361 | self.AddModelToTask(self.dynTaskName, self.rwStateEffector) 362 | self.AddModelToTask(self.dynTaskName, self.thrusterSet) 363 | self.AddModelToTask(self.envTaskName, self.eclipseObject) 364 | self.AddModelToTask(self.envTaskName, self.solarPanel) 365 | self.AddModelToTask(self.envTaskName, self.powerMonitor) 366 | self.AddModelToTask(self.envTaskName, self.powerSink) 367 | 368 | return 369 | 370 | 371 | def set_fsw(self): 372 | ''' 373 | Sets up the attitude guidance stack for the simulation. This simulator runs: 374 | inertial3Dpoint - Sets the attitude guidance objective to point the main panel at the sun. 375 | hillPointTask: Sets the attitude guidance objective to point a "camera" angle towards nadir. 376 | attitudeTrackingError: Computes the difference between estimated and guidance attitudes 377 | mrpFeedbackControl: Computes an appropriate control torque given an attitude error 378 | :return: 379 | ''' 380 | 381 | self.processName = self.DynamicsProcessName 382 | self.processTasksTimeStep = mc.sec2nano(self.fswRate) # 0.5 383 | self.dynProc.addTask(self.CreateNewTask("sunPointTask", self.processTasksTimeStep),taskPriority=100) 384 | self.dynProc.addTask(self.CreateNewTask("nadirPointTask", self.processTasksTimeStep),taskPriority=100) 385 | self.dynProc.addTask(self.CreateNewTask("mrpControlTask", self.processTasksTimeStep), taskPriority=50) 386 | self.dynProc.addTask(self.CreateNewTask("rwDesatTask", self.processTasksTimeStep), taskPriority=100) 387 | 388 | # Specify the vehicle configuration message to tell things what the vehicle inertia is 389 | vehicleConfigOut = fswMessages.VehicleConfigFswMsg() 390 | # use the same inertia in the FSW algorithm as in the simulation 391 | # Set inertia properties to those of a solid 6U cubeoid: 392 | width = self.initial_conditions.get("width") 393 | depth = self.initial_conditions.get("depth") 394 | height = self.initial_conditions.get("height") 395 | I = [1. / 12. * self.mass * (width ** 2. + depth ** 2.), 0., 0., 396 | 0., 1. / 12. * self.mass * (depth ** 2. + height ** 2.), 0., 397 | 0., 0., 1. / 12. * self.mass * (width ** 2. + height ** 2.)] 398 | 399 | vehicleConfigOut.ISCPntB_B = I 400 | unitTestSupport.setMessage(self.TotalSim, 401 | self.DynamicsProcessName, 402 | "adcs_config_data", 403 | vehicleConfigOut) 404 | 405 | 406 | # Sun pointing configuration 407 | self.sunPointData = inertial3D.inertial3DConfig() 408 | self.sunPointWrap = self.setModelDataWrap(self.sunPointData) 409 | self.sunPointWrap.ModelTag = "sunPoint" 410 | self.sunPointData.outputDataName = "att_reference" 411 | self.sunPointData.sigma_R0N = self.initial_conditions.get("sigma_R0N") 412 | 413 | # Earth pointing configuration 414 | self.hillPointData = hillPoint.hillPointConfig() 415 | self.hillPointWrap = self.setModelDataWrap(self.hillPointData) 416 | self.hillPointWrap.ModelTag = "hillPoint" 417 | self.hillPointData.outputDataName = "att_reference" 418 | self.hillPointData.inputNavDataName = self.simpleNavObject.outputTransName 419 | self.hillPointData.inputCelMessName = self.gravFactory.gravBodies['earth'].bodyInMsgName 420 | 421 | # Attitude error configuration 422 | self.trackingErrorData = attTrackingError.attTrackingErrorConfig() 423 | self.trackingErrorWrap = self.setModelDataWrap(self.trackingErrorData) 424 | self.trackingErrorWrap.ModelTag = "trackingError" 425 | self.trackingErrorData.inputNavName = self.simpleNavObject.outputAttName 426 | # Note: SimBase.DynModels.simpleNavObject.outputAttName = "simple_att_nav_output" 427 | self.trackingErrorData.inputRefName = "att_reference" 428 | self.trackingErrorData.outputDataName = "att_guidance" 429 | 430 | # add module that maps the Lr control torque into the RW motor torques 431 | self.rwMotorTorqueConfig = rwMotorTorque.rwMotorTorqueConfig() 432 | self.rwMotorTorqueWrap = self.setModelDataWrap(self.rwMotorTorqueConfig) 433 | self.rwMotorTorqueWrap.ModelTag = "rwMotorTorque" 434 | self.rwMotorTorqueConfig.outputDataName = self.rwStateEffector.InputCmds 435 | self.rwMotorTorqueConfig.rwParamsInMsgName = "rwConfig" 436 | self.rwMotorTorqueConfig.inputVehControlName = "commandedControlTorque" 437 | self.rwMotorTorqueConfig.controlAxes_B = self.initial_conditions.get("controlAxes_B") 438 | 439 | # Attitude controller configuration 440 | self.mrpFeedbackControlData = MRP_Feedback.MRP_FeedbackConfig() 441 | self.mrpFeedbackControlWrap = self.setModelDataWrap(self.mrpFeedbackControlData) 442 | self.mrpFeedbackControlWrap.ModelTag = "mrpFeedbackControl" 443 | self.mrpFeedbackControlData.inputGuidName = "att_guidance" 444 | self.mrpFeedbackControlData.vehConfigInMsgName = "adcs_config_data" 445 | self.mrpFeedbackControlData.outputDataName = self.rwMotorTorqueConfig.inputVehControlName 446 | self.mrpFeedbackControlData.K = self.initial_conditions.get("K") 447 | self.mrpFeedbackControlData.Ki = self.initial_conditions.get("Ki") 448 | self.mrpFeedbackControlData.P = self.initial_conditions.get("P") 449 | self.mrpFeedbackControlData.integralLimit = 2. / self.mrpFeedbackControlData.Ki * 0.1 450 | 451 | # Momentum dumping configuration 452 | self.thrDesatControlConfig = thrMomentumManagement.thrMomentumManagementConfig() 453 | self.thrDesatControlWrap = self.setModelDataWrap(self.thrDesatControlConfig) 454 | self.thrDesatControlWrap.ModelTag = "thrMomentumManagement" 455 | self.thrDesatControlConfig.hs_min = self.initial_conditions.get("hs_min") # Nms 456 | self.thrDesatControlConfig.rwSpeedsInMsgName = self.rwStateEffector.OutputDataString 457 | self.thrDesatControlConfig.rwConfigDataInMsgName = self.rwConfigMsgName 458 | self.thrDesatControlConfig.deltaHOutMsgName = "wheelDeltaH" 459 | 460 | # setup the thruster force mapping module 461 | self.thrForceMappingConfig = thrForceMapping.thrForceMappingConfig() 462 | self.thrForceMappingWrap = self.setModelDataWrap(self.thrForceMappingConfig) 463 | self.thrForceMappingWrap.ModelTag = "thrForceMapping" 464 | self.thrForceMappingConfig.inputVehControlName = self.thrDesatControlConfig.deltaHOutMsgName 465 | self.thrForceMappingConfig.inputThrusterConfName = self.thrusterConfigMsgName 466 | self.thrForceMappingConfig.inputVehicleConfigDataName = self.mrpFeedbackControlData.vehConfigInMsgName 467 | self.thrForceMappingConfig.outputDataName = "delta_p_achievable" 468 | self.thrForceMappingConfig.controlAxes_B = self.initial_conditions.get("controlAxes_B") 469 | self.thrForceMappingConfig.thrForceSign = self.initial_conditions.get("thrForceSign") 470 | 471 | self.thrDumpConfig = thrMomentumDumping.thrMomentumDumpingConfig() 472 | self.thrDumpWrap = self.setModelDataWrap(self.thrDumpConfig) 473 | self.thrDumpConfig.deltaHInMsgName = self.thrDesatControlConfig.deltaHOutMsgName 474 | self.thrDumpConfig.thrusterImpulseInMsgName = "delta_p_achievable" 475 | self.thrDumpConfig.thrusterOnTimeOutMsgName = self.thrusterSet.InputCmds 476 | self.thrDumpConfig.thrusterConfInMsgName = self.thrusterConfigMsgName 477 | self.thrDumpConfig.maxCounterValue = self.initial_conditions.get("maxCounterValue") 478 | self.thrDumpConfig.thrMinFireTime = self.initial_conditions.get("thrMinFireTime") 479 | 480 | # Add models to tasks 481 | self.AddModelToTask("sunPointTask", self.sunPointWrap, self.sunPointData) 482 | self.AddModelToTask("nadirPointTask", self.hillPointWrap, self.hillPointData) 483 | 484 | self.AddModelToTask("mrpControlTask", self.mrpFeedbackControlWrap, self.mrpFeedbackControlData) 485 | self.AddModelToTask("mrpControlTask", self.trackingErrorWrap, self.trackingErrorData) 486 | self.AddModelToTask("mrpControlTask", self.rwMotorTorqueWrap, self.rwMotorTorqueConfig) 487 | 488 | self.AddModelToTask("rwDesatTask", self.thrDesatControlWrap, self.thrDesatControlConfig) 489 | self.AddModelToTask("rwDesatTask", self.thrForceMappingWrap, self.thrForceMappingConfig) 490 | self.AddModelToTask("rwDesatTask", self.thrDumpWrap, self.thrDumpConfig) 491 | 492 | 493 | def set_logging(self): 494 | ''' 495 | Logs simulation outputs to return as observations. This simulator observes: 496 | mrp_bn - inertial to body MRP 497 | error_mrp - Attitude error given current guidance objective 498 | power_level - current W-Hr from the battery 499 | r_bn - inertial position of the s/c relative to Earth 500 | :return: 501 | ''' 502 | 503 | ## Set the sampling time to the duration of a timestep: 504 | samplingTime = mc.sec2nano(self.step_duration) 505 | 506 | # Log planet, sun positions 507 | 508 | #self.TotalSim.logThisMessage("earth_planet_data", samplingTime) 509 | # self.TotalSim.logThisMessage("sun_planet_data", samplingTime) 510 | # Log inertial attitude, position 511 | self.TotalSim.logThisMessage(self.scObject.scStateOutMsgName, samplingTime) 512 | self.TotalSim.logThisMessage(self.simpleNavObject.outputTransName, 513 | samplingTime) 514 | self.TotalSim.logThisMessage(self.simpleNavObject.outputAttName, 515 | samplingTime) 516 | self.TotalSim.logThisMessage(self.rwStateEffector.OutputDataString, 517 | samplingTime) 518 | # Log FSW error portrait 519 | self.TotalSim.logThisMessage("att_reference", samplingTime) 520 | self.TotalSim.logThisMessage(self.trackingErrorData.outputDataName, samplingTime) 521 | # self.TotalSim.logThisMessage(self.mrpFeedbackControlData.outputDataName, samplingTime) 522 | 523 | # Log system power status 524 | self.TotalSim.logThisMessage(self.powerMonitor.batPowerOutMsgName, 525 | samplingTime) 526 | 527 | # Eclipse indicator 528 | self.TotalSim.logThisMessage(self.solarPanel.sunEclipseInMsgName, samplingTime) 529 | 530 | # Desat debug parameters 531 | #self.TotalSim.logThisMessage(self.thrDesatControlConfig.deltaHOutMsgName, samplingTime) 532 | 533 | return 534 | 535 | def run_sim(self, action): 536 | ''' 537 | Executes the sim for a specified duration given a mode command. 538 | :param action: 539 | :param duration: 540 | :return: 541 | ''' 542 | 543 | self.modeRequest = str(action) 544 | 545 | self.sim_over = False 546 | 547 | currentResetTime = mc.sec2nano(self.simTime) 548 | if self.modeRequest == "0": 549 | #print('starting nadir pointing...') 550 | # Set up a nadir pointing mode 551 | self.dynProc.enableAllTasks() 552 | self.hillPointWrap.Reset(currentResetTime) 553 | self.trackingErrorWrap.Reset(currentResetTime) 554 | 555 | self.disableTask('sunPointTask') 556 | self.disableTask('rwDesatTask') 557 | 558 | self.enableTask('nadirPointTask') 559 | self.enableTask('mrpControlTask') 560 | 561 | elif self.modeRequest == "1": 562 | #print('starting sun pointing...') 563 | # Set up a sun pointing mode 564 | self.dynProc.enableAllTasks() 565 | self.sunPointWrap.Reset(currentResetTime) 566 | self.trackingErrorWrap.Reset(currentResetTime) 567 | 568 | self.disableTask('nadirPointTask') 569 | self.disableTask('rwDesatTask') 570 | 571 | self.enableTask('sunPointTask') 572 | self.enableTask('mrpControlTask') 573 | 574 | elif self.modeRequest == "2": 575 | # Set up a desat mode 576 | self.dynProc.enableAllTasks() 577 | self.sunPointWrap.Reset(currentResetTime) 578 | self.trackingErrorWrap.Reset(currentResetTime) 579 | 580 | self.thrDesatControlWrap.Reset(currentResetTime) 581 | self.thrDumpWrap.Reset(currentResetTime) 582 | 583 | self.disableTask('nadirPointTask') 584 | self.disableTask('sunPointTask') 585 | 586 | self.enableTask('sunPointTask') 587 | self.enableTask('mrpControlTask') 588 | self.enableTask('rwDesatTask') 589 | 590 | self.simTime += self.step_duration 591 | simulationTime = mc.sec2nano(self.simTime) 592 | 593 | # Execute the sim 594 | self.ConfigureStopTime(simulationTime) 595 | self.ExecuteSimulation() 596 | 597 | # Pull logged message data and return it as an observation 598 | simDict = self.pullMultiMessageLogData([ 599 | # 'sun_planet_data.PositionVector', 600 | # self.scObject.scStateOutMsgName + '.sigma_BN', 601 | self.scObject.scStateOutMsgName + '.r_BN_N', 602 | # self.scObject.scStateOutMsgName + '.v_BN_N', 603 | "att_reference.sigma_RN", 604 | self.simpleNavObject.outputAttName + '.omega_BN_B', 605 | self.trackingErrorData.outputDataName + '.sigma_BR', 606 | self.rwStateEffector.OutputDataString + '.wheelSpeeds', 607 | self.powerMonitor.batPowerOutMsgName + '.storageLevel', 608 | self.solarPanel.sunEclipseInMsgName + '.shadowFactor' 609 | ], [ 610 | # list(range(3)), 611 | list(range(3)), 612 | # list(range(3)), 613 | # list(range(3)), 614 | list(range(3)), list(range(3)), list(range(3)), list(range(3)), 615 | list(range(1)), list(range(1))], 616 | [ 617 | # 'double','double','double', 618 | 'double', 619 | 'double','double', 'double','double','double', 'double'], 1) 620 | 621 | # Observations 622 | attErr = simDict[self.trackingErrorData.outputDataName + '.sigma_BR'] 623 | attRef = simDict["att_reference.sigma_RN"] 624 | attRate = simDict[self.simpleNavObject.outputAttName + '.omega_BN_B'] 625 | storedCharge = simDict[self.powerMonitor.batPowerOutMsgName + '.storageLevel'] 626 | eclipseIndicator = simDict[self.solarPanel.sunEclipseInMsgName + '.shadowFactor'] 627 | wheelSpeeds = simDict[self.rwStateEffector.OutputDataString+'.wheelSpeeds'] 628 | 629 | # Debug info 630 | # sunPosition = simDict['sun_planet_data.PositionVector'] 631 | # inertialAtt = simDict[self.scObject.scStateOutMsgName + '.sigma_BN'] 632 | inertialPos = simDict[self.scObject.scStateOutMsgName + '.r_BN_N'] 633 | # inertialVel = simDict[self.scObject.scStateOutMsgName + '.v_BN_N'] 634 | 635 | # debug = np.hstack([inertialAtt[-1,1:4],inertialPos[-1,1:4],inertialVel[-1,1:4],attRef[-1,1:4], sunPosition[-1,1:4]]) 636 | obs = np.hstack([np.linalg.norm(attErr[-1,1:4]), np.linalg.norm(attRate[-1,1:4]), np.linalg.norm(wheelSpeeds[-1,1:4]), 637 | storedCharge[-1,1]/3600., eclipseIndicator[-1,1]]) 638 | self.obs = obs.reshape(len(obs), 1) 639 | self.sim_states = []#debug.reshape(len(debug), 1) 640 | 641 | if np.linalg.norm(inertialPos[-1,1:4]) < (orbitalMotion.REQ_EARTH/1000.): 642 | self.sim_over = True 643 | 644 | return self.obs, self.sim_states, self.sim_over 645 | 646 | def close_gracefully(self): 647 | """ 648 | makes sure spice gets shut down right when we close. 649 | :return: 650 | """ 651 | self.gravFactory.unloadSpiceKernels() 652 | return 653 | 654 | def create_leoPowerAttSimulator(): 655 | return LEOPowerAttitudeSimulator(0.1, 0.1, 60.) 656 | 657 | if __name__=="__main__": 658 | """ 659 | Test execution of the simulator with random actions and plot the observation space. 660 | """ 661 | sim = LEOPowerAttitudeSimulator(0.1,1.0, 60.) 662 | obs = [] 663 | states = [] 664 | normWheelSpeed = [] 665 | actList = [] 666 | from matplotlib import pyplot as plt 667 | from random import randrange 668 | plt.figure() 669 | 670 | tFinal = 2*180 671 | for ind in range(0,tFinal): 672 | act = 0#randrange(3) 673 | actList.append(act) 674 | ob, state, _ = sim.run_sim(act) 675 | #normWheelSpeed.append(np.linalg.norm(abs(ob[3:6]))) 676 | obs.append(ob) 677 | states.append(state) 678 | obs = np.asarray(obs) 679 | states = np.asarray(states) 680 | 681 | plt.plot(range(0,tFinal),obs[:,0], label="sigma_BR") 682 | plt.plot(range(0,tFinal),obs[:,1], label="omega_BN") 683 | plt.plot(range(0,tFinal),obs[:,2], label="omega_rw") 684 | plt.plot(range(0,tFinal), obs[:,3], label= "J_bat") 685 | plt.plot(range(0,tFinal),obs[:,4], label="eclipse_ind") 686 | plt.legend() 687 | 688 | 689 | # plt.figure() 690 | #plt.plot(states[:, 3]/1000., states[:, 4]/1000., label="Orbit") 691 | #plt.plot(states[:,12]/1000., states[:,13]/1000, label="Sun Position") 692 | # plt.legend() 693 | 694 | plt.show() 695 | 696 | 697 | 698 | 699 | 700 | -------------------------------------------------------------------------------- /basilisk_env/simulators/opNavSimulator.py: -------------------------------------------------------------------------------- 1 | # 3rd party modules 2 | import numpy as np 3 | import socket, errno 4 | 5 | # Basilisk modules 6 | from Basilisk.utilities import SimulationBaseClass 7 | from Basilisk.utilities import macros as mc 8 | from Basilisk.utilities import unitTestSupport 9 | from Basilisk.utilities import orbitalMotion 10 | from Basilisk.utilities import RigidBodyKinematics as rbk 11 | from numpy.random import uniform 12 | import matplotlib as mpl 13 | from Basilisk import __path__ 14 | bskPath = __path__[0] 15 | # Get current file path 16 | import sys, os, inspect, time, signal, subprocess 17 | filename = inspect.getframeinfo(inspect.currentframe()).filename 18 | path = os.path.dirname(os.path.abspath(filename)) 19 | sys.path.append(path + '/opNav_models') 20 | from BSK_masters import BSKSim, BSKScenario 21 | import BSK_OpNavDynamics, BSK_OpNavFsw 22 | 23 | mpl.rcParams.update({'font.size' : 8 }) 24 | #seaborn-colorblind, 'seaborn-paper', 'bmh', 'tableau-colorblind10', 'seaborn-deep', 'myStyle', 'aiaa' 25 | 26 | try: 27 | plt.style.use("myStyle") 28 | except: 29 | pass 30 | params = {'axes.labelsize': 8,'axes.titlesize':8, 'legend.fontsize': 8, 'xtick.labelsize': 7, 'ytick.labelsize': 7, 'text.usetex': True} 31 | mpl.rcParams.update(params) 32 | 33 | def socket_in_use(address, port): 34 | """ 35 | Checks whether a given address+port is in use. 36 | """ 37 | 38 | s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 39 | 40 | try: 41 | s.bind((address, port)) 42 | return False 43 | except socket.error as e: 44 | if e.errno == errno.EADDRINUSE: 45 | print("Port is already in use") 46 | return True 47 | else: 48 | # something else raised the socket.error exception 49 | print(e) 50 | return True 51 | 52 | 53 | class viz_manager(object): 54 | """ 55 | Manages an instance of the Vizard astrodynamics visualizer. 56 | """ 57 | def __init__(self): 58 | 59 | self.viz_init = False 60 | self.fail_count = 0 61 | self.fail_limit = 20 62 | 63 | def createViz(self, port=5556): 64 | """ 65 | Creates a Viz instance. 66 | """ 67 | if self.viz_init == False: 68 | # Check if the desired port is open: 69 | #if not socket_in_use(os.environ['viz_address'],port) and self.fail_count < self.fail_limit: 70 | self.viz_proc = subprocess.Popen([os.environ['viz_app'], "--args", "-opNavMode", f"tcp://{os.environ['viz_address']}:{port}"], stdout=subprocess.DEVNULL) # ,, "-batchmode" 71 | self.viz_init=True 72 | return port 73 | # else: 74 | # self.fail_count = self.fail_count + 1 75 | # self.createViz(port=port+1) 76 | else: 77 | pass 78 | 79 | 80 | 81 | def stopViz(self): 82 | """ 83 | Kills all existing vizard instances. 84 | """ 85 | self.viz_proc.kill() # Kill the viz process 86 | # If we're on WSL, also invoke kill: 87 | proc = subprocess.Popen(["/mnt/c/Windows/System32/TASKKILL.exe","/IM","Vizard.exe", "/F"]) 88 | time.sleep(1) # Give taskill some time to kill the task 89 | self.viz_init = False 90 | return 91 | 92 | class scenario_OpNav(BSKSim): 93 | ''' 94 | Simulates a spacecraft in LEO with atmospheric drag and J2. 95 | 96 | Dynamics Components 97 | - Forces: J2, Atmospheric Drag w/ COM offset 98 | - Environment: Exponential density model; eclipse 99 | - Actuators: ExternalForceTorque 100 | - Sensors: SimpleNav 101 | - Systems: SimpleBattery, SimpleSink, SimpleSolarPanel 102 | 103 | FSW Components: 104 | - MRP_Feedback controller 105 | - inertial3d (sun pointing), hillPoint (nadir pointing) 106 | - [WIP, not implemented] inertialEKF attitude filter 107 | 108 | Action Space (discrete, 0 or 1): 109 | 0 - orients the spacecraft towards the sun. 110 | 1 - orients the spacecraft towards Earth. 111 | [WIP, not implemented] 2 - Runs inertialEKF to better estimate the spacecraft state. 112 | 113 | Observation Space: 114 | eclipseIndicator - float [0,1] - indicates the flux mitigator due to eclipse. 115 | storedCharge - float [0,batCapacity] - indicates the s/c battery charge level in W-s. 116 | |sigma_RB| - float [0,1] - norm of the spacecraft error MRP with respect to the last reference frame specified. 117 | |diag(filter_covar)| - float - norm of the diagonal of the EKF covariance matrix at that time. 118 | 119 | Reward Function: 120 | r = 1/(1+ | sigma_RB|) if action=1 121 | Intended to provide a rich reward in action 1 when the spacecraft is pointed towards the earth, decaying as sigma^2 122 | as the pointing error increases. 123 | 124 | :return: 125 | ''' 126 | def __init__(self, dynRate, fswRate, step_duration): 127 | ''' 128 | Creates the simulation, but does not initialize the initial conditions. 129 | ''' 130 | super(scenario_OpNav, self).__init__(BSKSim) 131 | self.fswRate = fswRate 132 | self.dynRate = dynRate 133 | self.step_duration = step_duration 134 | 135 | 136 | 137 | self.set_DynModel(BSK_OpNavDynamics) 138 | self.set_FswModel(BSK_OpNavFsw) 139 | self.initInterfaces() 140 | self.filterUse = "relOD" 141 | self.configure_initial_conditions() 142 | self.get_DynModel().vizInterface.opNavMode = 1 143 | 144 | self.viz_manager = viz_manager() 145 | self.viz_port = self.viz_manager.createViz(port=5000) 146 | 147 | 148 | self.simTime = 0.0 149 | self.numModes = 50 150 | self.modeCounter = 0 151 | 152 | self.obs = np.zeros([4,1]) 153 | self.sim_states = np.zeros([12,1]) 154 | 155 | self.set_logging() 156 | # self.previousPointingGoal = "sunPointTask" 157 | 158 | self.modeRequest = 'OpNavOD' 159 | self.InitializeSimulationAndDiscover() 160 | 161 | return 162 | 163 | def configure_initial_conditions(self): 164 | # Configure Dynamics initial conditions 165 | oe = orbitalMotion.ClassicElements() 166 | # oe.a = uniform(17000 * 1E3, 22000 * 1E3, 1) 167 | # oe.e = uniform(0, 0.6, 1) 168 | # oe.i = uniform(-20 * mc.D2R, 20 * mc.D2R, 1) 169 | # oe.Omega = uniform(0 * mc.D2R, 360 * mc.D2R, 1) 170 | # oe.omega = uniform(0 * mc.D2R, 360 * mc.D2R, 1) 171 | # oe.f = uniform(0 * mc.D2R, 360 * mc.D2R, 1) 172 | 173 | oe.a = 18000 * 1E3 # meters 174 | oe.e = 0.6 175 | oe.i = 10 * mc.D2R 176 | oe.Omega = 25. * mc.D2R 177 | oe.omega = 190. * mc.D2R 178 | oe.f = 80. * mc.D2R # 90 good 179 | mu = self.get_DynModel().marsGravBody.mu 180 | 181 | rN, vN = orbitalMotion.elem2rv(mu, oe) 182 | orbitalMotion.rv2elem(mu, rN, vN) 183 | 184 | print("ICs \n") 185 | print(rN , "\n") 186 | print(vN , "\n") 187 | 188 | rError = uniform(100000,-100000, 3) 189 | vError = uniform(1000,-1000, 3) 190 | MRP = [0, 0, 0] 191 | self.get_FswModel().relativeODData.stateInit = (rN + rError).tolist() + (vN + vError).tolist() 192 | self.get_DynModel().scObject.hub.r_CN_NInit = unitTestSupport.np2EigenVectorXd(rN) # m - r_CN_N 193 | self.get_DynModel().scObject.hub.v_CN_NInit = unitTestSupport.np2EigenVectorXd(vN) # m/s - v_CN_N 194 | self.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B 195 | self.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B 196 | 197 | qNoiseIn = np.identity(6) 198 | qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-3 * 1E-3 199 | qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1E-4 * 1E-4 200 | self.get_FswModel().relativeODData.qNoise = qNoiseIn.reshape(36).tolist() 201 | self.get_FswModel().imageProcessing.noiseSF = 1 202 | self.get_FswModel().relativeODData.noiseSF = 5 203 | 204 | 205 | def set_logging(self): 206 | ''' 207 | Logs simulation outputs to return as observations. This simulator observes: 208 | mrp_bn - inertial to body MRP 209 | error_mrp - Attitude error given current guidance objective 210 | power_level - current W-Hr from the battery 211 | r_bn - inertial position of the s/c relative to Earth 212 | :return: 213 | ''' 214 | 215 | ## Set the sampling time to the duration of a timestep: 216 | samplingTime = mc.sec2nano(self.dynRate) 217 | 218 | # Log planet, sun positions 219 | self.TotalSim.logThisMessage(self.get_FswModel().relativeODData.filtDataOutMsgName, samplingTime) 220 | self.TotalSim.logThisMessage(self.get_DynModel().SimpleNavObject.outputAttName, samplingTime) 221 | self.TotalSim.logThisMessage(self.get_DynModel().scObject.scStateOutMsgName, samplingTime) 222 | 223 | return 224 | 225 | def run_sim(self, action): 226 | ''' 227 | Executes the sim for a specified duration given a mode command. 228 | :param action: 229 | :param duration: 230 | :return: 231 | ''' 232 | 233 | # self.modeRequest = str(action) 234 | 235 | self.sim_over = False 236 | self.modeCounter+=1 237 | 238 | currentResetTime = mc.sec2nano(self.simTime) 239 | if str(action) == "0": 240 | # self.get_DynModel().cameraMod.cameraIsOn = 1 241 | # self.modeRequest = 'OpNavOD' 242 | 243 | self.fswProc.disableAllTasks() 244 | self.enableTask('opNavPointTaskCheat') 245 | self.enableTask('mrpFeedbackRWsTask') 246 | self.enableTask('opNavODTask') 247 | 248 | 249 | elif str(action) == "1": 250 | self.get_DynModel().cameraMod.cameraIsOn = 0 251 | # self.modeRequest = "sunSafePoint" 252 | self.fswProc.disableAllTasks() 253 | self.enableTask('sunSafePointTask') 254 | self.enableTask('mrpFeedbackRWsTask') 255 | 256 | self.simTime += self.step_duration 257 | simulationTime = mc.min2nano(self.simTime) 258 | 259 | # Execute the sim 260 | self.ConfigureStopTime(simulationTime) 261 | self.ExecuteSimulation() 262 | 263 | NUM_STATES = 6 264 | # Pull logged message data and return it as an observation 265 | simDict = self.pullMultiMessageLogData([ 266 | self.get_DynModel().scObject.scStateOutMsgName + '.r_BN_N', 267 | self.get_DynModel().scObject.scStateOutMsgName + '.v_BN_N', 268 | self.get_DynModel().scObject.scStateOutMsgName + '.sigma_BN', 269 | self.get_DynModel().SimpleNavObject.outputAttName + '.vehSunPntBdy', 270 | self.get_FswModel().relativeODData.filtDataOutMsgName + ".state", 271 | self.get_FswModel().relativeODData.filtDataOutMsgName + ".covar" 272 | ], [list(range(3)), list(range(3)), list(range(3)), list(range(3)), list(range(NUM_STATES)), list(range(NUM_STATES*NUM_STATES))], ['float']*6,numRecords=1) 273 | 274 | # sunHead = simDict["sun_planet_data" + ".PositionVector"] 275 | sunHead_B = simDict[self.get_DynModel().SimpleNavObject.outputAttName + '.vehSunPntBdy'] 276 | position_N = simDict[self.get_DynModel().scObject.scStateOutMsgName + '.r_BN_N'] 277 | sigma_BN = simDict[self.get_DynModel().scObject.scStateOutMsgName + '.sigma_BN'] 278 | velocity_N = simDict[self.get_DynModel().scObject.scStateOutMsgName + '.v_BN_N'] 279 | navState = simDict[self.get_FswModel().relativeODData.filtDataOutMsgName + ".state"] 280 | navCovar = simDict[self.get_FswModel().relativeODData.filtDataOutMsgName + ".covar"] 281 | 282 | # mu = self.get_DynModel().marsGravBody.mu 283 | # oe = orbitalMotion.rv2elem_parab(mu, navState[-1,1:4], navState[-1,4:7]) 284 | covarVec = np.array([np.sqrt(navCovar[-1,1]), np.sqrt(navCovar[-1,2 + NUM_STATES]), np.sqrt(navCovar[-1,3 + 2*NUM_STATES])]) 285 | 286 | BN = rbk.MRP2C(sigma_BN[-1,1:4]) 287 | pos_B = -np.dot(BN, navState[-1,1:4]/np.linalg.norm(navState[-1,1:4])) 288 | sunHeadNorm = sunHead_B[-1,1:4]/np.linalg.norm(sunHead_B[-1,1:4]) 289 | sunMarsAngle = np.dot(pos_B, sunHeadNorm) 290 | 291 | debug = np.hstack([navState[-1,1:4], position_N[-1,1:4], velocity_N[-1,1:4], sigma_BN[-1,1:4]]) 292 | obs = np.hstack([sunMarsAngle, covarVec/np.linalg.norm(navState[-1,1:4])]) 293 | self.obs = obs.reshape(len(obs), 1) 294 | self.sim_states = debug.reshape(len(debug), 1) 295 | 296 | if self.modeCounter >= self.numModes: 297 | self.sim_over = True 298 | 299 | return self.obs, self.sim_states, self.sim_over 300 | 301 | def close_gracefully(self): 302 | """ 303 | makes sure spice gets shut down right when we close. 304 | :return: 305 | """ 306 | self.get_DynModel().SpiceObject.unloadSpiceKernel(self.get_DynModel().SpiceObject.SPICEDataPath, 'de430.bsp') 307 | self.get_DynModel().SpiceObject.unloadSpiceKernel(self.get_DynModel().SpiceObject.SPICEDataPath, 'naif0012.tls') 308 | self.get_DynModel().SpiceObject.unloadSpiceKernel(self.get_DynModel().SpiceObject.SPICEDataPath, 'de-403-masses.tpc') 309 | self.get_DynModel().SpiceObject.unloadSpiceKernel(self.get_DynModel().SpiceObject.SPICEDataPath, 'pck00010.tpc') 310 | 311 | self.viz_manager.stopViz() 312 | # try: 313 | # os.kill(self.child.pid + 1, signal.SIGKILL) 314 | # print("Closing Vizard") 315 | # except: 316 | # print("IDK how to turn this thing off") 317 | return 318 | 319 | def create_scenario_OpNav(): 320 | return scenario_OpNav(1., 5., 50.) 321 | 322 | if __name__=="__main__": 323 | """ 324 | Test execution of the simulator with random actions and plot the observation space. 325 | """ 326 | 327 | actHist = [1, 1, 0, 0, 1, 1, 1, 0, 0, 1] 328 | sim = scenario_OpNav(0.5, 5., 50.) 329 | obs = [] 330 | states = [] 331 | rewardList = [] 332 | normWheelSpeed = [] 333 | actList = [] 334 | from matplotlib import pyplot as plt 335 | from random import randrange 336 | 337 | tFinal = len(actHist) 338 | rewardList.append(np.nan) 339 | for ind in range(0,len(actHist)): 340 | act = actHist[ind]#(ind-1)%2 #randrange(2) 341 | print("act = ",act) 342 | actList.append(act) 343 | ob, state, _ = sim.run_sim(act) 344 | reward = 0 345 | if act == 1: 346 | real = np.array([state[3],state[4], state[5]]) 347 | nav = np.array([state[0],state[1], state[2]]) 348 | nav -= real 349 | nav *= 1./np.linalg.norm(real) 350 | reward = np.linalg.norm(1./ (1. + np.linalg.norm(nav)**2.0)) 351 | rewardList.append(reward) 352 | 353 | obs.append(ob) 354 | states.append(state) 355 | obs = np.asarray(obs) 356 | states = np.asarray(states) 357 | 358 | try: 359 | child.terminate() 360 | except: 361 | print("os.kill failed; fear for your lives") 362 | 363 | 364 | """Check results here""" 365 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 366 | colorList = [] 367 | for i in range(10): 368 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 369 | 370 | totalReward = [0] 371 | for ind in range(1, tFinal+1): 372 | totalReward.append(totalReward[-1] + rewardList[ind]) 373 | 374 | plt.figure(num=22, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 375 | plt.plot(rewardList, label='Mode-wise Reward', color=colorList[3]) 376 | plt.plot(totalReward, label='Summed Reward', color=colorList[7]) 377 | ax = plt.gca() 378 | for ind in range(0, tFinal): 379 | if actHist[ind] == 0: 380 | ax.axvspan(ind, ind + 1, color=colorList[1], alpha=0.05) 381 | if actHist[ind] == 1: 382 | ax.axvspan(ind, ind + 1, color=colorList[8], alpha=0.05) 383 | plt.ylabel('Reward') 384 | plt.xlabel('Modes (-)') 385 | plt.legend(loc='best') 386 | plt.tight_layout() 387 | plt.savefig('OpNav_Reward_History.pdf') 388 | 389 | nav = np.zeros(len(states[:,0])) 390 | covar = np.zeros(len(states[:,0])) 391 | 392 | nav1 = np.copy(states[:,0]) 393 | nav1 -= states[:,3] 394 | nav2 = np.copy(states[:, 1]) 395 | nav2 -= states[:, 4] 396 | nav3 = np.copy(states[:, 2]) 397 | nav3 -= states[:, 5] 398 | 399 | for i in range(len(states[:,0])): 400 | nav[i] = np.linalg.norm(np.array([nav1[i],nav2[i],nav3[i]]))/ np.linalg.norm(np.array([nav1[0],nav2[0],nav3[0]])) 401 | covar[i] = np.linalg.norm(np.array([obs[i, 1],obs[i, 2],obs[i, 3]]))/np.linalg.norm(np.array([obs[0, 1],obs[0, 2],obs[0, 3]])) 402 | 403 | plt.figure(num=1, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 404 | plt.plot(range(1,tFinal+1), nav, label="$\hat{\mathbf{r}}$", color = colorList[3]) 405 | plt.plot(range(1,tFinal+1), covar, label="$\hat{\mathrm{P}}$", color = colorList[8]) 406 | ax = plt.gca() 407 | for ind in range(0, tFinal): 408 | if actHist[ind] == 0: 409 | ax.axvspan(ind, ind + 1, color=colorList[1], alpha=0.05) 410 | if actHist[ind] == 1: 411 | ax.axvspan(ind, ind + 1, color=colorList[8], alpha=0.05) 412 | plt.legend(loc='best') 413 | plt.ylabel('Normalized States') 414 | plt.xlabel('Modes (-)') 415 | plt.savefig('ActionsNav.pdf') 416 | 417 | plt.figure(num=11, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 418 | plt.plot(range(1,tFinal+1),obs[:,0], label="Angle") 419 | plt.plot(range(1,tFinal+1),covar, label="$\hat{\mathrm{P}}$") 420 | ax = plt.gca() 421 | for ind in range(0, tFinal): 422 | if actHist[ind] == 0: 423 | ax.axvspan(ind, ind + 1, color=colorList[1], alpha=0.05) 424 | if actHist[ind] == 1: 425 | ax.axvspan(ind, ind + 1, color=colorList[8], alpha=0.05) 426 | plt.legend(loc='best') 427 | plt.ylabel('Eclipse Angle vs Covar') 428 | plt.xlabel('Modes (-)') 429 | plt.savefig('AngleCovar.pdf') 430 | # plt.figure() 431 | # plt.plot(range(0,tFinal),nav1, label="nav1", color = 'r') 432 | # plt.plot(range(0,tFinal), nav2, label="nav2", color = 'g') 433 | # plt.plot(range(0,tFinal),nav3, label="nav3", color = 'b') 434 | # plt.plot(range(0, tFinal), obs[:, 1], label="cov1", color = 'r') 435 | # plt.plot(range(0, tFinal), obs[:, 2], label="cov2", color = 'g') 436 | # plt.plot(range(0, tFinal), obs[:, 3], label="cov3", color = 'b') 437 | # plt.legend() 438 | 439 | 440 | plt.figure(num=2, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 441 | plt.plot(range(1,tFinal+1),states[:,9], label="$\sigma_1$", color = colorList[5]) 442 | plt.plot(range(1,tFinal+1),states[:,10], label="$\sigma_2$", color = colorList[6]) 443 | plt.plot(range(1,tFinal+1),states[:,11], label="$\sigma_3$", color = colorList[7]) 444 | ax = plt.gca() 445 | for ind in range(0, tFinal): 446 | if actHist[ind] == 0: 447 | ax.axvspan(ind, ind + 1, color=colorList[1], alpha=0.05) 448 | if actHist[ind] == 1: 449 | ax.axvspan(ind, ind + 1, color=colorList[8], alpha=0.05) 450 | plt.legend(loc='best') 451 | plt.ylabel('Attitude') 452 | plt.xlabel('Modes (-)') 453 | plt.savefig('AttModes.pdf') 454 | 455 | 456 | plt.show() 457 | 458 | 459 | 460 | 461 | 462 | -------------------------------------------------------------------------------- /basilisk_env/simulators/opNav_models/BSK_OpNavDynamics.py: -------------------------------------------------------------------------------- 1 | # 2 | # ISC License 3 | # 4 | # Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder 5 | # 6 | # Permission to use, copy, modify, and/or distribute this software for any 7 | # purpose with or without fee is hereby granted, provided that the above 8 | # copyright notice and this permission notice appear in all copies. 9 | # 10 | # THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 11 | # WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 12 | # MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 13 | # ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 14 | # WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 15 | # ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 16 | # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 17 | # 18 | r""" 19 | Overview 20 | -------- 21 | 22 | ``OpNavScenarios/models/BSK_OpNavDynamics.py`` is similar to the :ref:`Folder_BskSim` versions seen previously. 23 | The main additions are 24 | the instantiation of :ref:`vizInterface`, and the camera module. 25 | 26 | 27 | """ 28 | 29 | 30 | import numpy as np 31 | import math, sys, os, inspect 32 | from Basilisk.utilities import macros as mc 33 | from Basilisk.utilities import unitTestSupport as sp 34 | from Basilisk.simulation import (spacecraftPlus, gravityEffector, extForceTorque, simple_nav, spice_interface, 35 | reactionWheelStateEffector, coarse_sun_sensor, eclipse, alg_contain, bore_ang_calc, 36 | thrusterDynamicEffector, ephemeris_converter, vizInterface, 37 | camera) 38 | from Basilisk.utilities import simIncludeThruster, simIncludeRW, simIncludeGravBody, unitTestSupport 39 | from Basilisk.utilities import RigidBodyKinematics as rbk 40 | from Basilisk.topLevelModules import pyswice 41 | from Basilisk import __path__ 42 | 43 | from Basilisk.fswAlgorithms import attTrackingError 44 | 45 | bskPath = __path__[0] 46 | filename = inspect.getframeinfo(inspect.currentframe()).filename 47 | path = os.path.dirname(os.path.abspath(filename)) 48 | 49 | class BSKDynamicModels(): 50 | """ 51 | BSK Dynamics model for the op nav simulations 52 | """ 53 | def __init__(self, SimBase, dynRate, viz_port): 54 | # Define process name, task name and task time-step 55 | self.processName = SimBase.DynamicsProcessName 56 | self.taskName = "DynamicsTask" 57 | self.taskCamera = "CameraTask" 58 | self.processTasksTimeStep = mc.sec2nano(dynRate) 59 | self.viz_port = viz_port 60 | # Create task 61 | SimBase.dynProc.addTask(SimBase.CreateNewTask(self.taskName, self.processTasksTimeStep), 1000) 62 | SimBase.dynProc.addTask(SimBase.CreateNewTask(self.taskCamera, mc.sec2nano(60)), 999) 63 | 64 | # Instantiate Dyn modules as objects 65 | self.simBasePath = bskPath 66 | self.cameraMRP_CB =[] 67 | self.cameraRez = [] 68 | 69 | self.SpiceObject = spice_interface.SpiceInterface() 70 | self.scObject = spacecraftPlus.SpacecraftPlus() 71 | self.gravFactory = simIncludeGravBody.gravBodyFactory() 72 | self.extForceTorqueObject = extForceTorque.ExtForceTorque() 73 | self.SimpleNavObject = simple_nav.SimpleNav() 74 | self.TruthNavObject = simple_nav.SimpleNav() 75 | self.vizInterface = vizInterface.VizInterface() 76 | self.instrumentSunBore = bore_ang_calc.BoreAngCalc() 77 | self.eclipseObject = eclipse.Eclipse() 78 | self.CSSConstellationObject = coarse_sun_sensor.CSSConstellation() 79 | self.rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector() 80 | self.thrustersDynamicEffector = thrusterDynamicEffector.ThrusterDynamicEffector() 81 | self.cameraMod = camera.Camera() 82 | 83 | self.ephemObject = ephemeris_converter.EphemerisConverter() 84 | 85 | # Initialize all modules and write init one-time messages 86 | self.InitAllDynObjects() 87 | # self.WriteInitDynMessages(SimBase) 88 | 89 | self.truthRefErrors = attTrackingError.attTrackingErrorConfig() 90 | self.truthRefErrorsWrap = alg_contain.AlgContain(self.truthRefErrors, 91 | attTrackingError.Update_attTrackingError, 92 | attTrackingError.SelfInit_attTrackingError, 93 | attTrackingError.CrossInit_attTrackingError, 94 | attTrackingError.Reset_attTrackingError) 95 | self.truthRefErrorsWrap.ModelTag = "truthRefErrors" 96 | 97 | self.InitAllAnalysisObjects() 98 | 99 | # Assign initialized modules to tasks 100 | SimBase.AddModelToTask(self.taskName, self.scObject, None, 201) 101 | SimBase.AddModelToTask(self.taskName, self.SimpleNavObject, None, 109) 102 | SimBase.AddModelToTask(self.taskName, self.TruthNavObject, None, 110) 103 | SimBase.AddModelToTask(self.taskName, self.SpiceObject, 200) 104 | SimBase.AddModelToTask(self.taskName, self.ephemObject, 199) 105 | SimBase.AddModelToTask(self.taskName, self.CSSConstellationObject, None, 299) 106 | SimBase.AddModelToTask(self.taskName, self.eclipseObject, None, 204) 107 | SimBase.AddModelToTask(self.taskName, self.rwStateEffector, None, 301) 108 | SimBase.AddModelToTask(self.taskName, self.extForceTorqueObject, None, 300) 109 | SimBase.AddModelToTask(self.taskName, self.vizInterface, None, 100) 110 | SimBase.AddModelToTask(self.taskCamera, self.cameraMod, None, 99) 111 | 112 | # ------------------------------------------------------------------------------------------- # 113 | # These are module-initialization methods 114 | 115 | def SetCamera(self): 116 | self.cameraMod.imageInMsgName = "unity_image" 117 | self.cameraMod.imageOutMsgName = "opnav_image" 118 | self.cameraMod.cameraOutMsgName = "camera_config_data" 119 | self.cameraMod.saveImages = 0 120 | self.cameraMod.saveDir = 'Test/' 121 | 122 | # Noise parameters 123 | # self.cameraMod.gaussian = 2 124 | # self.cameraMod.darkCurrent = 0 125 | # self.cameraMod.saltPepper = 0.5 126 | # self.cameraMod.cosmicRays = 1 127 | self.cameraMod.blurParam = 3 128 | 129 | # Camera config 130 | self.cameraMod.cameraIsOn = 1 131 | self.cameraMod.cameraID = 1 132 | self.cameraRate = 60 133 | self.cameraMod.renderRate = int(mc.sec2nano(self.cameraRate)) # in 134 | self.cameraMRP_CB = [0.,0.,0.] # Arbitrary camera orientation 135 | self.cameraMod.sigma_CB = self.cameraMRP_CB 136 | self.cameraMod.cameraPos_B = [0., 0.2, 0.2] # in meters 137 | self.cameraRez = [512, 512] #[1024,1024] # in pixels 138 | self.cameraSize = [10.*1E-3, self.cameraRez[1]/self.cameraRez[0]*10.*1E-3] # in m 139 | self.cameraMod.resolution = self.cameraRez 140 | self.cameraMod.fieldOfView = np.deg2rad(55) 141 | self.cameraMod.parentName = 'inertial' 142 | self.cameraMod.skyBox = 'black' 143 | self.cameraFocal = self.cameraSize[1]/2./np.tan(self.cameraMod.fieldOfView/2.) #in m 144 | 145 | 146 | def SetVizInterface(self): 147 | fileName = os.path.splitext(sys.argv[0])[0] + '_UnityViz.bin' 148 | home = os.path.dirname(fileName) 149 | if len(home) != 0: 150 | home += '/' 151 | namePath, name = os.path.split(fileName) 152 | if not os.path.isdir(home + '_VizFiles'): 153 | os.mkdir(home + '_VizFiles') 154 | fileName = home + '_VizFiles/' + name 155 | 156 | scData = vizInterface.VizSpacecraftData() 157 | scData.spacecraftName = 'inertial' 158 | self.vizInterface.scData.push_back(scData) 159 | self.vizInterface.comAddress = os.environ['viz_address'] 160 | self.vizInterface.comPortNumber = str(self.viz_port) 161 | self.vizInterface.opNavMode = 2 162 | self.vizInterface.opnavImageOutMsgName = "unity_image"#"opnav_image"# 163 | self.vizInterface.spiceInMsgName = vizInterface.StringVector(["earth_planet_data", 164 | "mars barycenter_planet_data", 165 | "sun_planet_data", 166 | "jupiter barycenter_planet_data" 167 | ]) 168 | self.vizInterface.planetNames = vizInterface.StringVector( 169 | ["earth", "mars barycenter", "sun", "jupiter barycenter"]) 170 | 171 | # self.vizInterface.spiceInMsgName = vizInterface.StringVector(["mars barycenter_planet_data"]) 172 | # self.vizInterface.planetNames = vizInterface.StringVector(["mars barycenter"]) 173 | # vizMessager.numRW = 4 174 | self.vizInterface.protoFilename = fileName 175 | 176 | def SetSpacecraftHub(self): 177 | self.scObject.ModelTag = "spacecraftBody" 178 | # -- Crate a new variable for the sim sc inertia I_sc. Note: this is currently accessed from FSWClass 179 | self.I_sc = [900., 0., 0., 180 | 0., 800., 0., 181 | 0., 0., 600.] 182 | self.scObject.hub.mHub = 750.0 # kg - spacecraft mass 183 | self.scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] # m - position vector of body-fixed point B relative to CM 184 | self.scObject.hub.IHubPntBc_B = sp.np2EigenMatrix3d(self.I_sc) 185 | self.scObject.scStateOutMsgName = "inertial_state_output" 186 | 187 | 188 | def SetTruthErrorsData(self): 189 | self.truthRefErrors.inputRefName = "att_ref_output" 190 | self.truthRefErrors.inputNavName = "truth_att_nav_output" 191 | self.truthRefErrors.outputDataName = "truth_att_errors" 192 | 193 | 194 | def SetgravityEffector(self): 195 | self.earthGravBody = gravityEffector.GravBodyData() 196 | self.earthGravBody.bodyInMsgName = "earth_planet_data" 197 | self.earthGravBody.mu = 0.3986004415E+15 # meters! 198 | self.earthGravBody.isCentralBody = False 199 | self.earthGravBody.useSphericalHarmParams = False 200 | 201 | self.sunGravBody = gravityEffector.GravBodyData() 202 | self.sunGravBody.bodyInMsgName = "sun_planet_data" 203 | self.sunGravBody.mu = 1.32712440018E20 # meters! 204 | self.sunGravBody.isCentralBody = False 205 | self.sunGravBody.useSphericalHarmParams = False 206 | 207 | self.marsGravBody = gravityEffector.GravBodyData() 208 | self.marsGravBody.bodyInMsgName = "mars barycenter_planet_data" 209 | self.marsGravBody.mu = 4.2828371901284001E+13 # meters! 210 | self.marsGravBody.isCentralBody = True 211 | self.marsGravBody.useSphericalHarmParams = True 212 | gravityEffector.loadGravFromFile( 213 | self.simBasePath + '/supportData/LocalGravData/GGM2BData.txt', 214 | self.marsGravBody.spherHarm, 2) 215 | 216 | self.jupiterGravBody = gravityEffector.GravBodyData() 217 | self.jupiterGravBody.bodyInMsgName = "jupiter barycenter_planet_data" 218 | self.jupiterGravBody.mu = 1.266865349093058E17 # meters! 219 | self.jupiterGravBody.isCentralBody = False 220 | self.jupiterGravBody.useSphericalHarmParams = False 221 | 222 | self.scObject.gravField.gravBodies = spacecraftPlus.GravBodyVector([self.earthGravBody, 223 | self.sunGravBody, self.jupiterGravBody, 224 | self.marsGravBody]) 225 | # self.scObject.gravField.gravBodies = spacecraftPlus.GravBodyVector([self.marsGravBody]) 226 | 227 | def SetEclipseObject(self): 228 | self.eclipseObject.sunInMsgName = 'sun_planet_data' 229 | self.eclipseObject.addPlanetName('mars barycenter') 230 | self.eclipseObject.addPositionMsgName(self.scObject.scStateOutMsgName) 231 | 232 | def SetExternalForceTorqueObject(self): 233 | self.extForceTorqueObject.ModelTag = "externalDisturbance" 234 | self.scObject.addDynamicEffector(self.extForceTorqueObject) 235 | 236 | def SetSimpleNavObject(self): 237 | self.SimpleNavObject.ModelTag = "SimpleNavigation" 238 | posSigma = 10.0 239 | velSigma = 0.001 240 | attSigma = 1.0 / 36000.0 * math.pi / 180.0 241 | attRateSig = 0.00005 * math.pi / 180.0 242 | sunSig = 0.1 * math.pi / 180.0 243 | DVsig = 0.005 244 | PMatrix = np.diag( 245 | [posSigma, posSigma, posSigma, velSigma, velSigma, velSigma, attSigma, attSigma, attSigma, attRateSig, 246 | attRateSig, attRateSig, sunSig, sunSig, sunSig, DVsig, DVsig, DVsig]) 247 | errorBounds = [100000.0, 100000.0, 100000.0, # Position 248 | 0.1, 0.1, 0.1, # Velocity 249 | 1E-18 * math.pi / 180.0, 1E-18 * math.pi / 180.0, 1E-18 * math.pi / 180.0, # Attitude 250 | 1E-18 * math.pi / 180.0, 1E-18 * math.pi / 180.0, 1E-18 * math.pi / 180.0, # Attitude Rate 251 | 5.0 * math.pi / 180.0, 5.0 * math.pi / 180.0, 5.0 * math.pi / 180.0, # Sun vector 252 | 0.5, 0.5, 0.5] # Accumulated DV 253 | # PMatrix = np.zeros_like(np.eye(18)) 254 | # errorBounds = [0.0] * 18 # Accumulated DV 255 | self.SimpleNavObject.walkBounds = np.array(errorBounds) 256 | self.SimpleNavObject.PMatrix = PMatrix 257 | self.SimpleNavObject.crossTrans = True 258 | self.SimpleNavObject.crossAtt = False 259 | 260 | def SetTruthNavObject(self): 261 | self.TruthNavObject.ModelTag = "TruthNavigation" 262 | PMatrix = np.zeros_like(np.eye(18)) 263 | errorBounds = [0.0] * 18 # Accumulated DV 264 | self.TruthNavObject.walkBounds = np.array(errorBounds) 265 | self.TruthNavObject.PMatrix = PMatrix 266 | self.TruthNavObject.outputAttName = "truth_att_output" 267 | self.TruthNavObject.outputTransName = "truth_trans_output" 268 | 269 | def SetReactionWheelDynEffector(self): 270 | # Make a fresh RW factory instance, this is critical to run multiple times 271 | rwFactory = simIncludeRW.rwFactory() 272 | 273 | # specify RW momentum capacity 274 | maxRWMomentum = 50. # Nms 275 | 276 | # Define orthogonal RW pyramid 277 | # -- Pointing directions 278 | rwElAngle = np.array([40.0, 40.0, 40.0, 40.0])*mc.D2R 279 | rwAzimuthAngle = np.array([45.0, 135.0, 225.0, 315.0])*mc.D2R 280 | rwPosVector = [[0.8, 0.8, 1.79070], 281 | [0.8, -0.8, 1.79070], 282 | [-0.8, -0.8, 1.79070], 283 | [-0.8, 0.8, 1.79070] 284 | ] 285 | 286 | for elAngle, azAngle, posVector in zip(rwElAngle, rwAzimuthAngle, rwPosVector): 287 | gsHat = (rbk.Mi(-azAngle,3).dot(rbk.Mi(elAngle,2))).dot(np.array([1,0,0])) 288 | rwFactory.create('Honeywell_HR16', 289 | gsHat, 290 | maxMomentum=maxRWMomentum, 291 | rWB_B=posVector) 292 | 293 | rwFactory.addToSpacecraft("RWStateEffector", self.rwStateEffector, self.scObject) 294 | 295 | def SetACSThrusterStateEffector(self): 296 | # Make a fresh TH factory instance, this is critical to run multiple times 297 | thFactory = simIncludeThruster.thrusterFactory() 298 | 299 | # 8 thrusters are modeled that act in pairs to provide the desired torque 300 | thPos = [ 301 | [825.5/1000.0, 880.3/1000.0, 1765.3/1000.0], 302 | [825.5/1000.0, 880.3/1000.0, 260.4/1000.0], 303 | [880.3/1000.0, 825.5/1000.0, 1765.3/1000.0], 304 | [880.3/1000.0, 825.5/1000.0, 260.4/1000.0], 305 | [-825.5/1000.0, -880.3/1000.0, 1765.3/1000.0], 306 | [-825.5/1000.0, -880.3/1000.0, 260.4/1000.0], 307 | [-880.3/1000.0, -825.5/1000.0, 1765.3/1000.0], 308 | [-880.3/1000.0, -825.5/1000.0, 260.4/1000.0] 309 | ] 310 | thDir = [ 311 | [0.0, -1.0, 0.0], 312 | [0.0, -1.0, 0.0], 313 | [-1.0, 0.0, 0.0], 314 | [-1.0, 0.0, 0.0], 315 | [0.0, 1.0, 0.0], 316 | [0.0, 1.0, 0.0], 317 | [1.0, 0.0, 0.0], 318 | [1.0, 0.0, 0.0] 319 | ] 320 | for pos_B, dir_B in zip(thPos, thDir): 321 | thFactory.create( 322 | 'MOOG_Monarc_1' 323 | , pos_B 324 | , dir_B 325 | ) 326 | # create thruster object container and tie to spacecraft object 327 | thFactory.addToSpacecraft("Thrusters", 328 | self.thrustersDynamicEffector, 329 | self.scObject) 330 | 331 | def SetCSSConstellation(self): 332 | self.CSSConstellationObject.ModelTag = "cssConstellation" 333 | self.CSSConstellationObject.outputConstellationMessage = "CSSConstellation_output" 334 | 335 | # define single CSS element 336 | CSS_default = coarse_sun_sensor.CoarseSunSensor() 337 | CSS_default.fov = 80. * mc.D2R # half-angle field of view value 338 | CSS_default.scaleFactor = 2.0 339 | 340 | # setup CSS sensor normal vectors in body frame components 341 | nHat_B_List = [ 342 | [0.0, 0.707107, 0.707107], 343 | [0.707107, 0., 0.707107], 344 | [0.0, -0.707107, 0.707107], 345 | [-0.707107, 0., 0.707107], 346 | [0.0, -0.965926, -0.258819], 347 | [-0.707107, -0.353553, -0.612372], 348 | [0., 0.258819, -0.965926], 349 | [0.707107, -0.353553, -0.612372] 350 | ] 351 | numCSS = len(nHat_B_List) 352 | 353 | # store all 354 | cssList = [] 355 | for nHat_B, i in zip(nHat_B_List,range(1,numCSS+1)): 356 | CSS = coarse_sun_sensor.CoarseSunSensor(CSS_default) 357 | CSS.ModelTag = "CSS" + str(i) + "_sensor" 358 | CSS.cssDataOutMsgName = "CSS" + str(i) + "_output" 359 | CSS.nHat_B = np.array(nHat_B) 360 | CSS.sunEclipseInMsgName = "eclipse_data_0" 361 | cssList.append(CSS) 362 | 363 | # assign the list of CSS devices to the CSS array class 364 | self.CSSConstellationObject.sensorList = coarse_sun_sensor.CSSVector(cssList) 365 | 366 | def SetEphemConvert(self): 367 | # Initialize the ephermis module 368 | self.ephemObject.ModelTag = 'EphemData' 369 | planets = ["mars barycenter"] 370 | messageMap = {} 371 | for planet in planets: 372 | messageMap[planet + '_planet_data'] = planet + '_ephemeris_data' 373 | self.ephemObject.messageNameMap = ephemeris_converter.map_string_string(messageMap) 374 | 375 | def SetinstrumentSunBore(self): 376 | self.instrumentSunBore.ModelTag = "instrumentBoreSun" 377 | self.instrumentSunBore.StateString = "inertial_state_output" 378 | self.instrumentSunBore.celBodyString = "sun_planet_data" 379 | self.instrumentSunBore.OutputDataString = "instrument_sun_bore" 380 | self.instrumentSunBore.boreVec_B = [0.0, 1.0, 0.0] 381 | 382 | def SetSimpleGrav(self): 383 | # clear prior gravitational body and SPICE setup definitions 384 | self.marsGravBody = gravityEffector.GravBodyData() 385 | self.marsGravBody.bodyInMsgName = "mars barycenter_planet_data" 386 | self.marsGravBody.mu = 4.2828371901284001E+13 # meters! 387 | self.marsGravBody.isCentralBody = True 388 | self.marsGravBody.useSphericalHarmParams = False 389 | 390 | # attach gravity model to spaceCraftPlus 391 | self.scObject.gravField.gravBodies = spacecraftPlus.GravBodyVector([self.marsGravBody]) 392 | 393 | def SetSpiceObject(self): 394 | self.SpiceObject.ModelTag = "SpiceInterfaceData" 395 | self.SpiceObject.SPICEDataPath = self.simBasePath + '/supportData/EphemerisData/' 396 | self.SpiceObject.UTCCalInit = "2019 DECEMBER 12 18:00:00.0" 397 | self.SpiceObject.outputBufferCount = 2 398 | self.SpiceObject.planetNames = spice_interface.StringVector(["earth", "mars barycenter", "sun", "jupiter barycenter"]) 399 | # self.SpiceObject.planetNames = spice_interface.StringVector(["mars barycenter"]) 400 | self.SpiceObject.referenceBase = "J2000" 401 | self.SpiceObject.zeroBase = "mars barycenter" 402 | 403 | pyswice.furnsh_c(self.SpiceObject.SPICEDataPath + 'de430.bsp') # solar system bodies 404 | pyswice.furnsh_c(self.SpiceObject.SPICEDataPath + 'naif0012.tls') # leap second file 405 | pyswice.furnsh_c(self.SpiceObject.SPICEDataPath + 'de-403-masses.tpc') # solar system masses 406 | pyswice.furnsh_c(self.SpiceObject.SPICEDataPath + 'pck00010.tpc') # generic Planetary Constants Kernel 407 | 408 | # Global call to initialize every module 409 | def InitAllDynObjects(self): 410 | self.SetSpacecraftHub() 411 | # self.SetgravityEffector() 412 | self.SetSimpleGrav() 413 | self.SetEclipseObject() 414 | self.SetExternalForceTorqueObject() 415 | self.SetSimpleNavObject() 416 | self.SetReactionWheelDynEffector() 417 | self.SetACSThrusterStateEffector() 418 | self.SetCSSConstellation() 419 | self.SetVizInterface() 420 | self.SetEphemConvert() 421 | self.SetCamera() 422 | 423 | self.SetSpiceObject() 424 | 425 | 426 | def InitAllAnalysisObjects(self): 427 | self.SetTruthNavObject() 428 | self.SetTruthErrorsData() 429 | self.SetinstrumentSunBore() 430 | 431 | # Global call to create every required one-time message 432 | def WriteInitDynMessages(self, SimBase): 433 | msgName = 'mars barycenter_planet_data' 434 | ephemData = spice_interface.SpicePlanetStateSimMsg() 435 | ephemData.J2000Current = 0.0 436 | ephemData.PositionVector = [0.0, 0.0, 0.0] 437 | ephemData.VelocityVector = [0.0, 0.0, 0.0] 438 | ephemData.J20002Pfix = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] 439 | ephemData.J20002Pfix_dot = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] 440 | ephemData.PlanetName = 'mars barycenter' 441 | # setting the msg structure name is required below to all the planet msg to be logged 442 | unitTestSupport.setMessage(SimBase.TotalSim, self.processName, msgName, 443 | ephemData, "SpicePlanetStateSimMsg") 444 | return 445 | 446 | 447 | -------------------------------------------------------------------------------- /basilisk_env/simulators/opNav_models/BSK_OpNavFsw.py: -------------------------------------------------------------------------------- 1 | ''' ''' 2 | ''' 3 | ISC License 4 | 5 | Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder 6 | 7 | Permission to use, copy, modify, and/or distribute this software for any 8 | purpose with or without fee is hereby granted, provided that the above 9 | copyright notice and this permission notice appear in all copies. 10 | 11 | THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 12 | WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 13 | MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 14 | ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 15 | WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 16 | ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 17 | OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 18 | 19 | ''' 20 | 21 | import math 22 | import numpy as np 23 | 24 | from Basilisk.fswAlgorithms import (hillPoint, inertial3D, attTrackingError, MRP_Feedback, 25 | rwMotorTorque, fswMessages, opNavPoint, velocityPoint, 26 | sunSafePoint, cssWlsEst, headingSuKF, limbFinding, horizonOpNav, 27 | centerRadiusCNN, faultDetection) 28 | from Basilisk.fswAlgorithms import pixelLineConverter, houghCircles, relativeODuKF, pixelLineBiasUKF #FSW for OpNav 29 | from Basilisk.utilities import RigidBodyKinematics as rbk 30 | from Basilisk.utilities import fswSetupRW, unitTestSupport, orbitalMotion, macros 31 | from Basilisk import __path__ 32 | bskPath = __path__[0] 33 | 34 | class BSKFswModels(): 35 | def __init__(self, SimBase, fswRate): 36 | # Define process name and default time-step for all FSW tasks defined later on 37 | self.processName = SimBase.FSWProcessName 38 | self.processTasksTimeStep = macros.sec2nano(fswRate) # 0.5 39 | 40 | # Create module data and module wraps 41 | self.inertial3DData = inertial3D.inertial3DConfig() 42 | self.inertial3DWrap = SimBase.setModelDataWrap(self.inertial3DData) 43 | self.inertial3DWrap.ModelTag = "inertial3D" 44 | 45 | self.hillPointData = hillPoint.hillPointConfig() 46 | self.hillPointWrap = SimBase.setModelDataWrap(self.hillPointData) 47 | self.hillPointWrap.ModelTag = "hillPoint" 48 | 49 | self.sunSafePointData = sunSafePoint.sunSafePointConfig() 50 | self.sunSafePointWrap = SimBase.setModelDataWrap(self.sunSafePointData) 51 | self.sunSafePointWrap.ModelTag = "sunSafePoint" 52 | 53 | self.opNavPointData = opNavPoint.OpNavPointConfig() 54 | self.opNavPointWrap = SimBase.setModelDataWrap(self.opNavPointData) 55 | self.opNavPointWrap.ModelTag = "opNavPoint" 56 | 57 | self.velocityPointData = velocityPoint.velocityPointConfig() 58 | self.velocityPointWrap = SimBase.setModelDataWrap(self.velocityPointData) 59 | self.velocityPointWrap.ModelTag = "velocityPoint" 60 | 61 | self.cssWlsEstData = cssWlsEst.CSSWLSConfig() 62 | self.cssWlsEstWrap = SimBase.setModelDataWrap(self.cssWlsEstData) 63 | self.cssWlsEstWrap.ModelTag = "cssWlsEst" 64 | 65 | self.trackingErrorData = attTrackingError.attTrackingErrorConfig() 66 | self.trackingErrorWrap = SimBase.setModelDataWrap(self.trackingErrorData) 67 | self.trackingErrorWrap.ModelTag = "trackingError" 68 | 69 | self.trackingErrorCamData = attTrackingError.attTrackingErrorConfig() 70 | self.trackingErrorCamWrap = SimBase.setModelDataWrap(self.trackingErrorCamData) 71 | self.trackingErrorCamWrap.ModelTag = "trackingErrorCam" 72 | 73 | self.mrpFeedbackRWsData = MRP_Feedback.MRP_FeedbackConfig() 74 | self.mrpFeedbackRWsWrap = SimBase.setModelDataWrap(self.mrpFeedbackRWsData) 75 | self.mrpFeedbackRWsWrap.ModelTag = "mrpFeedbackRWs" 76 | 77 | self.rwMotorTorqueData = rwMotorTorque.rwMotorTorqueConfig() 78 | self.rwMotorTorqueWrap = SimBase.setModelDataWrap(self.rwMotorTorqueData) 79 | self.rwMotorTorqueWrap.ModelTag = "rwMotorTorque" 80 | 81 | self.imageProcessing = houghCircles.HoughCircles() 82 | self.imageProcessing.ModelTag = "houghCircles" 83 | 84 | self.opNavCNN = centerRadiusCNN.CenterRadiusCNN() 85 | self.opNavCNN.ModelTag = "opNavCNN" 86 | 87 | self.pixelLineData = pixelLineConverter.PixelLineConvertData() 88 | self.pixelLineWrap = SimBase.setModelDataWrap(self.pixelLineData) 89 | self.pixelLineWrap.ModelTag = "pixelLine" 90 | 91 | self.opNavFaultData = faultDetection.FaultDetectionData() 92 | self.opNavFaultWrap = SimBase.setModelDataWrap(self.opNavFaultData) 93 | self.opNavFaultWrap.ModelTag = "OpNav_Fault" 94 | 95 | self.relativeODData = relativeODuKF.RelODuKFConfig() 96 | self.relativeODWrap = SimBase.setModelDataWrap(self.relativeODData) 97 | self.relativeODWrap.ModelTag = "relativeOD" 98 | 99 | self.headingUKFData = headingSuKF.HeadingSuKFConfig() 100 | self.headingUKFWrap = SimBase.setModelDataWrap(self.headingUKFData) 101 | self.headingUKFWrap.ModelTag = "headingUKF" 102 | 103 | # Initialize all modules 104 | self.InitAllFSWObjects(SimBase) 105 | 106 | # Create tasks 107 | SimBase.fswProc.addTask(SimBase.CreateNewTask("hillPointTask", self.processTasksTimeStep), 20) 108 | SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavPointTask", self.processTasksTimeStep), 20) 109 | SimBase.fswProc.addTask(SimBase.CreateNewTask("headingPointTask", self.processTasksTimeStep), 20) 110 | SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavPointTaskCheat", self.processTasksTimeStep), 20) 111 | SimBase.fswProc.addTask(SimBase.CreateNewTask("mrpFeedbackRWsTask", self.processTasksTimeStep), 15) 112 | SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavODTask", self.processTasksTimeStep), 5) 113 | SimBase.fswProc.addTask(SimBase.CreateNewTask("imageProcTask", self.processTasksTimeStep), 9) 114 | SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavODTaskB", self.processTasksTimeStep), 9) 115 | SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavAttODTask", self.processTasksTimeStep), 9) 116 | SimBase.fswProc.addTask(SimBase.CreateNewTask("cnnAttODTask", self.processTasksTimeStep), 9) 117 | SimBase.fswProc.addTask(SimBase.CreateNewTask("opNavFaultDet", self.processTasksTimeStep), 9) 118 | SimBase.fswProc.addTask(SimBase.CreateNewTask("attODFaultDet", self.processTasksTimeStep), 9) 119 | SimBase.fswProc.addTask(SimBase.CreateNewTask("cnnFaultDet", self.processTasksTimeStep), 9) 120 | 121 | SimBase.fswProc.addTask(SimBase.CreateNewTask("sunSafePointTask", self.processTasksTimeStep), 20) 122 | SimBase.fswProc.addTask(SimBase.CreateNewTask("mrpSteeringRWsTask", self.processTasksTimeStep), 10) 123 | 124 | SimBase.AddModelToTask("hillPointTask", self.hillPointWrap, self.hillPointData, 10) 125 | SimBase.AddModelToTask("hillPointTask", self.trackingErrorCamWrap, self.trackingErrorCamData, 9) 126 | 127 | SimBase.AddModelToTask("sunSafePointTask", self.sunSafePointWrap, self.sunSafePointData, 10) 128 | SimBase.AddModelToTask("sunSafePointTask", self.cssWlsEstWrap, self.cssWlsEstData, 9) 129 | SimBase.AddModelToTask("sunSafePointTask", self.relativeODWrap, self.relativeODData, 5) 130 | 131 | SimBase.AddModelToTask("opNavPointTask", self.imageProcessing, None, 15) 132 | SimBase.AddModelToTask("opNavPointTask", self.pixelLineWrap, self.pixelLineData, 12) 133 | SimBase.AddModelToTask("opNavPointTask", self.opNavPointWrap, self.opNavPointData, 9) 134 | 135 | SimBase.AddModelToTask("headingPointTask", self.imageProcessing, None, 15) 136 | SimBase.AddModelToTask("headingPointTask", self.pixelLineWrap, self.pixelLineData, 12) 137 | SimBase.AddModelToTask("headingPointTask", self.headingUKFWrap, self.headingUKFData, 10) 138 | SimBase.AddModelToTask("headingPointTask", self.opNavPointWrap, self.opNavPointData, 9) 139 | 140 | SimBase.AddModelToTask("opNavPointTaskCheat", self.hillPointWrap, self.hillPointData, 10) 141 | SimBase.AddModelToTask("opNavPointTaskCheat", self.trackingErrorCamWrap, self.trackingErrorCamData, 9) 142 | 143 | SimBase.AddModelToTask("opNavODTask", self.imageProcessing, None, 15) 144 | SimBase.AddModelToTask("opNavODTask", self.pixelLineWrap, self.pixelLineData, 14) 145 | SimBase.AddModelToTask("opNavODTask", self.relativeODWrap, self.relativeODData, 13) 146 | 147 | SimBase.AddModelToTask("imageProcTask", self.imageProcessing, None, 15) 148 | 149 | SimBase.AddModelToTask("opNavAttODTask", self.imageProcessing, None, 15) 150 | SimBase.AddModelToTask("opNavAttODTask", self.pixelLineWrap, self.pixelLineData, 14) 151 | SimBase.AddModelToTask("opNavAttODTask", self.opNavPointWrap, self.opNavPointData, 10) 152 | # SimBase.AddModelToTask("opNavAttODTask", self.pixelLineFilterWrap, self.pixelLineFilterData, 9) 153 | SimBase.AddModelToTask("opNavAttODTask", self.relativeODWrap, self.relativeODData, 9) 154 | 155 | SimBase.AddModelToTask("cnnAttODTask", self.opNavCNN, None, 15) 156 | SimBase.AddModelToTask("cnnAttODTask", self.pixelLineWrap, self.pixelLineData, 14) 157 | SimBase.AddModelToTask("cnnAttODTask", self.opNavPointWrap, self.opNavPointData, 10) 158 | # SimBase.AddModelToTask("opNavAttODTask", self.pixelLineFilterWrap, self.pixelLineFilterData, 9) 159 | SimBase.AddModelToTask("cnnAttODTask", self.relativeODWrap, self.relativeODData, 9) 160 | 161 | SimBase.AddModelToTask("mrpFeedbackRWsTask", self.mrpFeedbackRWsWrap, self.mrpFeedbackRWsData, 9) 162 | SimBase.AddModelToTask("mrpFeedbackRWsTask", self.rwMotorTorqueWrap, self.rwMotorTorqueData, 8) 163 | 164 | SimBase.AddModelToTask("cnnFaultDet", self.opNavCNN, None, 25) 165 | SimBase.AddModelToTask("cnnFaultDet", self.pixelLineWrap, self.pixelLineData, 20) 166 | SimBase.AddModelToTask("cnnFaultDet", self.imageProcessing, None, 18) 167 | SimBase.AddModelToTask("cnnFaultDet", self.pixelLineWrap, self.pixelLineData, 16) 168 | SimBase.AddModelToTask("cnnFaultDet", self.opNavFaultWrap, self.opNavFaultData, 14) 169 | SimBase.AddModelToTask("cnnFaultDet", self.opNavPointWrap, self.opNavPointData, 10) 170 | SimBase.AddModelToTask("cnnFaultDet", self.relativeODWrap, self.relativeODData, 9) 171 | 172 | # Create events to be called for triggering GN&C maneuvers 173 | SimBase.fswProc.disableAllTasks() 174 | 175 | SimBase.createNewEvent("initiateStandby", self.processTasksTimeStep, True, 176 | ["self.modeRequest == 'standby'"], 177 | ["self.fswProc.disableAllTasks()", 178 | ]) 179 | 180 | SimBase.createNewEvent("initiateSunSafePoint", self.processTasksTimeStep, True, 181 | ["self.modeRequest == 'sunSafePoint'"], 182 | ["self.fswProc.disableAllTasks()", 183 | "self.enableTask('sunSafePointTask')", 184 | "self.enableTask('mrpFeedbackRWsTask')"]) 185 | 186 | SimBase.createNewEvent("prepOpNav", self.processTasksTimeStep, True, 187 | ["self.modeRequest == 'prepOpNav'"], 188 | ["self.fswProc.disableAllTasks()", 189 | "self.enableTask('opNavPointTaskCheat')", 190 | "self.enableTask('mrpFeedbackRWsTask')"]) 191 | 192 | SimBase.createNewEvent("imageGen", self.processTasksTimeStep, True, 193 | ["self.modeRequest == 'imageGen'"], 194 | ["self.fswProc.disableAllTasks()", 195 | "self.enableTask('imageProcTask')", 196 | "self.enableTask('opNavPointTaskCheat')", 197 | "self.enableTask('mrpFeedbackRWsTask')"]) 198 | 199 | SimBase.createNewEvent("pointOpNav", self.processTasksTimeStep, True, 200 | ["self.modeRequest == 'pointOpNav'"], 201 | ["self.fswProc.disableAllTasks()", 202 | "self.enableTask('opNavPointTask')", 203 | "self.enableTask('mrpFeedbackRWsTask')"]) 204 | 205 | SimBase.createNewEvent("pointHead", self.processTasksTimeStep, True, 206 | ["self.modeRequest == 'pointHead'"], 207 | ["self.fswProc.disableAllTasks()", 208 | "self.enableTask('headingPointTask')", 209 | "self.enableTask('mrpFeedbackRWsTask')"]) 210 | # 211 | # SimBase.createNewEvent("pointLimb", self.processTasksTimeStep, True, 212 | # ["self.modeRequest == 'pointLimb'"], 213 | # ["self.fswProc.disableAllTasks()", 214 | # "self.enableTask('opNavPointLimbTask')", 215 | # "self.enableTask('mrpFeedbackRWsTask')"]) 216 | 217 | SimBase.createNewEvent("OpNavOD", self.processTasksTimeStep, True, 218 | ["self.modeRequest == 'OpNavOD'"], 219 | ["self.fswProc.disableAllTasks()", 220 | "self.enableTask('opNavPointTaskCheat')", 221 | "self.enableTask('mrpFeedbackRWsTask')", 222 | "self.enableTask('opNavODTask')"]) 223 | 224 | # SimBase.createNewEvent("DoubleOD", self.processTasksTimeStep, True, 225 | # ["self.modeRequest == 'DoubleOD'"], 226 | # ["self.fswProc.disableAllTasks()", 227 | # "self.enableTask('opNavPointTaskCheat')", 228 | # "self.enableTask('mrpFeedbackRWsTask')", 229 | # "self.enableTask('opNavODTaskLimb')", 230 | # "self.enableTask('opNavODTask')"]) 231 | # 232 | # SimBase.createNewEvent("OpNavODLimb", self.processTasksTimeStep, True, 233 | # ["self.modeRequest == 'OpNavODLimb'"], 234 | # ["self.fswProc.disableAllTasks()", 235 | # "self.enableTask('opNavPointTaskCheat')", 236 | # "self.enableTask('mrpFeedbackRWsTask')", 237 | # "self.enableTask('opNavODTaskLimb')"]) 238 | 239 | SimBase.createNewEvent("OpNavODB", self.processTasksTimeStep, True, 240 | ["self.modeRequest == 'OpNavODB'"], 241 | ["self.fswProc.disableAllTasks()", 242 | "self.enableTask('opNavPointTaskCheat')", 243 | "self.enableTask('mrpFeedbackRWsTask')", 244 | "self.enableTask('opNavODTaskB')"]) 245 | 246 | SimBase.createNewEvent("OpNavAttOD", self.processTasksTimeStep, True, 247 | ["self.modeRequest == 'OpNavAttOD'"], 248 | ["self.fswProc.disableAllTasks()", 249 | "self.enableTask('opNavAttODTask')", 250 | "self.enableTask('mrpFeedbackRWsTask')"]) 251 | 252 | # SimBase.createNewEvent("OpNavAttODLimb", self.processTasksTimeStep, True, 253 | # ["self.modeRequest == 'OpNavAttODLimb'"], 254 | # ["self.fswProc.disableAllTasks()", 255 | # "self.enableTask('opNavAttODLimbTask')", 256 | # "self.enableTask('mrpFeedbackRWsTask')"]) 257 | 258 | SimBase.createNewEvent("CNNAttOD", self.processTasksTimeStep, True, 259 | ["self.modeRequest == 'CNNAttOD'"], 260 | ["self.fswProc.disableAllTasks()", 261 | "self.enableTask('cnnAttODTask')", 262 | "self.enableTask('mrpFeedbackRWsTask')"]) 263 | 264 | SimBase.createNewEvent("FaultDet", self.processTasksTimeStep, True, 265 | ["self.modeRequest == 'FaultDet'"], 266 | ["self.fswProc.disableAllTasks()", 267 | "self.enableTask('attODFaultDet')", 268 | "self.enableTask('mrpFeedbackRWsTask')"]) 269 | 270 | SimBase.createNewEvent("ODFaultDet", self.processTasksTimeStep, True, 271 | ["self.modeRequest == 'ODFaultDet'"], 272 | ["self.fswProc.disableAllTasks()", 273 | "self.enableTask('opNavPointTaskCheat')", 274 | "self.enableTask('mrpFeedbackRWsTask')", 275 | "self.enableTask('opNavFaultDet')"]) 276 | 277 | SimBase.createNewEvent("FaultDetCNN", self.processTasksTimeStep, True, 278 | ["self.modeRequest == 'FaultDetCNN'"], 279 | ["self.fswProc.disableAllTasks()", 280 | "self.enableTask('cnnFaultDet')", 281 | "self.enableTask('mrpFeedbackRWsTask')"]) 282 | 283 | # ------------------------------------------------------------------------------------------- # 284 | # These are module-initialization methods 285 | def SetHillPointGuidance(self, SimBase): 286 | self.hillPointData.outputDataName = "att_reference" 287 | self.hillPointData.inputNavDataName = SimBase.DynModels.SimpleNavObject.outputTransName 288 | self.hillPointData.inputCelMessName = "mars barycenter_ephemeris_data" 289 | 290 | def SetSunSafePointGuidance(self, SimBase): 291 | """Define the sun safe pointing guidance module""" 292 | self.sunSafePointData.attGuidanceOutMsgName = "att_guidance" 293 | self.sunSafePointData.imuInMsgName = SimBase.DynModels.SimpleNavObject.outputAttName 294 | self.sunSafePointData.sunDirectionInMsgName = self.cssWlsEstData.navStateOutMsgName 295 | self.sunSafePointData.sHatBdyCmd = [0.0, 0.0, 1.0] 296 | 297 | def SetOpNavPointGuidance(self, SimBase): 298 | self.opNavPointData.attGuidanceOutMsgName = "att_guidance" 299 | self.opNavPointData.imuInMsgName = SimBase.DynModels.SimpleNavObject.outputAttName 300 | self.opNavPointData.cameraConfigMsgName = "camera_config_data" 301 | self.opNavPointData.opnavDataInMsgName = "output_nav_msg" #"heading_filtered" 302 | self.opNavPointData.smallAngle = 0.001*np.pi/180. 303 | self.opNavPointData.timeOut = 1000 # Max time in sec between images before engaging search 304 | # self.opNavPointData.opNavAxisSpinRate = 0.1*np.pi/180. 305 | self.opNavPointData.omega_RN_B = [0.001, 0.0, -0.001] 306 | self.opNavPointData.alignAxis_C = [0.,0.,1] 307 | 308 | def SetHeadingUKF(self): 309 | self.headingUKFData.opnavOutMsgName = "heading_filtered" 310 | self.headingUKFData.filtDataOutMsgName = "heading_filter_data" 311 | self.headingUKFData.opnavDataInMsgName = "output_nav_msg" 312 | # self.headingUKFData.cameraConfigMsgName = "camera_config_data" 313 | 314 | self.headingUKFData.alpha = 0.02 315 | self.headingUKFData.beta = 2.0 316 | self.headingUKFData.kappa = 0.0 317 | 318 | # filterObject.state = [0.0, 0., 0., 0., 0.] 319 | self.headingUKFData.stateInit = [0.0, 0.0, 1.0, 0.0, 0.0] 320 | self.headingUKFData.covarInit = [0.2, 0.0, 0.0, 0.0, 0.0, 321 | 0.0, 0.2, 0.0, 0.0, 0.0, 322 | 0.0, 0.0, 0.2, 0.0, 0.0, 323 | 0.0, 0.0, 0.0, 0.005, 0.0, 324 | 0.0, 0.0, 0.0, 0.0, 0.005] 325 | 326 | qNoiseIn = np.identity(5) 327 | qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-6 * 1E-6 328 | qNoiseIn[3:5, 3:5] = qNoiseIn[3:5, 3:5] * 1E-6 * 1E-6 329 | self.headingUKFData.qNoise = qNoiseIn.reshape(25).tolist() 330 | # self.headingUKFData.qObsVal = 0.001 331 | 332 | def SetAttitudeTrackingError(self, SimBase): 333 | self.trackingErrorData.inputNavName = SimBase.DynModels.SimpleNavObject.outputAttName 334 | # Note: SimBase.DynModels.SimpleNavObject.outputAttName = "simple_att_nav_output" 335 | self.trackingErrorData.inputRefName = "att_reference" 336 | self.trackingErrorData.outputDataName = "att_guidance" 337 | 338 | ## Celestial point to Mars 339 | def SetCelTwoBodyMarsPoint(self): 340 | self.celTwoBodyMarsData.inputNavDataName = "simple_trans_nav_output" 341 | self.celTwoBodyMarsData.inputCelMessName = "mars barycenter_ephemeris_data" 342 | self.celTwoBodyMarsData.outputDataName = "att_ref_output" 343 | self.celTwoBodyMarsData.singularityThresh = 1.0 * math.pi / 180.0 344 | 345 | def SetAttTrackingErrorCam(self, SimBase): 346 | self.trackingErrorCamData.inputRefName = "att_reference" 347 | self.trackingErrorCamData.inputNavName = "simple_att_nav_output" 348 | self.trackingErrorCamData.outputDataName = "att_guidance" 349 | 350 | M2 = rbk.euler2(90 * macros.D2R) #rbk.euler2(-90 * macros.D2R) # 351 | M3 = rbk.euler1(90 * macros.D2R) #rbk.euler3(90 * macros.D2R) # 352 | M_cam = rbk.MRP2C(SimBase.DynModels.cameraMRP_CB) 353 | 354 | MRP = rbk.C2MRP(np.dot(np.dot(M3, M2), M_cam)) # This assures that the s/c does not control to the hill frame, but to a rotated frame such that the camera is pointing to the planet 355 | self.trackingErrorCamData.sigma_R0R = MRP 356 | # self.trackingErrorCamData.sigma_R0R = [1./3+0.1, 1./3-0.1, 0.1-1/3] 357 | 358 | def SetCSSWlsEst(self, SimBase): 359 | cssConfig = fswMessages.CSSConfigFswMsg() 360 | totalCSSList = [] 361 | nHat_B_vec = [ 362 | [0.0, 0.707107, 0.707107], 363 | [0.707107, 0., 0.707107], 364 | [0.0, -0.707107, 0.707107], 365 | [-0.707107, 0., 0.707107], 366 | [0.0, -0.965926, -0.258819], 367 | [-0.707107, -0.353553, -0.612372], 368 | [0., 0.258819, -0.965926], 369 | [0.707107, -0.353553, -0.612372] 370 | ] 371 | for CSSHat in nHat_B_vec: 372 | CSSConfigElement = fswMessages.CSSUnitConfigFswMsg() 373 | CSSConfigElement.CBias = 1.0 374 | CSSConfigElement.nHat_B = CSSHat 375 | totalCSSList.append(CSSConfigElement) 376 | cssConfig.cssVals = totalCSSList 377 | 378 | cssConfig.nCSS = len(SimBase.DynModels.CSSConstellationObject.sensorList) 379 | cssConfigSize = cssConfig.getStructSize() 380 | SimBase.TotalSim.CreateNewMessage("FSWProcess", "css_config_data", cssConfigSize, 2, "CSSConstellation") 381 | SimBase.TotalSim.WriteMessageData("css_config_data", cssConfigSize, 0, cssConfig) 382 | 383 | self.cssWlsEstData.cssDataInMsgName = SimBase.DynModels.CSSConstellationObject.outputConstellationMessage 384 | self.cssWlsEstData.cssConfigInMsgName = "css_config_data" 385 | self.cssWlsEstData.navStateOutMsgName = "sun_point_data" 386 | 387 | def SetMRPFeedbackControl(self, SimBase): 388 | self.mrpFeedbackControlData.inputGuidName = "att_guidance" 389 | self.mrpFeedbackControlData.vehConfigInMsgName = "adcs_config_data" 390 | self.mrpFeedbackControlData.outputDataName = SimBase.DynModels.extForceTorqueObject.cmdTorqueInMsgName 391 | # Note: SimBase.DynModels.extForceTorqueObject.cmdTorqueInMsgName = "extTorquePntB_B_cmds" 392 | 393 | self.mrpFeedbackControlData.K = 3.5 394 | self.mrpFeedbackControlData.Ki = -1.0 # Note: make value negative to turn off integral feedback 395 | self.mrpFeedbackControlData.P = 30.0 396 | self.mrpFeedbackControlData.integralLimit = 2. / self.mrpFeedbackControlData.Ki * 0.1 397 | 398 | 399 | def SetMRPFeedbackRWA(self): 400 | self.mrpFeedbackRWsData.K = 3.5 401 | self.mrpFeedbackRWsData.Ki = -1 # Note: make value negative to turn off integral feedback 402 | self.mrpFeedbackRWsData.P = 30.0 403 | self.mrpFeedbackRWsData.integralLimit = 2. / self.mrpFeedbackRWsData.Ki * 0.1 404 | 405 | self.mrpFeedbackRWsData.vehConfigInMsgName = "adcs_config_data" 406 | self.mrpFeedbackRWsData.inputRWSpeedsName = "reactionwheel_output_states" 407 | self.mrpFeedbackRWsData.rwParamsInMsgName = "rwa_config_data" 408 | self.mrpFeedbackRWsData.inputGuidName = "att_guidance" 409 | self.mrpFeedbackRWsData.outputDataName = "controlTorqueRaw" 410 | 411 | 412 | def SetVehicleConfiguration(self, SimBase): 413 | vehicleConfigOut = fswMessages.VehicleConfigFswMsg() 414 | # use the same inertia in the FSW algorithm as in the simulation 415 | vehicleConfigOut.ISCPntB_B = [900.0, 0.0, 0.0, 0.0, 800.0, 0.0, 0.0, 0.0, 600.0] 416 | unitTestSupport.setMessage(SimBase.TotalSim, 417 | SimBase.FSWProcessName, 418 | "adcs_config_data", 419 | vehicleConfigOut) 420 | 421 | def SetRWConfigMsg(self, SimBase): 422 | # Configure RW pyramid exactly as it is in the Dynamics (i.e. FSW with perfect knowledge) 423 | rwElAngle = np.array([40.0, 40.0, 40.0, 40.0]) * macros.D2R 424 | rwAzimuthAngle = np.array([45.0, 135.0, 225.0, 315.0]) * macros.D2R 425 | wheelJs = 50.0 / (6000.0 * math.pi * 2.0 / 60) 426 | 427 | fswSetupRW.clearSetup() 428 | for elAngle, azAngle in zip(rwElAngle, rwAzimuthAngle): 429 | gsHat = (rbk.Mi(-azAngle, 3).dot(rbk.Mi(elAngle, 2))).dot(np.array([1, 0, 0])) 430 | fswSetupRW.create(gsHat, # spin axis 431 | wheelJs, # kg*m^2 432 | 0.2) # Nm uMax 433 | 434 | fswSetupRW.writeConfigMessage("rwa_config_data", SimBase.TotalSim, SimBase.FSWProcessName) 435 | 436 | 437 | def SetRWMotorTorque(self, SimBase): 438 | controlAxes_B = [ 439 | 1.0, 0.0, 0.0 440 | , 0.0, 1.0, 0.0 441 | , 0.0, 0.0, 1.0 442 | ] 443 | self.rwMotorTorqueData.controlAxes_B = controlAxes_B 444 | self.rwMotorTorqueData.inputVehControlName = "controlTorqueRaw" 445 | self.rwMotorTorqueData.outputDataName = SimBase.DynModels.rwStateEffector.InputCmds # "reactionwheel_cmds" 446 | self.rwMotorTorqueData.rwParamsInMsgName = "rwa_config_data" 447 | 448 | def SetCNNOpNav(self): 449 | self.opNavCNN.imageInMsgName = "opnav_image" 450 | self.opNavCNN.opnavCirclesOutMsgName = "circles_data" 451 | self.opNavCNN.pixelNoise = [5,5,5] 452 | self.opNavCNN.pathToNetwork = bskPath + "/../../src/fswAlgorithms/imageProcessing/centerRadiusCNN/CAD.onnx" 453 | 454 | def SetImageProcessing(self): 455 | self.imageProcessing.imageInMsgName = "opnav_image" 456 | self.imageProcessing.opnavCirclesOutMsgName = "circles_data" 457 | 458 | self.imageProcessing.saveImages = 0 459 | self.imageProcessing.expectedCircles = 1 460 | self.imageProcessing.cannyThresh = 200 461 | self.imageProcessing.voteThresh = 25 462 | self.imageProcessing.houghMinDist = 50 463 | self.imageProcessing.houghMinRadius = 20 464 | self.imageProcessing.blurrSize = 9 465 | self.imageProcessing.noiseSF = 1 466 | self.imageProcessing.dpValue = 1 467 | self.imageProcessing.saveDir = 'Test/' 468 | self.imageProcessing.houghMaxRadius = 0#int(512 / 1.25) 469 | 470 | def SetPixelLineConversion(self): 471 | self.pixelLineData.circlesInMsgName = "circles_data" 472 | self.pixelLineData.cameraConfigMsgName = "camera_config_data" 473 | self.pixelLineData.attInMsgName = "simple_att_nav_output" 474 | self.pixelLineData.planetTarget = 2 475 | self.pixelLineData.opNavOutMsgName = "output_nav_msg" 476 | 477 | def SetLimbFinding(self): 478 | self.limbFinding.imageInMsgName = "opnav_image" 479 | self.limbFinding.opnavLimbOutMsgName = "limb_data" 480 | 481 | self.limbFinding.saveImages = 0 482 | self.limbFinding.cannyThreshLow = 50 483 | self.limbFinding.cannyThreshHigh = 100 484 | self.limbFinding.blurrSize = 5 485 | self.limbFinding.limbNumThresh = 0 486 | 487 | def SetHorizonNav(self): 488 | self.horizonNavData.limbInMsgName = "limb_data" 489 | self.horizonNavData.cameraConfigMsgName = "camera_config_data" 490 | self.horizonNavData.attInMsgName = "simple_att_nav_output" 491 | self.horizonNavData.planetTarget = 2 492 | self.horizonNavData.noiseSF = 1 #2 should work though 493 | self.horizonNavData.opNavOutMsgName = "output_nav_msg" 494 | 495 | def SetRelativeODFilter(self): 496 | self.relativeODData.navStateOutMsgName = "relod_state_estimate" 497 | self.relativeODData.filtDataOutMsgName = "relod_filter_data" 498 | self.relativeODData.opNavInMsgName = "output_nav_msg" 499 | 500 | self.relativeODData.planetIdInit = 2 501 | self.relativeODData.alpha = 0.02 502 | self.relativeODData.beta = 2.0 503 | self.relativeODData.kappa = 0.0 504 | self.relativeODData.noiseSF = 7.5 505 | 506 | mu = 42828.314 * 1E9 # m^3/s^2 507 | elementsInit = orbitalMotion.ClassicElements() 508 | elementsInit.a = 10000 * 1E3 # m 509 | elementsInit.e = 0.2 510 | elementsInit.i = 10 * macros.D2R 511 | elementsInit.Omega = 25. * macros.D2R 512 | elementsInit.omega = 10. * macros.D2R 513 | elementsInit.f = 40 * macros.D2R 514 | r, v = orbitalMotion.elem2rv(mu, elementsInit) 515 | 516 | self.relativeODData.stateInit = r.tolist() + v.tolist() 517 | self.relativeODData.covarInit = [1. * 1E6, 0.0, 0.0, 0.0, 0.0, 0.0, 518 | 0.0, 1. * 1E6, 0.0, 0.0, 0.0, 0.0, 519 | 0.0, 0.0, 1. * 1E6, 0.0, 0.0, 0.0, 520 | 0.0, 0.0, 0.0, 0.02 * 1E6, 0.0, 0.0, 521 | 0.0, 0.0, 0.0, 0.0, 0.02 * 1E6, 0.0, 522 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.02 * 1E6] 523 | 524 | qNoiseIn = np.identity(6) 525 | qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-3 * 1E-3 526 | qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1E-4 * 1E-4 527 | self.relativeODData.qNoise = qNoiseIn.reshape(36).tolist() 528 | 529 | def SetFaultDetection(self): 530 | self.opNavFaultData.navMeasPrimaryMsgName = "primary_opnav" 531 | self.opNavFaultData.navMeasSecondaryMsgName = "secondary_opnav" 532 | self.opNavFaultData.cameraConfigMsgName = "camera_config_data" 533 | self.opNavFaultData.attInMsgName = "simple_att_nav_output" 534 | self.opNavFaultData.opNavOutMsgName = "output_nav_msg" 535 | self.opNavFaultData.sigmaFault = 0.3 536 | self.opNavFaultData.faultMode = 0 537 | 538 | def SetPixelLineFilter(self): 539 | self.pixelLineFilterData.navStateOutMsgName = "pixelLine_state_estimate" 540 | self.pixelLineFilterData.filtDataOutMsgName = "pixelLine_filter_data" 541 | self.pixelLineFilterData.circlesInMsgName = "circles_data" 542 | self.pixelLineFilterData.cameraConfigMsgName = "camera_config_data" 543 | self.pixelLineFilterData.attInMsgName = "simple_att_nav_output" 544 | 545 | self.pixelLineFilterData.planetIdInit = 2 546 | self.pixelLineFilterData.alpha = 0.02 547 | self.pixelLineFilterData.beta = 2.0 548 | self.pixelLineFilterData.kappa = 0.0 549 | self.pixelLineFilterData.gamma = 0.9 550 | 551 | mu = 42828.314 * 1E9 # m^3/s^2 552 | elementsInit = orbitalMotion.ClassicElements() 553 | elementsInit.a = 10000 * 1E3 # m 554 | elementsInit.e = 0.2 555 | elementsInit.i = 10 * macros.D2R 556 | elementsInit.Omega = 25. * macros.D2R 557 | elementsInit.omega = 10. * macros.D2R 558 | elementsInit.f = 40 * macros.D2R 559 | r, v = orbitalMotion.elem2rv(mu, elementsInit) 560 | bias = [1, 1, 2] 561 | 562 | self.pixelLineFilterData.stateInit = r.tolist() + v.tolist() + bias 563 | self.pixelLineFilterData.covarInit = [10. * 1E6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 564 | 0.0, 10. * 1E6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 565 | 0.0, 0.0, 10. * 1E6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 566 | 0.0, 0.0, 0.0, 0.01 * 1E6, 0.0, 0.0, 0.0, 0.0, 0.0, 567 | 0.0, 0.0, 0.0, 0.0, 0.01 * 1E6, 0.0, 0.0, 0.0, 0.0, 568 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.01 * 1E6, 0.0, 0.0, 0.0, 569 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1, 0.0, 0.0, 570 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1, 0.0, 571 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1] 572 | 573 | 574 | qNoiseIn = np.identity(9) 575 | qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-3 * 1E-3 576 | qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1E-4 * 1E-4 577 | qNoiseIn[6:9, 6:9] = qNoiseIn[6:9, 6:9] * 1E-8 * 1E-8 578 | self.pixelLineFilterData.qNoise = qNoiseIn.reshape(9 * 9).tolist() 579 | 580 | 581 | # Global call to initialize every module 582 | def InitAllFSWObjects(self, SimBase): 583 | self.SetHillPointGuidance(SimBase) 584 | self.SetCSSWlsEst(SimBase) 585 | self.SetAttitudeTrackingError(SimBase) 586 | self.SetVehicleConfiguration(SimBase) 587 | self.SetRWConfigMsg(SimBase) 588 | self.SetMRPFeedbackRWA() 589 | self.SetRWMotorTorque(SimBase) 590 | self.SetAttTrackingErrorCam(SimBase) 591 | self.SetImageProcessing() 592 | self.SetPixelLineConversion() 593 | 594 | self.SetCNNOpNav() 595 | self.SetRelativeODFilter() 596 | self.SetFaultDetection() 597 | # J. Christian methods 598 | # self.SetLimbFinding() 599 | # self.SetHorizonNav() 600 | 601 | self.SetOpNavPointGuidance(SimBase) 602 | self.SetHeadingUKF() 603 | # self.SetPixelLineFilter() 604 | 605 | self.SetSunSafePointGuidance(SimBase) 606 | 607 | 608 | #BSKFswModels() 609 | -------------------------------------------------------------------------------- /basilisk_env/simulators/opNav_models/BSK_masters.py: -------------------------------------------------------------------------------- 1 | ''' ''' 2 | ''' 3 | ISC License 4 | 5 | Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder 6 | 7 | Permission to use, copy, modify, and/or distribute this software for any 8 | purpose with or without fee is hereby granted, provided that the above 9 | copyright notice and this permission notice appear in all copies. 10 | 11 | THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 12 | WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 13 | MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 14 | ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 15 | WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 16 | ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 17 | OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 18 | 19 | ''' 20 | 21 | # Import architectural modules 22 | from Basilisk.utilities import SimulationBaseClass, macros 23 | from Basilisk.simulation import sim_model 24 | 25 | # Get current file path 26 | import sys, os, inspect 27 | filename = inspect.getframeinfo(inspect.currentframe()).filename 28 | path = os.path.dirname(os.path.abspath(filename)) 29 | 30 | # Import Dynamics and FSW models 31 | sys.path.append(path + '/models') 32 | 33 | 34 | class BSKSim(SimulationBaseClass.SimBaseClass): 35 | def __init__(self, fswRate=0.1, dynRate=0.1, viz_port=5000): 36 | self.dynRate = dynRate 37 | self.fswRate = fswRate 38 | self.viz_port = viz_port 39 | # Create a sim module as an empty container 40 | SimulationBaseClass.SimBaseClass.__init__(self) 41 | self.TotalSim.terminateSimulation() 42 | 43 | self.DynModels = [] 44 | self.FSWModels = [] 45 | 46 | self.dynamics_added = False 47 | self.fsw_added = False 48 | 49 | def get_DynModel(self): 50 | assert (self.dynamics_added is True), "It is mandatory to use a dynamics model as an argument" 51 | return self.DynModels 52 | 53 | def set_DynModel(self, dynModel): 54 | self.dynamics_added = True 55 | self.DynamicsProcessName = 'DynamicsProcess' #Create simulation process name 56 | self.dynProc = self.CreateNewProcess(self.DynamicsProcessName, 100) #Create process 57 | self.DynModels = dynModel.BSKDynamicModels(self, self.dynRate, self.viz_port) #Create Dynamics and FSW classes 58 | 59 | def get_FswModel(self): 60 | assert (self.fsw_added is True), "A flight software model has not been added yet" 61 | return self.FSWModels 62 | 63 | def set_FswModel(self, fswModel): 64 | self.fsw_added = True 65 | self.FSWProcessName = "FSWProcess" #Create simulation process name 66 | self.fswProc = self.CreateNewProcess(self.FSWProcessName, 10) #Create processe 67 | self.FSWModels = fswModel.BSKFswModels(self, self.fswRate) #Create Dynamics and FSW classes 68 | 69 | def initInterfaces(self): 70 | # Define process message interfaces. 71 | assert (self.fsw_added is True and self.dynamics_added is True), "Must have dynamics and fsw modules to interface" 72 | self.dyn2FSWInterface = sim_model.SysInterface() 73 | self.fsw2DynInterface = sim_model.SysInterface() 74 | 75 | # Discover interfaces between processes 76 | self.dyn2FSWInterface.addNewInterface(self.DynamicsProcessName, self.FSWProcessName) 77 | self.fsw2DynInterface.addNewInterface(self.FSWProcessName, self.DynamicsProcessName) 78 | self.dynProc.addInterfaceRef(self.fsw2DynInterface) 79 | self.fswProc.addInterfaceRef(self.dyn2FSWInterface) 80 | 81 | class BSKScenario(object): 82 | def __init__(self, masterSim): 83 | self.name = "scenario" 84 | self.masterSim = masterSim 85 | 86 | def configure_initial_conditions(self): 87 | """ 88 | Developer must override this method in their BSK_Scenario derived subclass. 89 | """ 90 | pass 91 | 92 | def log_outputs(self): 93 | """ 94 | Developer must override this method in their BSK_Scenario derived subclass. 95 | """ 96 | pass 97 | 98 | def pull_outputs(self): 99 | """ 100 | Developer must override this method in their BSK_Scenario derived subclass. 101 | """ 102 | pass 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /basilisk_env/simulators/opNav_models/OpNav_Plotting.py: -------------------------------------------------------------------------------- 1 | ''' ''' 2 | ''' 3 | ISC License 4 | 5 | Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder 6 | 7 | Permission to use, copy, modify, and/or distribute this software for any 8 | purpose with or without fee is hereby granted, provided that the above 9 | copyright notice and this permission notice appear in all copies. 10 | 11 | THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 12 | WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 13 | MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 14 | ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 15 | WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 16 | ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 17 | OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 18 | 19 | ''' 20 | import matplotlib as mpl 21 | import matplotlib.pyplot as plt 22 | import matplotlib.animation as animation 23 | 24 | from mpl_toolkits.mplot3d import Axes3D 25 | from matplotlib.patches import Ellipse 26 | import numpy as np 27 | import scipy.optimize 28 | import os 29 | from Basilisk.utilities import macros as mc 30 | from Basilisk.utilities import unitTestSupport 31 | from Basilisk.utilities import RigidBodyKinematics 32 | from Basilisk.utilities import orbitalMotion 33 | 34 | 35 | # --------------------------------- COMPONENTS & SUBPLOT HANDLING ----------------------------------------------- # 36 | color_x = 'dodgerblue' 37 | color_y = 'salmon' 38 | color_z = 'lightgreen' 39 | m2km = 1.0 / 1000.0 40 | ns2min = 1/60.*1E-9 41 | 42 | mpl.rcParams.update({'font.size' : 8 }) 43 | #seaborn-colorblind, 'seaborn-paper', 'bmh', 'tableau-colorblind10', 'seaborn-deep', 'myStyle', 'aiaa' 44 | 45 | plt.style.use("myStyle") 46 | params = {'axes.labelsize': 8,'axes.titlesize':8, 'legend.fontsize': 8, 'xtick.labelsize': 7, 'ytick.labelsize': 7, 'text.usetex': True} 47 | mpl.rcParams.update(params) 48 | 49 | def fit_sin(tt, yy): 50 | '''Fit sin to the input time sequence, and return fitting parameters "amp", "omega", "phase", "offset", "freq", "period" and "fitfunc"''' 51 | tt = np.array(tt) 52 | yy = np.array(yy) 53 | ff = np.fft.fftfreq(len(tt), (tt[1]-tt[0])) # assume uniform spacing 54 | Fyy = abs(np.fft.fft(yy)) 55 | guess_freq = abs(ff[np.argmax(Fyy[1:])+1]) # excluding the zero frequency "peak", which is related to offset 56 | guess_amp = np.std(yy) * 2.**0.5 57 | guess_offset = np.mean(yy) 58 | guess = np.array([guess_amp, 2.*np.pi*guess_freq, 0., guess_offset]) 59 | 60 | def sinfunc(t, A, w, p, c): return A * np.sin(w*t + p) + c 61 | popt, pcov = scipy.optimize.curve_fit(sinfunc, tt, yy, p0=guess) 62 | A, w, p, c = popt 63 | f = w/(2.*np.pi) 64 | fitfunc = lambda t: A * np.sin(w*t + p) + c 65 | return {"amp": A, "omega": w, "phase": p, "offset": c, "freq": f, "period": 1./f, "fitfunc": fitfunc, "maxcov": np.max(pcov), "rawres": (guess,popt,pcov)} 66 | 67 | def show_all_plots(): 68 | plt.show() 69 | 70 | def clear_all_plots(): 71 | plt.close("all") 72 | 73 | def save_all_plots(fileName, figureNames): 74 | figureList = {} 75 | numFigures = len(figureNames) 76 | for i in range(0, numFigures): 77 | pltName = fileName + "_" + figureNames[i] 78 | figureList[pltName] = plt.figure(i+1) 79 | return figureList 80 | 81 | def omegaTrack(rError, covar): 82 | 83 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 84 | colorList = [] 85 | for i in range(10): 86 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 87 | 88 | t = rError[:, 0] * ns2min 89 | 90 | plt.figure(num=2210101, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 91 | plt.plot(t , rError[:, 1]*180./np.pi, color = colorList[2] , label= 'Error') 92 | plt.plot(t, 3*np.sqrt(covar[:, 0,0])*180./np.pi, color=colorList[8], linestyle = '--', label = 'Covar (3-$\sigma$)') 93 | plt.plot(t, -3*np.sqrt(covar[:, 0,0])*180./np.pi, color=colorList[8], linestyle = '--') 94 | plt.legend(loc='best') 95 | plt.ylabel('$\mathbf{\omega}_{' + str(2) +'}$ Error in $\mathcal{C}$ ($^\circ$/s)') 96 | plt.ylim([-0.07,0.07]) 97 | plt.xlabel('Time (min)') 98 | plt.savefig('RateCam1.pdf') 99 | 100 | plt.figure(num=2210201, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 101 | plt.plot(t , rError[:, 2]*180./np.pi, color = colorList[2]) 102 | plt.plot(t, 3*np.sqrt(covar[:, 1,1])*180./np.pi, color=colorList[8], linestyle = '--') 103 | plt.plot(t, -3*np.sqrt(covar[:, 1,1])*180./np.pi, color=colorList[8], linestyle = '--') 104 | plt.ylabel('$\mathbf{\omega}_{' + str(3) +'}$ Error in $\mathcal{C}$ ($^\circ$/s)') 105 | plt.ylim([-0.07,0.07]) 106 | plt.xlabel('Time (min)') 107 | plt.savefig('RateCam2.pdf') 108 | 109 | 110 | 111 | def vecTrack(ref, track, covar): 112 | 113 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 114 | colorList = [] 115 | for i in range(10): 116 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 117 | 118 | rError = np.copy(ref) 119 | rError[:,1:] -= track[:,1:] 120 | errorDeg = np.zeros([len(rError[:, 0]), 2]) 121 | covDeg = np.zeros([len(rError[:, 0]), 2]) 122 | for i in range(len(errorDeg[:, 0])): 123 | errorDeg[i, 0] = rError[i, 0] 124 | covDeg[i, 0] = rError[i, 0] 125 | errorDeg[i, 1] = np.arccos(np.dot(ref[i, 1:4], track[i, 1:4]))*180./np.pi 126 | covDeg[i, 1] = np.arccos(np.dot(ref[i, 1:4], track[i, 1:4])) 127 | covarVec = np.array( 128 | [track[i, 1] + np.sqrt(covar[i, 0,0]), track[i, 2] + np.sqrt(covar[i, 1,1]), 129 | track[i, 3] + np.sqrt(covar[i, 2,2])]) 130 | covarVec = covarVec / np.linalg.norm(covarVec) 131 | covDeg[i, 1] = 3 * np.arccos(np.dot(covarVec, track[i, 1:4]))*180./np.pi 132 | 133 | t = ref[:, 0] * ns2min 134 | plt.figure(num=101011, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 135 | plt.plot(t , errorDeg[:, 1], color = colorList[1] , label= "Off-point") 136 | plt.plot(t, covDeg[:,1], color=colorList[5], linestyle = '--', label = 'Covar (3-$\sigma$)') 137 | plt.legend(loc='upper right') 138 | plt.ylabel('Mean $\hat{\mathbf{h}}$ Error in $\mathcal{C}$ ($^\circ$)') 139 | plt.xlabel('Time (min)') 140 | plt.ylim([0,2]) 141 | plt.savefig('HeadingDeg.pdf') 142 | 143 | plt.figure(num=10101, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 144 | plt.plot(t , rError[:, 1], color = colorList[2] , label= 'Error') 145 | plt.plot(t, 3*np.sqrt(covar[:, 0,0]), color=colorList[8], linestyle = '--', label = 'Covar (3-$\sigma$)') 146 | plt.plot(t, -3*np.sqrt(covar[:, 0,0]), color=colorList[8], linestyle = '--') 147 | plt.legend() 148 | plt.ylabel('$\hat{\mathbf{h}}_{' + str(1) +'}$ Error in $\mathcal{C}$ (-)') 149 | plt.ylim([-0.04,0.04]) 150 | plt.xlabel('Time (min)') 151 | plt.savefig('HeadingCam1.pdf') 152 | 153 | plt.figure(num=10201, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 154 | plt.plot(t , rError[:, 2], color = colorList[2] ) 155 | plt.plot(t, 3*np.sqrt(covar[:, 1,1]), color=colorList[8], linestyle = '--') 156 | plt.plot(t, -3*np.sqrt(covar[:, 1,1]), color=colorList[8], linestyle = '--') 157 | plt.ylabel('$\hat{\mathbf{h}}_{' + str(2) +'}$ Error in $\mathcal{C}$ (-)') 158 | plt.ylim([-0.04,0.04]) 159 | plt.xlabel('Time (min)') 160 | plt.savefig('HeadingCam2.pdf') 161 | 162 | plt.figure(num=10301, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 163 | plt.plot(t , rError[:, 3], color = colorList[2]) 164 | plt.plot(t, 3*np.sqrt(covar[:, 2,2]), color=colorList[8], linestyle = '--') 165 | plt.plot(t, -3*np.sqrt(covar[:, 2,2]), color=colorList[8], linestyle = '--') 166 | plt.ylabel('$\hat{\mathbf{h}}_{' + str(3) +'}$ Error in $\mathcal{C}$ (-)') 167 | plt.ylim([-0.04,0.04]) 168 | plt.xlabel('Time (min)') 169 | plt.savefig('HeadingCam3.pdf') 170 | 171 | 172 | def plot_faults(dataFaults, valid1, valid2): 173 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 174 | colorList = [] 175 | for i in range(10): 176 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 177 | 178 | for i in range(len(dataFaults[:,0])): 179 | if (dataFaults[i,0]*ns2min%1 != 0): 180 | valid1[i, 1] = np.nan 181 | valid2[i, 1] = np.nan 182 | 183 | plt.figure(10101, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 184 | plt.xlabel('Time (min)') 185 | plt.ylabel('Detected Fault') 186 | plt.scatter(valid1[:,0]* ns2min, valid1[:,1], color = colorList[5], alpha = 0.1, label = "Limb") 187 | plt.scatter(valid2[:,0]* ns2min, valid2[:,1], color = colorList[8], alpha = 0.1, label = "Circ") 188 | plt.scatter(dataFaults[:,0]* ns2min, dataFaults[:,1], marker = '.', color = colorList[2], label = "Faults") 189 | plt.legend() 190 | plt.savefig('FaultsDetected.pdf') 191 | return 192 | 193 | def diff_methods(vec1, meth1, meth2, val1, val2): 194 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 195 | colorList = [] 196 | for i in range(10): 197 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 198 | 199 | validIdx1 = [] 200 | for i in range(len(val1[:,0])): 201 | if np.abs(val1[i,1] - 1) < 0.01: 202 | validIdx1.append(i) 203 | validIdx2 = [] 204 | for i in range(len(val2[:,0])): 205 | if np.abs(val2[i,1] - 1) < 0.01: 206 | validIdx2.append(i) 207 | 208 | diff1 = np.full([len(validIdx1), 4], np.nan) 209 | diffNorms1 = np.full([len(validIdx1), 2], np.nan) 210 | diff2 = np.full([len(validIdx2), 4], np.nan) 211 | diffNorms2 = np.full([len(validIdx2), 2], np.nan) 212 | for i in range(len(validIdx1)): 213 | diff1[i,0] = vec1[validIdx1[i],0] 214 | diff1[i,1:] = vec1[validIdx1[i],1:] - meth1[validIdx1[i],1:] 215 | diffNorms1[i,0] = vec1[validIdx1[i],0] 216 | diffNorms1[i,1] = np.linalg.norm(vec1[validIdx1[i],1:]) - np.linalg.norm(meth1[validIdx1[i],1:]) 217 | for i in range(len(validIdx2)): 218 | diff2[i,0] = vec1[validIdx2[i],0] 219 | diff2[i,1:] = vec1[validIdx2[i],1:] - meth2[validIdx2[i],1:] 220 | diffNorms2[i,0] = vec1[validIdx2[i],0] 221 | diffNorms2[i,1] = np.linalg.norm(vec1[validIdx2[i],1:]) - np.linalg.norm(meth2[validIdx2[i],1:]) 222 | plt.figure(1, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 223 | plt.xlabel('Time') 224 | plt.plot(diff1[:, 0] * ns2min, diff1[:, 1] * m2km, color = colorList[1], label="$\mathbf{r}_\mathrm{Limb}$") 225 | plt.plot(diff1[:, 0] * ns2min, diff1[:, 2] * m2km, color = colorList[5]) 226 | plt.plot(diff1[:, 0] * ns2min, diff1[:, 3] * m2km, color = colorList[8]) 227 | plt.plot(diff2[:, 0] * ns2min, diff2[:, 1] * m2km, color=colorList[1], label="$\mathbf{r}_\mathrm{Circ}$", linestyle ='--', linewidth=2) 228 | plt.plot(diff2[:, 0] * ns2min, diff2[:, 2] * m2km, color=colorList[5], linestyle ='--', linewidth=2) 229 | plt.plot(diff2[:, 0] * ns2min, diff2[:, 3] * m2km, color=colorList[8], linestyle ='--', linewidth=2) 230 | plt.legend() 231 | plt.ylabel("$\mathbf{r}_{\mathrm{true}} - \mathbf{r}_{\mathrm{opnav}}$ (km)") 232 | plt.xlabel("Time (min)") 233 | plt.savefig('MeasErrorComponents.pdf') 234 | 235 | plt.figure(2, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 236 | plt.xlabel('Time') 237 | plt.plot(diff1[:, 0] * ns2min, diffNorms1[:,1] * m2km, color = colorList[1]) 238 | plt.plot(diff2[:, 0] * ns2min, diffNorms2[:,1] * m2km, color = colorList[1], linestyle="--", linewidth=2) 239 | plt.ylabel("$|\mathbf{r}_{\mathrm{true}}|$ - $|\mathbf{r}_{\mathrm{opnav}}|$ (km)") 240 | plt.xlabel("Time (min)") 241 | plt.savefig('MeasErrorNorm.pdf') 242 | 243 | def diff_vectors(vec1, vec2, valid, string): 244 | assert len(vec1[0,:]) == len(vec2[0,:]), print("Vectors need to be the same size") 245 | 246 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 247 | colorList = [] 248 | for i in range(10): 249 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 250 | 251 | validIdx = [] 252 | for i in range(len(valid[:,0])): 253 | if np.abs(valid[i,1] - 1) < 0.01: 254 | validIdx.append(i) 255 | diff = np.full([len(validIdx), 4], np.nan) 256 | diffNorms = np.full([len(validIdx), 2], np.nan) 257 | m2km2 = m2km*m2km 258 | for i in range(len(validIdx)): 259 | diff[i,0] = vec1[validIdx[i],0] 260 | diff[i,1:] = vec1[validIdx[i],1:] - vec2[validIdx[i],1:] 261 | diffNorms[i,0] = vec1[validIdx[i],0] 262 | diffNorms[i,1] = np.linalg.norm(vec1[validIdx[i],1:]) - np.linalg.norm(vec2[validIdx[i],1:]) 263 | # plt.figure(1, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 264 | plt.figure(1, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 265 | plt.xlabel('Time') 266 | plt.plot(diff[:, 0] * ns2min, diff[:, 1] * m2km2, color = colorList[1], label="$x_\mathrm{"+string+"}$") 267 | plt.plot(diff[:, 0] * ns2min, np.mean(diff[:, 1]) * m2km2 * np.ones(len(diff[:, 0])), color = colorList[1], linestyle = '--') 268 | plt.plot(diff[:, 0] * ns2min, diff[:, 2] * m2km2, color = colorList[5], label="$y_\mathrm{"+string+"}$") 269 | plt.plot(diff[:, 0] * ns2min, np.mean(diff[:, 2]) * m2km2 * np.ones(len(diff[:, 0])), color = colorList[5], linestyle = '--') 270 | plt.plot(diff[:, 0] * ns2min, diff[:, 3] * m2km2, color = colorList[8], label="$z_\mathrm{"+string+"}$") 271 | plt.plot(diff[:, 0] * ns2min, np.mean(diff[:, 3]) * m2km2 * np.ones(len(diff[:, 0])), color = colorList[8], linestyle = '--') 272 | plt.legend() 273 | plt.ylabel("$\mathbf{r}_{\mathrm{true}} - \mathbf{r}_{\mathrm{opnav}}$")# ($10^3$km)") 274 | plt.xlabel("Time (min)") 275 | plt.savefig('MeasErrorComponents.pdf') 276 | 277 | # plt.figure(2, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 278 | plt.figure(2, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 279 | plt.xlabel('Time') 280 | plt.plot(diff[:, 0] * ns2min, diffNorms[:,1] * m2km2, color = colorList[1]) 281 | plt.plot(diff[:, 0] * ns2min, np.mean(diffNorms[:,1]) * m2km2 * np.ones(len(diff[:, 0])), color = colorList[1], linestyle="--") 282 | plt.ylabel("$|\mathbf{r}_{\mathrm{true}}|$ - $|\mathbf{r}_{\mathrm{opnav}}|$") 283 | plt.xlabel("Time (min)") 284 | plt.savefig('MeasErrorNorm.pdf') 285 | return 286 | 287 | def nav_percentages(truth, states, covar, valid, string): 288 | numStates = len(states[0:,1:]) 289 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 290 | colorList = [] 291 | for i in range(10): 292 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 293 | 294 | validIdx = [] 295 | for i in range(len(valid[:,0])): 296 | if np.abs(valid[i,1] - 1) < 0.01: 297 | validIdx.append(i) 298 | diffPos = np.full([len(validIdx), 2], np.nan) 299 | diffVel = np.full([len(validIdx), 2], np.nan) 300 | covarPos = np.full([len(validIdx), 2], np.nan) 301 | covarVel = np.full([len(validIdx), 2], np.nan) 302 | 303 | m2km2 = m2km 304 | for i in range(len(validIdx)): 305 | diffPos[i,0] = states[validIdx[i],0] 306 | diffPos[i,1] = np.linalg.norm(states[validIdx[i],1:4] - truth[validIdx[i],1:4])/np.linalg.norm(truth[validIdx[i],1:4])*100 307 | diffVel[i,0] = states[validIdx[i],0] 308 | diffVel[i,1] = np.linalg.norm(states[validIdx[i],4:7] - truth[validIdx[i],4:7])/np.linalg.norm(truth[validIdx[i],4:7])*100 309 | covarPos[i,0] = states[validIdx[i],0] 310 | posVec = np.sqrt(np.array([covar[validIdx[i],1], covar[validIdx[i],1 + 6+1], covar[validIdx[i],1 + 2*(6+1)]])) 311 | covarPos[i,1] = 3*np.linalg.norm(posVec)/np.linalg.norm(truth[validIdx[i],1:4])*100 312 | covarVel[i,0] = states[validIdx[i],0] 313 | velVec = np.sqrt(np.array([covar[validIdx[i],1 + 3*(6+1)], covar[validIdx[i],1 + 4*(6+1)], covar[validIdx[i],1 + 5*(6+1)]])) 314 | covarVel[i,1] = 3*np.linalg.norm(velVec)/np.linalg.norm(truth[validIdx[i],4:7])*100 315 | plt.figure(101, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 316 | # plt.figure(101, figsize=(3.5, 2), facecolor='w', edgecolor='k') 317 | plt.plot(diffPos[:, 0] * ns2min, diffPos[:, 1] , color = colorList[1], label = "Error") 318 | plt.plot(covarPos[:, 0] * ns2min, covarPos[:,1], color = colorList[8], linestyle = '--', label='Covar ($3\sigma$)') 319 | plt.legend(loc='upper right') 320 | plt.ylabel("$\mathbf{r}_\mathrm{"+string+"}$ errors ($\%$)") 321 | plt.xlabel("Time (min)") 322 | # plt.ylim([0,3.5]) 323 | plt.savefig('PercentErrorPos.pdf') 324 | 325 | plt.figure(102, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 326 | # plt.figure(102, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 327 | plt.plot(diffVel[:, 0] * ns2min, diffVel[:, 1], color = colorList[1]) 328 | plt.plot(covarVel[:, 0] * ns2min, covarVel[:,1], color = colorList[8], linestyle = '--') 329 | plt.ylabel("$\dot{\mathbf{r}}_\mathrm{"+string+ "}$ errors ($\%$)") 330 | plt.xlabel("Time (min)") 331 | # plt.ylim([0,15]) 332 | plt.savefig('PercentErrorVel.pdf') 333 | 334 | 335 | RMSPos = np.sqrt(sum(diffPos[:, 1] ** 2)/len(diffPos[:, 1])) 336 | RMSPosCov = np.sqrt(sum((covarPos[:, 1]) ** 2)/len(covarPos[:, 1])) 337 | RMSVel = np.sqrt(sum(diffVel[:, 1] ** 2)/len(diffVel[:, 1])) 338 | RMSVelCov = np.sqrt(sum((covarVel[:, 1]) ** 2)/len(covarVel[:, 1])) 339 | 340 | print('-------- Pos RMS '+ string + '--------') 341 | print(RMSPos) 342 | print(RMSPosCov) 343 | print('------------------------') 344 | print('-------- Vel RMS '+ string + '--------') 345 | print(RMSVel) 346 | print(RMSVelCov) 347 | print('------------------------') 348 | # RMS Errors Computed for heading and rate 349 | unitTestSupport.writeTeXSnippet('RMSPos_'+ string, str(round(RMSPos,3)), os.path.dirname(__file__)) 350 | unitTestSupport.writeTeXSnippet('RMSPosCov_'+ string, str(round(RMSPosCov,3)),os.path.dirname(__file__)) 351 | unitTestSupport.writeTeXSnippet('RMSVel_'+ string, str(round(RMSVel,3)), os.path.dirname(__file__)) 352 | unitTestSupport.writeTeXSnippet('RMSVelCov_'+ string, str(round(RMSVelCov,3)),os.path.dirname(__file__)) 353 | return 354 | 355 | def plot_orbit(r_BN): 356 | plt.figure() 357 | plt.xlabel('$R_x$, km') 358 | plt.ylabel('$R_y$, km') 359 | plt.plot(r_BN[:, 1] * m2km, r_BN[:, 2] * m2km, color_x) 360 | plt.scatter(0, 0) 361 | plt.title('Spacecraft Orbit') 362 | return 363 | 364 | def plot_rw_motor_torque(timeData, dataUsReq, dataRW, numRW): 365 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 366 | colorList = [] 367 | for i in range(10): 368 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 369 | plt.figure(22, figsize=(3, 1.8), facecolor='w', edgecolor='k') 370 | for idx in range(1, numRW): 371 | plt.plot(timeData, dataUsReq[:, idx], 372 | '--', 373 | color=colorList[idx*2], 374 | label=r'$\hat u_{s,' + str(idx) + '}$') 375 | plt.plot(timeData, dataRW[idx - 1][:, 1], 376 | color=colorList[idx*2], 377 | label='$u_{s,' + str(idx) + '}$') 378 | plt.legend(loc='lower right') 379 | plt.savefig('RWMotorTorque.pdf') 380 | plt.xlabel('Time (min)') 381 | plt.ylabel('RW Motor Torque (Nm)') 382 | 383 | 384 | def plot_TwoOrbits(r_BN, r_BN2): 385 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 386 | colorList = [] 387 | for i in range(10): 388 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 389 | 390 | # fig = plt.figure(5, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 391 | fig = plt.figure(5, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 392 | ax = fig.gca(projection='3d') 393 | ax.set_xlabel('$R_x$, km') 394 | ax.set_ylabel('$R_y$, km') 395 | ax.set_zlabel('$R_z$, km') 396 | ax.plot(r_BN[:, 1] * m2km, r_BN[:, 2] * m2km, r_BN[:, 3] * m2km, color=colorList[1], label="True") 397 | for i in range(len(r_BN2[:,0])): 398 | if np.abs(r_BN2[i, 1])>0 or np.abs(r_BN2[i, 2])>0: 399 | ax.scatter(r_BN2[i, 1] * m2km, r_BN2[i, 2] * m2km, r_BN2[i, 3] * m2km, color=colorList[8], label="Measured") 400 | ax.scatter(0, 0, color='r') 401 | # ax.set_title('Orbit and Measurements') 402 | return 403 | 404 | def plot_attitude_error(timeLineSet, dataSigmaBR): 405 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 406 | colorList = [] 407 | for i in range(10): 408 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 409 | 410 | plt.figure(5555, figsize=(3, 1.8), facecolor='w', edgecolor='k') 411 | plt.rcParams["font.size"] = "8" 412 | fig = plt.gcf() 413 | ax = fig.gca() 414 | vectorData = unitTestSupport.pullVectorSetFromData(dataSigmaBR) 415 | sNorm = np.array([np.linalg.norm(v) for v in vectorData]) 416 | plt.plot(timeLineSet, sNorm, 417 | color=colorList[0], 418 | ) 419 | plt.xlabel('Time (min)') 420 | plt.xlim([40, 100]) 421 | plt.ylabel('Attitude Error Norm $|\sigma_{C/R}|$') 422 | plt.savefig('AttErrorNorm.pdf') 423 | ax.set_yscale('log') 424 | 425 | 426 | def plot_rate_error(timeLineSet, dataOmegaBR): 427 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 428 | colorList = [] 429 | for i in range(10): 430 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 431 | plt.figure(666666, figsize=(3, 1.8), facecolor='w', edgecolor='k') 432 | plt.rcParams["font.size"] = "8" 433 | styleList = ['-', '--', ':'] 434 | for idx in range(1, 4): 435 | plt.plot(timeLineSet, dataOmegaBR[:, idx], 436 | color=colorList[idx*2], 437 | label='$\omega_{BR,' + str(idx) + '}$', 438 | linestyle= styleList[idx-1] ) 439 | plt.legend(loc='lower right') 440 | plt.xlim([40, 100]) 441 | plt.xlabel('Time (min)') 442 | plt.savefig('RateErrorControl.pdf') 443 | plt.ylabel('Rate Tracking Error (rad/s) ') 444 | return 445 | 446 | 447 | def plot_rw_cmd_torque(timeData, dataUsReq, numRW): 448 | plt.figure() 449 | for idx in range(1, 4): 450 | plt.plot(timeData, dataUsReq[:, idx], 451 | '--', 452 | color=unitTestSupport.getLineColor(idx, numRW), 453 | label='$\hat u_{s,' + str(idx) + '}$') 454 | plt.legend(loc='lower right') 455 | plt.xlabel('Time (min)') 456 | plt.savefig('RWMotorTorque.pdf') 457 | plt.ylabel('RW Motor Torque (Nm)') 458 | 459 | def plot_rw_speeds(timeData, dataOmegaRW, numRW): 460 | plt.figure() 461 | for idx in range(1, numRW + 1): 462 | plt.plot(timeData, dataOmegaRW[:, idx] / mc.RPM, 463 | color=unitTestSupport.getLineColor(idx, numRW), 464 | label='$\Omega_{' + str(idx) + '}$') 465 | plt.legend(loc='upper right') 466 | plt.xlabel('Time [min]') 467 | plt.ylabel('RW Speed (RPM) ') 468 | 469 | def plotStateCovarPlot(x, Pflat): 470 | 471 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 472 | colorList = [] 473 | for i in range(10): 474 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 475 | 476 | numStates = len(x[0,:])-1 477 | 478 | P = np.zeros([len(Pflat[:,0]),numStates,numStates]) 479 | t= np.zeros(len(Pflat[:,0])) 480 | for i in range(len(Pflat[:,0])): 481 | t[i] = x[i, 0]*ns2min 482 | if not np.isnan(Pflat[i,1]): 483 | Plast = Pflat[i,1:].reshape([numStates,numStates]) 484 | P[i, :, :] = Plast 485 | x0 = x[i,7:10] 486 | if numStates == 6 or numStates == 3: 487 | P[i,:,:] = Pflat[i,1:].reshape([numStates,numStates]) 488 | 489 | 490 | if numStates == 9 : 491 | plt.figure(10, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 492 | plt.plot(t , x[:, 1]*m2km, label='$r_1$', color = colorList[1]) 493 | plt.plot(t , 3 * np.sqrt(P[:, 0, 0])*m2km, '--', label='Covar (3-$\sigma$)', color = colorList[8]) 494 | plt.plot(t ,- 3 * np.sqrt(P[:, 0, 0])*m2km, '--', color = colorList[8]) 495 | plt.legend(loc='best') 496 | plt.ylabel('Position Error (km)') 497 | plt.grid() 498 | 499 | plt.figure(11, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 500 | plt.plot(t , x[:, 4]*m2km, color = colorList[1]) 501 | plt.plot(t , 3 * np.sqrt(P[:, 3, 3])*m2km, '--', color = colorList[8]) 502 | plt.plot(t ,- 3 * np.sqrt(P[:, 3, 3])*m2km, '--', color = colorList[8]) 503 | plt.ylabel('Rate Error (km/s)') 504 | plt.grid() 505 | 506 | plt.figure(12, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 507 | plt.plot(t , x[:, 7], color = colorList[1]) 508 | plt.plot(t , x0[0] + 3 * np.sqrt(P[:, 6, 6]), '--', color = colorList[8]) 509 | plt.plot(t , x0[0] - 3 * np.sqrt(P[:, 6, 6]), '--', color = colorList[8]) 510 | plt.title('First bias component (km/s)') 511 | plt.grid() 512 | 513 | plt.figure(13, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 514 | plt.plot(t , x[:, 2]*m2km, color = colorList[1]) 515 | plt.plot(t , 3 * np.sqrt(P[:, 1, 1])*m2km, '--', color = colorList[8]) 516 | plt.plot(t ,- 3 * np.sqrt(P[:, 1, 1])*m2km, '--', color = colorList[8]) 517 | plt.title('Second pos component (km)') 518 | plt.grid() 519 | 520 | plt.figure(14, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 521 | plt.plot(t , x[:, 5]*m2km, color = colorList[1]) 522 | plt.plot(t , 3 * np.sqrt(P[:, 4, 4])*m2km, '--', color = colorList[8]) 523 | plt.plot(t ,- 3 * np.sqrt(P[:, 4, 4])*m2km, '--', color = colorList[8]) 524 | plt.xlabel('Time (min)') 525 | plt.title('Second rate component (km/s)') 526 | plt.grid() 527 | 528 | plt.figure(15, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 529 | plt.plot(t , x[:, 8], color = colorList[1]) 530 | plt.plot(t , x0[1] + 3 * np.sqrt(P[:, 7, 7]), '--', color = colorList[8]) 531 | plt.plot(t , x0[1] - 3 * np.sqrt(P[:, 7, 7]), '--', color = colorList[8]) 532 | plt.title('Second bias component (km/s)') 533 | plt.grid() 534 | 535 | plt.figure(16, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 536 | plt.plot(t , x[:, 3]*m2km, color = colorList[1]) 537 | plt.plot(t , 3 * np.sqrt(P[:, 2, 2])*m2km, '--', color = colorList[8]) 538 | plt.plot(t ,-3 * np.sqrt(P[:, 2, 2])*m2km, '--', color = colorList[8]) 539 | plt.xlabel('Time (min)') 540 | plt.title('Third pos component (km)') 541 | plt.grid() 542 | 543 | plt.figure(17, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 544 | plt.plot(t , x[:, 6]*m2km, color = colorList[1]) 545 | plt.plot(t , 3 * np.sqrt(P[:, 5, 5])*m2km, '--', color = colorList[8]) 546 | plt.plot(t , -3 * np.sqrt(P[:, 5, 5])*m2km, '--', color = colorList[8]) 547 | plt.xlabel('Time (min)') 548 | plt.title('Third rate component (km/s)') 549 | plt.grid() 550 | 551 | plt.figure(18, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 552 | plt.plot(t , x[:, 9], color = colorList[1]) 553 | plt.plot(t , x0[2] + 3 * np.sqrt(P[:, 8, 8]), '--', color = colorList[8]) 554 | plt.plot(t , x0[2] - 3 * np.sqrt(P[:, 8, 8]), '--', color = colorList[8]) 555 | plt.title('Third bias component (km/s)') 556 | plt.grid() 557 | 558 | if numStates == 6 or numStates ==3: 559 | plt.figure(20, figsize=(2.4, 1.4), facecolor='w', edgecolor='k') 560 | plt.plot(t , x[:, 1]*m2km, label='State Error', color = colorList[1]) 561 | plt.plot(t , 3 * np.sqrt(P[:, 0, 0])*m2km, '--', label='Covar (3-$\sigma$)', color = colorList[8]) 562 | plt.plot(t ,- 3 * np.sqrt(P[:, 0, 0])*m2km, '--', color = colorList[8]) 563 | plt.legend(loc='best') 564 | plt.ylabel('$r_1$ Error (km)') 565 | plt.savefig('Filterpos1.pdf') 566 | 567 | if numStates == 6: 568 | plt.figure(21, figsize=(2.4, 1.4), facecolor='w', edgecolor='k') 569 | plt.plot(t , x[:, 4]*m2km, color = colorList[1]) 570 | plt.plot(t , 3 * np.sqrt(P[:, 3, 3])*m2km, '--', color = colorList[8]) 571 | plt.plot(t ,- 3 * np.sqrt(P[:, 3, 3])*m2km, '--', color = colorList[8]) 572 | plt.ylabel('$v_1$ Error (km/s)') 573 | plt.savefig('Filtervel1.pdf') 574 | 575 | if numStates == 6 or numStates ==3: 576 | plt.figure(22, figsize=(2.4, 1.4), facecolor='w', edgecolor='k') 577 | plt.plot(t , x[:, 2]*m2km, color = colorList[1]) 578 | plt.plot(t , 3 * np.sqrt(P[:, 1, 1])*m2km, '--', color = colorList[8]) 579 | plt.plot(t ,- 3 * np.sqrt(P[:, 1, 1])*m2km, '--', color = colorList[8]) 580 | plt.ylabel('$r_2$ Error (km)') 581 | plt.savefig('Filterpos2.pdf') 582 | 583 | if numStates == 6: 584 | plt.figure(23, figsize=(2.4, 1.4), facecolor='w', edgecolor='k') 585 | plt.plot(t , x[:, 5]*m2km, color = colorList[1]) 586 | plt.plot(t , 3 * np.sqrt(P[:, 4, 4])*m2km, '--', color = colorList[8]) 587 | plt.plot(t ,- 3 * np.sqrt(P[:, 4, 4])*m2km, '--', color = colorList[8]) 588 | plt.ylabel('$v_2$ Error (km/s)') 589 | plt.savefig('Filtervel2.pdf') 590 | 591 | if numStates == 6 or numStates ==3: 592 | plt.figure(24, figsize=(2.4, 1.4), facecolor='w', edgecolor='k') 593 | plt.plot(t , x[:, 3]*m2km, color = colorList[1]) 594 | plt.plot(t , 3 * np.sqrt(P[:, 2, 2])*m2km, '--', color = colorList[8]) 595 | plt.plot(t ,-3 * np.sqrt(P[:, 2, 2])*m2km, '--', color = colorList[8]) 596 | plt.xlabel('Time (min)') 597 | plt.ylabel('$r_3$ Error (km)') 598 | plt.savefig('Filterpos3.pdf') 599 | 600 | if numStates == 6: 601 | plt.figure(25, figsize=(2.4, 1.4), facecolor='w', edgecolor='k') 602 | plt.plot(t , x[:, 6]*m2km, color = colorList[1]) 603 | plt.plot(t , 3 * np.sqrt(P[:, 5, 5])*m2km, '--', color = colorList[8]) 604 | plt.plot(t , -3 * np.sqrt(P[:, 5, 5])*m2km, '--', color = colorList[8]) 605 | plt.xlabel('Time (min)') 606 | plt.ylabel('$v_3$ Error (km/s)') 607 | plt.savefig('Filtervel3.pdf') 608 | 609 | 610 | def centerXY(centerPoints, size): 611 | 612 | t= centerPoints[:, 0]*ns2min 613 | subtime = [] 614 | subX = [] 615 | subY = [] 616 | 617 | center = np.full([len(t), 3], np.nan) 618 | for i in range(len(centerPoints[:,0])): 619 | if centerPoints[i, 1:3].any() > 1E-10: 620 | subtime.append(centerPoints[i, 0]*ns2min) 621 | center[i,1:] = centerPoints[i,1:3] - size/2 + 0.5 622 | subX.append(center[i,1]) 623 | subY.append(center[i, 2]) 624 | 625 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 626 | colorList = [] 627 | for i in range(10): 628 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 629 | 630 | # xCurve = fit_sin(subtime, subX) 631 | # yCurve = fit_sin(subtime, subY) 632 | # 633 | # print "Periods in Errors are , " + str(xCurve["period"]) + " and " + str(yCurve["period"]) + " s" 634 | 635 | # plt.figure(100, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 636 | plt.figure(100, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 637 | plt.plot(subtime, subX, ".", label = "X-center", color = colorList[8]) 638 | # plt.plot(t, xCurve["fitfunc"](t), "r", label="X-curve") 639 | plt.plot(subtime, subY, ".", label = "Y-center", color = colorList[1]) 640 | # plt.plot(t, yCurve["fitfunc"](t), "b", label="Y-curve") 641 | plt.legend(loc='best') 642 | plt.ylabel("Pixels") 643 | plt.title('First unit Vec component in C frame (-)') 644 | plt.savefig('CentersXY.pdf') 645 | 646 | 647 | def pixelAndPos(x, r, centers, size): 648 | 649 | t= x[:, 0]*ns2min 650 | r_norm = np.zeros([len(r[:,0]), 5]) 651 | r_norm[:,0] = r[:,0] 652 | centerPoints = np.full([len(t), 3], np.nan) 653 | maxDiff = np.zeros(3) 654 | for i in range(3): 655 | values = [] 656 | for j in range(len(x[:, 0])): 657 | if not np.isnan(x[j,i+1]): 658 | values.append(x[j,i+1]) 659 | try: 660 | maxDiff[i] = np.max(np.abs(values)) 661 | except ValueError: 662 | maxDiff[i] =1 663 | for i in range(len(x[:,0])): 664 | r_norm[i,1:4] = r[i,1:]/np.linalg.norm(r[i,1:]) * maxDiff 665 | r_norm[i,4] = np.linalg.norm(r[i,1:]) 666 | if centers[i, 1:3].any() > 1E-10: 667 | centerPoints[i,1:] = centers[i,1:3] - size/2 668 | for i in range(2): 669 | values = [] 670 | for j in range(len(centerPoints[:, 0])): 671 | if not np.isnan(centerPoints[j, i + 1]): 672 | values.append(centerPoints[j, i + 1]) 673 | try: 674 | maxCenter = np.max(np.abs(values)) 675 | except ValueError: 676 | maxCenter = 1 677 | centerPoints[:, i+1]/=maxCenter/maxDiff[i] 678 | 679 | 680 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 681 | colorList = [] 682 | for i in range(10): 683 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 684 | 685 | # plt.figure(200, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 686 | plt.figure(200, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 687 | plt.plot(t, x[:, 1], ".", label='Truth - Meas', color = colorList[1]) 688 | plt.plot(t, r_norm[:, 1], label = "True Pos", color = colorList[5]) 689 | plt.plot(t, centerPoints[:,1], ".", label = "X-center", color = colorList[8]) 690 | # plt.plot(t, x1["fitfunc"](t), "r--", label = "Fit") 691 | plt.legend(loc='best') 692 | plt.title('First unit Vec component in C frame (-)') 693 | 694 | # plt.figure(201, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 695 | plt.figure(201, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 696 | plt.plot(t, x[:, 2], ".", color = colorList[1]) 697 | plt.plot(t, r_norm[:, 2], color = colorList[5]) 698 | plt.plot(t, centerPoints[:,2], ".", label = "X-center", color = colorList[8]) 699 | # plt.plot(t, x2["fitfunc"](t), "r--") 700 | plt.title('Second unit Vec component in C frame (-)') 701 | 702 | # plt.figure(202, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 703 | plt.figure(202, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 704 | plt.plot(t, x[:, 3], ".", color = colorList[1]) 705 | plt.plot(t, r_norm[:, 3], color = colorList[5]) 706 | # plt.plot(t, x3["fitfunc"](t), "r--") 707 | plt.xlabel('Time (min)') 708 | plt.title('Third unit Vec component in C frame (-)') 709 | 710 | # plt.figure(203, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 711 | plt.figure(203, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 712 | plt.plot(t, x[:, 4]*m2km, ".", color = colorList[1]) 713 | plt.xlabel('Time (min)') 714 | plt.title('Norm in C frame (km)') 715 | 716 | 717 | 718 | def imgProcVsExp(true, centers, radii, size): 719 | 720 | 721 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 722 | colorList = [] 723 | for i in range(10): 724 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 725 | 726 | t= true[:, 0]*ns2min 727 | centerline = np.ones([len(t),2])*(size/2+0.5) 728 | found = 0 729 | for i in range(len(t)): 730 | if np.abs(centers[i,1]) <1E-10 and np.abs(centers[i,2]) < 1E-10: 731 | centers[i,1:3] = np.array([np.nan,np.nan]) 732 | radii[i,1] = np.nan 733 | else: 734 | found = 1 735 | if found == 0: 736 | centerline[i,:] = np.array([np.nan,np.nan]) 737 | 738 | plt.figure(301, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 739 | # plt.figure(301, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 740 | plt.rcParams["font.size"] = "8" 741 | plt.plot(t, true[:, 1], "+", label='Truth Xpix', color = colorList[1]) 742 | plt.plot(t, centerline[:,0], "--", label='X-center', color = colorList[9]) 743 | plt.plot(t, centers[:, 1], '.', label = "ImagProc Xpix", color = colorList[5], alpha=0.7) 744 | plt.legend(loc='best') 745 | try: 746 | plt.ylim([centerline[-1,1]-15, centerline[-1,1]+15]) 747 | except ValueError: 748 | pass 749 | plt.ylabel('X (px)') 750 | plt.xlabel('Time (min)') 751 | plt.grid(b=None, which='major', axis='y') 752 | plt.savefig('Xpix.pdf') 753 | 754 | plt.figure(302, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 755 | # plt.figure(302, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 756 | plt.plot(t, true[:, 2], "+", label='Truth Ypix', color = colorList[1]) 757 | plt.plot(t, centerline[:,1], "--", label='Y-center', color = colorList[9]) 758 | plt.plot(t, centers[:, 2], '.', label = "ImagProc Ypix", color = colorList[5], alpha=0.7) 759 | plt.legend(loc='best') 760 | try: 761 | plt.ylim([centerline[-1,1]-15, centerline[-1,1]+15]) 762 | except ValueError: 763 | pass 764 | plt.ylabel('Y (px)') 765 | plt.xlabel('Time (min)') 766 | plt.grid(b=None, which='major', axis='y') 767 | plt.savefig('Ypix.pdf') 768 | 769 | plt.figure(312, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 770 | # plt.figure(312, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 771 | plt.plot(t, true[:, 3], "+", label='Truth $\\rho$', color = colorList[1]) 772 | plt.plot(t, radii[:, 1],'.', label = "ImagProc $\\rho$", color = colorList[5], alpha=0.7) 773 | plt.legend(loc='best') 774 | plt.ylabel('$\\rho$ (px)') 775 | plt.xlabel('Time (min)') 776 | plt.grid(b=None, which='minor', axis='y') 777 | plt.savefig('Rhopix.pdf') 778 | 779 | 780 | plt.figure(303, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 781 | # plt.figure(303, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 782 | plt.plot(t, true[:, 1] - centers[:, 1], ".", label='$\mathrm{X}_\mathrm{true} - \mathrm{X}_\mathrm{hough}$', color = colorList[1]) 783 | plt.legend(loc='best') 784 | plt.ylabel('X error (px)') 785 | plt.grid() 786 | plt.savefig('Xerror.pdf') 787 | 788 | plt.figure(304, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 789 | # plt.figure(304, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 790 | plt.plot(t, true[:, 2] - centers[:, 2], ".", label='$\mathrm{Y}_\mathrm{true} - \mathrm{Y}_\mathrm{hough}$', color = colorList[1]) 791 | plt.legend(loc='best') 792 | plt.ylabel('Y error (px)') 793 | plt.grid() 794 | plt.savefig('Yerror.pdf') 795 | 796 | plt.figure(305, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 797 | # plt.figure(305, figsize=(3.5, 2.), facecolor='w', edgecolor='k') 798 | plt.plot(t, true[:, 3] - radii[:, 1], ".", label='$\mathrm{\\rho}_\mathrm{true} - \mathrm{\\rho}_\mathrm{hough}$', color = colorList[1]) 799 | plt.legend(loc='best') 800 | plt.ylabel('Radius error (px)') 801 | plt.xlabel("Time (min)") 802 | plt.grid() 803 | plt.savefig('Rhoerror.pdf') 804 | 805 | 806 | def plotPostFitResiduals(Res, noise): 807 | 808 | MeasNoise = np.zeros([len(Res[:,0]), 3]) 809 | Noiselast = np.zeros([3,3]) 810 | 811 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(10) 812 | colorList = [] 813 | for i in range(10): 814 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 815 | 816 | t= np.zeros(len(Res[:,0])) 817 | for i in range(len(Res[:,0])): 818 | t[i] = Res[i, 0]*ns2min 819 | if isinstance(noise, float): 820 | MeasNoise[i, :] = np.array([3*np.sqrt(noise),3*np.sqrt(noise),3*np.sqrt(noise)]) 821 | else: 822 | if not np.isnan(noise[i, 1]): 823 | Noiselast = noise[i, 1:].reshape([3, 3]) 824 | MeasNoise[i, :] = np.array([3*np.sqrt(Noiselast[0,0]), 3*np.sqrt(Noiselast[1,1]), 3*np.sqrt(Noiselast[2,2])])/5 825 | if np.isnan(noise[i, 1]): 826 | MeasNoise[i, :] = np.array([3*np.sqrt(Noiselast[0,0]), 3*np.sqrt(Noiselast[1,1]), 3*np.sqrt(Noiselast[2,2])])/5 827 | # Don't plot zero values, since they mean that no measurement is taken 828 | for j in range(len(Res[0,:])-1): 829 | if -1E-10 < Res[i,j+1] < 1E-10: 830 | Res[i, j+1] = np.nan 831 | if len(Res[0,:])-1 == 3: 832 | plt.figure(401, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 833 | plt.plot(t , Res[:, 1]*m2km, ".", label='Residual', color = colorList[1]) 834 | plt.plot(t , MeasNoise[:,0]*m2km, '--', label='Noise ($3\sigma$)', color = colorList[8]) 835 | plt.plot(t , -MeasNoise[:,0]*m2km, '--', color = colorList[8]) 836 | plt.legend(loc=2) 837 | max = np.amax(MeasNoise[:,0]) 838 | if max >1E-15: 839 | plt.ylim([-2*max*m2km, 2*max*m2km]) 840 | plt.ylabel('$r_1$ Measured (km)') 841 | plt.xlabel("Time (min)") 842 | plt.grid() 843 | plt.savefig('Res1.pdf') 844 | 845 | plt.figure(402, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 846 | plt.plot(t , Res[:, 2]*m2km, ".", color = colorList[1]) 847 | plt.plot(t , MeasNoise[:,1]*m2km, '--', color = colorList[8]) 848 | plt.plot(t , -MeasNoise[:,1]*m2km, '--', color = colorList[8]) 849 | max = np.amax(MeasNoise[:,1]) 850 | if max >1E-15: 851 | plt.ylim([-2*max*m2km, 2*max*m2km]) 852 | plt.ylabel('$r_2$ Measured (km)') 853 | plt.xlabel("Time (min)") 854 | plt.grid() 855 | plt.savefig('Res2.pdf') 856 | 857 | 858 | plt.figure(403, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 859 | plt.plot(t , Res[:, 3]*m2km, ".", color = colorList[1]) 860 | plt.plot(t , MeasNoise[:,2]*m2km, '--', color = colorList[8]) 861 | plt.plot(t , -MeasNoise[:,2]*m2km, '--', color = colorList[8]) 862 | max = np.amax(MeasNoise[:,2]) 863 | if max >1E-15: 864 | plt.ylim([-2*max*m2km, 2*max*m2km]) 865 | plt.ylabel('$r_3$ Measured (km)') 866 | plt.xlabel("Time (min)") 867 | plt.grid() 868 | plt.savefig('Res3.pdf') 869 | 870 | 871 | if len(Res[0,:])-1 == 6: 872 | plt.figure(405, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 873 | plt.plot(t , Res[:, 1], "b.") 874 | plt.plot(t , MeasNoise[:,0], 'r--') 875 | plt.plot(t , -MeasNoise[:,0], 'r--') 876 | plt.legend(loc='best') 877 | plt.title('X-pixel component') 878 | plt.grid() 879 | 880 | plt.figure(406, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 881 | plt.plot(t , Res[:, 4], "b.") 882 | plt.plot(t , MeasNoise[:,0], 'r--') 883 | plt.plot(t , -MeasNoise[:,0], 'r--') 884 | plt.title('X-bias component') 885 | plt.grid() 886 | 887 | plt.figure(407, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 888 | plt.plot(t , Res[:, 2], "b.") 889 | plt.plot(t , MeasNoise[:,1], 'r--') 890 | plt.plot(t , -MeasNoise[:,1], 'r--') 891 | plt.title('Y-pixel component') 892 | plt.grid() 893 | 894 | plt.figure(408, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 895 | plt.plot(t , Res[:, 5], "b.") 896 | plt.plot(t , MeasNoise[:,1], 'r--') 897 | plt.plot(t , -MeasNoise[:,1], 'r--') 898 | plt.xlabel('Time (min)') 899 | plt.title('Y-bias component') 900 | plt.grid() 901 | 902 | plt.figure(409, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 903 | plt.plot(t , Res[:, 3], "b.") 904 | plt.plot(t , MeasNoise[:,2], 'r--') 905 | plt.plot(t , -MeasNoise[:,2], 'r--') 906 | plt.xlabel('Time (min)') 907 | plt.ylabel('Rho-component') 908 | plt.grid() 909 | 910 | plt.figure(410, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 911 | plt.plot(t , Res[:, 6], "b.") 912 | plt.plot(t , MeasNoise[:,2], 'r--') 913 | plt.plot(t , -MeasNoise[:,2], 'r--') 914 | plt.xlabel('Time (min)') 915 | plt.ylabel('Rho-bias component') 916 | plt.grid() 917 | 918 | def plot_cirlces(centers, radii, validity, resolution): 919 | circleIndx = [] 920 | for i in range(len(centers[:,0])): 921 | if validity[i, 1] == 1: 922 | circleIndx.append(i) 923 | 924 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(len(circleIndx)+1) 925 | colorList = [] 926 | for i in range(len(circleIndx)): 927 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 928 | 929 | plt.figure(500, figsize=(3, 3), facecolor='w', edgecolor='k') 930 | ax = plt.gca() 931 | for i in range(len(circleIndx)): 932 | if i%30 ==0: 933 | ell_xy = Ellipse(xy=(centers[circleIndx[i],1], centers[circleIndx[i],2]), 934 | width=radii[circleIndx[i],1], height=radii[circleIndx[i],1], 935 | angle=0, linestyle='-', linewidth=3, color=colorList[i], alpha=0.7) 936 | ell_xy.set(facecolor='none') 937 | ax.add_patch(ell_xy) 938 | ax.invert_yaxis() 939 | ax.set_xlim(0, resolution[0]) 940 | ax.set_ylim(resolution[1],0) 941 | plt.xlabel('X-axis (px)') 942 | plt.ylabel('Y-axis (px)') 943 | plt.axis("equal") 944 | plt.savefig('Circles.pdf') 945 | 946 | 947 | def plot_limb(limbPoints, numLimb, validity, resolution): 948 | indx = [] 949 | time = [] 950 | for i in range(len(limbPoints[:,0])): 951 | if validity[i, 1] == 1: 952 | indx.append(i) 953 | 954 | numBerPoints = [] 955 | colorsInt = len(mpl.cm.get_cmap("inferno").colors)/(len(indx)+1) 956 | colorList = [] 957 | for i in range(len(indx)): 958 | colorList.append(mpl.cm.get_cmap("inferno").colors[int(i*colorsInt)]) 959 | numBerPoints.append(numLimb[indx[i], 1]) 960 | time.append(numLimb[indx[i], 0]*1E-9/60) 961 | 962 | limbX = np.zeros([len(indx), 2*2000]) 963 | limbY = np.zeros([len(indx), 2*2000]) 964 | for i in range(len(indx)): 965 | for j in range(1, int(numLimb[indx[i], 1])): 966 | if np.abs(limbPoints[indx[i], 2*j] + limbPoints[indx[i], 2*j-1])>1E-1: 967 | limbX[i,j] = limbPoints[indx[i], 2*j-1] 968 | limbY[i,j] = limbPoints[indx[i], 2*j] 969 | plt.figure(600, figsize=(3, 3), facecolor='w', edgecolor='k') 970 | ax = plt.gca() 971 | for i in range(len(indx)): 972 | if i%30 ==0: 973 | ax.scatter(limbX[i,:int(numLimb[indx[i],1])-1], limbY[i,:int(numLimb[indx[i],1])-1], color=colorList[i], marker='.', alpha=0.2) 974 | ax.invert_yaxis() 975 | ax.set_xlim(0, resolution[0]) 976 | ax.set_ylim(resolution[1],0) 977 | plt.xlabel('X-axis (px)') 978 | plt.ylabel('Y-axis (px)') 979 | plt.axis("equal") 980 | plt.savefig('Limbs.pdf') 981 | 982 | plt.figure(601, figsize=(2.7, 1.6), facecolor='w', edgecolor='k') 983 | plt.plot(time, numBerPoints, color=colorList[1]) 984 | plt.xlabel('Time (min)') 985 | plt.ylabel('Limb Size (px)') 986 | plt.savefig('LimbPoints.pdf') 987 | 988 | class AnimatedCircles(object): 989 | """An animated scatter plot using matplotlib.animations.FuncAnimation.""" 990 | def __init__(self, size, centers, radii, validity): 991 | self.sensorSize = size 992 | self.idx = 0 993 | self.i = 0 994 | # Setup the figure and axes... 995 | self.fig, self.ax = plt.subplots(num='Circles Animation')#, figsize=(10, 10), dpi=80, facecolor='w') 996 | self.ax.invert_yaxis() 997 | self.ax.set_xlim(0, self.sensorSize[0]) 998 | self.ax.set_ylim(self.sensorSize[1],0) 999 | self.ax.set_aspect("equal") 1000 | # Then setup FuncAnimation. 1001 | self.setData(centers, radii, validity) 1002 | self.ani = animation.FuncAnimation(self.fig, self.update, interval=100, 1003 | init_func=self.setup_plot, frames=len(self.circleIndx), blit=True) 1004 | self.ani.save('../../TestImages/circlesAnimated.gif', writer='imagemagick', fps=60) 1005 | 1006 | def setData(self, centers, radii, validity): 1007 | self.circleIndx = [] 1008 | for i in range(len(centers[:,0])): 1009 | if validity[i, 1] == 1: 1010 | self.circleIndx.append(i) 1011 | 1012 | self.centers = np.zeros([len(self.circleIndx), 3]) 1013 | self.radii = np.zeros([len(self.circleIndx), 2]) 1014 | for i in range(len(self.circleIndx)): 1015 | self.centers[i,0] = centers[self.circleIndx[i],0] 1016 | self.centers[i, 1:] = centers[self.circleIndx[i], 1:3] 1017 | self.radii[i,0] = centers[self.circleIndx[i],0] 1018 | self.radii[i, 1] = radii[self.circleIndx[i], 1] 1019 | self.colorList = mpl.cm.get_cmap("inferno").colors 1020 | 1021 | 1022 | def setup_plot(self): 1023 | """Initial drawing of the scatter plot.""" 1024 | self.scat = self.ax.scatter(self.centers[0,1], self.centers[0,2], c=self.colorList[0], s=self.radii[0,1]) 1025 | # For FuncAnimation's sake, we need to return the artist we'll be using 1026 | # Note that it expects a sequence of artists, thus the trailing comma. 1027 | return self.scat, 1028 | 1029 | def data_stream(self, i): 1030 | while self.idx < len(self.circleIndx): 1031 | xy = np.array([[self.sensorSize[0]/2+0.5, self.sensorSize[0]/2+0.5],[self.centers[i,1], self.centers[i,2]],[self.centers[i,1], self.centers[i,2]]]) 1032 | s = np.array([1, 1, self.radii[i,1]]) 1033 | c = [self.colorList[-1], self.colorList[i],self.colorList[i]] 1034 | yield np.c_[xy[:,0], xy[:,1], s[:]], c 1035 | 1036 | def update(self, i): 1037 | """Update the scatter plot.""" 1038 | data, color = next(self.data_stream(i)) 1039 | 1040 | # Set x and y data... 1041 | self.scat.set_offsets(data[:, :2]) 1042 | # Set sizes... 1043 | self.scat.set_sizes((data[:, 2]/2.)**2.) 1044 | # Set colors.. 1045 | # self.scat.set_array(data[:, 3]) 1046 | self.scat.set_edgecolor(color) 1047 | self.scat.set_facecolor("none") 1048 | # We need to return the updated artist for FuncAnimation to draw.. 1049 | # Note that it expects a sequence of artists, thus the trailing comma. 1050 | return self.scat, 1051 | 1052 | def StateErrorCovarPlot(x, Pflat, FilterType, show_plots): 1053 | 1054 | nstates = int(np.sqrt(len(Pflat[0,:])-1)) 1055 | mpl.rc("figure", figsize=(3, 1.8)) 1056 | 1057 | colorsInt = int(len(mpl.cm.get_cmap().colors)/10) 1058 | colorList = [] 1059 | for i in range(10): 1060 | colorList.append(mpl.cm.get_cmap().colors[i*colorsInt]) 1061 | 1062 | P = np.zeros([len(Pflat[:,0]),nstates,nstates]) 1063 | t= np.zeros(len(Pflat[:,0])) 1064 | for i in range(len(Pflat[:,0])): 1065 | t[i] = x[i, 0]*ns2min 1066 | P[i,:,:] = Pflat[i,1:37].reshape([nstates,nstates]) 1067 | for j in range(len(P[0,0,:])): 1068 | P[i,j,j] = np.sqrt(P[i,j,j]) 1069 | 1070 | for i in range(3): 1071 | if i ==0: 1072 | plt.figure(num=None, facecolor='w', edgecolor='k') 1073 | plt.plot(t , x[:, i+1], color = colorList[0], label='Error') 1074 | plt.plot(t , 3 * P[:, i, i],color = colorList[-1], linestyle = '--', label='Covar (3-$\sigma$)') 1075 | plt.plot(t , -3 * P[:, i, i], color = colorList[-1], linestyle = '--') 1076 | plt.legend(loc='best') 1077 | plt.ylabel('$d_' + str(i+1) + '$ Error (-)') 1078 | plt.ylim([-0.1,0.1]) 1079 | plt.xlabel('Time (min)') 1080 | unitTestSupport.saveFigurePDF('StateCovarHeading'+ FilterType + str(i) , plt, './') 1081 | if show_plots: 1082 | plt.show() 1083 | plt.close("all") 1084 | else: 1085 | plt.figure(num=None, facecolor='w', edgecolor='k') 1086 | plt.plot(t , x[:, i+1], color = colorList[0]) 1087 | plt.plot(t , 3 * P[:, i, i],color = colorList[-1], linestyle = '--') 1088 | plt.plot(t , -3 * P[:, i, i], color = colorList[-1], linestyle = '--') 1089 | plt.ylabel('$d_' + str(i+1) + '$ Error (-)') 1090 | plt.xlabel('Time (min)') 1091 | plt.ylim([-0.1,0.1]) 1092 | unitTestSupport.saveFigurePDF('StateCovarHeading'+ FilterType + str(i) , plt, './') 1093 | if show_plots: 1094 | plt.show() 1095 | plt.close("all") 1096 | 1097 | if nstates>3: 1098 | for i in range(3,nstates): 1099 | if i == 3: 1100 | plt.figure(num=None, facecolor='w', edgecolor='k') 1101 | plt.plot(t, x[:, i + 1], color = colorList[0], label='Error') 1102 | plt.plot(t, 3 * P[:, i, i], color = colorList[-1],linestyle = '--', label='Covar (3-$\sigma$)') 1103 | plt.plot(t, -3 * P[:, i, i], color = colorList[-1],linestyle = '--') 1104 | plt.ylim([-0.0007,0.0007]) 1105 | # plt.legend(loc='best') 1106 | if nstates == 5: 1107 | plt.ylabel('$\omega_' + str(i - 1) + '$ Error (-)') 1108 | else: 1109 | plt.ylabel('$d\'_' + str(i-2) + '$ Error (-)') 1110 | plt.xlabel('Time (min)') 1111 | unitTestSupport.saveFigurePDF('StateCovarRate' + FilterType + str(i), plt, './') 1112 | if show_plots: 1113 | plt.show() 1114 | plt.close("all") 1115 | else: 1116 | plt.figure(num=None, facecolor='w', edgecolor='k') 1117 | plt.plot(t, x[:, i + 1], color = colorList[0]) 1118 | plt.plot(t, 3 * P[:, i, i], color = colorList[-1],linestyle = '--') 1119 | plt.plot(t, -3 * P[:, i, i], color = colorList[-1],linestyle = '--') 1120 | plt.ylim([-0.0007,0.0007]) 1121 | if nstates == 5: 1122 | plt.ylabel('$\omega_' + str(i - 1) + '$ Error (-)') 1123 | else: 1124 | plt.ylabel('$d\'_' + str(i-2) + '$ Error (-)') 1125 | plt.xlabel('Time (min)') 1126 | unitTestSupport.saveFigurePDF('StateCovarRate' + FilterType + str(i), plt, './') 1127 | if show_plots: 1128 | plt.show() 1129 | plt.close("all") 1130 | plt.close("all") 1131 | 1132 | def PostFitResiduals(Res, covar_B, FilterType, show_plots): 1133 | 1134 | mpl.rc("figure", figsize=(3, 1.8)) 1135 | 1136 | colorsInt = int(len(mpl.cm.get_cmap().colors) / 10) 1137 | colorList = [] 1138 | for i in range(10): 1139 | colorList.append(mpl.cm.get_cmap().colors[i * colorsInt]) 1140 | 1141 | MeasNoise = np.zeros([len(Res[:, 0]), 3]) 1142 | prevNoise = np.zeros(3) 1143 | t = np.zeros(len(Res[:, 0])) 1144 | constantVal = np.array([np.nan] * 4) 1145 | for i in range(len(Res[:, 0])): 1146 | t[i] = Res[i, 0] * ns2min 1147 | if np.abs(covar_B[i, 1]) < 1E-15 and prevNoise[0] == 0: 1148 | MeasNoise[i,:] = np.full(3, np.nan) 1149 | elif np.abs(covar_B[i, 1]) < 1E-15 and prevNoise[0] != 0: 1150 | MeasNoise[i,:] = prevNoise 1151 | else: 1152 | MeasNoise[i,0] = 3 * np.sqrt(covar_B[i, 1]) 1153 | MeasNoise[i,1] = 3 * np.sqrt(covar_B[i, 1 + 4]) 1154 | MeasNoise[i,2] = 3 * np.sqrt(covar_B[i, 1 + 8]) 1155 | prevNoise = MeasNoise[i,:] 1156 | 1157 | # Don't plot constant values, they mean no measurement is taken 1158 | if i > 0: 1159 | for j in range(1, 4): 1160 | constantRes = np.abs(Res[i, j] - Res[i - 1, j]) 1161 | if constantRes < 1E-10 or np.abs(constantVal[j - 1] - Res[i, j]) < 1E-10: 1162 | constantVal[j - 1] = Res[i, j] 1163 | Res[i, j] = np.nan 1164 | 1165 | for i in range(3): 1166 | if i == 0: 1167 | plt.figure(num=None, dpi=80, facecolor='w', edgecolor='k') 1168 | plt.plot(t, Res[:, i + 1], color=colorList[0], linestyle='', marker='.', label='Residual') 1169 | plt.plot(t, MeasNoise[:,i], color=colorList[-1], linestyle="--", label='Noise (3-$\sigma$)') 1170 | plt.plot(t, -MeasNoise[:,i], color=colorList[-1], linestyle="--", ) 1171 | plt.legend(loc='best') 1172 | plt.ylabel('$r_' + str(i + 1) + '$ (-)') 1173 | plt.xlabel('Time (min)') 1174 | plt.ylim([-2*MeasNoise[-1,i], 2*MeasNoise[-1,i]]) 1175 | unitTestSupport.saveFigurePDF('PostFit' + FilterType + str(i), plt, './') 1176 | if show_plots: 1177 | plt.show() 1178 | plt.close("all") 1179 | else: 1180 | plt.figure(num=None, dpi=80, facecolor='w', edgecolor='k') 1181 | plt.plot(t, Res[:, i + 1], color=colorList[0], linestyle='', marker='.') 1182 | plt.plot(t, MeasNoise[:,i], color=colorList[-1], linestyle="--") 1183 | plt.plot(t, -MeasNoise[:,i], color=colorList[-1], linestyle="--", ) 1184 | plt.ylabel('$r_' + str(i + 1) + '$ (-)') 1185 | plt.xlabel('Time min') 1186 | plt.ylim([-2*MeasNoise[-1,i], 2*MeasNoise[-1,i]]) 1187 | unitTestSupport.saveFigurePDF('PostFit' + FilterType + str(i), plt, './') 1188 | if show_plots: 1189 | plt.show() 1190 | plt.close("all") 1191 | 1192 | plt.close("all") 1193 | 1194 | class AnimatedLimb(object): 1195 | """An animated scatter plot using matplotlib.animations.FuncAnimation.""" 1196 | def __init__(self, size, limb, centers, validity): 1197 | self.stream = self.data_stream() 1198 | self.sensorSize = size 1199 | self.idx = 0 1200 | self.i = 0 1201 | # Setup the figure and axes... 1202 | self.fig, self.ax = plt.subplots(num='Limb Animation') # , figsize=(10, 10), dpi=80, facecolor='w') 1203 | self.ax.invert_yaxis() 1204 | self.ax.set_xlim(0, self.sensorSize[0]) 1205 | self.ax.set_ylim(self.sensorSize[1], 0) 1206 | self.ax.set_aspect("equal") 1207 | # Then setup FuncAnimation. 1208 | self.setData(limb, validity) 1209 | # Setup the figure and axes... 1210 | self.fig, self.ax = plt.subplots() 1211 | # Then setup FuncAnimation. 1212 | self.ani = animation.FuncAnimation(self.fig, self.update, interval=100, 1213 | init_func=self.setup_plot, blit=True) 1214 | 1215 | def setup_plot(self): 1216 | """Initial drawing of the scatter plot.""" 1217 | self.scat = self.ax.scatter(self.centers[0,1], self.centers[0,2], c=self.colorList[0], s=self.radii[0,1]) 1218 | # For FuncAnimation's sake, we need to return the artist we'll be using 1219 | # Note that it expects a sequence of artists, thus the trailing comma. 1220 | return self.scat, 1221 | 1222 | 1223 | def setData(self, centers, limbPoints, validity): 1224 | self.pointIndx = [] 1225 | for i in range(len(limbPoints[:,0])): 1226 | if validity[i, 1] == 1: 1227 | self.pointIndx.append(i) 1228 | self.centers = np.zeros([len(self.pointIndx), 2]) 1229 | self.points = np.zeros([len(self.pointIndx), 2*1000]) 1230 | for i in range(len(self.pointIndx)): 1231 | self.centers[i, 1:] = centers[self.pointIndx[i], 1:2] 1232 | self.points[i,0] = limbPoints[self.pointIndx[i],1:] 1233 | self.colorList = mpl.cm.get_cmap("inferno").colors 1234 | 1235 | 1236 | def data_stream(self, i): 1237 | """Generate a random walk (brownian motion). Data is scaled to produce 1238 | a soft "flickering" effect.""" 1239 | limb = [] 1240 | while self.idx < len(self.circleIndx): 1241 | xy = np.array([[self.sensorSize[0]/2+0.5, self.sensorSize[0]/2+0.5],[self.centers[i,1], self.centers[i,2]]]) 1242 | for j in range(len(self.points[i,:])/2): 1243 | if self.points[i,j] >1E-1 or self.points[i,j+1] >1E-1: 1244 | limb.append([self.points[i,j], self.points[i,j+1]]) 1245 | c = [self.colorList[-1], self.colorList[i], self.colorList[i]] 1246 | yield np.c_[xy[:,0], xy[:,1], limb[:,0], limb[:,1]], c 1247 | 1248 | def update(self, i): 1249 | """Update the scatter plot.""" 1250 | data, color = next(self.data_stream(i)) 1251 | 1252 | # Set x and y data... 1253 | self.scat.set_offsets(data[:, :2]) 1254 | # Set sizes... 1255 | self.scat.set_offsets(data[:, 2:4]) 1256 | # Set colors.. 1257 | # self.scat.set_array(data[:, 3]) 1258 | self.scat.set_edgecolor(color) 1259 | self.scat.set_facecolor("none") 1260 | # We need to return the updated artist for FuncAnimation to draw.. 1261 | # Note that it expects a sequence of artists, thus the trailing comma. 1262 | return self.scat, -------------------------------------------------------------------------------- /basilisk_env/simulators/opNav_models/opnav_boi-v1k.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/atharris/basilisk_env/8fa610520859eb020ad226e9dedb38fb1c1ad678/basilisk_env/simulators/opNav_models/opnav_boi-v1k.zip -------------------------------------------------------------------------------- /basilisk_env/simulators/scenario_OpNavOD.py: -------------------------------------------------------------------------------- 1 | 2 | # Import utilities 3 | from Basilisk.utilities import orbitalMotion, macros, unitTestSupport 4 | from Basilisk.utilities import RigidBodyKinematics as rbk 5 | 6 | 7 | # Get current file path 8 | import sys, os, inspect, time, signal, subprocess 9 | filename = inspect.getframeinfo(inspect.currentframe()).filename 10 | path = os.path.dirname(os.path.abspath(filename)) 11 | 12 | # Import master classes: simulation base class and scenario base class 13 | sys.path.append(path + '/..') 14 | from BSK_masters import BSKSim, BSKScenario 15 | import BSK_OpNavDynamics, BSK_OpNavFsw 16 | import numpy as np 17 | 18 | # Import plotting file for your scenario 19 | sys.path.append(path + '/../plotting') 20 | import OpNav_Plotting as BSK_plt 21 | 22 | # Create your own scenario child class 23 | class scenario_OpNav(BSKScenario): 24 | def __init__(self, masterSim): 25 | super(scenario_OpNav, self).__init__(masterSim) 26 | self.name = 'scenario_opnav' 27 | self.masterSim = masterSim 28 | self.filterUse = "relOD" 29 | 30 | def configure_initial_conditions(self): 31 | print('%s: configure_initial_conditions' % self.name) 32 | 33 | # Configure Dynamics initial conditions 34 | oe = orbitalMotion.ClassicElements() 35 | oe.a = 18000 * 1E3 # meters 36 | oe.e = 0.6 37 | oe.i = 10 * macros.D2R 38 | oe.Omega = 25. * macros.D2R 39 | oe.omega = 190. * macros.D2R 40 | oe.f = 80. * macros.D2R # 90 good 41 | mu = self.masterSim.get_DynModel().marsGravBody.mu 42 | 43 | rN, vN = orbitalMotion.elem2rv(mu, oe) 44 | orbitalMotion.rv2elem(mu, rN, vN) 45 | bias = [0, 0, -2] 46 | 47 | MRP= [0,0,0] 48 | if self.filterUse =="relOD": 49 | self.masterSim.get_FswModel().relativeODData.stateInit = rN.tolist() + vN.tolist() 50 | if self.filterUse == "bias": 51 | self.masterSim.get_FswModel().pixelLineFilterData.stateInit = rN.tolist() + vN.tolist() + bias 52 | self.masterSim.get_DynModel().scObject.hub.r_CN_NInit = unitTestSupport.np2EigenVectorXd(rN) # m - r_CN_N 53 | self.masterSim.get_DynModel().scObject.hub.v_CN_NInit = unitTestSupport.np2EigenVectorXd(vN) # m/s - v_CN_N 54 | self.masterSim.get_DynModel().scObject.hub.sigma_BNInit = [[MRP[0]], [MRP[1]], [MRP[2]]] # sigma_BN_B 55 | self.masterSim.get_DynModel().scObject.hub.omega_BN_BInit = [[0.0], [0.0], [0.0]] # rad/s - omega_BN_B 56 | 57 | qNoiseIn = np.identity(6) 58 | qNoiseIn[0:3, 0:3] = qNoiseIn[0:3, 0:3] * 1E-3 * 1E-3 59 | qNoiseIn[3:6, 3:6] = qNoiseIn[3:6, 3:6] * 1E-4 * 1E-4 60 | self.masterSim.get_FswModel().relativeODData.qNoise = qNoiseIn.reshape(36).tolist() 61 | self.masterSim.get_FswModel().imageProcessing.noiseSF = 0.5 62 | def log_outputs(self): 63 | print('%s: log_outputs' % self.name) 64 | 65 | # Dynamics process outputs: log messages below if desired. 66 | 67 | # FSW process outputs 68 | samplingTime = self.masterSim.get_FswModel().processTasksTimeStep 69 | # self.masterSim.TotalSim.logThisMessage(self.masterSim.get_FswModel().trackingErrorCamData.outputDataName, samplingTime) 70 | # self.masterSim.TotalSim.logThisMessage(self.masterSim.get_FswModel().trackingErrorData.outputDataName, samplingTime) 71 | 72 | if self.filterUse == "relOD": 73 | self.masterSim.TotalSim.logThisMessage(self.masterSim.get_FswModel().relativeODData.filtDataOutMsgName, samplingTime) 74 | self.masterSim.TotalSim.logThisMessage(self.masterSim.get_FswModel().pixelLineData.opNavOutMsgName, samplingTime) 75 | if self.filterUse == "bias": 76 | self.masterSim.TotalSim.logThisMessage(self.masterSim.get_FswModel().pixelLineFilterData.filtDataOutMsgName, samplingTime) 77 | 78 | self.masterSim.TotalSim.logThisMessage(self.masterSim.get_DynModel().scObject.scStateOutMsgName,samplingTime) 79 | self.masterSim.TotalSim.logThisMessage(self.masterSim.get_FswModel().imageProcessing.opnavCirclesOutMsgName, samplingTime) 80 | return 81 | 82 | def pull_outputs(self, showPlots): 83 | print('%s: pull_outputs' % self.name) 84 | 85 | # Dynamics process outputs: pull log messages below if any 86 | ## Spacecraft true states 87 | position_N = self.masterSim.pullMessageLogData( 88 | self.masterSim.get_DynModel().scObject.scStateOutMsgName + ".r_BN_N", range(3)) 89 | velocity_N = self.masterSim.pullMessageLogData( 90 | self.masterSim.get_DynModel().scObject.scStateOutMsgName + ".v_BN_N", range(3)) 91 | ## Attitude 92 | sigma_BN = self.masterSim.pullMessageLogData( 93 | self.masterSim.get_DynModel().scObject.scStateOutMsgName + ".sigma_BN", range(3)) 94 | ## Image processing 95 | circleCenters = self.masterSim.pullMessageLogData( 96 | self.masterSim.get_FswModel().imageProcessing.opnavCirclesOutMsgName+ ".circlesCenters", range(2*10)) 97 | circleRadii = self.masterSim.pullMessageLogData( 98 | self.masterSim.get_FswModel().imageProcessing.opnavCirclesOutMsgName+ ".circlesRadii", range(10)) 99 | validCircle = self.masterSim.pullMessageLogData( 100 | self.masterSim.get_FswModel().imageProcessing.opnavCirclesOutMsgName+ ".valid", range(1)) 101 | if self.filterUse == "bias": 102 | NUM_STATES = 9 103 | ## Navigation results 104 | navState = self.masterSim.pullMessageLogData( 105 | self.masterSim.get_FswModel().pixelLineFilterData.filtDataOutMsgName + ".state", range(NUM_STATES)) 106 | navCovar = self.masterSim.pullMessageLogData( 107 | self.masterSim.get_FswModel().pixelLineFilterData.filtDataOutMsgName + ".covar", 108 | range(NUM_STATES * NUM_STATES)) 109 | navPostFits = self.masterSim.pullMessageLogData( 110 | self.masterSim.get_FswModel().pixelLineFilterData.filtDataOutMsgName + ".postFitRes", range(NUM_STATES - 3)) 111 | if self.filterUse == "relOD": 112 | NUM_STATES = 6 113 | ## Navigation results 114 | navState = self.masterSim.pullMessageLogData( 115 | self.masterSim.get_FswModel().relativeODData.filtDataOutMsgName + ".state", range(NUM_STATES)) 116 | navCovar = self.masterSim.pullMessageLogData( 117 | self.masterSim.get_FswModel().relativeODData.filtDataOutMsgName + ".covar", 118 | range(NUM_STATES * NUM_STATES)) 119 | navPostFits = self.masterSim.pullMessageLogData( 120 | self.masterSim.get_FswModel().relativeODData.filtDataOutMsgName + ".postFitRes", range(NUM_STATES - 3)) 121 | measPos = self.masterSim.pullMessageLogData( 122 | self.masterSim.get_FswModel().pixelLineData.opNavOutMsgName + ".r_BN_N", range(3)) 123 | r_C = self.masterSim.pullMessageLogData( 124 | self.masterSim.get_FswModel().pixelLineData.opNavOutMsgName + ".r_BN_C", range(3)) 125 | measCovar = self.masterSim.pullMessageLogData( 126 | self.masterSim.get_FswModel().pixelLineData.opNavOutMsgName + ".covar_N", range(3*3)) 127 | covar_C = self.masterSim.pullMessageLogData( 128 | self.masterSim.get_FswModel().pixelLineData.opNavOutMsgName + ".covar_C", range(3*3)) 129 | 130 | sigma_CB = self.masterSim.get_DynModel().cameraMRP_CB 131 | sizeMM = self.masterSim.get_DynModel().cameraSize 132 | sizeOfCam = self.masterSim.get_DynModel().cameraRez 133 | focal = self.masterSim.get_DynModel().cameraFocal #in m 134 | 135 | pixelSize = [] 136 | pixelSize.append(sizeMM[0] / sizeOfCam[0]) 137 | pixelSize.append(sizeMM[1] / sizeOfCam[1]) 138 | 139 | dcm_CB = rbk.MRP2C(sigma_CB) 140 | # Plot results 141 | BSK_plt.clear_all_plots() 142 | stateError = np.zeros([len(position_N[:,0]), NUM_STATES+1]) 143 | navCovarLong = np.full([len(position_N[:,0]), 1+NUM_STATES*NUM_STATES], np.nan) 144 | navCovarLong[:,0] = np.copy(position_N[:,0]) 145 | stateError[:, 0:4] = np.copy(position_N) 146 | stateError[:,4:7] = np.copy(velocity_N[:,1:]) 147 | pixCovar = np.ones([len(circleCenters[:,0]), 3*3+1]) 148 | pixCovar[:,0] = circleCenters[:,0] 149 | pixCovar[:,1:]*=np.array([1,0,0,0,1,0,0,0,2]) 150 | if self.filterUse == "relOD": 151 | measError = np.full([len(measPos[:,0]), 4], np.nan) 152 | measError[:,0] = measPos[:,0] 153 | measError_C = np.full([len(measPos[:,0]), 5], np.nan) 154 | measError_C[:,0] = measPos[:,0] 155 | trueRhat_C = np.full([len(circleCenters[:,0]), 4], np.nan) 156 | trueR_C = np.full([len(circleCenters[:,0]), 4], np.nan) 157 | trueCircles = np.full([len(circleCenters[:,0]), 4], np.nan) 158 | trueCircles[:,0] = circleCenters[:,0] 159 | trueRhat_C[:,0] = circleCenters[:,0] 160 | trueR_C[:,0] = circleCenters[:,0] 161 | 162 | centerBias = np.copy(circleCenters) 163 | radBias = np.copy(circleRadii) 164 | 165 | switchIdx = 0 166 | 167 | Rmars = 3396.19*1E3 168 | for j in range(len(stateError[:, 0])): 169 | if stateError[j, 0] in navState[:, 0]: 170 | stateError[j, 1:4] -= navState[j - switchIdx, 1:4] 171 | stateError[j, 4:] -= navState[j - switchIdx, 4:] 172 | else: 173 | stateError[j, 1:] = np.full(NUM_STATES, np.nan) 174 | switchIdx += 1 175 | for i in range(len(circleCenters[:,0])): 176 | if circleCenters[i,1:].any() > 1E-8 or circleCenters[i,1:].any() < -1E-8: 177 | if self.filterUse == "bias": 178 | centerBias[i,1:3] = np.round(navState[i, 7:9]) 179 | radBias[i,1] = np.round(navState[i, -1]) 180 | if self.filterUse == "relOD": 181 | measError[i, 1:4] = position_N[i +switchIdx, 1:4] - measPos[i, 1:4] 182 | measError_C[i, 4] = np.linalg.norm(position_N[i +switchIdx, 1:4]) - np.linalg.norm(r_C[i, 1:4]) 183 | measError_C[i, 1:4] = trueRhat_C[i,1:] - r_C[i, 1:4]/np.linalg.norm(r_C[i, 1:4]) 184 | trueR_C[i, 1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i + switchIdx, 1:4])), 185 | position_N[i + switchIdx, 1:4]) 186 | trueRhat_C[i,1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[i +switchIdx, 1:4])) ,position_N[i +switchIdx, 1:4])/np.linalg.norm(position_N[i +switchIdx, 1:4]) 187 | trueCircles[i,3] = focal*np.tan(np.arcsin(Rmars/np.linalg.norm(position_N[i,1:4])))/pixelSize[0] 188 | trueRhat_C[i,1:] *= focal/trueRhat_C[i,3] 189 | trueCircles[i, 1] = trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0]/2 - 0.5 190 | trueCircles[i, 2] = trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1]/2 - 0.5 191 | else: 192 | if self.filterUse == "relOD": 193 | measCovar[i,1:] = np.full(3*3, np.nan) 194 | covar_C[i, 1:] = np.full(3 * 3, np.nan) 195 | navCovarLong[switchIdx:,:] = np.copy(navCovar) 196 | 197 | timeData = position_N[:, 0] * macros.NANO2MIN 198 | 199 | if self.filterUse == "relOD": 200 | BSK_plt.plot_TwoOrbits(navState, measPos) 201 | # BSK_plt.AnimatedCircles(sizeOfCam, circleCenters, circleRadii, validCircle) 202 | BSK_plt.diff_vectors(trueR_C, r_C, validCircle, "Circ") 203 | BSK_plt.plot_cirlces(circleCenters, circleRadii, validCircle, sizeOfCam) 204 | BSK_plt.plotStateCovarPlot(stateError, navCovarLong) 205 | if self.filterUse == "bias": 206 | circleCenters[i,1:] += centerBias[i,1:] 207 | circleRadii[i,1:] += radBias[i,1:] 208 | BSK_plt.plotPostFitResiduals(navPostFits, pixCovar) 209 | BSK_plt.imgProcVsExp(trueCircles, circleCenters, circleRadii, np.array(sizeOfCam)) 210 | if self.filterUse == "relOD": 211 | BSK_plt.plotPostFitResiduals(navPostFits, measCovar) 212 | figureList = {} 213 | if showPlots: 214 | BSK_plt.show_all_plots() 215 | else: 216 | fileName = os.path.basename(os.path.splitext(__file__)[0]) 217 | figureNames = ["attitudeErrorNorm", "rwMotorTorque", "rateError", "rwSpeed"] 218 | figureList = BSK_plt.save_all_plots(fileName, figureNames) 219 | 220 | return figureList 221 | 222 | 223 | def run(showPlots): 224 | 225 | # Instantiate base simulation 226 | TheBSKSim = BSKSim(fswRate=0.5, dynRate=0.5) 227 | TheBSKSim.set_DynModel(BSK_OpNavDynamics) 228 | TheBSKSim.set_FswModel(BSK_OpNavFsw) 229 | TheBSKSim.initInterfaces() 230 | 231 | # Configure a scenario in the base simulation 232 | TheScenario = scenario_OpNav(TheBSKSim) 233 | TheScenario.filterUse = "relOD" 234 | TheScenario.log_outputs() 235 | TheScenario.configure_initial_conditions() 236 | 237 | TheBSKSim.get_DynModel().cameraMod.saveImages = 0 238 | TheBSKSim.get_DynModel().vizInterface.opNavMode = 2 239 | 240 | if TheBSKSim.get_DynModel().vizInterface.opNavMode == 2: 241 | appPath = '/Applications/OpNavScene.app' 242 | if TheBSKSim.get_DynModel().vizInterface.opNavMode == 1: 243 | appPath = '/Applications/Vizard.app' 244 | if TheBSKSim.get_DynModel().vizInterface.opNavMode > 0: 245 | child = subprocess.Popen(["open", appPath, "--args", "-opNavMode", "tcp://localhost:5556"]) # ,, "-batchmode" 246 | print("Vizard spawned with PID = " + str(child.pid)) 247 | 248 | # Configure FSW mode 249 | TheScenario.masterSim.modeRequest = 'prepOpNav' 250 | # Initialize simulation 251 | TheBSKSim.InitializeSimulationAndDiscover() 252 | # Configure run time and execute simulation 253 | simulationTime = macros.min2nano(10.) 254 | TheBSKSim.ConfigureStopTime(simulationTime) 255 | print('Starting Execution') 256 | t1 = time.time() 257 | TheBSKSim.ExecuteSimulation() 258 | if TheScenario.filterUse == "relOD": 259 | TheScenario.masterSim.modeRequest = 'OpNavOD' 260 | else: 261 | TheScenario.masterSim.modeRequest = 'OpNavODB' 262 | simulationTime = macros.min2nano(600.) 263 | TheBSKSim.ConfigureStopTime(simulationTime) 264 | TheBSKSim.ExecuteSimulation() 265 | t2 = time.time() 266 | print('Finished Execution in ', t2-t1, ' seconds. Post-processing results') 267 | 268 | try: 269 | os.kill(child.pid + 1, signal.SIGKILL) 270 | except: 271 | print("IDK how to turn this thing off") 272 | 273 | # Pull the results of the base simulation running the chosen scenario 274 | figureList = TheScenario.pull_outputs(showPlots) 275 | return figureList 276 | 277 | 278 | if __name__ == "__main__": 279 | run(True) 280 | -------------------------------------------------------------------------------- /envSetup.sh: -------------------------------------------------------------------------------- 1 | pip install numpy 2 | pip install stable-baselines 3 | pip install keras 4 | pip install tensorflow 5 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup(name='basilisk_env', 4 | version='0.0.1', 5 | install_requires=['gym','matplotlib','numpy','pandas'] 6 | ) 7 | --------------------------------------------------------------------------------