Source code for scenarioSepMomentumManagement

#
#  ISC License
#
#  Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
#
#  Permission to use, copy, modify, and/or distribute this software for any
#  purpose with or without fee is hereby granted, provided that the above
#  copyright notice and this permission notice appear in all copies.
#
#  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
#  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
#  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
#  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
#  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
#  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
#  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
#

r"""
Overview
--------

This script shows how to use a solar electric propulsion (SEP) thruster mounted on a dual-gimbaled platform to perform
continuous momentum management of a spacecraft actuated with reaction wheels (RWs). The spacecraft is also equipped
two large rotating solar arrays (SAs) which can continuously track the Sun to ensure maximum power generation.
The goals for the SEP are to continuously point the thruster along the requested inertial thrust direction, while also
maneuvering the gimbal in order to manage the momentum build-up on RWs due to external unmodeled perturbations. As a
consequence, the optimal reference attitude for the spacecraft changes as the dual-gimbaled platform is articulated.
In this script, the unmodeled perturbation consists in the solar radiation pressure (SRP) torque acting on the
system, and modeled using :ref:`facetSRPDynamicEffector`. The main flight software modules used in this script are
the following:

- :ref:`oneAxisSolarArrayPoint`: computes the reference attitude for a spacecraft with multiple pointing requirements.
  For this application, the first requirement is to align the thruster with the requested inertial direction; the
  second requirement is to have the solar array drive axis as close to orthogonal as possible to the sunline.
- :ref:`thrusterPlatformReference`: computes the reference tip and tilt angles for the dual-gimbaled platform on which
  the SEP thruster is mounted on. Based on nominal expected thruster behavior, this module computes the gimbal angles
  that ensure that the resulting thruster torque feeds back on RW momentum build-up, therefore ensuring that the total
  net momentum is continuously dumped.
- :ref:`thrustCMEstimation`: estimates the location of the system's center of mass (CM). In the presence of an
  unmodeled disturbance such as SRP the estimate is biased, and the estimated point is the location of a point
  :math:`C^*` such that, when the thruster is fired through this point, the resulting torque counterbalances external
  unmodeled perturbations.

To ensure that attitude convergence is reached, in order for :ref:`thrustCMEstimation` to process meaningful torque
measurements, :ref:`thrusterPlatformReference` is run at the frequency of one update per hour, as opposed to the
frequency of one update every other second (0.5 Hz) for every other flight software module. Dynamics frequency is 2 Hz.

The script is found in the folder ``basilisk/examples`` and executed by using::

      python3 scenarioSepMomentumManagement.py

Illustration of Simulation Results
----------------------------------

The most interesting result of this analysis is shown comparing RW speeds with and without continuous momentum
management. In the first plot, the thruster is fired through the system's center of mass and therefore the thrust is not
used to perform momentum management. Exact knowledge of the system's CM location is used here. The wheel speeds increase
linearly over time, eventually needing momentum dumping. In the second plot, the thruster is used to perform continuous
momentum management, and the CM location is sequentially estimated. Wheel speeds oscillate in the beginning when the CM
location is still poorly known, until finally settling once the estimate becomes accurate.

.. image:: /_images/Scenarios/scenarioSepMomentumManagement300.svg
   :align: center

.. image:: /_images/Scenarios/scenarioSepMomentumManagement311.svg
   :align: center

The following two plots show the angle between the thrust vector and the true system CM. In the first plot, this
angle immediately drops to zero, because knowledge of the CM is exact, and the guidance algorithm correctly aligns the
thruster with the CM. In the second plot, the offset angle varies as the algorithm determines the location of the
equilibrium point :math:`C^*`. At steady state, the thruster is fired at a small, constant offset with respect to the
true CM.

.. image:: /_images/Scenarios/scenarioSepMomentumManagement600.svg
   :align: center

.. image:: /_images/Scenarios/scenarioSepMomentumManagement611.svg
   :align: center

The final two plots show the net external torques about the CM, projected on the plane orthogonal to the thrust vector
:math:`\boldsymbol{t}`. In the second plot, because the thruster is fired through the CM, the only contribution is given
by the SRP torque. In the first plot, when the thruster is fired through the equilibrium point :math:`C^*`, the thruster
torque exactly counters the action of the SRP torque according to:

.. math::
    \boldsymbol{L} = \boldsymbol{L}_\text{SRP} - (\boldsymbol{L}_\text{SRP} \cdot \boldsymbol{\hat{t}})\boldsymbol{\hat{t}} +
    \boldsymbol{r}_{C^*/C} \times \boldsymbol{t} = 0.

.. image:: /_images/Scenarios/scenarioSepMomentumManagement1000.svg
   :align: center

.. image:: /_images/Scenarios/scenarioSepMomentumManagement1011.svg
   :align: center

"""

import os

import matplotlib.pyplot as plt
import numpy as np
from Basilisk import __path__
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import (mrpFeedback, attTrackingError, oneAxisSolarArrayPoint, rwMotorTorque,
                                    hingedRigidBodyPIDMotor, solarArrayReference, thrusterPlatformReference,
                                    thrusterPlatformState, thrustCMEstimation, torqueScheduler)
from Basilisk.simulation import (reactionWheelStateEffector, simpleNav, simpleMassProps, spacecraft,
                                 spinningBodyOneDOFStateEffector,
                                 spinningBodyTwoDOFStateEffector, thrusterStateEffector, facetSRPDynamicEffector)
from Basilisk.utilities import (SimulationBaseClass, macros, orbitalMotion, simIncludeGravBody, simIncludeRW,
                                unitTestSupport, vizSupport, RigidBodyKinematics as rbk)

bskPath = __path__[0]
fileName = os.path.basename(os.path.splitext(__file__)[0])


