Merge pull request #20 from ut-issl/feature/add-relative-position-sensor

Add relative position sensor

s2e-ff/CMakeLists.txt

@@ -82,6 +82,8 @@ set(SOURCE_FILES
   src/S2eFf.cpp
   src/Components/AOCS/RelativeDistanceSensor.cpp
   src/Components/AOCS/InitializeRelativeDistanceSensor.cpp
+  src/Components/AOCS/RelativePositionSensor.cpp
+  src/Components/AOCS/InitializeRelativePositionSensor.cpp
   src/Components/IdealComponents/ForceGenerator.cpp
   src/Components/IdealComponents/InitializeForceGenerator.cpp
   src/Simulation/Case/FfCase.cpp

s2e-ff/data/ini/Components/RelativePositionSensor.ini

@@ -0,0 +1,51 @@
+[RelativePositionSensor]
+// Target satellite ID
+target_sat_id = 1
+
+// Normally, reference_sat_id is set the id of mounted satellite
+reference_sat_id = 0
+
+// Users can choose the frame for error settings
+// INERTIAL, RTN, or BODY
+error_frame = BODY
+
+[ComponentBase]
+// Prescaler with respect to the component update period
+prescaler = 1
+
+[SensorBase]
+// The coordinate of the error is selected by the error_frame
+// Scale factor [-]
+scale_factor_c(0) = 1;
+scale_factor_c(1) = 0;
+scale_factor_c(2) = 0;
+scale_factor_c(3) = 0;
+scale_factor_c(4) = 1;
+scale_factor_c(5) = 0;
+scale_factor_c(6) = 0;
+scale_factor_c(7) = 0;
+scale_factor_c(8) = 1;
+
+// Constant bias noise [m]
+constant_bias_c(0) = 10.0
+constant_bias_c(1) = 10.0
+constant_bias_c(2) = 10.0
+
+// Standard deviation of normal random noise [m]
+normal_random_standard_deviation_c(0) = 1.0
+normal_random_standard_deviation_c(1) = 1.0
+normal_random_standard_deviation_c(2) = 1.0
+
+// Standard deviation for random walk noise [m]
+random_walk_standard_deviation_c(0) = 0.0
+random_walk_standard_deviation_c(1) = 0.0
+random_walk_standard_deviation_c(2) = 0.0
+
+// Limit of random walk noise [m]
+random_walk_limit_c(0) = 0.0
+random_walk_limit_c(1) = 0.0
+random_walk_limit_c(2) = 0.0
+
+// Range [m]
+range_to_const = 1000000.0  // smaller than range_to_zero_m
+range_to_zero = 10000000.0

s2e-ff/data/ini/FfSat.ini

@@ -139,4 +139,5 @@ structure_file = ../../data/ini/FfSatStructure.ini
 [COMPONENTS_FILE]
 // Users can add the path for component initialize files here.
 relative_distance_sensor_file = ../../data/ini/Components/RelativeDistanceSensor.ini
+relative_position_sensor_file = ../../data/ini/Components/RelativePositionSensor.ini
 force_generator_file = ../../data/ini/Components/ForceGenerator.ini

s2e-ff/src/Components/AOCS/InitializeRelativePositionSensor.cpp

@@ -0,0 +1,40 @@
+#include "InitializeRelativePositionSensor.hpp"
+
+#include <Interface/InitInput/IniAccess.h>
+
+#include "../Abstract/InitializeSensorBase.hpp"
+
+RelativePositionSensor InitializeRelativePositionSensor(ClockGenerator* clock_gen, const std::string file_name, const double compo_step_time_s,
+                                                        const RelativeInformation& rel_info, const Dynamics& dynamics) {
+  // General
+  IniAccess ini_file(file_name);
+
+  // CompoBase
+  int prescaler = ini_file.ReadInt("ComponentBase", "prescaler");
+  if (prescaler <= 1) prescaler = 1;
+
+  // SensorBase
+  SensorBase<3> sensor_base = ReadSensorBaseInformation<3>(file_name, compo_step_time_s * (double)(prescaler));
+
+  // RelativePositionSensor
+  char section[30] = "RelativePositionSensor";
+  int target_sat_id = ini_file.ReadInt(section, "target_sat_id");
+  int reference_sat_id = ini_file.ReadInt(section, "reference_sat_id");
+  std::string error_frame_string = ini_file.ReadString(section, "error_frame");
+  RelativePositionSensorErrorFrame error_frame;
+
+  if (error_frame_string == "INERTIAL") {
+    error_frame = RelativePositionSensorErrorFrame::INERTIAL;
+  } else if (error_frame_string == "RTN") {
+    error_frame = RelativePositionSensorErrorFrame::RTN;
+  } else if (error_frame_string == "BODY") {
+    error_frame = RelativePositionSensorErrorFrame::BODY;
+  } else {
+    std::cerr << "Warnings: InitializeRelativePositionSensor: The error frame setting was failed. It is automatically set as BODY frame." << std::endl;
+    error_frame = RelativePositionSensorErrorFrame::BODY;
+  }
+
+  RelativePositionSensor relative_position_sensor(prescaler, clock_gen, sensor_base, target_sat_id, reference_sat_id, error_frame, rel_info, dynamics);
+
+  return relative_position_sensor;
+}

s2e-ff/src/Components/AOCS/InitializeRelativePositionSensor.hpp

@@ -0,0 +1,6 @@
+#pragma once
+
+#include "RelativePositionSensor.hpp"
+
+RelativePositionSensor InitializeRelativePositionSensor(ClockGenerator* clock_gen, const std::string file_name, const double compo_step_time_s,
+                                                        const RelativeInformation& rel_info, const Dynamics& dynamics);

s2e-ff/src/Components/AOCS/RelativePositionSensor.cpp

@@ -0,0 +1,70 @@
+#include "RelativePositionSensor.hpp"
+
+RelativePositionSensor::RelativePositionSensor(const int prescaler, ClockGenerator* clock_gen, SensorBase& sensor_base, const int target_sat_id,
+                                               const int reference_sat_id, const RelativePositionSensorErrorFrame error_frame, 
+                                               const RelativeInformation& rel_info, const Dynamics& dynamics)
+    : ComponentBase(prescaler, clock_gen),
+      SensorBase(sensor_base),
+      target_sat_id_(target_sat_id),
+      reference_sat_id_(reference_sat_id),
+      error_frame_(error_frame),
+      rel_info_(rel_info),
+      dynamics_(dynamics) {}
+
+RelativePositionSensor::~RelativePositionSensor() {}
+
+void RelativePositionSensor::MainRoutine(int count) {
+  UNUSED(count);
+
+  // Get true value
+  measured_target_position_i_m_ = rel_info_.GetRelativePosition_i_m(target_sat_id_, reference_sat_id_);
+  measured_target_position_rtn_m_ = rel_info_.GetRelativePosition_rtn_m(target_sat_id_, reference_sat_id_);
+  libra::Quaternion q_i2b = dynamics_.GetAttitude().GetQuaternion_i2b();
+  measured_target_position_body_m_ = q_i2b.frame_conv(measured_target_position_i_m_);
+
+  // Add noise at body frame and frame conversion
+  libra::Quaternion q_i2rtn = dynamics_.GetOrbit().CalcQuaternionI2LVLH();
+  switch (error_frame_)
+  {
+  case RelativePositionSensorErrorFrame::INERTIAL :
+    measured_target_position_i_m_ = Measure(measured_target_position_i_m_);
+    // Frame conversion
+    measured_target_position_rtn_m_ = q_i2rtn.frame_conv(measured_target_position_i_m_);
+    measured_target_position_body_m_ = q_i2b.frame_conv(measured_target_position_i_m_);
+    break;
+  case RelativePositionSensorErrorFrame::RTN :
+    measured_target_position_rtn_m_ = Measure(measured_target_position_rtn_m_);
+    // Frame conversion
+    measured_target_position_i_m_ = q_i2rtn.frame_conv_inv(measured_target_position_rtn_m_);
+    measured_target_position_body_m_ = q_i2b.frame_conv(measured_target_position_i_m_);
+    break;
+  case RelativePositionSensorErrorFrame::BODY :
+    measured_target_position_body_m_ = Measure(measured_target_position_body_m_);
+    // Frame conversion
+    measured_target_position_i_m_ = q_i2b.frame_conv_inv(measured_target_position_body_m_);
+    measured_target_position_rtn_m_ = q_i2rtn.frame_conv(measured_target_position_i_m_);
+    break;  
+  default:
+    break;
+  }
+}
+
+std::string RelativePositionSensor::GetLogHeader() const {
+  std::string str_tmp = "";
+  std::string head = "RelativePositionSensor_";
+  str_tmp += WriteVector(head + "position", "i", "m", 3);
+  str_tmp += WriteVector(head + "position", "rtn", "m", 3);
+  str_tmp += WriteVector(head + "position", "b", "m", 3);
+
+  return str_tmp;
+}
+
+std::string RelativePositionSensor::GetLogValue() const {
+  std::string str_tmp = "";
+
+  str_tmp += WriteVector(measured_target_position_i_m_);
+  str_tmp += WriteVector(measured_target_position_rtn_m_);
+  str_tmp += WriteVector(measured_target_position_body_m_);
+
+  return str_tmp;
+}

s2e-ff/src/Components/AOCS/RelativePositionSensor.hpp

@@ -0,0 +1,48 @@
+#ifndef RELATIVE_POSITION_SENSOR_H_
+#define RELATIVE_POSITION_SENSOR_H_
+
+#include <Component/Abstract/ComponentBase.h>
+#include <Component/Abstract/SensorBase.h>
+#include <Interface/LogOutput/ILoggable.h>
+#include <RelativeInformation/RelativeInformation.h>
+
+enum class RelativePositionSensorErrorFrame {
+  INERTIAL,
+  RTN,
+  BODY
+};
+
+class RelativePositionSensor : public ComponentBase, public SensorBase<3>, public ILoggable {
+ public:
+  RelativePositionSensor(const int prescaler, ClockGenerator* clock_gen, SensorBase& sensor_base, const int target_sat_id, const int reference_sat_id,
+                         const RelativePositionSensorErrorFrame error_frame, const RelativeInformation& rel_info, const Dynamics& dynamics);
+  ~RelativePositionSensor();
+  // ComponentBase
+  void MainRoutine(int count) override;
+
+  // ILoggable
+  virtual std::string GetLogHeader() const;
+  virtual std::string GetLogValue() const;
+
+  // Getter
+  inline libra::Vector<3> GetMeasuredTargetPosition_i_m() const { return measured_target_position_i_m_; }
+  inline libra::Vector<3> GetMeasuredTargetPosition_rtn_m() const { return measured_target_position_rtn_m_; }
+  inline libra::Vector<3> GetMeasuredTargetPosition_b_m() const { return measured_target_position_body_m_; }
+
+  // Setter
+  void SetTargetSatId(const int target_sat_id) { target_sat_id_ = target_sat_id; }
+
+ protected:
+  int target_sat_id_;
+  const int reference_sat_id_;
+  RelativePositionSensorErrorFrame error_frame_;
+
+  libra::Vector<3> measured_target_position_i_m_{0.0};
+  libra::Vector<3> measured_target_position_rtn_m_{0.0};
+  libra::Vector<3> measured_target_position_body_m_{0.0};
+
+  const RelativeInformation& rel_info_;
+  const Dynamics& dynamics_;
+};
+
+#endif

s2e-ff/src/Simulation/Spacecraft/FfComponents.cpp

@@ -3,6 +3,7 @@
 #include <Interface/InitInput/IniAccess.h>
 
 #include "../../Components/AOCS/InitializeRelativeDistanceSensor.hpp"
+#include "../../Components/AOCS/InitializeRelativePositionSensor.hpp"
 #include "../../Components/IdealComponents/InitializeForceGenerator.hpp"
 
 FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure, const LocalEnvironment* local_env, const GlobalEnvironment* glo_env,
@@ -18,6 +19,10 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure,
   const std::string rel_dist_file = sat_file.ReadString("COMPONENTS_FILE", "relative_distance_sensor_file");
   relative_distance_sensor_ = new RelativeDistanceSensor(InitializeRelativeDistanceSensor(clock_gen, rel_dist_file, compo_step_sec, *rel_info_));
 
+  const std::string rel_pos_file = sat_file.ReadString("COMPONENTS_FILE", "relative_position_sensor_file");
+  relative_position_sensor_ =
+      new RelativePositionSensor(InitializeRelativePositionSensor(clock_gen, rel_pos_file, compo_step_sec, *rel_info_, *dynamics_));
+
   const std::string force_generator_file = sat_file.ReadString("COMPONENTS_FILE", "force_generator_file");
   force_generator_ = new ForceGenerator(InitializeForceGenerator(clock_gen, force_generator_file, dynamics_));
 
@@ -31,6 +36,7 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure,
 
 FfComponents::~FfComponents() {
   delete relative_distance_sensor_;
+  delete relative_position_sensor_;
   delete force_generator_;
   // OBC must be deleted the last since it has com ports
   delete obc_;
@@ -50,5 +56,6 @@ Vector<3> FfComponents::GenerateTorque_Nm_b() {
 
 void FfComponents::LogSetup(Logger& logger) {
   logger.AddLoggable(relative_distance_sensor_);
+  logger.AddLoggable(relative_position_sensor_);
   logger.AddLoggable(force_generator_);
 }

s2e-ff/src/Simulation/Spacecraft/FfComponents.hpp

@@ -9,6 +9,7 @@
 
 // include for components
 #include "../../Components/AOCS/RelativeDistanceSensor.hpp"
+#include "../../Components/AOCS/RelativePositionSensor.hpp"
 #include "../../Components/IdealComponents/ForceGenerator.hpp"
 #include "OBC.h"
 
@@ -25,6 +26,7 @@ class FfComponents : public InstalledComponents {
   // Components
   OBC* obc_;
   RelativeDistanceSensor* relative_distance_sensor_;
+  RelativePositionSensor* relative_position_sensor_;
   ForceGenerator* force_generator_;
 
   // References