diff --git a/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini b/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini index a3ee4282..efd9554e 100644 --- a/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini +++ b/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 diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp index e9405b31..8634ac78 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp +++ b/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 + 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& 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(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; +} diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp index 3f4a2cd1..129b04ff 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp +++ b/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 #include #include +#include #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& 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_ diff --git a/s2e-ff/src/simulation/spacecraft/ff_components.cpp b/s2e-ff/src/simulation/spacecraft/ff_components.cpp index 6fdbf55c..7e2957bb 100644 --- a/s2e-ff/src/simulation/spacecraft/ff_components.cpp +++ b/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_));