Add noise characteristics of the qpd sensor

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

@@ -1,3 +1,46 @@
+[SENSOR_BASE_QPD_POSITIONING_SENSOR]
+////////////////////////////////////////////////////////////////////////////////////////////////
+// Generally, the noise characteristics of the QPD sensor output values change according to the
+// line-of-sight direction and position displacement in both the y-axis and z-axis direction.
+// Therefore, this section only represents base noise characteristics.
+// Detailed noise characteristics are defined in each section of the QPD positioning sensor.
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+// 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 [V]
+constant_bias_c_V(0) = 0.0
+constant_bias_c_V(1) = 0.0
+constant_bias_c_V(2) = 0.0
+
+// Standard deviation of normal random noise [V]
+normal_random_standard_deviation_c_V(0) = 1.0
+normal_random_standard_deviation_c_V(1) = 1.0
+normal_random_standard_deviation_c_V(2) = 1.0
+
+// Standard deviation for random walk noise [V]
+random_walk_standard_deviation_c_V(0) = 0.0
+random_walk_standard_deviation_c_V(1) = 0.0
+random_walk_standard_deviation_c_V(2) = 0.0
+
+// Limit of random walk noise [V]
+random_walk_limit_c_V(0) = 0.0
+random_walk_limit_c_V(1) = 0.0
+random_walk_limit_c_V(2) = 0.0
+
+// Range [m]
+range_to_constant_V = 10.0  // smaller than range_to_zero_V
+range_to_zero_V = 10.0
+
 [QPD_POSITIONING_SENSOR_0]
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // This quadrant photodiode sensor is a sensor that observes position displacement on 
@@ -6,6 +49,8 @@
 // Detailed information about the qpd positioning sensor is descrived in qpd_positioning_sensor.hpp file.
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
+prescaler = 1
+
 quaternion_b2c(0) = 0.0
 quaternion_b2c(1) = 0.0
 quaternion_b2c(2) = 1.0
@@ -35,3 +80,8 @@ qpd_laser_receivable_angle_rad = 0.785
 // Voltage threshold ot the quadrant photodiode sensor [V]
 // Less than this value, the QPD positioning sensor cannot determine the position displacement.
 qpd_sensor_output_voltage_threshold_V = 0.09
+
+// The standard deviations of the QPD sensor output change according to the line-of-sight distance.
+// The following coefficients are required to model the standard deviations of the QPD sensor output.
+qpd_sensor_output_std_scale_factor = 1.0e-5
+qpd_sensor_output_std_constant_V = 2.0e-3

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

@@ -5,9 +5,12 @@
 
 #include "./qpd_positioning_sensor.hpp"
 
