Merge pull request #595 from ut-issl/feature/add-position-observer

Add orbit observer

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

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

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()

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
 

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 <library/initialize/initialize_file_access.hpp>
+#include <library/randomization/global_randomization.hpp>
+
+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;
+}

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 <dynamics/orbit/orbit.hpp>
+#include <library/logger/loggable.hpp>
+#include <library/math/vector.hpp>
+#include <library/randomization/normal_randomization.hpp>
+
+#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_