Add GNSS receiver pseudorange calculation

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.plot(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()

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

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),
@@ -24,6 +25,7 @@ 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) {
@@ -36,7 +38,8 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo
 
 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),
@@ -45,6 +48,7 @@ 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) {
@@ -64,6 +68,18 @@ void GnssReceiver::MainRoutine(const int time_count) {
   math::Quaternion quaternion_i2b = dynamics_->GetAttitude().GetQuaternion_i2b();
   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;
+  }
+
   if (is_gnss_visible_) {
     // Antenna of GNSS-R can detect GNSS signal
     position_ecef_m_ = dynamics_->GetOrbit().GetPosition_ecef_m();
@@ -217,6 +233,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 +256,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 +281,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 +310,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 +323,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 +337,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;
 }
 

src/components/real/aocs/gnss_receiver.hpp

@@ -18,6 +18,15 @@
 
 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
@@ -54,6 +63,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 +72,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 +85,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 +94,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
   /**
@@ -139,6 +151,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
+  double pseudorange_noise_standard_deviation_m_;           //!< Random noise for pseudorange [m]
+  math::Vector<kNumberOfGpsSatellite> pseudorange_list_m_;  //!< 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]