Modify pseudorange calculation

src/components/real/aocs/gnss_receiver.cpp

@@ -71,13 +71,13 @@ void GnssReceiver::MainRoutine(const int time_count) {
   // 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 = math::Vector<3>(0.0);
-    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;
+    math::Vector<3> gnss_satellite_position_i_m = gnss_satellites_->GetPosition_eci_m(i);
+    math::Vector<3> position_true_i_m = dynamics_->GetOrbit().GetPosition_i_m();
+    double geometric_distance_m = (gnss_satellite_position_i_m - position_true_i_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_) {