Source code for test_thrustCMEstimation

#
#  ISC License
#
#  Copyright (c) 2023 Laboratory for Atmospheric and Space Physics, 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.
#

import os

import matplotlib.pyplot as plt
import numpy as np
import pytest
from Basilisk import __path__
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import thrustCMEstimation
from Basilisk.utilities import SimulationBaseClass, macros, unitTestSupport

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


[docs] @pytest.mark.parametrize("dT", [10]) # s @pytest.mark.parametrize("accuracy", [1e-12]) def test_thrustCMEstimation(show_plots, dT, accuracy): r""" **Validation Test Description** This unit test script tests the correctness of the center of mass (CM) estimate computed by :ref:`thrustCMEstimation` when multiple torque measurements are provided. **Test Parameters** This tests feeds several thrust vectors and thrust application points to the CM estimator module. These simulate the behavior of a gimbaled thruster. Args: dT (sec): time interval between two consecutive torque measurements are processed accuracy: tolerance on the result. **Description of Variables Being Tested** This Unit Test checks the correctness of the estimation based on the following considerations: - post-fit residuals are smaller than the associate pre-fit residuals for every measurement update; - post-fit residuals are within :math:`3\sigma` bounds of the measurement noise covariance; - the error on the state estimate is within :math:`3\sigma` bounds of the estimated covariance. """ # each test method requires a single assert method to be called thrustCMEstimationTestFunction(show_plots, dT, accuracy)
def thrustCMEstimationTestFunction(show_plots, dT, accuracy): r_CB_B = np.array([0, 0, 0]) # exact CM location r_TB_B = np.array([[6, 5, 4], # simulated thrust application point [5, 4, 6], [4, 6, 5], [-6, 5, 4]]) T_B = np.array([[1, 2, 3], # simulated thrust vector [2, 3, 1], [3, 1, 2], [1, -2, 3]]) unitTaskName = "unitTask" unitProcessName = "TestProcess" # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread testProcessRate = macros.sec2nano(dT) # update process rate update time testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # setup the FSW algorithm tasks # setup thrustCMEstimation module cmEstimation = thrustCMEstimation.ThrustCMEstimation() cmEstimation.ModelTag = "cmEstimator" cmEstimation.attitudeTol = 1e-4 cmEstimation.r_CB_B = [0.01, -0.025, 0.04] cmEstimation.P0 = [0.0025, 0.0025, 0.0025] cmEstimation.R0 = [1e-8, 1e-8, 1e-8] unitTestSim.AddModelToTask(unitTaskName, cmEstimation) # Write attitude guidance msg vehConfig = messaging.VehicleConfigMsgPayload() vehConfig.CoM = r_CB_B vehConfigMsg = messaging.VehicleConfigMsg().write(vehConfig) cmEstimation.vehConfigInMsg.subscribeTo(vehConfigMsg) # Write attitude guidance msg attGuidance = messaging.AttGuidMsgPayload() attGuidance.sigma_BR = [0, 0, 0] attGuidance.omega_BR_B = [0, 0, 0] attGuidMsg = messaging.AttGuidMsg().write(attGuidance) cmEstimation.attGuidInMsg.subscribeTo(attGuidMsg) # Write THR Config Msg in body frame coordinates B thrBConfig = messaging.THRConfigMsgPayload() thrConfigBMsg = messaging.THRConfigMsg() cmEstimation.thrusterConfigBInMsg.subscribeTo(thrConfigBMsg) # Write integral feedback torque intTorque = messaging.CmdTorqueBodyMsgPayload() intTorque.torqueRequestBody = [0, 0, 0] intFeedbackTorqueMsg = messaging.CmdTorqueBodyMsg() cmEstimation.intFeedbackTorqueInMsg.subscribeTo(intFeedbackTorqueMsg) # # Setup data logging before the simulation is initialized # cmEstimateLog = cmEstimation.cmEstDataOutMsg.recorder() unitTestSim.AddModelToTask(unitTaskName, cmEstimateLog) t = dT R0 = np.array([[cmEstimation.R0[0][0], 0, 0], [0, cmEstimation.R0[1][0], 0], [0, 0, cmEstimation.R0[2][0]]]) unitTestSim.InitializeSimulation() unitTestSim.ConfigureStopTime(macros.sec2nano(t-0.1)) for i in range(len(r_TB_B)): thrBConfig.timeTag = macros.sec2nano(t) thrBConfig.rThrust_B = r_TB_B[i] thrBConfig.maxThrust = np.linalg.norm(T_B[i]) thrBConfig.tHatThrust_B = T_B[i] / thrBConfig.maxThrust thrConfigBMsg.write(thrBConfig) intTorque.timeTag = macros.sec2nano(t) uMeasNoise = np.random.multivariate_normal([0,0,0], R0, size=1) intTorque.torqueRequestBody = -np.cross(r_TB_B[i], T_B[i]) + uMeasNoise[0] intFeedbackTorqueMsg.write(intTorque) t += dT unitTestSim.ExecuteSimulation() unitTestSim.ConfigureStopTime(macros.sec2nano(t-0.1)) # retrieve the logged data stateErr = cmEstimateLog.stateError sigma = cmEstimateLog.covariance preFit = cmEstimateLog.preFitRes postFit = cmEstimateLog.postFitRes # check that post-fit residuals are smaller in magnitude that pre-fit residuals at each measurement for i in range(len(r_TB_B)): np.testing.assert_array_less(np.linalg.norm(postFit[i]), np.linalg.norm(preFit[i]) + accuracy, verbose=True) # check that components of post-fit residuals are within 3-sigma bounds of measurement covariance for i in range(len(r_TB_B)): for j in range(3): np.testing.assert_array_less(postFit[i][j], 3*(R0[j][j])**0.5 + accuracy, verbose=True) # check that components of state errors are within 3-sigma bounds of state covariance for i in range(len(r_TB_B)): for j in range(3): np.testing.assert_array_less(stateErr[i][j], 3*sigma[i][j] + accuracy, verbose=True) return # # This statement below ensures that the unit test scrip can be run as a # stand-along python script # if __name__ == "__main__": test_thrustCMEstimation( True, # show_plots 10, # dTsim 1e-12 # accuracy )