-QpdPositioningSensor::QpdPositioningSensor(const int prescaler, ClockGenerator* clock_gen, const std::string file_name, const Dynamics& dynamics,
-                                           const FfInterSpacecraftCommunication& inter_spacecraft_communication, const size_t id)
-    : Component(prescaler, clock_gen), dynamics_(dynamics), inter_spacecraft_communication_(inter_spacecraft_communication) {
+#include <components/base/initialize_sensor.hpp>
+
+QpdPositioningSensor::QpdPositioningSensor(const int prescaler, ClockGenerator* clock_gen, Sensor& sensor_base, const std::string file_name,
+                                           const Dynamics& dynamics, const FfInterSpacecraftCommunication& inter_spacecraft_communication,
+                                           const size_t id)
+    : Component(prescaler, clock_gen), Sensor(sensor_base), dynamics_(dynamics), inter_spacecraft_communication_(inter_spacecraft_communication) {
   Initialize(file_name, id);
 }
 
@@ -140,6 +143,7 @@ double QpdPositioningSensor::CalcDisplacement(const libra::Vector<3> point_posit
 void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const double distance_from_beam_waist_m,
                                             const double qpd_y_axis_displacement_m, const double qpd_z_axis_displacement_m) {
   qpd_sensor_radius_m_ = (double)(((int32_t)(qpd_sensor_radius_m_ / qpd_sensor_integral_step_m_)) * qpd_sensor_integral_step_m_);
+  libra::Vector<3> qpd_standard_deviation_V{0.0};
   for (size_t y_axis_step = 0; y_axis_step <= (size_t)(qpd_sensor_radius_m_ / qpd_sensor_integral_step_m_) * 2; y_axis_step++) {
     double y_axis_pos_m = qpd_sensor_integral_step_m_ * y_axis_step - qpd_sensor_radius_m_;
     double z_axis_range_max_m = (double)((int32_t)(sqrt(pow(qpd_sensor_radius_m_, 2.0) - pow(y_axis_pos_m, 2.0)) / qpd_sensor_integral_step_m_) *
@@ -148,13 +152,39 @@ void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const d
       double z_axis_pos_m = qpd_sensor_integral_step_m_ * z_axis_step - z_axis_range_max_m;
       double deviation_from_optical_axis_m =
           sqrt(pow(y_axis_pos_m - qpd_y_axis_displacement_m, 2.0) + pow(z_axis_pos_m - qpd_z_axis_displacement_m, 2.0));
-      double temp = qpd_sensor_sensitivity_coefficient_V_W_ *
-                    laser_emitter->CalcIntensity_W_m2(distance_from_beam_waist_m, deviation_from_optical_axis_m) * qpd_sensor_integral_step_m_ *
-                    qpd_sensor_integral_step_m_;
+      double temp1 = qpd_sensor_sensitivity_coefficient_V_W_ *
+                     laser_emitter->CalcIntensity_W_m2(distance_from_beam_waist_m, deviation_from_optical_axis_m) * qpd_sensor_integral_step_m_ *
+                     qpd_sensor_integral_step_m_;
+      double temp2 = 2 * (y_axis_pos_m - qpd_y_axis_displacement_m + z_axis_pos_m - qpd_z_axis_displacement_m) /
+                     pow(laser_emitter->CalcBeamWidthRadius_m(distance_from_beam_waist_m), 2.0) * temp1;
+
+      qpd_sensor_output_y_axis_V_ += CalcSign(-y_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * temp1;
+      qpd_sensor_output_z_axis_V_ += CalcSign(z_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * temp1;
+      qpd_sensor_output_sum_V_ += temp1;
 
-      qpd_sensor_output_y_axis_V_ += CalcSign(-y_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * temp;
-      qpd_sensor_output_z_axis_V_ += CalcSign(z_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * temp;
-      qpd_sensor_output_sum_V_ += temp;
+      qpd_standard_deviation_V[0] += CalcSign(-y_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * temp2;
+      qpd_standard_deviation_V[1] += CalcSign(z_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * temp2;
+      qpd_standard_deviation_V[2] += temp2;
+    }
+  }
+  for (int std_id = 0; std_id < 3; ++std_id) {
+    qpd_standard_deviation_V[std_id] =
+        qpd_sensor_output_std_scale_factor_ * fabs(qpd_standard_deviation_V[std_id]) + qpd_sensor_output_std_constant_V_;
+  }
+  libra::Vector<3> qpd_sensor_noise_base = Measure(libra::Vector<3>{0.0});
+  qpd_sensor_output_y_axis_V_ += qpd_sensor_noise_base[0] * qpd_standard_deviation_V[0];
+  qpd_sensor_output_z_axis_V_ += qpd_sensor_noise_base[1] * qpd_standard_deviation_V[1];
+  qpd_sensor_output_sum_V_ += qpd_sensor_noise_base[2] * qpd_standard_deviation_V[2];
+  if (fabs(qpd_sensor_output_y_axis_V_) > qpd_sensor_output_sum_V_) {
+    qpd_sensor_output_y_axis_V_ -= 2 * qpd_sensor_noise_base[0] * qpd_standard_deviation_V[0];
+    if (fabs(qpd_sensor_output_y_axis_V_) > qpd_sensor_output_sum_V_) {
+      qpd_sensor_output_sum_V_ -= 2 * qpd_sensor_noise_base[2] * qpd_standard_deviation_V[2];
+    }
+  }
+  if (fabs(qpd_sensor_output_z_axis_V_) > qpd_sensor_output_sum_V_) {
+    qpd_sensor_output_z_axis_V_ -= 2 * qpd_sensor_noise_base[1] * qpd_standard_deviation_V[1];
+    if (fabs(qpd_sensor_output_z_axis_V_) > qpd_sensor_output_sum_V_) {
+      qpd_sensor_output_sum_V_ -= 2 * qpd_sensor_noise_base[2] * qpd_standard_deviation_V[2];
     }
   }
 }
@@ -213,8 +243,24 @@ void QpdPositioningSensor::Initialize(const std::string file_name, const size_t
   qpd_positioning_threshold_m_ = ini_file.ReadDouble(section_name.c_str(), "qpd_positioning_threshold_m");
   qpd_laser_receivable_angle_rad_ = ini_file.ReadDouble(section_name.c_str(), "qpd_laser_receivable_angle_rad");
   qpd_sensor_output_voltage_threshold_V_ = ini_file.ReadDouble(section_name.c_str(), "qpd_sensor_output_voltage_threshold_V");
+  qpd_sensor_output_std_scale_factor_ = ini_file.ReadDouble(section_name.c_str(), "qpd_sensor_output_std_scale_factor");
+  qpd_sensor_output_std_constant_V_ = ini_file.ReadDouble(section_name.c_str(), "qpd_sensor_output_std_constant_V");
 
   x_axis_direction_c_[0] = 1.0;
   y_axis_direction_c_[1] = 1.0;
   z_axis_direction_c_[2] = 1.0;
 }
+
+QpdPositioningSensor InitializeQpdPositioningSensor(ClockGenerator* clock_gen, const std::string file_name, double compo_step_time_s,
+                                                    const Dynamics& dynamics, const FfInterSpacecraftCommunication& inter_spacecraft_communication,
+                                                    const size_t id) {
+  IniAccess ini_file(file_name);
+  std::string name = "QPD_POSITIONING_SENSOR_";
+  const std::string section_name = name + std::to_string(static_cast<long long>(id));
+  int prescaler = ini_file.ReadInt(section_name.c_str(), "prescaler");
+
+  Sensor<3> sensor_base = ReadSensorInformation<3>(file_name, compo_step_time_s * (double)(prescaler), "QPD_POSITIONING_SENSOR", "V");
+  QpdPositioningSensor qpd_positioning_sensor(prescaler, clock_gen, sensor_base, file_name, dynamics, inter_spacecraft_communication, id);
+
+  return qpd_positioning_sensor;
+}

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

@@ -7,6 +7,7 @@
 #define S2E_COMPONENTS_QPD_POSITIONING_SENSOR_HPP_
 
 #include <components/base/component.hpp>
+#include <components/base/sensor.hpp>
 #include <dynamics/dynamics.hpp>
 #include <library/logger/logger.hpp>
 #include <library/math/vector.hpp>
@@ -19,13 +20,13 @@
  * @brief Quadrant photodiode sensor
  * @note  This component not only calculate the QPD sensor output values, but also calculate position displacements.
  */
-class QpdPositioningSensor : public Component, public ILoggable {
+class QpdPositioningSensor : public Component, public Sensor<3>, public ILoggable {
  public:
   /**
    * @fn QpdPositioningSensor
    * @brief Constructor
    */
-  QpdPositioningSensor(const int prescaler, ClockGenerator* clock_gen, const std::string file_name, const Dynamics& dynamics,
+  QpdPositioningSensor(const int prescaler, ClockGenerator* clock_gen, Sensor& sensor_base, const std::string file_name, const Dynamics& dynamics,
                        const FfInterSpacecraftCommunication& inter_spacecraft_communication, const size_t id = 0);
   /**
    * @fn ~QpdPositioningSensor
@@ -82,6 +83,9 @@ class QpdPositioningSensor : public Component, public ILoggable {
   double qpd_sensor_output_z_axis_V_ = 0.0;  //!< Quadrant photodiode sensor output value corresponding to the y-axis direction [V]
   double qpd_sensor_output_sum_V_ = 0.0;     //!< Quadrant photodiode sensor output value corresponding to the sum of the light intensity [V]
 
+  double qpd_sensor_output_std_scale_factor_;
+  double qpd_sensor_output_std_constant_V_;
+
   double observed_y_axis_displacement_m_ = 0.0;  //!< Observed displacement in the y-axis direction [m]
   double observed_z_axis_displacement_m_ = 0.0;  //!< Observed displacement in the z-axis direction [m]
 
@@ -110,4 +114,8 @@ class QpdPositioningSensor : public Component, public ILoggable {
   void Initialize(const std::string file_name, const size_t id = 0);
 };
 
+QpdPositioningSensor InitializeQpdPositioningSensor(ClockGenerator* clock_gen, const std::string file_name, double compo_step_time_s,
+                                                    const Dynamics& dynamics, const FfInterSpacecraftCommunication& inter_spacecraft_communication,
+                                                    const size_t id = 0);
+
 #endif  // S2E_COMPONENTS_QPD_POSITIONING_SENSOR_HPP_

s2e-ff/src/simulation/spacecraft/ff_components.cpp

@@ -42,7 +42,8 @@ FfComponents::FfComponents(const Dynamics* dynamics, const Structure* structure,
   laser_distance_meter_ = new LaserDistanceMeter(1, clock_gen, ldm_file, *dynamics_, inter_spacecraft_communication_);
 
   const std::string qpd_file = sat_file.ReadString(section_name.c_str(), "qpd_positioning_sensor_file");
-  qpd_positioning_sensor_ = new QpdPositioningSensor(1, clock_gen, qpd_file, *dynamics_, inter_spacecraft_communication_);
+  qpd_positioning_sensor_ =
+      new QpdPositioningSensor(InitializeQpdPositioningSensor(clock_gen, qpd_file, compo_step_sec, *dynamics_, inter_spacecraft_communication_));
 
   const std::string force_generator_file = sat_file.ReadString(section_name.c_str(), "force_generator_file");
   force_generator_ = new ForceGenerator(InitializeForceGenerator(clock_gen, force_generator_file, dynamics_));