Modify pseudorange calculation

Author: fukudakazuya

src/components/real/aocs/gnss_receiver.cpp

@@ -25,7 +25,6 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo
       quaternion_b2c_(quaternion_b2c),
       half_width_deg_(half_width_deg),
       antenna_model_(antenna_model),
-      pseudorange_noise_standard_deviation_m_(pseudorange_noise_standard_deviation_m),
       dynamics_(dynamics),
       gnss_satellites_(gnss_satellites),
       simulation_time_(simulation_time) {
@@ -34,6 +33,7 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo
     velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i],
                                                      randomization::global_randomization.MakeSeed());
   }
+  pseudorange_random_noise_m_.SetParameters(0.0, pseudorange_noise_standard_deviation_m, randomization::global_randomization.MakeSeed());
 }
 
 GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clock_generator, PowerPort* power_port, const size_t component_id,
@@ -48,7 +48,6 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo
       quaternion_b2c_(quaternion_b2c),
       half_width_deg_(half_width_deg),
       antenna_model_(antenna_model),
-      pseudorange_noise_standard_deviation_m_(pseudorange_noise_standard_deviation_m),
       dynamics_(dynamics),
       gnss_satellites_(gnss_satellites),
       simulation_time_(simulation_time) {
@@ -57,6 +56,7 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo
     velocity_random_noise_ecef_m_s_[i].SetParameters(0.0, velocity_noise_standard_deviation_ecef_m_s[i],
                                                      randomization::global_randomization.MakeSeed());
   }
