Merge pull request #730 from ut-issl/feature/gnss-receiver-update-pseudorange

fukudakazuya · web-flow · commit 1577a5f48e50 · 2025-01-10T11:26:46.000+09:00
Add GNSS receiver pseudorange calculation
diff --git a/scripts/Plot/plot_gnss_receiver_pseudorange.py b/scripts/Plot/plot_gnss_receiver_pseudorange.py
@@ -0,0 +1,83 @@
+#
+# Plot GNSS Receiver Pseudorange
+#
+# arg[1] : read_file_tag : time tag for default CSV output log file. ex. 220627_142946
+#
+
+#
+# Import
+#
+# plots
+import matplotlib.pyplot as plt
+# numpy
+import numpy as np
+# local function
+from common import find_latest_log_tag
+from common import add_log_file_arguments
+from common import read_3d_vector_from_csv
+from common import read_scalar_from_csv
+# arguments
+import argparse
+
+# Arguments
+aparser = argparse.ArgumentParser()
+aparser = add_log_file_arguments(aparser)
+aparser.add_argument('--no-gui', action='store_true')
+args = aparser.parse_args()
+
+#
+# Read Arguments
+#
+# log file path
+path_to_logs = args.logs_dir
+
+read_file_tag = args.file_tag
+if read_file_tag == None:
+  print("file tag does not found. use latest.")
+  read_file_tag = find_latest_log_tag(path_to_logs)
+
+print("log: " + read_file_tag)
+
+#
+# CSV file name
+#
+read_file_name  = path_to_logs + '/' + 'logs_' + read_file_tag + '/' + read_file_tag + '_default.csv'
+
+#
+# Data read and edit
+#
+# Read S2E CSV
+time = read_scalar_from_csv(read_file_name, 'elapsed_time[s]')
+
+plt.figure(0)
+for gps_idx in range(32):
+  gps_str = 'GPS' + str(gps_idx)
+  pseudorange = read_scalar_from_csv(read_file_name, gps_str + '_pseudorange[m]')
+  plt.scatter(time[0][1:], pseudorange[0][1:], marker=".", label=gps_str)
+
+plt.title("GPS Psuedorange")
+plt.xlabel("Time [s]")
+plt.ylabel("Psuedorange [m]")
+plt.legend(fontsize=7, loc="upper right")
+
+
+plt.figure(1)
+spacecraft_position_ecef_m = read_3d_vector_from_csv(read_file_name, 'spacecraft_position_ecef', 'm')[:, :-1]
+for gps_idx in range(32):
+  gps_str = 'GPS' + str(gps_idx)
+  pseudorange = read_scalar_from_csv(read_file_name, gps_str + '_pseudorange[m]')[:, 1:]
+  gps_position = read_3d_vector_from_csv(read_file_name, gps_str + '_position_ecef', 'm')[:, 1:]
+  geometric_range = np.linalg.norm(gps_position - spacecraft_position_ecef_m, axis=0)
+  pseudorange_error = pseudorange - geometric_range
+  plt.scatter(time[0][:-1], pseudorange_error, marker=".", label=gps_str)
+
+plt.title("GPS Pseudorange Error")
+plt.xlabel("Time [s]")
+plt.ylabel("Pseudorange Error [m]")
+plt.legend(fontsize=7, loc="upper right")
+
+# Data save
+if args.no_gui:
+  plt.savefig(read_file_tag + "_gnss_receiver_pseudorange.png") # save last figure only
+else:
+  plt.show()
diff --git a/settings/sample_satellite/components/gnss_receiver.ini b/settings/sample_satellite/components/gnss_receiver.ini
@@ -24,6 +24,9 @@ antenna_model = SIMPLE
 // Antenna half width [deg]
 antenna_half_width_deg = 60
 
+// Random noise for pseudorange observation
+white_noise_standard_deviation_pseudorange_m = 10.0
+
 // Random noise for simple position observation
 white_noise_standard_deviation_position_ecef_m(0) = 2000.0
 white_noise_standard_deviation_position_ecef_m(1) = 1000.0
diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp
@@ -15,7 +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 math::Vector<3> position_noise_standard_deviation_ecef_m,
+                           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 dynamics::Dynamics* dynamics,
                            const environment::GnssSatellites* gnss_satellites, const environment::SimulationTime* simulation_time)
     : Component(prescaler, clock_generator),
@@ -32,11 +33,13 @@ 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,
                            const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c,
