Merge pull request #540 from ut-issl/feature/add-ideal-attitude-sensor

Add ideal attitude observers

data/sample/initialize_files/components/angular_velocity_observer.ini

@@ -0,0 +1,27 @@
+[COMPONENT_BASE]
+prescaler = 1
+
+[SENSOR_BASE_ANGULAR_VELOCITY_OBSERVER]
+// Constant bias noise at component frame [rad/s]
+constant_bias_c_rad_s(0) = 0.0
+constant_bias_c_rad_s(1) = 0.0
+constant_bias_c_rad_s(2) = 0.0
+
+// Standard deviation for random walk noise[rad/s]
+random_walk_standard_deviation_c_rad_s(0) = 0.0
+random_walk_standard_deviation_c_rad_s(1) = 0.0
+random_walk_standard_deviation_c_rad_s(2) = 0.0
+
+// Limit of random walk noise[rad/s]
+random_walk_limit_c_rad_s(0) = 0.0
+random_walk_limit_c_rad_s(1) = 0.0
+random_walk_limit_c_rad_s(2) = 0.0
+
+// Standard deviation of normal random noise[rad/s]
+normal_random_standard_deviation_c_rad_s(0) = 1e-3
+normal_random_standard_deviation_c_rad_s(1) = 1e-3
+normal_random_standard_deviation_c_rad_s(2) = 1e-3
+
+// Range [rad/s]
+range_to_constant_rad_s = 5.0  // smaller than range_to_zero
+range_to_zero_rad_s = 10.0

data/sample/initialize_files/components/attitude_observer.ini

@@ -0,0 +1,7 @@
+[ATTITUDE_OBSERVER]
+// Standard deviation of force direction error [deg]
+error_angle_standard_deviation_deg = 1 
+
+[COMPONENT_BASE]
+// Prescaler with respect to the component update period
+prescaler = 1

data/sample/initialize_files/sample_satellite.ini

@@ -144,6 +144,8 @@ rw_file = INI_FILE_DIR_FROM_EXE/components/reaction_wheel.ini
 thruster_file = INI_FILE_DIR_FROM_EXE/components/thruster.ini
 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
 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_angular_velocity_observer.py

