Merge pull request #602 from ut-issl/feature/final-check-gnss-receiver

Final check for GnssReceiver refactoring

scripts/Plot/plot_gnss_receiver.py

@@ -49,14 +49,14 @@
 # Read S2E CSV
 time = read_scalar_from_csv(read_file_name, 'elapsed_time[s]')
 
-measured_position_eci_m = read_3d_vector_from_csv(read_file_name, 'gnss_receiver1_measured_position_eci', 'm')
-true_position_eci_m = read_3d_vector_from_csv(read_file_name, 'spacecraft_position_i', 'm')
+measured_position_ecef_m = read_3d_vector_from_csv(read_file_name, 'gnss_receiver1_measured_position_ecef', 'm')
+true_position_ecef_m = read_3d_vector_from_csv(read_file_name, 'spacecraft_position_ecef', 'm')
 
 number_of_visible_satellites = read_scalar_from_csv(read_file_name, 'gnss_receiver1_number_of_visible_satellites')
 satellite_visible_flag = read_scalar_from_csv(read_file_name, 'gnss_receiver1_satellite_visible_flag')
 
 # Statistics
-error_m = measured_position_eci_m[:, 1:] - true_position_eci_m[:, 1:]
+error_m = measured_position_ecef_m[:, 1:] - true_position_ecef_m[:, 1:]
 average = [0.0, 0.0, 0.0]
 standard_deviation = [0.0, 0.0, 0.0]
 for i in range(3):
@@ -68,20 +68,20 @@
 #
 unit = ' m'
 fig, axis = plt.subplots(5, 1, squeeze = False, tight_layout = True, sharex = True)
