Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add effect of radio wave propagation time to geometric distance calculation #739

Merged
merged 12 commits into from
Feb 4, 2025
Merged

Add effect of radio wave propagation time to geometric distance calculation #739

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
6493fa2
Add pseudorange logging
fukudakazuya Jan 30, 2025
42dbb3d
Modify gnss receiver ini file
fukudakazuya Jan 30, 2025
e925fe8
Modify pseudorange logging
fukudakazuya Jan 31, 2025
379db4b
Add signal travel time
fukudakazuya Jan 31, 2025
3fe5cdb
Modify geometric calculation
fukudakazuya Jan 31, 2025
fe9bef9
Modify sagnac correction
fukudakazuya Feb 1, 2025
d2ddd5a
Modify gnss receiver
fukudakazuya Feb 3, 2025
0a037dd
Add function GetCurrentEpochTime
fukudakazuya Feb 3, 2025
a4802b8
Add constructor initialized with a double-precision time expression
fukudakazuya Feb 3, 2025
b485cb8
Modify ConvertCalendarDayToEpochTime
fukudakazuya Feb 3, 2025
1a7c206
Modify geometric calculation
fukudakazuya Feb 3, 2025
d6e356d
Modify EpochTime constructor
fukudakazuya Feb 4, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions settings/sample_satellite/components/gnss_receiver.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,9 @@ white_noise_standard_deviation_velocity_ecef_m_s(0) = 1.0
white_noise_standard_deviation_velocity_ecef_m_s(1) = 1.5
white_noise_standard_deviation_velocity_ecef_m_s(2) = 2.0

// Logging
pseudorange_logging = DISABLE

[POWER_PORT]
minimum_voltage_V = 3.3
assumed_power_consumption_W = 1.0
85 changes: 59 additions & 26 deletions src/components/real/aocs/gnss_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,14 +17,16 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo
const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c,
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)
const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const bool is_log_pseudorange_enabled,
const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites,
const environment::SimulationTime* simulation_time)
: Component(prescaler, clock_generator),
component_id_(component_id),
antenna_position_b_m_(antenna_position_b_m),
quaternion_b2c_(quaternion_b2c),
half_width_deg_(half_width_deg),
antenna_model_(antenna_model),
is_logged_pseudorange_(is_log_pseudorange_enabled),
dynamics_(dynamics),
gnss_satellites_(gnss_satellites),
simulation_time_(simulation_time) {
Expand All @@ -40,14 +42,16 @@ GnssReceiver::GnssReceiver(const int prescaler, environment::ClockGenerator* clo
const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c,
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)
const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const bool is_log_pseudorange_enabled,
const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites,
const environment::SimulationTime* simulation_time)
: Component(prescaler, clock_generator, power_port),
component_id_(component_id),
antenna_position_b_m_(antenna_position_b_m),
quaternion_b2c_(quaternion_b2c),
half_width_deg_(half_width_deg),
antenna_model_(antenna_model),
is_logged_pseudorange_(is_log_pseudorange_enabled),
dynamics_(dynamics),
gnss_satellites_(gnss_satellites),
simulation_time_(simulation_time) {
Expand Down Expand Up @@ -187,14 +191,30 @@ void GnssReceiver::SetGnssInfo(const math::Vector<3> antenna_to_satellite_i_m, c
}

double GnssReceiver::CalcGeometricDistance_m(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();
math::Vector<3> gnss_position_at_signal_arrival_ecef_m = gnss_satellites_->GetPosition_ecef_m(gnss_system_id);

environment::UTC current_utc = simulation_time_->GetCurrentUtc();
time_system::DateTime current_date_time((size_t)current_utc.year, (size_t)current_utc.month, (size_t)current_utc.day, (size_t)current_utc.hour,
(size_t)current_utc.minute, current_utc.second);
time_system::EpochTime current_epoch_time(current_date_time);
double signal_travel_time_s = (gnss_position_at_signal_arrival_ecef_m - position_true_ecef_m).CalcNorm() / environment::speed_of_light_m_s;
time_system::EpochTime signal_emission_epoch_time(current_epoch_time.GetTimeWithFraction_s() - signal_travel_time_s);

math::Vector<3> gnss_position_at_signal_emission_ecef_m = gnss_satellites_->GetPosition_ecef_m(gnss_system_id, signal_emission_epoch_time);

math::Vector<3> earth_angular_velocity_rad_s{0.0};
earth_angular_velocity_rad_s[2] = environment::earth_mean_angular_velocity_rad_s;

double sagnac_correction_m =
InnerProduct(OuterProduct(gnss_position_at_signal_emission_ecef_m, position_true_ecef_m), earth_angular_velocity_rad_s) /
environment::speed_of_light_m_s;

double geometric_distance_m = (gnss_position_at_signal_emission_ecef_m - position_true_ecef_m).CalcNorm() + sagnac_correction_m;
return geometric_distance_m;
}

double GnssReceiver::CalcPseudorange_m(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_m(gnss_system_id);
Expand Down Expand Up @@ -231,11 +251,24 @@ void GnssReceiver::ConvertJulianDayToGpsTime(const double julian_day) {
gps_time_s_ = (elapsed_day - (double)(gps_time_week_)*kDayInWeek) * kSecInDay;
}

typedef struct _gnss_receiver_param {
int prescaler;
AntennaModel antenna_model;
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;
bool is_log_pseudorange_enabled;
} GnssReceiverParam;

std::string GnssReceiver::GetLogHeader() const // For logs
{
std::string str_tmp = "";
const std::string sensor_id = std::to_string(static_cast<long long>(component_id_));
std::string sensor_name = "gnss_receiver" + sensor_id + "_";
GnssReceiverParam gnss_receiver_param;

str_tmp += logger::WriteScalar(sensor_name + "measured_utc_time_year");
str_tmp += logger::WriteScalar(sensor_name + "measured_utc_time_month");
Expand All @@ -250,8 +283,12 @@ 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");

if (is_logged_pseudorange_) {
printf("%d\n", gnss_receiver_param.is_log_pseudorange_enabled);
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;
Expand All @@ -260,6 +297,8 @@ std::string GnssReceiver::GetLogHeader() const // For logs
std::string GnssReceiver::GetLogValue() const // For logs
{
std::string str_tmp = "";
GnssReceiverParam gnss_receiver_param;

str_tmp += logger::WriteScalar(utc_.year);
str_tmp += logger::WriteScalar(utc_.month);
str_tmp += logger::WriteScalar(utc_.day);
Expand All @@ -273,8 +312,11 @@ 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);

if (is_logged_pseudorange_) {
for (size_t gps_index = 0; gps_index < kNumberOfGpsSatellite; gps_index++) {
str_tmp += logger::WriteScalar(pseudorange_list_m_[gps_index], 16);
}
}

return str_tmp;
Expand All @@ -292,17 +334,6 @@ AntennaModel SetAntennaModel(const std::string antenna_model) {
}
}

typedef struct _gnss_receiver_param {
int prescaler;
AntennaModel antenna_model;
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;

GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const environment::GnssSatellites* gnss_satellites, const size_t component_id) {
GnssReceiverParam gnss_receiver_param;

Expand Down Expand Up @@ -330,7 +361,8 @@ GnssReceiverParam ReadGnssReceiverIni(const std::string file_name, const environ
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);

gnss_receiver_param.is_log_pseudorange_enabled = gnssr_conf.ReadEnable(GSection, "pseudorange_logging");
printf("%d\n", gnss_receiver_param.is_log_pseudorange_enabled);
return gnss_receiver_param;
}

Expand All @@ -341,7 +373,8 @@ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, cons

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.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);
gr_param.velocity_noise_standard_deviation_ecef_m_s, gr_param.is_log_pseudorange_enabled, dynamics, gnss_satellites,
simulation_time);
return gnss_r;
}

