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_