-                           const double half_width_deg, const math::Vector<3> position_noise_standard_deviation_ecef_m,
+                           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 dynamics::Dynamics* dynamics,
                            const environment::GnssSatellites* gnss_satellites, const environment::SimulationTime* simulation_time)
     : Component(prescaler, clock_generator, power_port),
@@ -53,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) {
@@ -64,6 +68,9 @@ void GnssReceiver::MainRoutine(const int time_count) {
   math::Quaternion quaternion_i2b = dynamics_->GetAttitude().GetQuaternion_i2b();
   CheckAntenna(position_true_eci, quaternion_i2b);
 
+  // Pseudorange calculation
+  SetGnssObservationList();
+
   if (is_gnss_visible_) {
     // Antenna of GNSS-R can detect GNSS signal
     position_ecef_m_ = dynamics_->GetOrbit().GetPosition_ecef_m();
@@ -179,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_m(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_m(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_m(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_m(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];
@@ -217,6 +250,9 @@ std::string GnssReceiver::GetLogHeader() const  // For logs
   str_tmp += logger::WriteScalar(sensor_name + "measured_altitude", "m");
   str_tmp += logger::WriteScalar(sensor_name + "satellite_visible_flag");
   str_tmp += logger::WriteScalar(sensor_name + "number_of_visible_satellites");
+  for (size_t gps_index = 0; gps_index < kNumberOfGpsSatellite; gps_index++) {
+    str_tmp += logger::WriteScalar("GPS" + std::to_string(gps_index) + "_pseudorange", "m");
+  }
 
   return str_tmp;
 }
@@ -237,6 +273,9 @@ std::string GnssReceiver::GetLogValue() const  // For logs
   str_tmp += logger::WriteScalar(geodetic_position_.GetAltitude_m(), 10);
   str_tmp += logger::WriteScalar(is_gnss_visible_);
   str_tmp += logger::WriteScalar(visible_satellite_number_);
+  for (size_t gps_index = 0; gps_index < kNumberOfGpsSatellite; gps_index++) {
+    str_tmp += logger::WriteScalar(pseudorange_list_m_[gps_index], 16);
+  }
 
   return str_tmp;
 }
@@ -259,6 +298,7 @@ typedef struct _gnss_receiver_param {
   math::Vector<3> antenna_pos_b;
   math::Quaternion quaternion_b2c;
   double half_width_deg;
+  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;
 } GnssReceiverParam;
@@ -287,6 +327,7 @@ 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.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);
 
@@ -299,8 +340,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.position_noise_standard_deviation_ecef_m, gr_param.velocity_noise_standard_deviation_ecef_m_s,
-                      dynamics, gnss_satellites, simulation_time);
+                      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, dynamics, gnss_satellites, simulation_time);
   return gnss_r;
 }
 
@@ -313,8 +354,9 @@ 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.position_noise_standard_deviation_ecef_m,
-                      gr_param.velocity_noise_standard_deviation_ecef_m_s, dynamics, gnss_satellites, simulation_time);
+                      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, dynamics,
+                      gnss_satellites, simulation_time);
   return gnss_r;
 }
 
diff --git a/src/components/real/aocs/gnss_receiver.hpp b/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] 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]
    * @param [in] dynamics: Dynamics information
@@ -62,9 +63,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 math::Vector<3> position_noise_standard_deviation_ecef_m, const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s,
-               const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites,
-               const environment::SimulationTime* simulation_time);
+               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 dynamics::Dynamics* dynamics,
+               const environment::GnssSatellites* gnss_satellites, const environment::SimulationTime* simulation_time);
   /**
    * @fn GnssReceiver
    * @brief Constructor with power port
@@ -75,6 +76,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] 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]
    * @param [in] dynamics: Dynamics information
@@ -83,9 +85,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 math::Vector<3> position_noise_standard_deviation_ecef_m,
-               const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const dynamics::Dynamics* dynamics,
-               const environment::GnssSatellites* gnss_satellites, const environment::SimulationTime* simulation_time);
+               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 dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites,
+               const environment::SimulationTime* simulation_time);
 
   // Override functions for Component
   /**
@@ -130,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
 
@@ -139,6 +155,10 @@ class GnssReceiver : public Component, public logger::ILoggable {
   double half_width_deg_ = 0.0;           //!< Half width of the antenna cone model [deg]
   AntennaModel antenna_model_;            //!< Antenna model
 
+  // GNSS observation
+  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]
   randomization::NormalRand velocity_random_noise_ecef_m_s_[3];  //!< Random noise for velocity at the ECEF frame [m]
@@ -194,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_m(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_m(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