@@ -0,0 +1,94 @@
+#
+# Plot Angular Velocity Observer
+#
+# arg[1] : read_file_tag : time tag for default CSV output log file. ex. 220627_142946
+#
+
+#
+# Import
+#
+# plots
+import matplotlib.pyplot as plt
+# 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_angular_velocity_b_rad_s = read_3d_vector_from_csv(read_file_name, 'angular_velocity_observer_measured_value_b', 'rad/s')
+true_angular_velocity_b_rad_s = read_3d_vector_from_csv(read_file_name, 'spacecraft_angular_velocity_b', 'rad/s')
+
+# Statistics
+# We assume that the component frame and the body frame is same
+error_rad_s = measured_angular_velocity_b_rad_s - true_angular_velocity_b_rad_s
+average = [0.0, 0.0, 0.0]
+standard_deviation = [0.0, 0.0, 0.0]
+for i in range(3):
+  average[i] = error_rad_s[i].mean()
+  standard_deviation[i] = error_rad_s[i].std()
+
+#
+# Plot
+#
+unit = ' rad/s'
+
+fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True)
+axis[0, 0].plot(time[0], measured_angular_velocity_b_rad_s[0], marker=".", c="red",    label="MEASURED-X")
+axis[0, 0].plot(time[0], true_angular_velocity_b_rad_s[0], marker=".", c="orange",  label="TRUE-X")
+axis[0, 0].legend(loc = 'upper right')
+axis[0, 0].text(0.01, 0.99, "Error average:" + format(average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes)
+axis[0, 0].text(0.01, 0.89, "Standard deviation:" + format(standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes)
+
+axis[1, 0].plot(time[0], measured_angular_velocity_b_rad_s[1], marker=".", c="green",  label="MEASURED-Y")
+axis[1, 0].plot(time[0], true_angular_velocity_b_rad_s[1], marker=".", c="yellow",  label="TRUE-Y")
+axis[1, 0].legend(loc = 'upper right')
+axis[1, 0].text(0.01, 0.99, "Error average:" + format(average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes)
+axis[1, 0].text(0.01, 0.89, "Standard deviation:" + format(standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes)
+
+axis[2, 0].plot(time[0], measured_angular_velocity_b_rad_s[2], marker=".", c="blue",   label="MEASURED-Z")
+axis[2, 0].plot(time[0], true_angular_velocity_b_rad_s[2], marker=".", c="purple",  label="TRUE-Z")
+axis[2, 0].legend(loc = 'upper right')
+axis[2, 0].text(0.01, 0.99, "Error average:" + format(average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes)
+axis[2, 0].text(0.01, 0.89, "Standard deviation:" + format(standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes)
+
+fig.suptitle("Angular Velocity Observer")
+fig.supylabel("Angular Velocity" + unit)
+fig.supxlabel("Time [s]")
+
+# Data save
+if args.no_gui:
+  plt.savefig(read_file_tag + "_angular_velocity_observer.png") # save last figure only
+else:
+  plt.show()

scripts/Plot/plot_attitude_observer.py

@@ -0,0 +1,95 @@
+#
+# Plot Attitude Observer
+#
+# arg[1] : read_file_tag : time tag for default CSV output log file. ex. 220627_142946
+#
+
+#
+# Import
+#
+# plots
+import matplotlib.pyplot as plt
+# local function
+from common import find_latest_log_tag
+from common import add_log_file_arguments
+from common import read_quaternion_from_csv
+from common import read_scalar_from_csv
+from common import calc_error_angle_from_quaternions
+# 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_quaternion_i2b = read_quaternion_from_csv(read_file_name, 'attitude_observer_quaternion_i2b')
+true_quaternion_i2b = read_quaternion_from_csv(read_file_name, 'spacecraft_quaternion_i2b')
+
+# Statistics
+error_angle_rad = calc_error_angle_from_quaternions(measured_quaternion_i2b, true_quaternion_i2b)
+error_average = error_angle_rad.mean()
+standard_deviation = error_angle_rad.std()
+
+#
+# Plot
+#
+
+fig, axis = plt.subplots(4, 1, squeeze = False, tight_layout = True, sharex = True)
+axis[0, 0].plot(time[0], measured_quaternion_i2b[0], marker=".", c="red",    label="MEASURED-X")
+axis[0, 0].plot(time[0], true_quaternion_i2b[0], marker=".", c="orange",  label="TRUE-X")
+axis[0, 0].legend(loc = 'upper right')
+
+axis[1, 0].plot(time[0], measured_quaternion_i2b[1], marker=".", c="green",    label="MEASURED-Y")
+axis[1, 0].plot(time[0], true_quaternion_i2b[1], marker=".", c="yellow",  label="TRUE-Y")
+axis[1, 0].legend(loc = 'upper right')
+
+axis[2, 0].plot(time[0], measured_quaternion_i2b[2], marker=".", c="blue",    label="MEASURED-Z")
+axis[2, 0].plot(time[0], true_quaternion_i2b[2], marker=".", c="purple",  label="TRUE-Z")
+axis[2, 0].legend(loc = 'upper right')
+
+axis[3, 0].plot(time[0], measured_quaternion_i2b[3], marker=".", c="black",    label="MEASURED-W")
+axis[3, 0].plot(time[0], true_quaternion_i2b[3], marker=".", c="gray",  label="TRUE-W")
+axis[3, 0].legend(loc = 'upper right')
+
+fig.suptitle("Attitude Observer Quaternion")
+fig.supylabel("Quaternion")
+fig.supxlabel("Time [s]")
+
+unit = 'rad'
+plt.figure(0)
+plt.plot(time[0], error_angle_rad, marker=".", c="red")
+plt.title("Error angle \n" + "Error average:" + format(error_average, '+.2e') + unit + "\n Standard deviation:" + format(standard_deviation, '+.2e') + unit)
+plt.xlabel("Time [s]")
+plt.ylabel("Angle [rad]")
+
+# Data save
+if args.no_gui:
+  plt.savefig(read_file_tag + "_attitude_observer.png") # save last figure only
+else:
+  plt.show()

src/components/CMakeLists.txt

@@ -35,6 +35,8 @@ examples/example_i2c_target_for_hils.cpp
 
 ideal/force_generator.cpp
 ideal/torque_generator.cpp
+ideal/angular_velocity_observer.cpp
+ideal/attitude_observer.cpp
 
 real/mission/telescope.cpp
 

src/components/base/sensor_template_functions.hpp

@@ -83,9 +83,13 @@ Sensor<N> ReadSensorInformation(const std::string file_name, const double step_w
   libra::Vector<N * N> scale_factor_vector;
   ini_file.ReadVector(section.c_str(), "scale_factor_c", scale_factor_vector);
   libra::Matrix<N, N> scale_factor_c;
-  for (size_t i = 0; i < N; i++) {
-    for (size_t j = 0; j < N; j++) {
-      scale_factor_c[i][j] = scale_factor_vector[i * N + j];
+  if (scale_factor_vector.CalcNorm() == 0.0) {
+    scale_factor_c = libra::MakeIdentityMatrix<N>();
+  } else {
+    for (size_t i = 0; i < N; i++) {
+      for (size_t j = 0; j < N; j++) {
+        scale_factor_c[i][j] = scale_factor_vector[i * N + j];
+      }
     }
   }
 

src/components/ideal/angular_velocity_observer.cpp

@@ -0,0 +1,48 @@
+/*
+ * @file angular_velocity_observer.cpp
+ * @brief Ideal component which can observe angular velocity
+ */
+
+#include "angular_velocity_observer.hpp"
+
+#include <library/initialize/initialize_file_access.hpp>
+
+AngularVelocityObserver::AngularVelocityObserver(const int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const Attitude& attitude)
+    : Component(prescaler, clock_generator), Sensor(sensor_base), attitude_(attitude) {}
+
+void AngularVelocityObserver::MainRoutine(const int time_count) {
+  UNUSED(time_count);
+  angular_velocity_b_rad_s_ = Measure(attitude_.GetAngularVelocity_b_rad_s());
+}
+
+std::string AngularVelocityObserver::GetLogHeader() const {
+  std::string str_tmp = "";
+
+  std::string sensor_name = "angular_velocity_observer_";
+  str_tmp += WriteVector(sensor_name + "measured_value", "b", "rad/s", 3);
+
+  return str_tmp;
+}
+
+std::string AngularVelocityObserver::GetLogValue() const {
+  std::string str_tmp = "";
+
+  str_tmp += WriteVector(angular_velocity_b_rad_s_);
+
+  return str_tmp;
+}
+
+AngularVelocityObserver InitializeAngularVelocityObserver(ClockGenerator* clock_generator, const std::string file_name, double component_step_time_s,
+                                                          const Attitude& attitude) {
+  IniAccess ini_file(file_name);
+
+  int prescaler = ini_file.ReadInt("COMPONENT_BASE", "prescaler");
+  if (prescaler <= 1) prescaler = 1;
+
+  // Sensor
+  Sensor<3> sensor_base = ReadSensorInformation<3>(file_name, component_step_time_s * (double)(prescaler), "ANGULAR_VELOCITY_OBSERVER", "rad_s");
+
+  AngularVelocityObserver observer(prescaler, clock_generator, sensor_base, attitude);
+
+  return observer;
+}

src/components/ideal/angular_velocity_observer.hpp

@@ -0,0 +1,78 @@
+/*
+ * @file angular_velocity_observer.hpp
+ * @brief Ideal component which can observe angular velocity
+ */
+
+#ifndef S2E_COMPONENTS_IDEAL_ANGULAR_VELOCITY_OBSERVER_HPP_
+#define S2E_COMPONENTS_IDEAL_ANGULAR_VELOCITY_OBSERVER_HPP_
+
+#include <dynamics/dynamics.hpp>
+#include <library/logger/loggable.hpp>
+
+#include "../base/component.hpp"
+#include "../base/sensor.hpp"
+
+/*
+ * @class AngularVelocityObserver
+ * @brief Ideal component which can observe angular velocity
+ */
+class AngularVelocityObserver : public Component, public Sensor<3>, public ILoggable {
+ public:
+  /**
+   * @fn AngularVelocityObserver
+   * @brief Constructor
+   * @param [in] prescaler: Frequency scale factor for update
+   * @param [in] clock_generator: Clock generator
+   * @param [in] sensor_base: Sensor base information
+   * @param [in] dynamics: Dynamics information
+   */
+  AngularVelocityObserver(const int prescaler, ClockGenerator* clock_generator, Sensor& sensor_base, const Attitude& attitude);
+  /**
+   * @fn ~AngularVelocityObserver
+   * @brief Destructor
+   */
+  ~AngularVelocityObserver() {}
+
+  // Override functions for Component
+  /**
+   * @fn MainRoutine
+   * @brief Main routine to calculate force generation
+   */
+  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;
+
+  // Getter
+  /**
+   * @fn GetAngularVelocity_b_rad_s
+   * @brief Return observed angular velocity
+   */
+  inline libra::Vector<3> GetAngularVelocity_b_rad_s() const { return angular_velocity_b_rad_s_; }
+
+ protected:
+  libra::Vector<3> angular_velocity_b_rad_s_{0.0};  //!< Observed angular velocity [rad/s]
+  const Attitude& attitude_;                        //!< Dynamics information
+};
+
+/**
+ * @fn InitializeAngularVelocityObserver
+ * @brief Initialize function for AngularVelocityObserver
+ * @param [in] clock_generator: Clock generator
+ * @param [in] file_name: Path to the initialize file
+ * @param [in] component_step_time_s: Component step time [sec]
+ * @param [in] dynamics: Dynamics information
+ */
+AngularVelocityObserver InitializeAngularVelocityObserver(ClockGenerator* clock_generator, const std::string file_name, double component_step_time_s,
+                                                          const Attitude& attitude);
+
+#endif  // S2E_COMPONENTS_IDEAL_ANGULAR_VELOCITY_OBSERVER_HPP_