add noise

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

@@ -1,12 +1,21 @@
 [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;

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

@@ -20,8 +20,21 @@ RelativePositionSensor InitializeRelativePositionSensor(ClockGenerator* clock_ge
   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");
-
-  RelativePositionSensor relative_position_sensor(prescaler, clock_gen, sensor_base, target_sat_id, reference_sat_id, rel_info, dynamics);
+  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/RelativePositionSensor.cpp

@@ -1,11 +1,13 @@
 #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 RelativeInformation& rel_info, const Dynamics& dynamics)
+                                               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) {}
 
@@ -14,11 +16,37 @@ 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 {

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

@@ -6,10 +6,16 @@
 #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 RelativeInformation& rel_info, const Dynamics& dynamics);
+                         const RelativePositionSensorErrorFrame error_frame, const RelativeInformation& rel_info, const Dynamics& dynamics);
   ~RelativePositionSensor();
   // ComponentBase
   void MainRoutine(int count) override;
@@ -29,6 +35,7 @@ class RelativePositionSensor : public ComponentBase, public SensorBase<3>, publi
  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};