fix way of adding noises

s2e-ff/data/initialize_files/components/relative_attitude_sensor.ini

@@ -8,39 +8,5 @@ target_sat_id = 1
 // When this value is negative, reference_sat_id is automatically set as the mounting satellite ID
 reference_sat_id = -1
 
-[SENSOR_BASE_RELATIVE_ATTITUDE_SENSOR]
-// 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 [rad]
-constant_bias_c_rad(0) = 0.0
-constant_bias_c_rad(1) = 0.0
-constant_bias_c_rad(2) = 0.0
-
-// Standard deviation of normal random noise [rad]
-normal_random_standard_deviation_c_rad(0) = 0.0
-normal_random_standard_deviation_c_rad(1) = 0.0
-normal_random_standard_deviation_c_rad(2) = 0.0
-
-// Standard deviation for random walk noise [rad]
-random_walk_standard_deviation_c_rad(0) = 0.0
-random_walk_standard_deviation_c_rad(1) = 0.0
-random_walk_standard_deviation_c_rad(2) = 0.0
-
-// Limit of random walk noise [rad]
-random_walk_limit_c_rad(0) = 0.0
-random_walk_limit_c_rad(1) = 0.0
-random_walk_limit_c_rad(2) = 0.0
-
-// Range [rad]
-range_to_constant_rad = 6.283186  // smaller than range_to_zero_rad
-range_to_zero_rad = 7.0
+// Standard deviation of force direction error [deg]
+error_angle_standard_deviation_deg = 1.0

s2e-ff/src/components/aocs/relative_attitude_sensor.cpp

@@ -9,19 +9,34 @@
 #include <library/initialize/initialize_file_access.hpp>
 
 RelativeAttitudeSensor::RelativeAttitudeSensor(const int prescaler, ClockGenerator* clock_gen, Sensor& sensor_base, const int target_sat_id,
-                                               const int reference_sat_id, const RelativeInformation& rel_info)
-    : Component(prescaler, clock_gen), Sensor(sensor_base), target_sat_id_(target_sat_id), reference_sat_id_(reference_sat_id), rel_info_(rel_info) {}
+                                               const int reference_sat_id, const RelativeInformation& rel_info, const double standard_deviation_rad)
+    : Component(prescaler, clock_gen),
+      Sensor(sensor_base),
+      target_sat_id_(target_sat_id),
+      reference_sat_id_(reference_sat_id),
+      rel_info_(rel_info),
+      angle_noise_(0.0, standard_deviation_rad) {
+  direction_noise_.SetParameters(0.0, 1.0);
+}
 
 RelativeAttitudeSensor::~RelativeAttitudeSensor() {}
 
 void RelativeAttitudeSensor::MainRoutine(int count) {
   UNUSED(count);
+  // Error calculation
+  libra::Vector<3> random_direction;
+  random_direction[0] = direction_noise_;
+  random_direction[1] = direction_noise_;
+  random_direction[2] = direction_noise_;
+  random_direction = random_direction.CalcNormalizedVector();
+
+  double error_angle_rad = angle_noise_;
+  libra::Quaternion error_quaternion(random_direction, error_angle_rad);
 
   // Get true value
   measured_target_attitude_rb2tb_quaternion_ = rel_info_.GetRelativeAttitudeQuaternion(target_sat_id_, reference_sat_id_);
+  measured_target_attitude_rb2tb_quaternion_ = error_quaternion * measured_target_attitude_rb2tb_quaternion_;
   measured_target_attitude_rb2tb_rad_ = measured_target_attitude_rb2tb_quaternion_.ConvertToEuler();
-  measured_target_attitude_rb2tb_rad_ = Measure(measured_target_attitude_rb2tb_rad_);
-  measured_target_attitude_rb2tb_quaternion_ = libra::Quaternion::ConvertFromEuler(measured_target_attitude_rb2tb_rad_);
 }
 
 std::string RelativeAttitudeSensor::GetLogHeader() const {
@@ -54,6 +69,9 @@ RelativeAttitudeSensor InitializeRelativeAttitudeSensor(ClockGenerator* clock_ge
   int prescaler = ini_file.ReadInt(section, "prescaler");
   if (prescaler <= 1) prescaler = 1;
 
+  double error_angle_standard_deviation_deg = ini_file.ReadDouble(section, "error_angle_standard_deviation_deg");
+  double error_angle_standard_deviation_rad = libra::deg_to_rad * error_angle_standard_deviation_deg;
+
   // RelativeAttitudeSensor
   int target_sat_id = ini_file.ReadInt(section, "target_sat_id");
   int reference_sat_id = ini_file.ReadInt(section, "reference_sat_id");
@@ -64,7 +82,8 @@ RelativeAttitudeSensor InitializeRelativeAttitudeSensor(ClockGenerator* clock_ge
   // SensorBase
   Sensor<3> sensor_base = ReadSensorInformation<3>(file_name, compo_step_time_s * (double)(prescaler), section, "rad");
 
-  RelativeAttitudeSensor relative_attitude_sensor(prescaler, clock_gen, sensor_base, target_sat_id, reference_sat_id, rel_info);
+  RelativeAttitudeSensor relative_attitude_sensor(prescaler, clock_gen, sensor_base, target_sat_id, reference_sat_id, rel_info,
+                                                  error_angle_standard_deviation_rad);
 
   return relative_attitude_sensor;
 }

s2e-ff/src/components/aocs/relative_attitude_sensor.hpp

@@ -22,7 +22,7 @@ class RelativeAttitudeSensor : public Component, public Sensor<3>, public ILogga
    * @brief Constructor
    */
   RelativeAttitudeSensor(const int prescaler, ClockGenerator* clock_gen, Sensor& sensor_base, const int target_sat_id, const int reference_sat_id,
-                         const RelativeInformation& rel_info);
+                         const RelativeInformation& rel_info, const double standard_deviation_rad);
   /**
    * @fn ~RelativeAttitudeSensor
    * @brief Destructor
@@ -60,10 +60,14 @@ class RelativeAttitudeSensor : public Component, public Sensor<3>, public ILogga
   const int reference_sat_id_;  //!< Reference satellite ID
 
   // Measured value
-  libra::Quaternion measured_target_attitude_rb2tb_quaternion_;  //!< Measured quaternion of target body from reference body
+  libra::Quaternion measured_target_attitude_rb2tb_quaternion_ = {0.0, 0.0, 0.0, 1.0};  //!< Measured quaternion of target body from reference body
   libra::Vector<3> measured_target_attitude_rb2tb_rad_{
       0.0};  //!< Measured attitude of target body in BODY frame [rad], 3-2-1 Euler angle (1: roll, 2: pitch, 3: yaw order)
 
+  // Noises
+  libra::NormalRand angle_noise_;      //!< Normal random for magnitude noise
+  libra::NormalRand direction_noise_;  //!< Normal random for direction noise
+
   // References
   const RelativeInformation& rel_info_;  //!< Relative information
 };