Add GNSS satellite and GNSS receiver clock bias

example/settings/sample_satellite/components/gnss_receiver.ini

@@ -24,7 +24,10 @@ antenna_model = SIMPLE
 // Antenna half width [deg]
 antenna_half_width_deg = 60
 
-// Random noise for pseudorange observation
+// Random noise of receiver clock bias [s]
+white_noise_standard_deviation_receiver_clock_bias_s = 10.0E-9
+
+// Random noise for pseudorange observation [m]
 white_noise_standard_deviation_pseudorange_m = 10.0
 
 // Random noise for simple position observation

src/components/real/aocs/gnss_receiver.cpp

@@ -15,8 +15,8 @@ namespace s2e::components {
 
 GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, const size_t component_id,
                            const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c,
-                           const double half_width_deg, const double pseudorange_noise_standard_deviation_m,
-                           const math::Vector<3> position_noise_standard_deviation_ecef_m,
+                           const double half_width_deg, const double receiver_clock_bias_noise_standard_deviation_s,
+                           const double pseudorange_noise_standard_deviation_m, const math::Vector<3> position_noise_standard_deviation_ecef_m,
                            const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const bool is_log_pseudorange_enabled,
                            const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites,
                            const environment::SimulationTime* simulation_time)
@@ -36,12 +36,14 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo
                                                      randomization::global_randomization.MakeSeed());
   }
   pseudorange_random_noise_m_.SetParameters(0.0, pseudorange_noise_standard_deviation_m, randomization::global_randomization.MakeSeed());
+  receiver_clock_bias_random_noise_s_.SetParameters(0.0, receiver_clock_bias_noise_standard_deviation_s,
+                                                    randomization::global_randomization.MakeSeed());
 }
 
 GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id,
                            const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c,
-                           const double half_width_deg, const double pseudorange_noise_standard_deviation_m,
-                           const math::Vector<3> position_noise_standard_deviation_ecef_m,
+                           const double half_width_deg, const double receiver_clock_bias_noise_standard_deviation_s,
+                           const double pseudorange_noise_standard_deviation_m, const math::Vector<3> position_noise_standard_deviation_ecef_m,
                            const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const bool is_log_pseudorange_enabled,
                            const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites,
                            const environment::SimulationTime* simulation_time)
@@ -61,6 +63,8 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo
                                                      randomization::global_randomization.MakeSeed());
   }
   pseudorange_random_noise_m_.SetParameters(0.0, pseudorange_noise_standard_deviation_m, randomization::global_randomization.MakeSeed());
