From 1c28bc68b34b43a9e7c0d766fa26ec12696c994d Mon Sep 17 00:00:00 2001 From: fukudakazuya Date: Thu, 9 Jan 2025 20:29:34 +0900 Subject: [PATCH] Modify pseudorange calculation --- src/components/real/aocs/gnss_receiver.cpp | 41 ++++++++++++++------ src/components/real/aocs/gnss_receiver.hpp | 45 ++++++++++++++++------ 2 files changed, 63 insertions(+), 23 deletions(-) diff --git a/src/components/real/aocs/gnss_receiver.cpp b/src/components/real/aocs/gnss_receiver.cpp index ef21bcd14..b2dc7112a 100644 --- a/src/components/real/aocs/gnss_receiver.cpp +++ b/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]; diff --git a/src/components/real/aocs/gnss_receiver.hpp b/src/components/real/aocs/gnss_receiver.hpp index 695cf492a..b1f9f8ca4 100644 --- a/src/components/real/aocs/gnss_receiver.hpp +++ b/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; // pseudorange_list_m_; //!< Pseudorange list for each GPS satellite + randomization::NormalRand pseudorange_random_noise_m_; //!< Random noise for pseudorange [m] + std::vector 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