Merge pull request #82 from TomokiMochizuki/feature/add_noise_of_qpd_…

…sensor

Add noise characteristics of the qpd sensor

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

@@ -6,6 +6,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 +37,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_standard_deviation_scale_factor = 7.0e-6
+qpd_standard_deviation_constant_V = 2.0e-3

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

@@ -1,10 +1,12 @@
 /**
  * @file qpd_positioning_sensor.cpp
- * @brief Quadrant photodiode (QPD) sensor
+ * @brief Quadrant photodiode (QPD) positioning sensor
  */
 
 #include "./qpd_positioning_sensor.hpp"
 
+#include <components/base/initialize_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) {
@@ -71,7 +73,7 @@ void QpdPositioningSensor::MainRoutine(int count) {
     LaserEmitter laser_emitter = inter_spacecraft_communication_.GetLaserEmitter(laser_id);
     double laser_rayleigh_length_offset_m = laser_emitter.GetRayleighLengthOffset_m();
 
-    CalcSensorOutput(&laser_emitter, qpd_laser_distance_m - laser_rayleigh_length_offset_m, qpd_y_axis_displacement_m, qpd_z_axis_displacement_m);
+    CalcSensorOutput(&laser_emitter, qpd_laser_distance_m, laser_rayleigh_length_offset_m, qpd_y_axis_displacement_m, qpd_z_axis_displacement_m);
 
     if (qpd_sensor_output_sum_V_ < qpd_sensor_output_voltage_threshold_V_) {
       continue;
@@ -137,9 +139,14 @@ double QpdPositioningSensor::CalcDisplacement(const libra::Vector<3> point_posit
   return displacement_m;
 };
 
-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) {
+void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const double qpd_laser_distance_m,
+                                            const double laser_rayleigh_length_offset_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_);
+  const double distance_from_beam_waist_m = qpd_laser_distance_m - laser_rayleigh_length_offset_m;
+  double qpd_sensor_output_derivative_y_axis_V_m = 0.0;
+  double qpd_sensor_output_derivative_z_axis_V_m = 0.0;
+  double qpd_sensor_output_derivative_sum_V_m = 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 +155,53 @@ 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_;
 
-      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;
+      // Calculate a laser receiving amount at each point, and convert it to a voltage value.
+      double photovoltage_at_each_point = 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_;
+
+      // Calculate the variation of the laser light received at each point, and convert it to a voltage value.
+      double photovoltage_variation_at_each_point = 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) *
+                                                    photovoltage_at_each_point;
+
+      qpd_sensor_output_y_axis_V_ += CalcSign(-y_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * photovoltage_at_each_point;
+      qpd_sensor_output_z_axis_V_ += CalcSign(z_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * photovoltage_at_each_point;
+      qpd_sensor_output_sum_V_ += photovoltage_at_each_point;
+
+      qpd_sensor_output_derivative_y_axis_V_m += CalcSign(-y_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * photovoltage_variation_at_each_point;
+      qpd_sensor_output_derivative_z_axis_V_m += CalcSign(z_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * photovoltage_variation_at_each_point;
+      qpd_sensor_output_derivative_sum_V_m += photovoltage_variation_at_each_point;
+    }
+  }
+  if (qpd_sensor_output_sum_V_ < qpd_sensor_output_voltage_threshold_V_) return;
+
+  const double qpd_standard_deviation_y_axis_V = CalcStandardDeviation(qpd_sensor_output_derivative_y_axis_V_m, qpd_laser_distance_m);
+  const double qpd_standard_deviation_z_axis_V = CalcStandardDeviation(qpd_sensor_output_derivative_z_axis_V_m, qpd_laser_distance_m);
+  const double qpd_standard_deviation_sum_V = CalcStandardDeviation(qpd_sensor_output_derivative_sum_V_m, qpd_laser_distance_m);
+
+  // Add Noise to to the quadrant photodiode output values
+  qpd_sensor_output_random_noise_.SetParameters(0.0, qpd_standard_deviation_y_axis_V);
+  const double qpd_sensor_random_noise_y_axis = qpd_sensor_output_random_noise_;
+  qpd_sensor_output_random_noise_.SetParameters(0.0, qpd_standard_deviation_z_axis_V);
+  const double qpd_sensor_random_noise_z_axis = qpd_sensor_output_random_noise_;
+  qpd_sensor_output_random_noise_.SetParameters(0.0, qpd_standard_deviation_sum_V);
+  const double qpd_sensor_random_noise_sum = qpd_sensor_output_random_noise_;
+
+  qpd_sensor_output_y_axis_V_ += qpd_sensor_random_noise_y_axis;
+  qpd_sensor_output_z_axis_V_ += qpd_sensor_random_noise_z_axis;
+  qpd_sensor_output_sum_V_ += qpd_sensor_random_noise_sum;
+  if (fabs(qpd_sensor_output_y_axis_V_) > qpd_sensor_output_sum_V_) {
+    qpd_sensor_output_y_axis_V_ -= 2 * qpd_sensor_random_noise_y_axis;
+    if (fabs(qpd_sensor_output_y_axis_V_) > qpd_sensor_output_sum_V_) {
+      qpd_sensor_output_sum_V_ -= 2 * qpd_sensor_random_noise_sum;
+    }
+  }
+  if (fabs(qpd_sensor_output_z_axis_V_) > qpd_sensor_output_sum_V_) {
+    qpd_sensor_output_z_axis_V_ -= 2 * qpd_sensor_random_noise_z_axis;
+    if (fabs(qpd_sensor_output_z_axis_V_) > qpd_sensor_output_sum_V_) {
+      qpd_sensor_output_sum_V_ -= 2 * qpd_sensor_random_noise_sum;
     }
   }
 }
@@ -168,6 +215,12 @@ double QpdPositioningSensor::CalcSign(const double input_value, const double thr
   return 0.0;
 }
 
+double QpdPositioningSensor::CalcStandardDeviation(const double sensor_output_derivative, const double qpd_laser_distance_m) {
+  double standard_deviation =
+      qpd_standard_deviation_scale_factor_ * qpd_laser_distance_m * fabs(sensor_output_derivative) + qpd_standard_deviation_constant_V_;
+  return standard_deviation;
+}
+
 double QpdPositioningSensor::ObservePositionDisplacement(const double qpd_sensor_output_polarization, const double qpd_sensor_output_V,
                                                          const double qpd_sensor_output_sum_V, const std::vector<double>& qpd_voltage_ratio_list) {
   double observed_displacement_m = qpd_sensor_output_polarization * CalcSign(qpd_sensor_output_sum_V, 0.0) * qpd_positioning_threshold_m_;
@@ -213,8 +266,23 @@ 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_standard_deviation_scale_factor_ = ini_file.ReadDouble(section_name.c_str(), "qpd_standard_deviation_scale_factor");
+  qpd_standard_deviation_constant_V_ = ini_file.ReadDouble(section_name.c_str(), "qpd_standard_deviation_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");
+
+  QpdPositioningSensor qpd_positioning_sensor(prescaler, clock_gen, file_name, dynamics, inter_spacecraft_communication, id);
+
+  return qpd_positioning_sensor;
+}

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

@@ -1,6 +1,6 @@
 /**
  * @file qpd_positioning_sensor.hpp
- * @brief Quadrant photodiode (QPD) positioning system
+ * @brief Quadrant photodiode (QPD) positioning sensor
  */
 
 #ifndef S2E_COMPONENTS_QPD_POSITIONING_SENSOR_HPP_
@@ -10,6 +10,7 @@
 #include <dynamics/dynamics.hpp>
 #include <library/logger/logger.hpp>
 #include <library/math/vector.hpp>
+#include <library/randomization/normal_randomization.hpp>
 
 #include "../../library/math/translation_first_dual_quaternion.hpp"
 #include "../../simulation/case/ff_inter_spacecraft_communication.hpp"
@@ -78,9 +79,14 @@ class QpdPositioningSensor : public Component, public ILoggable {
 
   // This quadrant photodiode sensor is modeled after Thorlabs' PDQ80A product.
   // Therefore, the acquired values are not the raw values of the four photodiodes but the following three values:
-  double qpd_sensor_output_y_axis_V_ = 0.0;  //!< Quadrant photodiode sensor output value corresponding to the y-axis direction [V]
-  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_y_axis_V_ = 0.0;  //!< Quadrant photodiode sensor output value corresponding to the y-axis direction: E_y [V]
+  double qpd_sensor_output_z_axis_V_ = 0.0;  //!< Quadrant photodiode sensor output value corresponding to the y-axis direction: E_z [V]
+  double qpd_sensor_output_sum_V_ = 0.0;     //!< Quadrant photodiode sensor output value corresponding to the sum of the light intensity: E_sum [V]
+
+  // Noise parameters
+  libra::NormalRand qpd_sensor_output_random_noise_;  //!< Normal random noise for QPD sensor output value
+  double qpd_standard_deviation_scale_factor_;        //!< Scale factor of the standard deviation: Coefficient to express position dependency
+  double qpd_standard_deviation_constant_V_;          //!< Constant value of the standard deviation, which is constant regardless of its position
 
   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]
@@ -101,13 +107,18 @@ class QpdPositioningSensor : public Component, public ILoggable {
   double CalcDisplacement(const libra::Vector<3> point_position, const libra::Vector<3> origin_position,
                           const libra::Vector<3> displacement_direction);
 
-  void 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);
+  void CalcSensorOutput(LaserEmitter* laser_emitter, const double qpd_laser_distance_m, const double laser_rayleigh_length_offset_m,
+                        const double qpd_y_axis_displacement_m, const double qpd_z_axis_displacement_m);
   double ObservePositionDisplacement(const double qpd_sensor_output_polarization, const double qpd_sensor_output_V,
                                      const double qpd_sensor_output_sum_V, const std::vector<double>& qpd_ratio_reference_list);
   double CalcSign(const double input_value, const double threshold);
+  double CalcStandardDeviation(const double sensor_output_derivative, const double qpd_laser_distance_m);
 
   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_));