From 2ec66351cb97a82100f9c556f3a38f2f2c476a16 Mon Sep 17 00:00:00 2001 From: Tomoki Date: Tue, 9 Jan 2024 17:32:30 +0900 Subject: [PATCH 01/15] add noise characteristice of the qpd positioning sensor --- .../components/qpd_positioning_sensor.ini | 50 +++++++++++++++++ .../aocs/qpd_positioning_sensor.cpp | 54 +++++++++++++++---- .../aocs/qpd_positioning_sensor.hpp | 12 ++++- .../simulation/spacecraft/ff_components.cpp | 3 +- 4 files changed, 106 insertions(+), 13 deletions(-) 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..34b0ba1d 100644 --- a/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini +++ b/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 = 4.0e-2 diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp index e9405b31..029c9465 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp +++ b/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 + +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,15 +152,29 @@ 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; + 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_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]; } double QpdPositioningSensor::CalcSign(const double input_value, const double threshold) { @@ -213,8 +231,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(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; +} diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp index 3f4a2cd1..d3f57f4f 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp @@ -7,6 +7,7 @@ #define S2E_COMPONENTS_QPD_POSITIONING_SENSOR_HPP_ #include +#include #include #include #include @@ -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 @@ -92,6 +93,9 @@ class QpdPositioningSensor : public Component, public ILoggable { std::vector qpd_sensor_voltage_ratio_z_list_; //!< List of `qpd_sensor_output_z_axis_V / qpd_sensor_output_sum_V` at each point on the z-axis. + double qpd_sensor_output_std_scale_factor_; + double qpd_sensor_output_std_constant_V_; + // Reference const Dynamics& dynamics_; const FfInterSpacecraftCommunication& inter_spacecraft_communication_; @@ -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_ 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_)); From 17a2a1d3c0ee1c859c34d57946cad1b79f32381d Mon Sep 17 00:00:00 2001 From: Tomoki Date: Tue, 9 Jan 2024 18:13:26 +0900 Subject: [PATCH 02/15] fix way of adding noise --- .../components/qpd_positioning_sensor.ini | 2 +- .../src/components/aocs/qpd_positioning_sensor.cpp | 12 ++++++++++-- 2 files changed, 11 insertions(+), 3 deletions(-) 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 34b0ba1d..9949fc9d 100644 --- a/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini +++ b/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini @@ -84,4 +84,4 @@ 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 = 4.0e-2 +qpd_sensor_output_std_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 029c9465..c217fce8 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp @@ -172,9 +172,17 @@ void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const d 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_noise_base[0] * qpd_standard_deviation_V_[0]) > qpd_sensor_output_sum_V_) { + qpd_sensor_output_y_axis_V_ -= qpd_sensor_noise_base[0] * qpd_standard_deviation_V_[0]; + } else { + qpd_sensor_output_y_axis_V_ += qpd_sensor_noise_base[0] * qpd_standard_deviation_V_[0]; + } + if (fabs(qpd_sensor_output_z_axis_V_ + qpd_sensor_noise_base[1] * qpd_standard_deviation_V_[1]) > qpd_sensor_output_sum_V_) { + qpd_sensor_output_z_axis_V_ -= qpd_sensor_noise_base[1] * qpd_standard_deviation_V_[1]; + } else { + qpd_sensor_output_z_axis_V_ += qpd_sensor_noise_base[1] * qpd_standard_deviation_V_[1]; + } } double QpdPositioningSensor::CalcSign(const double input_value, const double threshold) { From d635483ed0fd6a1ab74ef3b832ad1415e8407367 Mon Sep 17 00:00:00 2001 From: Tomoki Date: Tue, 9 Jan 2024 18:33:39 +0900 Subject: [PATCH 03/15] fix small --- .../components/aocs/qpd_positioning_sensor.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp index c217fce8..04892dac 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp @@ -172,16 +172,20 @@ void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const d 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_noise_base[0] * qpd_standard_deviation_V_[0]) > qpd_sensor_output_sum_V_) { - qpd_sensor_output_y_axis_V_ -= qpd_sensor_noise_base[0] * qpd_standard_deviation_V_[0]; - } else { - qpd_sensor_output_y_axis_V_ += 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_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_noise_base[1] * qpd_standard_deviation_V_[1]) > qpd_sensor_output_sum_V_) { - qpd_sensor_output_z_axis_V_ -= qpd_sensor_noise_base[1] * qpd_standard_deviation_V_[1]; - } else { - qpd_sensor_output_z_axis_V_ += qpd_sensor_noise_base[1] * qpd_standard_deviation_V_[1]; + 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]; + } } } From a0d539d1cef86193fda9d1c635530269541b50a7 Mon Sep 17 00:00:00 2001 From: Tomoki Date: Tue, 9 Jan 2024 18:35:12 +0900 Subject: [PATCH 04/15] fix name of variables --- .../aocs/qpd_positioning_sensor.cpp | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp index 04892dac..6869e04e 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp @@ -143,7 +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}; + 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_) * @@ -162,29 +162,29 @@ void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const d 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_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; + 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_; + 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]; + 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]; + 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]; + 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_noise_base[1] * qpd_standard_deviation_V_[1]) > 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_noise_base[1] * qpd_standard_deviation_V[1]) > 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]; + qpd_sensor_output_sum_V_ -= 2 * qpd_sensor_noise_base[2] * qpd_standard_deviation_V[2]; } } } From 2b380fd18ed198e14eda89b3392f3c398652e431 Mon Sep 17 00:00:00 2001 From: Tomoki Date: Wed, 10 Jan 2024 17:19:02 +0900 Subject: [PATCH 05/15] fix small --- s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp index 6869e04e..54abc739 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp @@ -181,7 +181,7 @@ void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const d 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_noise_base[1] * qpd_standard_deviation_V[1]) > qpd_sensor_output_sum_V_) { + 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]; From ca508e07b607ef2dc14ffbc32e9fb46a1918d28d Mon Sep 17 00:00:00 2001 From: Tomoki Date: Thu, 11 Jan 2024 12:58:42 +0900 Subject: [PATCH 06/15] fix small --- s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp index d3f57f4f..a1a257f6 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp @@ -83,6 +83,9 @@ class QpdPositioningSensor : public Component, public Sensor<3>, public ILoggabl 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] @@ -93,9 +96,6 @@ class QpdPositioningSensor : public Component, public Sensor<3>, public ILoggabl std::vector qpd_sensor_voltage_ratio_z_list_; //!< List of `qpd_sensor_output_z_axis_V / qpd_sensor_output_sum_V` at each point on the z-axis. - double qpd_sensor_output_std_scale_factor_; - double qpd_sensor_output_std_constant_V_; - // Reference const Dynamics& dynamics_; const FfInterSpacecraftCommunication& inter_spacecraft_communication_; From a1881811a1d3a3e23723e5715de74671f51ac524 Mon Sep 17 00:00:00 2001 From: Tomoki Date: Fri, 12 Jan 2024 12:54:30 +0900 Subject: [PATCH 07/15] rename variable --- s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp index 54abc739..5d71c17f 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp @@ -167,9 +167,9 @@ void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const d 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_; + for (int standard_deviation_id = 0; standard_deviation_id < 3; ++standard_deviation_id) { + qpd_standard_deviation_V[standard_deviation_id] = + qpd_sensor_output_std_scale_factor_ * fabs(qpd_standard_deviation_V[standard_deviation_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]; From 9768aa5eb6c7dec8f8e41af0e2917584071fb040 Mon Sep 17 00:00:00 2001 From: Tomoki Date: Fri, 12 Jan 2024 16:09:06 +0900 Subject: [PATCH 08/15] fix small --- s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp index 5d71c17f..b1ec28bb 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp @@ -167,7 +167,7 @@ void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const d qpd_standard_deviation_V[2] += temp2; } } - for (int standard_deviation_id = 0; standard_deviation_id < 3; ++standard_deviation_id) { + for (size_t standard_deviation_id = 0; standard_deviation_id < 3; ++standard_deviation_id) { qpd_standard_deviation_V[standard_deviation_id] = qpd_sensor_output_std_scale_factor_ * fabs(qpd_standard_deviation_V[standard_deviation_id]) + qpd_sensor_output_std_constant_V_; } From 7d6481fffa8a5fa3738f2611503a8330f32a83ce Mon Sep 17 00:00:00 2001 From: Tomoki Date: Fri, 12 Jan 2024 19:19:12 +0900 Subject: [PATCH 09/15] remove Sensor class --- .../components/qpd_positioning_sensor.ini | 4 +- .../aocs/qpd_positioning_sensor.cpp | 61 +++++++++++-------- .../aocs/qpd_positioning_sensor.hpp | 21 ++++--- 3 files changed, 52 insertions(+), 34 deletions(-) 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 9949fc9d..ce45093b 100644 --- a/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini +++ b/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini @@ -83,5 +83,5 @@ 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 +qpd_standard_deviation_scale_factor_m = 1.0e-5 +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 b1ec28bb..c44b0ea9 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp @@ -7,10 +7,9 @@ #include -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) { +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) { Initialize(file_name, id); } @@ -143,7 +142,9 @@ 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}; + 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_) * @@ -162,29 +163,37 @@ void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const d 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_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; + qpd_sensor_output_derivative_y_axis_V_m += CalcSign(-y_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * temp2; + qpd_sensor_output_derivative_z_axis_V_m += CalcSign(z_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * temp2; + qpd_sensor_output_derivative_sum_V_m += temp2; } } - for (size_t standard_deviation_id = 0; standard_deviation_id < 3; ++standard_deviation_id) { - qpd_standard_deviation_V[standard_deviation_id] = - qpd_sensor_output_std_scale_factor_ * fabs(qpd_standard_deviation_V[standard_deviation_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]; + const double qpd_standard_deviation_y_axis_V = CalcStandardDeviation(qpd_sensor_output_derivative_y_axis_V_m); + const double qpd_standard_deviation_z_axis_V = CalcStandardDeviation(qpd_sensor_output_derivative_z_axis_V_m); + const double qpd_standard_deviation_sum_V = CalcStandardDeviation(qpd_sensor_output_derivative_sum_V_m); + + qpd_sensor_random_noise_y_axis_.SetParameters(0.0, qpd_standard_deviation_y_axis_V); + qpd_sensor_random_noise_z_axis_.SetParameters(0.0, qpd_standard_deviation_z_axis_V); + qpd_sensor_random_noise_sum_.SetParameters(0.0, qpd_standard_deviation_sum_V); + + // Add Noise to to the quadrant photodiode output values + const double qpd_sensor_random_noise_y_axis = qpd_sensor_random_noise_y_axis_; + const double qpd_sensor_random_noise_z_axis = qpd_sensor_random_noise_z_axis_; + const double qpd_sensor_random_noise_sum = qpd_sensor_random_noise_sum_; + + 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_noise_base[0] * qpd_standard_deviation_V[0]; + 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_noise_base[2] * qpd_standard_deviation_V[2]; + 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_noise_base[1] * qpd_standard_deviation_V[1]; + 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_noise_base[2] * qpd_standard_deviation_V[2]; + qpd_sensor_output_sum_V_ -= 2 * qpd_sensor_random_noise_sum; } } } @@ -198,6 +207,11 @@ double QpdPositioningSensor::CalcSign(const double input_value, const double thr return 0.0; } +double QpdPositioningSensor::CalcStandardDeviation(const double sensor_output_derivative) { + double standard_deviation = qpd_standard_deviation_scale_factor_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_; @@ -243,8 +257,8 @@ 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"); + qpd_standard_deviation_scale_factor_m_ = ini_file.ReadDouble(section_name.c_str(), "qpd_standard_deviation_scale_factor_m"); + 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; @@ -259,8 +273,7 @@ QpdPositioningSensor InitializeQpdPositioningSensor(ClockGenerator* clock_gen, c const std::string section_name = name + std::to_string(static_cast(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); + 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 a1a257f6..f782946d 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp @@ -7,10 +7,10 @@ #define S2E_COMPONENTS_QPD_POSITIONING_SENSOR_HPP_ #include -#include #include #include #include +#include #include "../../library/math/translation_first_dual_quaternion.hpp" #include "../../simulation/case/ff_inter_spacecraft_communication.hpp" @@ -20,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 Sensor<3>, public ILoggable { +class QpdPositioningSensor : public Component, public ILoggable { public: /** * @fn QpdPositioningSensor * @brief Constructor */ - QpdPositioningSensor(const int prescaler, ClockGenerator* clock_gen, Sensor& sensor_base, const std::string file_name, const Dynamics& dynamics, + QpdPositioningSensor(const int prescaler, ClockGenerator* clock_gen, const std::string file_name, const Dynamics& dynamics, const FfInterSpacecraftCommunication& inter_spacecraft_communication, const size_t id = 0); /** * @fn ~QpdPositioningSensor @@ -79,12 +79,16 @@ class QpdPositioningSensor : public Component, public Sensor<3>, public ILoggabl // 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] - double qpd_sensor_output_std_scale_factor_; - double qpd_sensor_output_std_constant_V_; + // Noise parameters + libra::NormalRand qpd_sensor_random_noise_y_axis_; //!< Normal random for QPD sensor output value: E_y + libra::NormalRand qpd_sensor_random_noise_z_axis_; //!< Normal random for QPD sensor output value: E_z + libra::NormalRand qpd_sensor_random_noise_sum_; //!< Normal random for QPD sensor output value: E_sum + double qpd_standard_deviation_scale_factor_m_; + double qpd_standard_deviation_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,6 +114,7 @@ class QpdPositioningSensor : public Component, public Sensor<3>, public ILoggabl 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); void Initialize(const std::string file_name, const size_t id = 0); }; From 71d696e0e3fb598aa4ac42dbc37cee90cf51e381 Mon Sep 17 00:00:00 2001 From: Tomoki Date: Fri, 12 Jan 2024 19:32:11 +0900 Subject: [PATCH 10/15] fix small --- .../components/aocs/qpd_positioning_sensor.cpp | 15 ++++++++------- .../components/aocs/qpd_positioning_sensor.hpp | 4 +--- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp index c44b0ea9..f8d34402 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp @@ -168,18 +168,19 @@ void QpdPositioningSensor::CalcSensorOutput(LaserEmitter* laser_emitter, const d qpd_sensor_output_derivative_sum_V_m += temp2; } } + 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); const double qpd_standard_deviation_z_axis_V = CalcStandardDeviation(qpd_sensor_output_derivative_z_axis_V_m); const double qpd_standard_deviation_sum_V = CalcStandardDeviation(qpd_sensor_output_derivative_sum_V_m); - qpd_sensor_random_noise_y_axis_.SetParameters(0.0, qpd_standard_deviation_y_axis_V); - qpd_sensor_random_noise_z_axis_.SetParameters(0.0, qpd_standard_deviation_z_axis_V); - qpd_sensor_random_noise_sum_.SetParameters(0.0, qpd_standard_deviation_sum_V); - // Add Noise to to the quadrant photodiode output values - const double qpd_sensor_random_noise_y_axis = qpd_sensor_random_noise_y_axis_; - const double qpd_sensor_random_noise_z_axis = qpd_sensor_random_noise_z_axis_; - const double qpd_sensor_random_noise_sum = qpd_sensor_random_noise_sum_; + 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; diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp index f782946d..63d93a48 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp @@ -84,9 +84,7 @@ class QpdPositioningSensor : public Component, public ILoggable { 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_random_noise_y_axis_; //!< Normal random for QPD sensor output value: E_y - libra::NormalRand qpd_sensor_random_noise_z_axis_; //!< Normal random for QPD sensor output value: E_z - libra::NormalRand qpd_sensor_random_noise_sum_; //!< Normal random for QPD sensor output value: E_sum + libra::NormalRand qpd_sensor_output_random_noise_; //!< Normal random for QPD sensor output value double qpd_standard_deviation_scale_factor_m_; double qpd_standard_deviation_constant_V_; From 4ef0f1a9878106629bc18b92a124017c0b4be882 Mon Sep 17 00:00:00 2001 From: Tomoki Date: Fri, 12 Jan 2024 19:37:52 +0900 Subject: [PATCH 11/15] fix comment --- s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp | 2 +- s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp index f8d34402..2671c0f1 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp @@ -1,6 +1,6 @@ /** * @file qpd_positioning_sensor.cpp - * @brief Quadrant photodiode (QPD) sensor + * @brief Quadrant photodiode (QPD) positioning sensor */ #include "./qpd_positioning_sensor.hpp" diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp index 63d93a48..bc96f347 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_ From e77a783d4587a74faeb7c00f972cba1ba7148491 Mon Sep 17 00:00:00 2001 From: Tomoki Date: Sat, 13 Jan 2024 10:14:58 +0900 Subject: [PATCH 12/15] [SENSOR_BASE_QPD_POSITIONING_SENSOR] remove --- .../components/qpd_positioning_sensor.ini | 43 ------------------- 1 file changed, 43 deletions(-) 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 ce45093b..eebd6bba 100644 --- a/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini +++ b/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini @@ -1,46 +1,3 @@ -[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 From 2258da628e15f588c884ef26589eddc947556e18 Mon Sep 17 00:00:00 2001 From: Tomoki Date: Sat, 13 Jan 2024 10:37:55 +0900 Subject: [PATCH 13/15] rename variables --- .../components/qpd_positioning_sensor.ini | 2 +- .../aocs/qpd_positioning_sensor.cpp | 52 +++++++++++-------- .../aocs/qpd_positioning_sensor.hpp | 8 +-- 3 files changed, 35 insertions(+), 27 deletions(-) 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 eebd6bba..daac4ebf 100644 --- a/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini +++ b/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini @@ -40,5 +40,5 @@ 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_m = 1.0e-5 +qpd_standard_deviation_scale_factor = 1.0e-5 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 2671c0f1..8634ac78 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.cpp @@ -73,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; @@ -139,9 +139,11 @@ 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; @@ -153,26 +155,31 @@ 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 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_derivative_y_axis_V_m += CalcSign(-y_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * temp2; - qpd_sensor_output_derivative_z_axis_V_m += CalcSign(z_axis_pos_m, qpd_sensor_integral_step_m_ / 2) * temp2; - qpd_sensor_output_derivative_sum_V_m += temp2; + + // 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); - const double qpd_standard_deviation_z_axis_V = CalcStandardDeviation(qpd_sensor_output_derivative_z_axis_V_m); - const double qpd_standard_deviation_sum_V = CalcStandardDeviation(qpd_sensor_output_derivative_sum_V_m); + 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); @@ -208,8 +215,9 @@ double QpdPositioningSensor::CalcSign(const double input_value, const double thr return 0.0; } -double QpdPositioningSensor::CalcStandardDeviation(const double sensor_output_derivative) { - double standard_deviation = qpd_standard_deviation_scale_factor_m_ * fabs(sensor_output_derivative) + qpd_standard_deviation_constant_V_; +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; } @@ -258,7 +266,7 @@ 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_m_ = ini_file.ReadDouble(section_name.c_str(), "qpd_standard_deviation_scale_factor_m"); + 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; diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp index bc96f347..5492580d 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp @@ -85,7 +85,7 @@ class QpdPositioningSensor : public Component, public ILoggable { // Noise parameters libra::NormalRand qpd_sensor_output_random_noise_; //!< Normal random for QPD sensor output value - double qpd_standard_deviation_scale_factor_m_; + double qpd_standard_deviation_scale_factor_; double qpd_standard_deviation_constant_V_; double observed_y_axis_displacement_m_ = 0.0; //!< Observed displacement in the y-axis direction [m] @@ -107,12 +107,12 @@ 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); + 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); }; From a70324f30ee0e3ef1b8659eaa36d3f0fbed3d27b Mon Sep 17 00:00:00 2001 From: Tomoki Date: Sat, 13 Jan 2024 10:44:56 +0900 Subject: [PATCH 14/15] add comment --- s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp index 5492580d..129b04ff 100644 --- a/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp +++ b/s2e-ff/src/components/aocs/qpd_positioning_sensor.hpp @@ -84,9 +84,9 @@ class QpdPositioningSensor : public Component, public ILoggable { 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 for QPD sensor output value - double qpd_standard_deviation_scale_factor_; - double qpd_standard_deviation_constant_V_; + 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] From 6d53ae5b68142927f320d10691e334aae37f4962 Mon Sep 17 00:00:00 2001 From: Tomoki Date: Sat, 13 Jan 2024 11:43:12 +0900 Subject: [PATCH 15/15] fix small --- .../data/initialize_files/components/qpd_positioning_sensor.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 daac4ebf..efd9554e 100644 --- a/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini +++ b/s2e-ff/data/initialize_files/components/qpd_positioning_sensor.ini @@ -40,5 +40,5 @@ 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 = 1.0e-5 +qpd_standard_deviation_scale_factor = 7.0e-6 qpd_standard_deviation_constant_V = 2.0e-3