+  receiver_clock_bias_random_noise_s_.SetParameters(0.0, receiver_clock_bias_noise_standard_deviation_s,
+                                                    randomization::global_randomization.MakeSeed());
 }
 
 void GnssReceiver::MainRoutine(const int time_count) {
@@ -215,11 +219,17 @@ double GnssReceiver::CalcGeometricDistance_m(const size_t gnss_system_id) {
   return geometric_distance_m;
 }
 
+double GnssReceiver::CalcClockBias_m(const size_t gnss_system_id) {
+  const double c_m_s = environment::speed_of_light_m_s;
+  double gnss_clock_bias_s = gnss_satellites_->GetClock_s(gnss_system_id);
+  return c_m_s * (receiver_clock_bias_random_noise_s_ - gnss_clock_bias_s);
+}
+
 double GnssReceiver::CalcPseudorange_m(const size_t gnss_system_id) {
-  // TODO: Add effect of clock bias
   // TODO: Add ionospheric delay
   double geometric_distance_m = CalcGeometricDistance_m(gnss_system_id);
-  double pseudorange_m = geometric_distance_m + pseudorange_random_noise_m_;
+  double clock_bias_m = CalcClockBias_m(gnss_system_id);
+  double pseudorange_m = geometric_distance_m + clock_bias_m + pseudorange_random_noise_m_;
   return pseudorange_m;
 }
 
@@ -258,6 +268,7 @@ typedef struct _gnss_receiver_param {
   math::Vector<3> antenna_pos_b;
   math::Quaternion quaternion_b2c;
   double half_width_deg;
+  double receiver_clock_bias_noise_standard_deviation_s;
   double pseudorange_noise_standard_deviation_m;
   math::Vector<3> position_noise_standard_deviation_ecef_m;
   math::Vector<3> velocity_noise_standard_deviation_ecef_m_s;
@@ -358,6 +369,8 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const environ
   gnssr_conf.ReadVector(GSection, "antenna_position_b_m", gnss_receiver_param.antenna_pos_b);
   gnssr_conf.ReadQuaternion(GSection, "quaternion_b2c", gnss_receiver_param.quaternion_b2c);
   gnss_receiver_param.half_width_deg = gnssr_conf.ReadDouble(GSection, "antenna_half_width_deg");
+  gnss_receiver_param.receiver_clock_bias_noise_standard_deviation_s =
+      gnssr_conf.ReadDouble(GSection, "white_noise_standard_deviation_receiver_clock_bias_s");
   gnss_receiver_param.pseudorange_noise_standard_deviation_m = gnssr_conf.ReadDouble(GSection, "white_noise_standard_deviation_pseudorange_m");
   gnssr_conf.ReadVector(GSection, "white_noise_standard_deviation_position_ecef_m", gnss_receiver_param.position_noise_standard_deviation_ecef_m);
   gnssr_conf.ReadVector(GSection, "white_noise_standard_deviation_velocity_ecef_m_s", gnss_receiver_param.velocity_noise_standard_deviation_ecef_m_s);
@@ -371,7 +384,8 @@ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, cons
   GnssReceiverParam gr_param = ReadGnssReceiverIni(file_name, gnss_satellites, component_id);
 
   GnssReceiver gnss_r(gr_param.prescaler, clock_generator, component_id, gr_param.antenna_model, gr_param.antenna_pos_b, gr_param.quaternion_b2c,
-                      gr_param.half_width_deg, gr_param.pseudorange_noise_standard_deviation_m, gr_param.position_noise_standard_deviation_ecef_m,
+                      gr_param.half_width_deg, gr_param.receiver_clock_bias_noise_standard_deviation_s,
+                      gr_param.pseudorange_noise_standard_deviation_m, gr_param.position_noise_standard_deviation_ecef_m,
                       gr_param.velocity_noise_standard_deviation_ecef_m_s, gr_param.is_log_pseudorange_enabled, dynamics, gnss_satellites,
                       simulation_time);
   return gnss_r;
@@ -386,9 +400,10 @@ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, Powe
   power_port->InitializeWithInitializeFile(file_name);
 
   GnssReceiver gnss_r(gr_param.prescaler, clock_generator, power_port, component_id, gr_param.antenna_model, gr_param.antenna_pos_b,
-                      gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.pseudorange_noise_standard_deviation_m,
-                      gr_param.position_noise_standard_deviation_ecef_m, gr_param.velocity_noise_standard_deviation_ecef_m_s,
-                      gr_param.is_log_pseudorange_enabled, dynamics, gnss_satellites, simulation_time);
+                      gr_param.quaternion_b2c, gr_param.half_width_deg, gr_param.receiver_clock_bias_noise_standard_deviation_s,
+                      gr_param.pseudorange_noise_standard_deviation_m, gr_param.position_noise_standard_deviation_ecef_m,
+                      gr_param.velocity_noise_standard_deviation_ecef_m_s, gr_param.is_log_pseudorange_enabled, dynamics, gnss_satellites,
+                      simulation_time);
   return gnss_r;
 }
 

src/components/real/aocs/gnss_receiver.hpp

@@ -54,6 +54,7 @@ class GnssReceiver : public Component, public logger::ILoggable {
    * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m]
    * @param [in] quaternion_b2c: Quaternion from body frame to component frame (antenna frame)
    * @param [in] half_width_deg: Half width of the antenna cone model [deg]
+   * @param [in] receiver_clock_bias_noise_standard_deviation_s: Standard deviation of normal random noise for receiver clock bias [s]
    * @param [in] pseudorange_noise_standard_deviation_m: Standard deviation of normal random noise for pseudorange [m]
    * @param [in] position_noise_standard_deviation_ecef_m: Standard deviation of normal random noise for position in the ECEF frame [m]
    * @param [in] velocity_noise_standard_deviation_ecef_m_s: Standard deviation of normal random noise for velocity in the ECEF frame [m/s]