+  pseudorange_random_noise_m_.SetParameters(0.0, pseudorange_noise_standard_deviation_m, randomization::global_randomization.MakeSeed());
 }
 
 void GnssReceiver::MainRoutine(const int time_count) {
@@ -69,16 +69,7 @@ void GnssReceiver::MainRoutine(const int time_count) {
   CheckAntenna(position_true_eci, quaternion_i2b);
 
   // Pseudorange calculation
-  size_t number_of_calculated_gnss_satellites = gnss_satellites_->GetNumberOfCalculatedSatellite();
-  for (size_t i = 0; i < number_of_calculated_gnss_satellites; i++) {
-    math::Vector<3> gnss_satellite_position_ecef_m = gnss_satellites_->GetPosition_ecef_m(i);
-    math::Vector<3> position_true_ecef_m = dynamics_->GetOrbit().GetPosition_ecef_m();
-    double geometric_distance_m = (gnss_satellite_position_ecef_m - position_true_ecef_m).CalcNorm();
-    randomization::NormalRand pseudorange_random_noise_m;
-    pseudorange_random_noise_m.SetParameters(0.0, pseudorange_noise_standard_deviation_m_, randomization::global_randomization.MakeSeed());
-    double pseudorange_m = geometric_distance_m + pseudorange_random_noise_m;
-    pseudorange_list_m_[i] = pseudorange_m;
-  }
+  SetGnssObservationList();
 
   if (is_gnss_visible_) {
     // Antenna of GNSS-R can detect GNSS signal
@@ -195,6 +186,32 @@ void GnssReceiver::SetGnssInfo(const math::Vector<3> antenna_to_satellite_i_m, c
   gnss_information_list_.push_back(gnss_info_new);
 }
 
+double GnssReceiver::CalcGeometricDistance(const size_t gnss_system_id) {
+  math::Vector<3> gnss_satellite_position_ecef_m = gnss_satellites_->GetPosition_ecef_m(gnss_system_id);
+  math::Vector<3> position_true_ecef_m = dynamics_->GetOrbit().GetPosition_ecef_m();
+  double geometric_distance_m = (gnss_satellite_position_ecef_m - position_true_ecef_m).CalcNorm();
+  return geometric_distance_m;
+}
+
+double GnssReceiver::CalcPseudorange(const size_t gnss_system_id) {
+  // TODO: Add effect of radio wave propagation time
+  // TODO: Add effect of clock bias
+  // TODO: Add ionospheric delay
+  double geometric_distance_m = CalcGeometricDistance(gnss_system_id);
+  double pseudorange_m = geometric_distance_m + pseudorange_random_noise_m_;
+  return pseudorange_m;
+}
+
+void GnssReceiver::SetGnssObservationList() {
+  // TODO: Add carrier phase observation
+  pseudorange_list_m_.assign(kTotalNumberOfGnssSatellite, 0.0);
+  for (size_t i = 0; i < gnss_information_list_.size(); i++) {
+    size_t gnss_system_id = gnss_information_list_[i].gnss_id;
+    double pseudorange_m = CalcPseudorange(gnss_system_id);
+    pseudorange_list_m_[gnss_system_id] = pseudorange_m;
+  }
+}
+
 void GnssReceiver::AddNoise(const math::Vector<3> position_true_ecef_m, const math::Vector<3> velocity_true_ecef_m_s) {
   for (size_t i = 0; i < 3; i++) {
     position_ecef_m_[i] = position_true_ecef_m[i] + position_random_noise_ecef_m_[i];

src/components/real/aocs/gnss_receiver.hpp

@@ -18,15 +18,6 @@
 
 namespace s2e::components {
 
-// GNSS satellite number definition
-// TODO: Move to initialized file?
-const size_t kNumberOfGpsSatellite = 32;      //!< Number of GPS satellites
-const size_t kNumberOfGlonassSatellite = 26;  //!< Number of GLONASS satellites
-const size_t kNumberOfGalileoSatellite = 28;  //!< Number of Galileo satellites
-const size_t kNumberOfBeidouSatellite = 62;   //!< Number of BeiDou satellites
-const size_t kNumberOfQzssSatellite = 5;      //!< Number of QZSS satellites
-const size_t kNumberOfNavicSatellite = 7;     //!< Number of NavIC satellites
-
 /**
  * @enum AntennaModel
  * @brief Antenna pattern model to emulate GNSS antenna
@@ -142,6 +133,19 @@ class GnssReceiver : public Component, public logger::ILoggable {
   virtual std::string GetLogValue() const;
 
  protected:
+  // GNSS satellite number definition
+  // TODO: Move to initialized file?
+  static const size_t kNumberOfGpsSatellite = 32;      //!< Number of GPS satellites
+  static const size_t kNumberOfGlonassSatellite = 26;  //!< Number of GLONASS satellites
+  static const size_t kNumberOfGalileoSatellite = 28;  //!< Number of Galileo satellites
+  static const size_t kNumberOfBeidouSatellite = 62;   //!< Number of BeiDou satellites
+  static const size_t kNumberOfQzssSatellite = 5;      //!< Number of QZSS satellites
+  static const size_t kNumberOfNavicSatellite = 7;     //!< Number of NavIC satellites
+
+  static const size_t kTotalNumberOfGnssSatellite = kNumberOfGpsSatellite + kNumberOfGlonassSatellite + kNumberOfGalileoSatellite +
+                                                    kNumberOfBeidouSatellite + kNumberOfQzssSatellite +
+                                                    kNumberOfNavicSatellite;  //<! Total number of GNSS satellites
+
   // Parameters for receiver
   const size_t component_id_;  //!< Receiver ID
 
@@ -152,8 +156,8 @@ class GnssReceiver : public Component, public logger::ILoggable {
   AntennaModel antenna_model_;            //!< Antenna model
 
   // GNSS observation
-  double pseudorange_noise_standard_deviation_m_;           //!< Random noise for pseudorange [m]
-  math::Vector<kNumberOfGpsSatellite> pseudorange_list_m_;  //!< Pseudorange list for each GPS satellite
+  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
 
   // Simple position observation
   randomization::NormalRand position_random_noise_ecef_m_[3];    //!< Random noise for position at the ECEF frame [m]
@@ -210,6 +214,25 @@ class GnssReceiver : public Component, public logger::ILoggable {
    * @param [in] gnss_system_id: ID of target GNSS satellite
    */
   void SetGnssInfo(const math::Vector<3> antenna_to_satellite_i_m, const math::Quaternion quaternion_i2b, const size_t gnss_system_id);
+  /**
+   * @fn CalcGeometricDistance
+   * @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(const size_t gnss_system_id);
+  /**
+   * @fn CalcPseudorange
+   * @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]
+   */
+  double CalcPseudorange(const size_t gnss_id);
+  /**
+   * @fn SetGnssObservationList
+   * @brief Calculate and set the GNSS observation list for each GNSS satellite
+   */
+  void SetGnssObservationList();
   /**
    * @fn AddNoise
    * @brief Substitutional method for "Measure" in other sensor models inherited Sensor class