Skip to content

Commit

Permalink
Modify pseudorange calculation
Browse files Browse the repository at this point in the history
  • Loading branch information
@fukudakazuya
fukudakazuya committed Jan 9, 2025
1 parent 48e6d6d commit 1c28bc6
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 23 deletions.
41 changes: 29 additions & 12 deletions src/components/real/aocs/gnss_receiver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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,
Expand All @@ -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) {
Expand All @@ -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) {
Expand All @@ -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
Expand Down Expand Up @@ -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];
Expand Down
45 changes: 34 additions & 11 deletions src/components/real/aocs/gnss_receiver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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; //<! Total number of GNSS satellites

// Parameters for receiver
const size_t component_id_; //!< Receiver ID

Expand All @@ -152,8 +156,8 @@ class GnssReceiver : public Component, public logger::ILoggable {
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
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]
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit 1c28bc6

Please sign in to comment.