Expand All @@ -355,8 +388,8 @@ GnssReceiver InitGnssReceiver(environment::ClockGenerator* clock_generator, Powe

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.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);
gr_param.position_noise_standard_deviation_ecef_m, gr_param.velocity_noise_standard_deviation_ecef_m_s,
gr_param.is_log_pseudorange_enabled, dynamics, gnss_satellites, simulation_time);
return gnss_r;
}

Expand Down
10 changes: 7 additions & 3 deletions src/components/real/aocs/gnss_receiver.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,15 +57,17 @@ class GnssReceiver : public Component, public logger::ILoggable {
* @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] is_log_pseudorange_enabled: Enable flag to log output pseudorange
* @param [in] dynamics: Dynamics information
* @param [in] gnss_satellites: GNSS Satellites information
* @param [in] simulation_time: Simulation time information
*/
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 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);
const math::Vector<3> velocity_noise_standard_deviation_ecef_m_s, const bool is_log_pseudorange_enabled,
const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites,
const environment::SimulationTime* simulation_time);
/**
* @fn GnssReceiver
* @brief Constructor with power port
Expand All @@ -79,6 +81,7 @@ class GnssReceiver : public Component, public logger::ILoggable {
* @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] is_log_pseudorange_enabled: Enable flag to log output pseudorange
* @param [in] dynamics: Dynamics information
* @param [in] gnss_satellites: GNSS Satellites information
* @param [in] simulation_time: Simulation time information
Expand All @@ -87,7 +90,7 @@ class GnssReceiver : public Component, public logger::ILoggable {
const AntennaModel antenna_model, const math::Vector<3> antenna_position_b_m, const math::Quaternion quaternion_b2c,
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 bool is_log_pseudorange_enabled, const dynamics::Dynamics* dynamics, const environment::GnssSatellites* gnss_satellites,
const environment::SimulationTime* simulation_time);

// Override functions for Component
Expand Down Expand Up @@ -158,6 +161,7 @@ class GnssReceiver : public Component, public logger::ILoggable {
// GNSS observation
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
bool is_logged_pseudorange_; //!< Flag for log output of pseudorange

// Simple position observation
randomization::NormalRand position_random_noise_ecef_m_[3]; //!< Random noise for position at the ECEF frame [m]
Expand Down
Loading