diff --git a/data/sample/initialize_files/components/orbit_observer.ini b/data/sample/initialize_files/components/orbit_observer.ini new file mode 100644 index 000000000..1c27ecf57 --- /dev/null +++ b/data/sample/initialize_files/components/orbit_observer.ini @@ -0,0 +1,19 @@ +[ORBIT_OBSERVER] +// Noise definition frame +// INERTIAL: Inertial frame +// RTN: RTN frame +noise_frame = INERTIAL + +// Standard deviation of position and velocity noise [m, m/s] +// The frame definition can be selected above +noise_standard_deviation(0) = 1000 // Position-X +noise_standard_deviation(1) = 2000 // Position-Y +noise_standard_deviation(2) = 3000 // Position-Z +noise_standard_deviation(3) = 30 // Velocity-X +noise_standard_deviation(4) = 20 // Velocity-Y +noise_standard_deviation(5) = 10 // Velocity-Z + + +[COMPONENT_BASE] +// Prescaler with respect to the component update period +prescaler = 1 diff --git a/data/sample/initialize_files/sample_satellite.ini b/data/sample/initialize_files/sample_satellite.ini index 69dcd5210..d0fa9c23a 100644 --- a/data/sample/initialize_files/sample_satellite.ini +++ b/data/sample/initialize_files/sample_satellite.ini @@ -146,6 +146,7 @@ force_generator_file = INI_FILE_DIR_FROM_EXE/components/force_generator.ini torque_generator_file = INI_FILE_DIR_FROM_EXE/components/torque_generator.ini angular_velocity_observer_file = INI_FILE_DIR_FROM_EXE/components/angular_velocity_observer.ini attitude_observer_file = INI_FILE_DIR_FROM_EXE/components/attitude_observer.ini +orbit_observer_file = INI_FILE_DIR_FROM_EXE/components/orbit_observer.ini antenna_file = INI_FILE_DIR_FROM_EXE/components/spacecraft_antenna.ini component_interference_file = INI_FILE_DIR_FROM_EXE/components/component_interference.ini telescope_file = INI_FILE_DIR_FROM_EXE/components/telescope.ini diff --git a/scripts/Plot/plot_orbit_observer.py b/scripts/Plot/plot_orbit_observer.py new file mode 100644 index 000000000..b84c02830 --- /dev/null +++ b/scripts/Plot/plot_orbit_observer.py @@ -0,0 +1,126 @@ +# +# Plot orbit observer +# +# arg[1] : read_file_tag : time tag for default CSV output log file. ex. 220627_142946 +# + +# +# Import +# +# plots +import matplotlib.pyplot as plt +# numpy +import numpy as np +# local function +from common import find_latest_log_tag +from common import add_log_file_arguments +from common import read_3d_vector_from_csv +from common import read_scalar_from_csv +# arguments +import argparse + +# Arguments +aparser = argparse.ArgumentParser() +aparser = add_log_file_arguments(aparser) +aparser.add_argument('--no-gui', action='store_true') +args = aparser.parse_args() + +# +# Read Arguments +# +# log file path +path_to_logs = args.logs_dir + +read_file_tag = args.file_tag +if read_file_tag == None: + print("file tag does not found. use latest.") + read_file_tag = find_latest_log_tag(path_to_logs) + +print("log: " + read_file_tag) + +# +# CSV file name +# +read_file_name = path_to_logs + '/' + 'logs_' + read_file_tag + '/' + read_file_tag + '_default.csv' + +# +# Data read and edit +# +# Read S2E CSV +time = read_scalar_from_csv(read_file_name, 'elapsed_time[s]') + +measured_position_eci_m = read_3d_vector_from_csv(read_file_name, 'orbit_observer_position_i', 'm') +true_position_eci_m = read_3d_vector_from_csv(read_file_name, 'spacecraft_position_i', 'm') + +measured_velocity_eci_m_s = read_3d_vector_from_csv(read_file_name, 'orbit_observer_velocity_i', 'm/s') +true_velocity_eci_m_s = read_3d_vector_from_csv(read_file_name, 'spacecraft_velocity_i', 'm/s') + +# Statistics +position_error_m = measured_position_eci_m[:, 1:] - true_position_eci_m[:, 1:] +position_average = [0.0, 0.0, 0.0] +position_standard_deviation = [0.0, 0.0, 0.0] +velocity_error_m = measured_velocity_eci_m_s[:, 1:] - true_velocity_eci_m_s[:, 1:] +velocity_average = [0.0, 0.0, 0.0] +velocity_standard_deviation = [0.0, 0.0, 0.0] +for i in range(3): + position_average[i] = position_error_m[i].mean() + position_standard_deviation[i] = position_error_m[i].std() + velocity_average[i] = velocity_error_m[i].mean() + velocity_standard_deviation[i] = velocity_error_m[i].std() + +# +# Plot +# +unit = ' m' +fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True) +axis[0, 0].plot(time[0], measured_position_eci_m[0], marker=".", c="red", label="MEASURED-X") +axis[0, 0].plot(time[0], true_position_eci_m[0], marker=".", c="orange", label="TRUE-X") +axis[0, 0].text(0.01, 0.99, "Error average:" + format(position_average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) +axis[0, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) +axis[0, 0].legend(loc = 'upper right') + +axis[1, 0].plot(time[0], measured_position_eci_m[1], marker=".", c="green", label="MEASURED-Y") +axis[1, 0].plot(time[0], true_position_eci_m[1], marker=".", c="yellow", label="TRUE-Y") +axis[1, 0].text(0.01, 0.99, "Error average:" + format(position_average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) +axis[1, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) +axis[1, 0].legend(loc = 'upper right') + +axis[2, 0].plot(time[0], measured_position_eci_m[2], marker=".", c="blue", label="MEASURED-Z") +axis[2, 0].plot(time[0], true_position_eci_m[2], marker=".", c="purple", label="TRUE-Z") +axis[2, 0].text(0.01, 0.99, "Error average:" + format(position_average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) +axis[2, 0].text(0.01, 0.79, "Standard deviation:" + format(position_standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) +axis[2, 0].legend(loc = 'upper right') + +fig.suptitle("Orbit observer position results @ ECI") +fig.supylabel("Position [m]") +fig.supxlabel("Time [s]") + +unit = ' m/s' +fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True) +axis[0, 0].plot(time[0], measured_velocity_eci_m_s[0], marker=".", c="red", label="MEASURED-X") +axis[0, 0].plot(time[0], true_velocity_eci_m_s[0], marker=".", c="orange", label="TRUE-X") +axis[0, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) +axis[0, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes) +axis[0, 0].legend(loc = 'upper right') + +axis[1, 0].plot(time[0], measured_velocity_eci_m_s[1], marker=".", c="green", label="MEASURED-Y") +axis[1, 0].plot(time[0], true_velocity_eci_m_s[1], marker=".", c="yellow", label="TRUE-Y") +axis[1, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) +axis[1, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes) +axis[1, 0].legend(loc = 'upper right') + +axis[2, 0].plot(time[0], measured_velocity_eci_m_s[2], marker=".", c="blue", label="MEASURED-Z") +axis[2, 0].plot(time[0], true_velocity_eci_m_s[2], marker=".", c="purple", label="TRUE-Z") +axis[2, 0].text(0.01, 0.99, "Error average:" + format(velocity_average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) +axis[2, 0].text(0.01, 0.79, "Standard deviation:" + format(velocity_standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes) +axis[2, 0].legend(loc = 'upper right') + +fig.suptitle("Orbit observer velocity results @ ECI") +fig.supylabel("Velocity [m/s]") +fig.supxlabel("Time [s]") + +# Data save +if args.no_gui: + plt.savefig(read_file_tag + "_orbit_observer.png") # save last figure only +else: + plt.show() diff --git a/src/components/CMakeLists.txt b/src/components/CMakeLists.txt index 697b2322f..2cc69c93d 100644 --- a/src/components/CMakeLists.txt +++ b/src/components/CMakeLists.txt @@ -37,6 +37,7 @@ ideal/force_generator.cpp ideal/torque_generator.cpp ideal/angular_velocity_observer.cpp ideal/attitude_observer.cpp +ideal/orbit_observer.cpp real/mission/telescope.cpp diff --git a/src/components/ideal/orbit_observer.cpp b/src/components/ideal/orbit_observer.cpp new file mode 100644 index 000000000..61fe2b002 --- /dev/null +++ b/src/components/ideal/orbit_observer.cpp @@ -0,0 +1,102 @@ +/* + * @file orbit_observer.cpp + * @brief Ideal component which can observe orbit + */ + +#include "orbit_observer.hpp" + +#include +#include + +OrbitObserver::OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame, + const libra::Vector<6> error_standard_deviation, const Orbit& orbit) + : Component(prescaler, clock_generator), noise_frame_(noise_frame), orbit_(orbit) { + for (size_t i = 0; i < 6; i++) { + normal_random_noise_[i].SetParameters(0.0, error_standard_deviation[i], global_randomization.MakeSeed()); + } +} + +void OrbitObserver::MainRoutine(const int time_count) { + UNUSED(time_count); + + // Calc noise + libra::Vector<3> position_error_i_m{0.0}; + libra::Vector<3> position_error_rtn_m{0.0}; + libra::Vector<3> velocity_error_i_m_s{0.0}; + libra::Vector<3> velocity_error_rtn_m_s{0.0}; + libra::Quaternion q_i2rtn = orbit_.CalcQuaternion_i2lvlh(); + switch (noise_frame_) { + case NoiseFrame::kInertial: + for (size_t axis = 0; axis < 3; axis++) { + position_error_i_m[axis] = normal_random_noise_[axis]; + velocity_error_i_m_s[axis] = normal_random_noise_[axis + 3]; + } + break; + case NoiseFrame::kRtn: + for (size_t axis = 0; axis < 3; axis++) { + position_error_rtn_m[axis] = normal_random_noise_[axis]; + velocity_error_rtn_m_s[axis] = normal_random_noise_[axis + 3]; + } + // Frame conversion + position_error_i_m = q_i2rtn.InverseFrameConversion(position_error_rtn_m); + // For zero bias noise, we do not need to care the frame rotation effect. + velocity_error_i_m_s = q_i2rtn.InverseFrameConversion(velocity_error_rtn_m_s); + + break; + default: + break; + } + + // Get observed value + observed_position_i_m_ = orbit_.GetPosition_i_m() + position_error_i_m; + observed_velocity_i_m_s_ = orbit_.GetVelocity_i_m_s() + velocity_error_i_m_s; +} + +std::string OrbitObserver::GetLogHeader() const { + std::string str_tmp = ""; + + std::string head = "orbit_observer_"; + str_tmp += WriteVector(head + "position", "i", "m", 3); + str_tmp += WriteVector(head + "velocity", "i", "m/s", 3); + + return str_tmp; +} + +std::string OrbitObserver::GetLogValue() const { + std::string str_tmp = ""; + + str_tmp += WriteVector(observed_position_i_m_, 16); + str_tmp += WriteVector(observed_velocity_i_m_s_, 16); + + return str_tmp; +} + +NoiseFrame SetNoiseFrame(const std::string noise_frame) { + if (noise_frame == "INERTIAL") { + return NoiseFrame::kInertial; + } else if (noise_frame == "RTN") { + return NoiseFrame::kRtn; + } else { + std::cerr << "[WARNINGS] Orbit observer noise frame is not defined!" << std::endl; + std::cerr << "The noise frame is automatically initialized as INERTIAL" << std::endl; + return NoiseFrame::kInertial; + } +} + +OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit) { + // General + IniAccess ini_file(file_name); + + // CompoBase + int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler"); + if (prescaler <= 1) prescaler = 1; + + // Noise + const NoiseFrame noise_frame = SetNoiseFrame(ini_file.ReadString("ORBIT_OBSERVER", "noise_frame")); + libra::Vector<6> noise_standard_deviation; + ini_file.ReadVector("ORBIT_OBSERVER", "noise_standard_deviation", noise_standard_deviation); + + OrbitObserver orbit_observer(prescaler, clock_generator, noise_frame, noise_standard_deviation, orbit); + + return orbit_observer; +} diff --git a/src/components/ideal/orbit_observer.hpp b/src/components/ideal/orbit_observer.hpp new file mode 100644 index 000000000..490489957 --- /dev/null +++ b/src/components/ideal/orbit_observer.hpp @@ -0,0 +1,108 @@ +/* + * @file orbit_observer.hpp + * @brief Ideal component which can observe orbit + */ + +#ifndef S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_ +#define S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_ + +#include +#include +#include +#include + +#include "../base/component.hpp" + +/** + * @enum NoiseFrame + * @brief Noise definition frame + */ +enum class NoiseFrame { + kInertial, //!< Inertial frame + kRtn, //!< RTN frame +}; + +/* + * @class OrbitObserver + * @brief Ideal component which can observe orbit + */ +class OrbitObserver : public Component, public ILoggable { + public: + /** + * @fn OrbitObserver + * @brief Constructor without power port + * @param [in] prescaler: Frequency scale factor for update + * @param [in] clock_generator: Clock generator + * @param [in] noise_frame: Error frame definition + * @param [in] error_standard_deviation: Position and Velocity standard deviation noise [m, m/s] + * @param [in] orbit: Orbit information + */ + OrbitObserver(const int prescaler, ClockGenerator* clock_generator, const NoiseFrame noise_frame, const libra::Vector<6> error_standard_deviation, + const Orbit& orbit); + + /** + * @fn ~AttitudeObserver + * @brief Destructor + */ + ~OrbitObserver() {} + + // Override functions for Component + /** + * @fn MainRoutine + * @brief Main routine for sensor observation + */ + void MainRoutine(const int time_count) override; + + // Override ILoggable + /** + * @fn GetLogHeader + * @brief Override GetLogHeader function of ILoggable + */ + virtual std::string GetLogHeader() const override; + /** + * @fn GetLogValue + * @brief Override GetLogValue function of ILoggable + */ + virtual std::string GetLogValue() const override; + + /** + * @fn GetPosition_i_m + * @brief Return observed position + */ + inline const libra::Vector<3> GetPosition_i_m() const { return observed_position_i_m_; }; + + /** + * @fn GetVelocity_i_m_s + * @brief Return observed velocity + */ + inline const libra::Vector<3> GetVelocity_i_m_s() const { return observed_velocity_i_m_s_; }; + + protected: + libra::Vector<3> observed_position_i_m_{0.0}; //!< Observed position @ inertial frame [m] + libra::Vector<3> observed_velocity_i_m_s_{0.0}; //!< Observed velocity @ inertial frame [m/s] + + NoiseFrame noise_frame_; //!< Noise definition frame + libra::NormalRand normal_random_noise_[6]; //!< Position and Velocity noise [m, m/s] + + // Observed variables + const Orbit& orbit_; //!< Orbit information +}; + +/** + * @fn SetNoiseFrame + * @brief Set NoiseFrame by string + * @param [in] noise_frame: Noise frame name + * @return noise frame + */ +NoiseFrame SetNoiseFrame(const std::string noise_frame); + +/** + * @fn InitStarSensor + * @brief Initialize functions for StarSensor without power port + * @param [in] clock_generator: Clock generator + * @param [in] file_name: Path to the initialize file + * @param [in] orbit: Orbit information + */ +OrbitObserver InitializeOrbitObserver(ClockGenerator* clock_generator, const std::string file_name, const Orbit& orbit); + +#endif // S2E_COMPONENTS_IDEAL_ORBIT_OBSERVER_HPP_ diff --git a/src/simulation_sample/spacecraft/sample_components.cpp b/src/simulation_sample/spacecraft/sample_components.cpp index d529d040a..ffe4023a5 100644 --- a/src/simulation_sample/spacecraft/sample_components.cpp +++ b/src/simulation_sample/spacecraft/sample_components.cpp @@ -106,6 +106,11 @@ SampleComponents::SampleComponents(const Dynamics* dynamics, Structure* structur configuration_->main_logger_->CopyFileToLogDirectory(file_name); attitude_observer_ = new AttitudeObserver(InitializeAttitudeObserver(clock_generator, file_name, dynamics_->GetAttitude())); + // Orbit Observer + file_name = iniAccess.ReadString("COMPONENT_FILES", "orbit_observer_file"); + configuration_->main_logger_->CopyFileToLogDirectory(file_name); + orbit_observer_ = new OrbitObserver(InitializeOrbitObserver(clock_generator, file_name, dynamics_->GetOrbit())); + // Antenna file_name = iniAccess.ReadString("COMPONENT_FILES", "antenna_file"); configuration_->main_logger_->CopyFileToLogDirectory(file_name); @@ -171,6 +176,7 @@ SampleComponents::~SampleComponents() { delete torque_generator_; delete angular_velocity_observer_; delete attitude_observer_; + delete orbit_observer_; delete antenna_; delete mtq_magnetometer_interference_; // delete change_structure_; @@ -215,4 +221,5 @@ void SampleComponents::LogSetup(Logger& logger) { logger.AddLogList(torque_generator_); logger.AddLogList(angular_velocity_observer_); logger.AddLogList(attitude_observer_); + logger.AddLogList(orbit_observer_); } diff --git a/src/simulation_sample/spacecraft/sample_components.hpp b/src/simulation_sample/spacecraft/sample_components.hpp index 2b7574f27..abc355bef 100644 --- a/src/simulation_sample/spacecraft/sample_components.hpp +++ b/src/simulation_sample/spacecraft/sample_components.hpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -114,6 +115,7 @@ class SampleComponents : public InstalledComponents { TorqueGenerator* torque_generator_; //!< Ideal Torque Generator AngularVelocityObserver* angular_velocity_observer_; //!< Ideal Angular velocity observer AttitudeObserver* attitude_observer_; //!< Ideal Attitude observer + OrbitObserver* orbit_observer_; //!< Ideal Orbit observer // Mission Telescope* telescope_; //!< Telescope