-axis[0, 0].plot(time[0], measured_position_eci_m[0], marker=".", c="red", label="MEASURED-X")
-axis[0, 0].plot(time[0], true_position_eci_m[0], marker=".", c="orange", label="TRUE-X")
+axis[0, 0].plot(time[0], measured_position_ecef_m[0], marker=".", c="red", label="MEASURED-X")
+axis[0, 0].plot(time[0], true_position_ecef_m[0], marker=".", c="orange", label="TRUE-X")
 axis[0, 0].text(0.01, 0.99, "Error average:" + format(average[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes)
 axis[0, 0].text(0.01, 0.79, "Standard deviation:" + format(standard_deviation[0], '+.2e') + unit, verticalalignment = 'top', transform = axis[0, 0].transAxes)
 axis[0, 0].legend(loc = 'upper right')
 
-axis[1, 0].plot(time[0], measured_position_eci_m[1], marker=".", c="green", label="MEASURED-Y")
-axis[1, 0].plot(time[0], true_position_eci_m[1], marker=".", c="yellow", label="TRUE-Y")
+axis[1, 0].plot(time[0], measured_position_ecef_m[1], marker=".", c="green", label="MEASURED-Y")
+axis[1, 0].plot(time[0], true_position_ecef_m[1], marker=".", c="yellow", label="TRUE-Y")
 axis[1, 0].text(0.01, 0.99, "Error average:" + format(average[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes)
 axis[1, 0].text(0.01, 0.79, "Standard deviation:" + format(standard_deviation[1], '+.2e') + unit, verticalalignment = 'top', transform = axis[1, 0].transAxes)
 axis[1, 0].legend(loc = 'upper right')
 
-axis[2, 0].plot(time[0], measured_position_eci_m[2], marker=".", c="blue", label="MEASURED-Z")
-axis[2, 0].plot(time[0], true_position_eci_m[2], marker=".", c="purple", label="TRUE-Z")
+axis[2, 0].plot(time[0], measured_position_ecef_m[2], marker=".", c="blue", label="MEASURED-Z")
+axis[2, 0].plot(time[0], true_position_ecef_m[2], marker=".", c="purple", label="TRUE-Z")
 axis[2, 0].text(0.01, 0.99, "Error average:" + format(average[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes)
 axis[2, 0].text(0.01, 0.79, "Standard deviation:" + format(standard_deviation[2], '+.2e') + unit, verticalalignment = 'top', transform = axis[2, 0].transAxes)
 axis[2, 0].legend(loc = 'upper right')
@@ -94,7 +94,7 @@
 axis[4, 0].legend(loc = 'upper right')
 axis[4, 0].set_ylim(0, max(number_of_visible_satellites[0]) + 2)
 
-fig.suptitle("GNSS Receiver Spacecraft position @ ECI")
+fig.suptitle("GNSS Receiver Spacecraft position @ ECEF")
 fig.supylabel("Position [m]")
 fig.supxlabel("Time [s]")
 

src/components/real/aocs/gnss_receiver.cpp

@@ -106,12 +106,13 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m,
  // Cone model
  gnss_information_list_.clear();
 
- // antenna normal vector at inertial frame
- libra::Vector<3> antenna_direction_c(0.0);
- antenna_direction_c[2] = 1.0;
- libra::Vector<3> antenna_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_direction_c);
- libra::Vector<3> antenna_direction_i = quaternion_i2b.InverseFrameConversion(antenna_direction_b);
+ // Antenna pointing direction vector at inertial frame
+ libra::Vector<3> antenna_pointing_direction_c(0.0);
+ antenna_pointing_direction_c[2] = 1.0;
+ libra::Vector<3> antenna_pointing_direction_b = quaternion_b2c_.InverseFrameConversion(antenna_pointing_direction_c);
+ libra::Vector<3> antenna_pointing_direction_i = quaternion_i2b.InverseFrameConversion(antenna_pointing_direction_b);
 
+ // Antenna position vector at inertial frame
  libra::Vector<3> antenna_position_i_m = position_true_eci_m + quaternion_i2b.InverseFrameConversion(antenna_position_b_m_);
 
  // initialize
@@ -125,25 +126,28 @@ void GnssReceiver::CheckAntennaCone(const libra::Vector<3> position_true_eci_m,
  libra::Vector<3> antenna_to_gnss_satellite_i_m = gnss_satellite_position_i_m - antenna_position_i_m;
  libra::Vector<3> antenna_to_gnss_satellite_direction_i = antenna_to_gnss_satellite_i_m.CalcNormalizedVector();
 
- // check gnss satellites are visible from antenna
+ // Check GNSS satellites are visible from the receiver(not care antenna direction)
+ bool is_gnss_satellite_visible_from_receiver = false;
  double inner1 = InnerProduct(antenna_position_i_m, gnss_satellite_position_i_m);
- bool is_satellite_visible = false;
- if (inner1 > 0) {
- is_satellite_visible = true;
- } else {
- Vector<3> tmp =
- antenna_position_i_m + InnerProduct(-antenna_position_i_m, antenna_to_gnss_satellite_direction_i) * antenna_to_gnss_satellite_i_m;
- if (tmp.CalcNorm() < environment::earth_equatorial_radius_m) {
- // There is earth between antenna and gnss
- is_satellite_visible = false;
+ if (inner1 > 0.0) { // GNSS satellite and receiver are in the same hemisphere
+ is_gnss_satellite_visible_from_receiver = true;
+ } else { // GNSS satellite is in the another hemisphere
+ double angle_bw_earth_center_and_edge_rad = asin(environment::earth_equatorial_radius_m / antenna_position_i_m.CalcNorm());
+ double angle_bw_earth_center_and_gnss_rad =
+ acos(InnerProduct(-antenna_position_i_m.CalcNormalizedVector(), antenna_to_gnss_satellite_direction_i));
+
+ if (angle_bw_earth_center_and_edge_rad < angle_bw_earth_center_and_gnss_rad) {
+ // There is no Earth between receiver and GNSS satellite
+ is_gnss_satellite_visible_from_receiver = true;
  } else {
- // There is not earth between antenna and gnss
- is_satellite_visible = true;
+ // There is Earth between receiver and GNSS satellite
+ is_gnss_satellite_visible_from_receiver = false;
  }
  }
 
- double inner2 = InnerProduct(antenna_direction_i, antenna_to_gnss_satellite_direction_i);
- if (inner2 > cos(half_width_deg_ * libra::deg_to_rad) && is_satellite_visible) {
+ // Check GNSS satellites are in the antenna half width angle
+ double inner2 = InnerProduct(antenna_pointing_direction_i, antenna_to_gnss_satellite_direction_i);
+ if (inner2 > cos(half_width_deg_ * libra::deg_to_rad) && is_gnss_satellite_visible_from_receiver) {
  // is visible
  visible_satellite_number_++;
  SetGnssInfo(antenna_to_gnss_satellite_i_m, quaternion_i2b, i);
@@ -255,7 +259,7 @@ typedef struct _gnss_receiver_param {
  libra::Vector<3> velocity_noise_standard_deviation_ecef_m_s;
 } GnssReceiverParam;
 
-GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSatellites* gnss_satellites, const int component_id) {
+GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const GnssSatellites* gnss_satellites, const size_t component_id) {
  GnssReceiverParam gnss_receiver_param;
 
  IniAccess gnssr_conf(file_name);

src/dynamics/orbit/orbit.cpp

@@ -68,6 +68,7 @@ std::string Orbit::GetLogHeader() const {
  std::string str_tmp = "";
 
  str_tmp += WriteVector("spacecraft_position", "i", "m", 3);
+ str_tmp += WriteVector("spacecraft_position", "ecef", "m", 3);
  str_tmp += WriteVector("spacecraft_velocity", "i", "m/s", 3);
  str_tmp += WriteVector("spacecraft_velocity", "b", "m/s", 3);
  str_tmp += WriteVector("spacecraft_acceleration", "i", "m/s2", 3);
@@ -82,6 +83,7 @@ std::string Orbit::GetLogValue() const {
  std::string str_tmp = "";
 
  str_tmp += WriteVector(spacecraft_position_i_m_, 16);
+ str_tmp += WriteVector(spacecraft_position_ecef_m_, 16);
  str_tmp += WriteVector(spacecraft_velocity_i_m_s_, 10);
  str_tmp += WriteVector(spacecraft_velocity_b_m_s_, 10);
  str_tmp += WriteVector(spacecraft_acceleration_i_m_s2_, 10);