[docs] def run(momentumManagement, cmEstimation, showPlots): """ The scenario can be run with the followings setups parameters: Args: momentumManagement (bool): When false, the platform aligns the thruster with the CM location it receives as input. When true, the thruster is used to perform momentum management. cmEstimation (bool): When false, the platform is connected to the true CM location message. When true, the platform is connected to the estimated CM location. showPlots (bool): Determines if the script should display plots. """ # Create simulation variable names fswTask = "fswTask" pltRefTask = "pltRefTask" dynTask = "dynTask" simProcessName = "simProcess" # Create a sim module as an empty container scSim = SimulationBaseClass.SimBaseClass() scSim.SetProgressBar(True) # create the simulation process dynProcess = scSim.CreateNewProcess(simProcessName) # create the dynamics task and specify the simulation time and integration update time simulationTime = macros.hour2nano(6) simulationTimeStepDyn = macros.sec2nano(0.5) simulationTimeStepFsw = macros.sec2nano(2) simulationTimeStepPlt = macros.hour2nano(1) dynProcess.addTask(scSim.CreateNewTask(dynTask, simulationTimeStepDyn)) dynProcess.addTask(scSim.CreateNewTask(pltRefTask, simulationTimeStepPlt)) dynProcess.addTask(scSim.CreateNewTask(fswTask, simulationTimeStepFsw)) # # setup the simulation tasks/objects # # initialize spacecraft object and set properties scObject = spacecraft.Spacecraft() scObject.ModelTag = "Spacecraft" # add spacecraft object to the simulation process scSim.AddModelToTask(dynTask, scObject, 1) # setup Gravity Body gravFactory = simIncludeGravBody.gravBodyFactory() # Next a series of gravitational bodies are included gravBodies = gravFactory.createBodies(['sun']) gravBodies['sun'].isCentralBody = True mu = gravBodies['sun'].mu # The configured gravitational bodies are added to the spacecraft dynamics with the usual command: gravFactory.addBodiesTo(scObject) # Next, the default SPICE support module is created and configured. timeInitString = "2023 OCTOBER 22 00:00:00.0" # The following is a support macro that creates a `gravFactory.spiceObject` instance gravFactory.createSpiceInterface(bskPath +'/supportData/EphemerisData/', timeInitString, epochInMsg=True) # Sun is gravity center gravFactory.spiceObject.zeroBase = 'Sun' # The SPICE object is added to the simulation task list. scSim.AddModelToTask(fswTask, gravFactory.spiceObject, 2) # setup the orbit using classical orbit elements oe = orbitalMotion.ClassicElements() oe.a = 150e9 # meters oe.e = 0.001 oe.i = 0.0 * macros.D2R oe.Omega = 0.0 * macros.D2R oe.omega = 0.0 * macros.D2R oe.f = -135.0 * macros.D2R rN, vN = orbitalMotion.elem2rv(mu, oe) # To set the spacecraft initial conditions, the following initial position and velocity variables are set: scObject.hub.r_CN_NInit = rN # m - r_BN_N scObject.hub.v_CN_NInit = vN # m/s - v_BN_N scObject.hub.sigma_BNInit = [0, 0., 0.] # MRP set to customize initial inertial attitude scObject.hub.omega_BN_BInit = [[0.], [0.], [0.]] # rad/s - omega_CN_B # define the simulation inertia I = [ 1725, -5, -12, -5, 5525, 43, -12, 43, 4810] scObject.hub.mHub = 2500 # kg - spacecraft mass scObject.hub.r_BcB_B = [[0.008], [-0.010], [1.214]] # [m] - position vector of hub CM relative to the body-fixed point B scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) # # add RW devices # # Make RW factory instance rwFactory = simIncludeRW.rwFactory() # specify RW momentum capacity maxRWMomentum = 100. # Nms # Define orthogonal RW pyramid # -- Pointing directions rwElAngle = np.array([40.0, 40.0, 40.0, 40.0]) * macros.D2R rwAzimuthAngle = np.array([45.0, 135.0, 225.0, 315.0]) * macros.D2R rwPosVector = [[0.8, 0.8, 1.8], [0.8, -0.8, 1.8], [-0.8, -0.8, 1.8], [-0.8, 0.8, 1.8]] Gs = [] for elAngle, azAngle, posVector in zip(rwElAngle, rwAzimuthAngle, rwPosVector): gsHat = (rbk.Mi(-azAngle, 3).dot(rbk.Mi(elAngle, 2))).dot(np.array([1, 0, 0])) Gs.append(gsHat) rwFactory.create('Honeywell_HR16', gsHat, maxMomentum=maxRWMomentum, rWB_B=posVector, Omega=0.) numRW = rwFactory.getNumOfDevices() # create RW object container and tie to spacecraft object rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector() rwStateEffector.ModelTag = "RW_cluster" rwFactory.addToSpacecraft(scObject.ModelTag, rwStateEffector, scObject) # add RW object array to the simulation process scSim.AddModelToTask(dynTask, rwStateEffector, 2) # Setup the FSW RW configuration message. fswRwConfigMsg = rwFactory.getConfigMessage() # add the simple Navigation sensor module sNavObject = simpleNav.SimpleNav() sNavObject.ModelTag = "SimpleNavigation" scSim.AddModelToTask(dynTask, sNavObject) # Set up the simple mass props object simpleMassPropsObject = simpleMassProps.SimpleMassProps() scSim.AddModelToTask(dynTask, simpleMassPropsObject) # Set up the rotating solar arrays numRSA = 2 RSAList = [] # 1st solar array RSAList.append(spinningBodyOneDOFStateEffector.SpinningBodyOneDOFStateEffector()) scSim.AddModelToTask(dynTask, RSAList[0]) RSAList[0].r_SB_B = [0.75, 0.0, 0.45] RSAList[0].r_ScS_S = [0.0, 3.75, 0.0] RSAList[0].sHat_S = [0, 1, 0] RSAList[0].dcm_S0B = [[0, 0, -1], [1, 0, 0], [0, -1, 0]] RSAList[0].IPntSc_S = [[250.0, 0.0, 0.0], [0.0, 250.0, 0.0], [0.0, 0.0, 500.0]] RSAList[0].mass = 85 RSAList[0].k = 0 RSAList[0].c = 0 RSAList[0].thetaInit = 0 RSAList[0].thetaDotInit = 0 RSAList[0].ModelTag = "solarArray1" scObject.addStateEffector(RSAList[0]) # 2nd solar array RSAList.append(spinningBodyOneDOFStateEffector.SpinningBodyOneDOFStateEffector()) scSim.AddModelToTask(dynTask, RSAList[1]) RSAList[1].r_SB_B = [-0.75, 0.0, 0.45] RSAList[1].r_ScS_S = [0.0, 3.75, 0.0] RSAList[1].sHat_S = [0, 1, 0] RSAList[1].dcm_S0B = [[0, 0, 1], [-1, 0, 0], [0, -1, 0]] RSAList[1].IPntSc_S = [[250.0, 0.0, 0.0], [0.0, 250.0, 0.0], [0.0, 0.0, 500.0]] RSAList[1].mass = 85 RSAList[1].k = 0 RSAList[1].c = 0 RSAList[1].thetaInit = 0 RSAList[1].thetaDotInit = 0 RSAList[1].ModelTag = "solarArray2" scObject.addStateEffector(RSAList[1]) # Set up the dual-gimbaled platform platform = spinningBodyTwoDOFStateEffector.SpinningBodyTwoDOFStateEffector() scSim.AddModelToTask(dynTask, platform) platform.theta1Init = 0 platform.theta1DotInit = 0 platform.theta2Init = 0 platform.theta2DotInit = 0 platform.mass1 = 0 platform.mass2 = 10 platform.k1 = 0 platform.k2 = 0 platform.r_S1B_B = [0, 0, 0] platform.r_S2S1_S1 = [0, 0, 0] platform.r_Sc1S1_S1 = [0, 0, 0] platform.r_Sc2S2_S2 = [0, 0, 0] platform.s1Hat_S1 = [1, 0, 0] platform.s2Hat_S2 = [0, 1, 0] platform.IS1PntSc1_S1 = [[2, 0, 0], [0, 3, 0], [0, 0, 4]] platform.IS2PntSc2_S2 = [[2, 0, 0], [0, 3, 0], [0, 0, 4]] platform.dcm_S10B = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] platform.dcm_S20S1 = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] platform.ModelTag = "platform1" scObject.addStateEffector(platform) # Set up the SEP thruster sepThruster = thrusterStateEffector.ThrusterStateEffector() scSim.AddModelToTask(dynTask, sepThruster) thruster = thrusterStateEffector.THRSimConfig() r_TF_F = [0, 0, 0] # Thruster application point in F frame coordinates tHat_F = [0, 0, 1] # Thrust unit direction vector in F frame coordinates thruster.thrLoc_B = r_TF_F thruster.thrDir_B = tHat_F thruster.MaxThrust = 0.27 thruster.steadyIsp = 1600 thruster.MinOnTime = 0.006 thruster.cutoffFrequency = 5 sepThruster.addThruster(thruster, platform.spinningBodyConfigLogOutMsgs[1]) sepThruster.kappaInit = messaging.DoubleVector([0.0]) sepThruster.ModelTag = "sepThruster" scObject.addStateEffector(sepThruster) # Write THR Config Msg THRConfig = messaging.THRConfigMsgPayload() THRConfig.rThrust_B = r_TF_F THRConfig.tHatThrust_B = tHat_F THRConfig.maxThrust = thruster.MaxThrust thrConfigFMsg = messaging.THRConfigMsg().write(THRConfig) # Set up the SRP dynamic effector SRP = facetSRPDynamicEffector.FacetSRPDynamicEffector() SRP.numFacets = 10 SRP.numArticulatedFacets = 4 scSim.AddModelToTask(dynTask, SRP) # Define the spacecraft geometry for populating the FacetedSRPSpacecraftGeometryData structure in the SRP module # Define the facet surface areas lenXHub = 1.50 # [m] lenYHub = 1.8 # [m] lenZHub = 2.86 # [m] area2 = np.pi*(0.5 * 7.262)*(0.5 * 7.262) # [m^2] facetAreas = [lenYHub * lenZHub, lenXHub * lenZHub, lenYHub * lenZHub, lenXHub * lenZHub, lenXHub * lenYHub, lenXHub * lenYHub, area2, area2, area2, area2] # Define the facet normals in B frame components facetNormal1 = np.array([1.0, 0.0, 0.0]) facetNormal2 = np.array([0.0, 1.0, 0.0]) facetNormal3 = np.array([-1.0, 0.0, 0.0]) facetNormal4 = np.array([0.0, -1.0, 0.0]) facetNormal5 = np.array([0.0, 0.0, 1.0]) facetNormal6 = np.array([0.0, 0.0, -1.0]) facetNormal7 = np.array([0.0, 1.0, 0.0]) facetNormal8 = np.array([0.0, -1.0, 0.0]) facetNormal9 = np.array([0.0, 1.0, 0.0]) facetNormal10 = np.array([0.0, -1.0, 0.0]) normals_B = [facetNormal1, facetNormal2, facetNormal3, facetNormal4, facetNormal5, facetNormal6, facetNormal7, facetNormal8, facetNormal9, facetNormal10] # Define the facet center of pressure locations with respect to point B in B frame components facetLoc1 = np.array([0.5 * lenXHub, 0.0, 0.5 * lenZHub]) # [m] facetLoc2 = np.array([0.0, 0.5 * lenYHub, 0.5 * lenZHub]) # [m] facetLoc3 = np.array([-0.5 * lenXHub, 0.0, 0.5 * lenZHub]) # [m] facetLoc4 = np.array([0.0, -0.5 * lenYHub, 0.5 * lenZHub]) # [m] facetLoc5 = np.array([0.0, 0.0, lenZHub]) # [m] facetLoc6 = np.array([0.0, 0.0, 0.0]) # [m] facetLoc7 = np.array([3.75 + 0.5 * lenXHub, 0.0, 0.45]) # [m] facetLoc8 = np.array([3.75 + 0.5 * lenXHub, 0.00, 0.45]) # [m] facetLoc9 = np.array([-(3.75 + 0.5 * lenXHub), 0.0, 0.45]) # [m] facetLoc10 = np.array([-(3.75 + 0.5 * lenXHub), 0.0, 0.45]) # [m] locationsPntB_B = [facetLoc1, facetLoc2, facetLoc3, facetLoc4, facetLoc5, facetLoc6, facetLoc7, facetLoc8, facetLoc9, facetLoc10] # Define facet articulation axes in B frame components rotAxes_B = [np.array([0.0, 0.0, 0.0]), np.array([0.0, 0.0, 0.0]), np.array([0.0, 0.0, 0.0]), np.array([0.0, 0.0, 0.0]), np.array([0.0, 0.0, 0.0]), np.array([0.0, 0.0, 0.0]), np.array([1.0, 0.0, 0.0]), np.array([1.0, 0.0, 0.0]), np.array([-1.0, 0.0, 0.0]), np.array([-1.0, 0.0, 0.0])] # Define the facet optical coefficients specCoeff = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) diffCoeff = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) # Populate the scGeometry structure with the facet information for i in range(len(facetAreas)): SRP.addFacet(facetAreas[i], specCoeff[i], diffCoeff[i], normals_B[i], locationsPntB_B[i], rotAxes_B[i]) SRP.ModelTag = "FacetSRP" SRP.addArticulatedFacet(RSAList[0].spinningBodyOutMsg) SRP.addArticulatedFacet(RSAList[0].spinningBodyOutMsg) SRP.addArticulatedFacet(RSAList[1].spinningBodyOutMsg) SRP.addArticulatedFacet(RSAList[1].spinningBodyOutMsg) scObject.addDynamicEffector(SRP) # # setup the FSW algorithm modules # # Set up thruster platform state module pltState = thrusterPlatformState.thrusterPlatformState() pltState.ModelTag = "thrusterPlatformState" pltState.sigma_MB = np.array([0, 0, 0]) pltState.r_BM_M = [0, 0, 0] pltState.r_FM_F = [0, 0, 0] scSim.AddModelToTask(fswTask, pltState, 30) # Set up the CM estimator module r_CB_B_0 = [0.04, -0.05, 1.25] cmEstimator = thrustCMEstimation.ThrustCMEstimation() cmEstimator.ModelTag = "cmEstimator" cmEstimator.attitudeTol = 1e-6 cmEstimator.r_CB_B = r_CB_B_0 # Real CoM_B location = [0.113244, 0.025605, 1.239834] cmEstimator.P0 = [0.0025, 0.0025, 0.0025] cmEstimator.R0 = [4e-8, 4e-8, 4e-8] scSim.AddModelToTask(fswTask, cmEstimator, None, 29) # create the FSW vehicle configuration message for CoM vehicleConfigData = messaging.VehicleConfigMsgPayload() vehicleConfigData.CoM_B = r_CB_B_0 # use the same initial CoM guess as the cmEstimator module vcMsg_CoM = messaging.VehicleConfigMsg_C() vcMsg_CoM.write(vehicleConfigData) # create the FSW vehicle configuration message for inertias vehicleConfigOut = messaging.VehicleConfigMsgPayload() vehicleConfigOut.ISCPntB_B = I # use the same inertia in the FSW algorithm as in the simulation vcMsg_I = messaging.VehicleConfigMsg().write(vehicleConfigOut) # Set up platform reference module pltReference = thrusterPlatformReference.thrusterPlatformReference() pltReference.ModelTag = 'thrusterPlatformReference' pltReference.sigma_MB = pltState.sigma_MB pltReference.r_BM_M = pltState.r_BM_M pltReference.r_FM_F = pltState.r_FM_F pltReference.theta1Max = np.pi/12 pltReference.theta2Max = np.pi/12 if momentumManagement: pltReference.K = 2.5e-4 else: pltReference.K = 0 pltReference.Ki = 0 scSim.AddModelToTask(pltRefTask, pltReference, 28) # Set up the two platform PD controllers pltController = [] for item in range(2): pltController.append(hingedRigidBodyPIDMotor.hingedRigidBodyPIDMotor()) pltController[item].ModelTag = "PltMototorGimbal"+str(item+1) pltController[item].K = 0.5 pltController[item].P = 3 scSim.AddModelToTask(fswTask, pltController[item], 27) # Set up the torque scheduler module pltTorqueScheduler = torqueScheduler.torqueScheduler() pltTorqueScheduler.ModelTag = "TorqueScheduler" pltTorqueScheduler.tSwitch = 60 pltTorqueScheduler.lockFlag = 0 scSim.AddModelToTask(fswTask, pltTorqueScheduler, 26) # Set up attitude guidance module sepPoint = oneAxisSolarArrayPoint.oneAxisSolarArrayPoint() sepPoint.ModelTag = "sepPointGuidance" sepPoint.a1Hat_B = [1, 0, 0] # solar array drive axis sepPoint.a2Hat_B = [0, 1, 0] # antiparallel direction to the sensitive surface sepPoint.hHat_N = [1, 0, 0] # random inertial thrust direction scSim.AddModelToTask(fswTask, sepPoint, 25) # Set up the solar array reference modules saReference = [] for item in range(numRSA): saReference.append(solarArrayReference.solarArrayReference()) saReference[item].ModelTag = "SolarArrayReference"+str(item+1) saReference[item].a1Hat_B = [(-1)**item, 0, 0] saReference[item].a2Hat_B = [0, 1, 0] scSim.AddModelToTask(fswTask, saReference[item], 24) # Set up solar array controller modules saController = [] for item in range(numRSA): saController.append(hingedRigidBodyPIDMotor.hingedRigidBodyPIDMotor()) saController[item].ModelTag = "SolarArrayMotor"+str(item+1) saController[item].K = 1.25 saController[item].P = 50 saController[item].I = 3e-3 scSim.AddModelToTask(fswTask, saController[item], 23) # Set up attitude tracking error attError = attTrackingError.attTrackingError() attError.ModelTag = "AttitudeTrackingError" scSim.AddModelToTask(fswTask, attError, 22) # Set up the MRP Feedback control module mrpControl = mrpFeedback.mrpFeedback() mrpControl.ModelTag = "mrpFeedback" mrpControl.Ki = 1e-5 mrpControl.P = 275 mrpControl.K = 9 mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1 mrpControl.controlLawType = 1 scSim.AddModelToTask(fswTask, mrpControl, 21) # add module that maps the Lr control torque into the RW motor torques rwMotorTorqueObj = rwMotorTorque.rwMotorTorque() rwMotorTorqueObj.ModelTag = "rwMotorTorque" rwMotorTorqueObj.controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] scSim.AddModelToTask(fswTask, rwMotorTorqueObj, 20) # Configure thruster on-time message thrOnTimeMsgData = messaging.THRArrayOnTimeCmdMsgPayload() thrOnTimeMsgData.OnTimeRequest = [3600*4*30] thrOnTimeMsg = messaging.THRArrayOnTimeCmdMsg().write(thrOnTimeMsgData) # Write cmEstimator output msg to the standalone message vcMsg_CoM # This is needed because platformReference runs on its own task at a different frequency, # but it receives inputs and provides outputs to modules that run on the main flight software task messaging.VehicleConfigMsg_C_addAuthor(cmEstimator.vehConfigOutMsgC, vcMsg_CoM) # Connect messages sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg) sNavObject.sunStateInMsg.subscribeTo(gravFactory.spiceObject.planetStateOutMsgs[0]) simpleMassPropsObject.scMassPropsInMsg.subscribeTo(scObject.scMassOutMsg) RSAList[0].motorTorqueInMsg.subscribeTo(saController[0].motorTorqueOutMsg) RSAList[1].motorTorqueInMsg.subscribeTo(saController[1].motorTorqueOutMsg) platform.motorTorqueInMsg.subscribeTo(pltTorqueScheduler.motorTorqueOutMsg) platform.motorLockInMsg.subscribeTo(pltTorqueScheduler.effectorLockOutMsg) SRP.sunInMsg.subscribeTo(gravFactory.spiceObject.planetStateOutMsgs[0]) pltState.thrusterConfigFInMsg.subscribeTo(thrConfigFMsg) pltState.hingedRigidBody1InMsg.subscribeTo(platform.spinningBodyOutMsgs[0]) pltState.hingedRigidBody2InMsg.subscribeTo(platform.spinningBodyOutMsgs[1]) cmEstimator.thrusterConfigBInMsg.subscribeTo(pltState.thrusterConfigBOutMsg) cmEstimator.intFeedbackTorqueInMsg.subscribeTo(mrpControl.intFeedbackTorqueOutMsg) cmEstimator.attGuidInMsg.subscribeTo(attError.attGuidOutMsg) cmEstimator.vehConfigInMsg.subscribeTo(simpleMassPropsObject.vehicleConfigOutMsg) if cmEstimation: pltReference.vehConfigInMsg.subscribeTo(vcMsg_CoM) # connect to this msg for estimated CM else: pltReference.vehConfigInMsg.subscribeTo(simpleMassPropsObject.vehicleConfigOutMsg) # connect to this msg for exact CM information pltReference.thrusterConfigFInMsg.subscribeTo(thrConfigFMsg) pltReference.rwConfigDataInMsg.subscribeTo(fswRwConfigMsg) pltReference.rwSpeedsInMsg.subscribeTo(rwStateEffector.rwSpeedOutMsg) pltTorqueScheduler.motorTorque1InMsg.subscribeTo(pltController[0].motorTorqueOutMsg) pltTorqueScheduler.motorTorque2InMsg.subscribeTo(pltController[1].motorTorqueOutMsg) sepPoint.attNavInMsg.subscribeTo(sNavObject.attOutMsg) sepPoint.bodyHeadingInMsg.subscribeTo(pltReference.bodyHeadingOutMsg) attError.attNavInMsg.subscribeTo(sNavObject.attOutMsg) attError.attRefInMsg.subscribeTo(sepPoint.attRefOutMsg) mrpControl.guidInMsg.subscribeTo(attError.attGuidOutMsg) mrpControl.vehConfigInMsg.subscribeTo(vcMsg_I) mrpControl.rwParamsInMsg.subscribeTo(fswRwConfigMsg) mrpControl.rwSpeedsInMsg.subscribeTo(rwStateEffector.rwSpeedOutMsg) rwMotorTorqueObj.rwParamsInMsg.subscribeTo(fswRwConfigMsg) rwMotorTorqueObj.vehControlInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg) rwStateEffector.rwMotorCmdInMsg.subscribeTo(rwMotorTorqueObj.rwMotorTorqueOutMsg) for item in range(numRSA): saReference[item].attNavInMsg.subscribeTo(sNavObject.attOutMsg) saReference[item].attRefInMsg.subscribeTo(sepPoint.attRefOutMsg) saReference[item].hingedRigidBodyInMsg.subscribeTo(RSAList[item].spinningBodyOutMsg) saController[item].hingedRigidBodyInMsg.subscribeTo(RSAList[item].spinningBodyOutMsg) saController[item].hingedRigidBodyRefInMsg.subscribeTo(saReference[item].hingedRigidBodyRefOutMsg) for item in range(2): pltController[item].hingedRigidBodyInMsg.subscribeTo(platform.spinningBodyOutMsgs[item]) pltController[0].hingedRigidBodyRefInMsg.subscribeTo(pltReference.hingedRigidBodyRef1OutMsg) pltController[1].hingedRigidBodyRefInMsg.subscribeTo(pltReference.hingedRigidBodyRef2OutMsg) sepThruster.cmdsInMsg.subscribeTo(thrOnTimeMsg) # # Setup data logging before the simulation is initialized # numDataPoints = simulationTime / simulationTimeStepFsw samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStepFsw, numDataPoints) vehConfigLog = simpleMassPropsObject.vehicleConfigOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, vehConfigLog) snTransLog = sNavObject.transOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, snTransLog) snAttLog = sNavObject.attOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, snAttLog) attErrorLog = attError.attGuidOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, attErrorLog) attRefLog = sepPoint.attRefOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, attRefLog) rwMotorLog = rwMotorTorqueObj.rwMotorTorqueOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, rwMotorLog) rwSpeedLog = rwStateEffector.rwSpeedOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, rwSpeedLog) thrLog = sepThruster.thrusterOutMsgs[0].recorder(samplingTime) scSim.AddModelToTask(dynTask, thrLog) cmEstLog = cmEstimator.cmEstDataOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, cmEstLog) srpForceLog = SRP.logger("forceExternal_B", samplingTime) scSim.AddModelToTask(dynTask, srpForceLog) srpTorqueLog = SRP.logger("torqueExternalPntB_B", samplingTime) scSim.AddModelToTask(dynTask, srpTorqueLog) mrpTorqueLog = mrpControl.cmdTorqueOutMsg.recorder(samplingTime) scSim.AddModelToTask(dynTask, mrpTorqueLog) # A message is created that stores an array of the Omega wheel speeds rwLogs = [] for item in range(numRW): rwLogs.append(rwStateEffector.rwOutMsgs[item].recorder(samplingTime)) scSim.AddModelToTask(dynTask, rwLogs[item]) saAngleLogs = [] saRefAngleLogs = [] for item in range(numRSA): saAngleLogs.append(RSAList[item].spinningBodyOutMsg.recorder(samplingTime)) scSim.AddModelToTask(dynTask, saAngleLogs[item]) saRefAngleLogs.append(saReference[item].hingedRigidBodyRefOutMsg.recorder(samplingTime)) scSim.AddModelToTask(dynTask, saRefAngleLogs[item]) pltAngleLogs = [] pltRefAngleLogs = [] pltRefAngleLogs.append(pltReference.hingedRigidBodyRef1OutMsg.recorder(samplingTime)) pltRefAngleLogs.append(pltReference.hingedRigidBodyRef2OutMsg.recorder(samplingTime)) for item in range(2): scSim.AddModelToTask(dynTask, pltRefAngleLogs[item]) pltAngleLogs.append(platform.spinningBodyOutMsgs[item].recorder(samplingTime)) scSim.AddModelToTask(dynTask, pltAngleLogs[item]) # initialize Simulation: This function runs the self_init() # cross_init() and reset() routines on each module. scSim.InitializeSimulation() # configure a simulation stop time and execute the simulation run scSim.ConfigureStopTime(simulationTime) scSim.ExecuteSimulation() # retrieve the logged data timeData = snAttLog.times() * macros.NANO2HOUR dataSigmaBN = snAttLog.sigma_BN dataSigmaRN = attRefLog.sigma_RN dataSigmaBR = attErrorLog.sigma_BR dataOmegaRW = rwSpeedLog.wheelSpeeds dataRealCM = vehConfigLog.CoM_B dataStateError = cmEstLog.stateError dataCovariance = cmEstLog.covariance dataPreFit = cmEstLog.preFitRes dataPostFit = cmEstLog.postFitRes dataRW = [] for i in range(numRW): dataRW.append(rwLogs[i].u_current) dataNu = [] dataNuRef = [] for item in range(2): dataNu.append(pltAngleLogs[item].theta) dataNuRef.append(pltRefAngleLogs[item].theta) dataAlpha = [] dataAlphaRef = [] for item in range(numRSA): dataAlpha.append(saAngleLogs[item].theta) dataAlphaRef.append(saRefAngleLogs[item].theta) # Map SRP torque from point B to CM dataSRPForce = srpForceLog.forceExternal_B dataSRPTorquePntB = srpTorqueLog.torqueExternalPntB_B dataSRPTorquePntC = [] for i in range(len(dataSRPForce)): dataSRPTorquePntC.append(dataSRPTorquePntB[i] - np.cross(dataRealCM[i], dataSRPForce[i])) dataSRPTorquePntC = np.array(dataSRPTorquePntC) thrLoc_F = thrLog.thrusterLocation thrDir_F = thrLog.thrusterDirection thrForce = thrLog.thrustForce thrVec_F = [] for i in range(len(thrForce)): thrVec_F.append(thrForce[i]*thrDir_F[i]) thrVec_F = np.array(thrVec_F) # Plot the results figureList = {} plot_attitude(timeData, dataSigmaBN, dataSigmaRN, figID=1) pltName = fileName+"1"+str(int(momentumManagement))+str(int(cmEstimation)) figureList[pltName] = plt.figure(1) plot_attitude_error(timeData, dataSigmaBR, figID=2) pltName = fileName+"2"+str(int(momentumManagement))+str(int(cmEstimation)) figureList[pltName] = plt.figure(2) plot_rw_speeds(timeData, dataOmegaRW, numRW, figID=3) pltName = fileName+"3"+str(int(momentumManagement))+str(int(cmEstimation)) figureList[pltName] = plt.figure(3) plot_solar_array_angle(timeData, dataAlpha, dataAlphaRef, figID=4) pltName = fileName+"4"+str(int(momentumManagement))+str(int(cmEstimation)) figureList[pltName] = plt.figure(4) plot_platform_angle(timeData, dataNu, dataNuRef, figID=5) pltName = fileName+"5"+str(int(momentumManagement))+str(int(cmEstimation)) figureList[pltName] = plt.figure(5) plot_thruster_cm_offset(timeData, dataRealCM, dataNu, platform.r_S1B_B, platform.dcm_S10B, thrLoc_F, thrDir_F, figID=6) pltName = fileName+"6"+str(int(momentumManagement))+str(int(cmEstimation)) figureList[pltName] = plt.figure(6) plot_thrust_to_momentum_angle(timeData, dataOmegaRW, Gs, dataNu, platform.dcm_S10B, thrDir_F, figID=7) pltName = fileName+"7"+str(int(momentumManagement))+str(int(cmEstimation)) figureList[pltName] = plt.figure(7) plot_external_torque(timeData, dataSRPTorquePntC, yString='SRP', figID=8) pltName = fileName+"8"+str(int(momentumManagement))+str(int(cmEstimation)) figureList[pltName] = plt.figure(8) plot_thr_torque(timeData, dataRealCM, dataNu, platform.r_S1B_B, platform.dcm_S10B, thrLoc_F, thrVec_F, figID=9) pltName = fileName+"9"+str(int(momentumManagement))+str(int(cmEstimation)) figureList[pltName] = plt.figure(9) plot_net_torques(timeData, dataRealCM, dataNu, platform.r_S1B_B, platform.dcm_S10B, thrLoc_F, thrVec_F, dataSRPTorquePntC, figID=10) pltName = fileName+"10"+str(int(momentumManagement))+str(int(cmEstimation)) figureList[pltName] = plt.figure(10) plot_state_errors(timeData, dataStateError, dataCovariance, figID=11) pltName = fileName+"11"+str(int(momentumManagement))+str(int(cmEstimation)) figureList[pltName] = plt.figure(11) plot_residuals(timeData, dataPreFit, dataPostFit, cmEstimator.R0[0][0]**0.5, figID=12) pltName = fileName+"12"+str(int(momentumManagement))+str(int(cmEstimation)) figureList[pltName] = plt.figure(12) if showPlots: plt.show() # close the plots being saved off to avoid over-writing old and new figures plt.close("all") return figureList
# Plotting functions
[docs] def plot_attitude(timeData, dataSigmaBN, dataSigmaRN, figID=None): """Plot the spacecraft attitude w.r.t. reference.""" plt.figure(figID, figsize=(5, 2.75)) for idx in range(3): plt.plot(timeData, dataSigmaBN[:, idx], color=unitTestSupport.getLineColor(idx, 3), label=r'$\sigma_{BN,' + str(idx + 1) + '}$') for idx in range(3): plt.plot(timeData, dataSigmaRN[:, idx], color=unitTestSupport.getLineColor(idx, 3), linestyle='dashed', label=r'$\sigma_{RN,' + str(idx + 1) + '}$') plt.legend(loc='lower right') plt.xlabel('Time [hours]') plt.ylabel(r'Attitude $\sigma$')
[docs] def plot_attitude_error(timeData, dataSigmaBR, figID=None): """Plot the spacecraft attitude error.""" plt.figure(figID, figsize=(5, 2.75)) for idx in range(3): plt.plot(timeData, dataSigmaBR[:, idx], color=unitTestSupport.getLineColor(idx, 3), label=r'$\sigma_' + str(idx + 1) + '$') plt.legend(loc='lower right') plt.xlabel('Time [hours]') plt.ylabel(r'Attitude Tracking Error $\sigma_{B/R}$')
[docs] def plot_rw_speeds(timeData, dataOmegaRW, numRW, figID=None): """Plot the RW spin rates.""" plt.figure(figID, figsize=(5, 2.75)) for idx in range(numRW): plt.plot(timeData, dataOmegaRW[:, idx] / macros.RPM, color=unitTestSupport.getLineColor(idx, numRW), label=r'$\Omega_{' + str(idx + 1) + '}$') plt.legend(loc='lower right') plt.xlabel('Time [hours]') plt.ylabel('RW Speed (RPM) ')
[docs] def plot_solar_array_angle(timeData, dataAngle, dataRefAngle, figID=None): """Plot the solar array angles w.r.t references.""" plt.figure(figID, figsize=(5, 2.75)) for i, angle in enumerate(dataAngle): plt.plot(timeData, angle / np.pi * 180, color='C'+str(i), label=r'$\alpha_' + str(i+1) + '$') for i, angle in enumerate(dataRefAngle): plt.plot(timeData, angle / np.pi * 180, color='C'+str(i), linestyle='dashed', label=r'$\alpha_{R,' + str(i+1) + '}$') plt.legend(loc='lower right') plt.xlabel('Time [hours]') plt.ylabel(r'Solar Array Angles [deg]')
[docs] def plot_platform_angle(timeData, dataAngle, dataRefAngle, figID=None): """Plot the platform tip and tilt angles w.r.t. references.""" plt.figure(figID, figsize=(5,2.75)) for i, angle in enumerate(dataAngle): plt.plot(timeData, angle / np.pi * 180, color='C'+str(i), label=r'$\nu_' + str(i+1) + '$') for i, angle in enumerate(dataRefAngle): plt.plot(timeData, angle / np.pi * 180, color='C'+str(i), linestyle='dashed', label=r'$\nu_{R,' + str(i+1) + '}$') plt.legend(loc='lower right') plt.xlabel('Time [hours]') plt.ylabel(r'Platform Angles [deg]')
[docs] def plot_thruster_cm_offset(timeData, dataCM, dataNu, dataMB_B, dataM0B, dataThrLoc_F, dataThrDir_F, figID=None): """Plot the angle between thrust vector and system CM.""" r_MB_B = np.array([dataMB_B[0][0], dataMB_B[1][0], dataMB_B[2][0]]) dataAngOffset = [] for i in range(len(timeData)): FM0 = rbk.euler1232C([dataNu[0][i], dataNu[1][i], 0.0]) FB = np.matmul(FM0, dataM0B) BF = FB.transpose() r_TM_B = np.matmul(BF, dataThrLoc_F[i]) r_CT_B = dataCM[i] - r_TM_B - r_MB_B thrDir_B = np.matmul(BF, dataThrDir_F[i]) dataAngOffset.append(np.arccos(min(max(np.dot(r_CT_B, thrDir_B) / np.linalg.norm(r_CT_B), -1), 1))) cross = np.cross(r_CT_B, thrDir_B) if np.arctan2(cross[1], cross[0]) < 0: dataAngOffset[-1] = -dataAngOffset[-1] dataAngOffset = np.array(dataAngOffset) * macros.R2D plt.figure(figID, figsize=(5, 2.75)) plt.plot(timeData, dataAngOffset, label=r'$\Delta \theta$') plt.legend(loc='lower right') plt.xlabel('Time [hours]') plt.ylabel('CM Offset Ang [deg]')
[docs] def plot_thrust_to_momentum_angle(timeData, dataOmegaRW, Gs, dataNu, dataM0B, dataThrDir_F, figID=None): """Plot the angle between thrust vector and net momentum on RWs.""" dataAngle = [] for i in range(len(timeData)): FM0 = rbk.euler1232C([dataNu[0][i], dataNu[1][i], 0.0]) FB = np.matmul(FM0, dataM0B) BF = FB.transpose() thrDir_B = np.matmul(BF, dataThrDir_F[i]) h_B = np.array([0, 0, 0]) for j in range(len(Gs)): h_B = h_B + dataOmegaRW[i][j] * Gs[j] h_B_norm = np.linalg.norm(h_B) if h_B_norm == 0: dataAngle.append(0.0) else: dataAngle.append(np.arccos(min(max(np.dot(h_B, thrDir_B) / np.linalg.norm(h_B), -1), 1))) cross = np.cross(h_B, thrDir_B) if np.arctan2(cross[1], cross[0]) < 0: dataAngle[-1] = -dataAngle[-1] dataAngle = np.array(dataAngle) * macros.R2D plt.figure(figID, figsize=(5, 2.75)) plt.plot(timeData, dataAngle, color='C3', label=r'$\Delta \phi$') plt.legend(loc='lower right') plt.xlabel('Time [hours]') plt.ylabel('Thr-to-Momentum Angle [deg]')
[docs] def plot_external_torque(timeData, dataTorque, yString=None, figID=None): """Plot the external torques.""" plt.figure(figID, figsize=(5, 2.75)) for idx in range(3): plt.plot(timeData, dataTorque[:, idx] * 1000, color=unitTestSupport.getLineColor(idx, 3), label=r'${}^BL_' + str(idx+1) + '$') plt.legend(loc='lower right') plt.xlabel('Time [hours]') if yString: plt.ylabel(yString + ' Torque [mNm]') else: plt.ylabel('Torque [mNm]')
[docs] def plot_thr_torque(timeData, dataCM, dataNu, dataMB_B, dataM0B, dataThrLoc_F, dataThrVec_F, figID=None): """Plot the thruster torque about CM.""" r_MB_B = np.array([dataMB_B[0][0], dataMB_B[1][0], dataMB_B[2][0]]) dataThrTorque = [] for i in range(len(timeData)): FM0 = rbk.euler1232C([dataNu[0][i], dataNu[1][i], 0.0]) FB = np.matmul(FM0, dataM0B) BF = FB.transpose() r_TM_B = np.matmul(BF, dataThrLoc_F[i]) r_TC_B = r_TM_B + r_MB_B - dataCM[i] thrVec_B = np.matmul(BF, dataThrVec_F[i]) dataThrTorque.append(np.cross(r_TC_B, thrVec_B)) dataThrTorque = np.array(dataThrTorque) plot_external_torque(timeData, dataThrTorque, yString=r'Thruster', figID=figID)
[docs] def plot_net_torques(timeData, dataCM, dataNu, dataMB_B, dataM0B, dataThrLoc_F, dataThrVec_F, dataSRP, figID=None): """Plot the net external torques in the plane perpendicular to the thrust vector.""" r_MB_B = np.array([dataMB_B[0][0], dataMB_B[1][0], dataMB_B[2][0]]) dataDeltaL = [] for i in range(len(timeData)): FM0 = rbk.euler1232C([dataNu[0][i], dataNu[1][i], 0.0]) FB = np.matmul(FM0, dataM0B) BF = FB.transpose() r_TM_B = np.matmul(BF, dataThrLoc_F[i]) r_TC_B = r_TM_B + r_MB_B - dataCM[i] thrVec_B = np.matmul(BF, dataThrVec_F[i]) thrTorque_B = np.cross(r_TC_B, thrVec_B) dataDeltaL.append(dataSRP[i] + thrTorque_B) dataDeltaL = np.array(dataDeltaL) plot_external_torque(timeData, dataDeltaL, yString=r'Net Ext.', figID=figID)
[docs] def plot_state_errors(timeData, data1, data2, figID=None): """Plot the error between estimated CM and true CM.""" plt.figure(figID, figsize=(5, 6)) plt.subplot(3,1,1) plt.plot(timeData, data1[:, 0]*1000, color='C0', linestyle='solid', label=r'$\Delta r_1$') plt.plot(timeData, 3*data2[:, 0]*1000, color='C0', linestyle='dashed', label=r'$\pm 3\sigma_1$') plt.plot(timeData, -3*data2[:, 0]*1000, color='C0', linestyle='dashed') plt.legend(loc='upper right') plt.ylabel('$r_{CM,1}$ [mm]') plt.grid() plt.subplot(3,1,2) plt.plot(timeData, data1[:, 1]*1000, color='C1', linestyle='solid', label=r'$\Delta r_2$') plt.plot(timeData, 3*data2[:, 1]*1000, color='C1', linestyle='dashed', label=r'$\pm 3\sigma_2$') plt.plot(timeData, -3*data2[:, 1]*1000, color='C1', linestyle='dashed') plt.legend(loc='upper right') plt.ylabel('$r_{CM,2}$ [mm]') plt.grid() plt.subplot(3,1,3) plt.plot(timeData, data1[:, 2]*1000, color='C2', linestyle='solid', label=r'$\Delta r_3$') plt.plot(timeData, 3*data2[:, 2]*1000, color='C2', linestyle='dashed', label=r'$\pm 3\sigma_3$') plt.plot(timeData, -3*data2[:, 2]*1000, color='C2', linestyle='dashed') plt.legend(loc='upper right') plt.ylabel('$r_{CM,3}$ [mm]') plt.grid() plt.xlabel('Time [hours]')
[docs] def plot_residuals(timeData, preFit, postFit, R, figID=None): """Plot pre-fit and post-fit residuals on integral feedback torque measurements.""" plt.figure(figID, figsize=(5, 6)) plt.subplot(2, 1, 1) plt.plot(timeData, preFit[:, 0]*1e3, color='C0', linestyle='solid', label=r'$\rho_1$') plt.plot(timeData, preFit[:, 1]*1e3, color='C1', linestyle='solid', label=r'$\rho_2$') plt.plot(timeData, preFit[:, 2]*1e3, color='C2', linestyle='solid', label=r'$\rho_3$') plt.ylabel('Pre-Fit residuals [mNm]') plt.legend(loc='upper right') plt.grid() plt.subplot(2,1,2) plt.plot(timeData, postFit[:, 0]*1e3, color='C0', linestyle='dashed', label=r'$\rho_1$') plt.plot(timeData, postFit[:, 1]*1e3, color='C1', linestyle='dashed', label=r'$\rho_2$') plt.plot(timeData, postFit[:, 2]*1e3, color='C2', linestyle='dashed', label=r'$\rho_3$') plt.plot([timeData[0],timeData[-1]],[3000*R,3000*R], color='C3', linestyle='dashed', label=r'$\pm 3\sigma_R$') plt.plot([timeData[0],timeData[-1]],[-3000*R,-3000*R], color='C3', linestyle='dashed') plt.legend(loc='upper right') plt.ylabel('Post-Fit residuals [mNm]') plt.grid() plt.xlabel('Time [hours]')
if __name__ == "__main__": run( True, True, True )