@@ -64,9 +65,9 @@ class GnssReceiver : public Component, public logger::ILoggable {
    */
   GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, const size_t component_id, const AntennaModel antenna_model,
                const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c, const double half_width_deg,
-               const double pseudorange_noise_standard_deviation_m, const math::Vector<3> position_noise_standard_deviation_ecef_m,
-               const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const bool is_log_pseudorange_enabled,
-               const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites,
+               const double receiver_clock_bias_noise_standard_deviation_s, const double pseudorange_noise_standard_deviation_m,
+               const math::Vector<3> position_noise_standard_deviation_ecef_m, const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s,
+               const bool is_log_pseudorange_enabled, const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites,
                const environment::SimulationTime* simulation_time);
   /**
    * @fn GnssReceiver
@@ -78,6 +79,7 @@ class GnssReceiver : public Component, public logger::ILoggable {
    * @param [in] antenna_position_b_m: GNSS antenna position at the body-fixed frame [m]
    * @param [in] quaternion_b2c: Quaternion from body frame to component frame (antenna frame)
    * @param [in] half_width_deg: Half width of the antenna cone model [rad]
+   * @param [in] receiver_clock_bias_noise_standard_deviation_s: Standard deviation of normal random noise for receiver clock bias [s]
    * @param [in] pseudorange_noise_standard_deviation_m: Standard deviation of normal random noise for pseudorange [m]
    * @param [in] position_noise_standard_deviation_ecef_m: Standard deviation of normal random noise for position in the ECEF frame [m]
    * @param [in] velocity_noise_standard_deviation_ecef_m_s: Standard deviation of normal random noise for velocity in the ECEF frame [m/s]
@@ -88,9 +90,10 @@ class GnssReceiver : public Component, public logger::ILoggable {
    */
   GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id,
                const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c,
-               const double half_width_deg, const double pseudorange_noise_standard_deviation_m,
-               const math::Vector<3> position_noise_standard_deviation_ecef_m, const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s,
-               const bool is_log_pseudorange_enabled, const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites,
+               const double half_width_deg, const double receiver_clock_bias_noise_standard_deviation_s,
+               const double pseudorange_noise_standard_deviation_m, const math::Vector<3> position_noise_standard_deviation_ecef_m,
+               const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const bool is_log_pseudorange_enabled,
+               const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites,
                const environment::SimulationTime* simulation_time);
 
   // Override functions for Component
@@ -159,6 +162,7 @@ class GnssReceiver : public Component, public logger::ILoggable {
   AntennaModel antenna_model_;            //!< Antenna model
 
   // GNSS observation
+  randomization::NormalRand receiver_clock_bias_random_noise_s_;              //!< Random noise for receiver clock bias [s]
   randomization::NormalRand pseudorange_random_noise_m_;                      //!< Random noise for pseudorange [m]
   std::vector<double> pseudorange_list_m_{kTotalNumberOfGnssSatellite, 0.0};  //!< Pseudorange list for each GPS satellite
   bool is_logged_pseudorange_;                                                //!< Flag for log output of pseudorange
@@ -219,14 +223,21 @@ class GnssReceiver : public Component, public logger::ILoggable {
    */
   void SetGnssInfo(const math::Vector<3> antenna_to_satellite_i_m, const math::Quaternion quaternion_i2b, const size_t gnss_system_id);
   /**
-   * @fn CalcGeometricDistance
+   * @fn CalcGeometricDistance_m
    * @brief Calculate the geometric distance between the GNSS satellite and the GNSS receiver antenna
    * @param [in] gnss_system_id: ID of target GNSS satellite
    * @return Geometric distance between the GNSS satellite and the GNSS receiver antenna [m]
    */
   double CalcGeometricDistance_m(const size_t gnss_system_id);
   /**
-   * @fn CalcPseudorange
+   * @fn CalcClockBias_m
+   * @brief Calculate the clock bias term of the GNSS satellite and the GNSS receiver antenna
+   * @param [in] gnss_system_id: ID of target GNSS satellite
+   * @return Clock bias term of the GNSS satellite and the GNSS receiver antenna [m]
+   */
+  double CalcClockBias_m(const size_t gnss_system_id);
+  /**
+   * @fn CalcPseudorange_m
    * @brief Calculate the pseudorange between the GNSS satellite and the GNSS receiver antenna
    * @param [in] gnss_system_id: ID of target GNSS satellite
    * @return Pseudorange between the GNSS satellite and the GNSS receiver antenna [m]