Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/refactor-keidai' into guillemc
Browse files Browse the repository at this point in the history
  • Loading branch information
@gcasadesus
gcasadesus committed Dec 24, 2024
2 parents af4b2f3 + f08ebce commit 0c5c175
Show file tree
Hide file tree
Showing 8 changed files with 242 additions and 189 deletions.
41 changes: 23 additions & 18 deletions examples/cpp/state_estimation/example_ekf_elfo_gps.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -200,7 +200,7 @@ void PrintProgressHeader() {
std::cout << "Run Simulation" << std::endl;
std::cout << " " << std::endl;
std::cout << " " << std::endl;
std::cout << "Time [min] | Pos Err [m] | Vel Err [mm/s] | Clk Bias Err [ms]" << std::endl;
std::cout << "Time [min] | Pos Err [m] | Vel Err [mm/s] | Clk Bias Err [m]" << std::endl;
std::cout << "--------------------------------------------------------------" << std::endl;
}

Expand Down Expand Up @@ -472,7 +472,11 @@ int main() {
* Simulation Parameters
*********************************************/
// Get initial epoch
auto gps_const = GnssConstellation();
GnssConstellation gps_const = GnssConstellation();
Ptr<CartesianTwoBodyDynamics> dyn_earth_tb = std::make_shared<CartesianTwoBodyDynamics>(
GM_EARTH); // use 2d earth dynamics to propagate GPS constellation
Ptr<GnssChannel> channel = std::make_shared<GnssChannel>();
gps_const.InitializeWithTle("GPS", "gps.txt", dyn_earth_tb, channel); // example gps file
double epoch0 = gps_const.GetEpoch();

// Time
Expand Down Expand Up @@ -521,17 +525,22 @@ int main() {
double vel_err = 1e-3; // Initial Velocity error [km/s]
double clk_bias_err = 1e-6; // Initial Clock bias error [s]
double clk_drift_err = 1e-9; // Initial Clock drift error [s/s]
double sigma_acc = 1e-12; // Process noise Acceleration [km/s^2] <-- tune
double sigma_acc = 1e-10; // Process noise Acceleration [km/s^2] <-- tune
// this for optimal performance!

// Debug mode
bool plot_results = false;
bool debug_jacobian = false;
bool print_debug = false;
bool debug_ekf = false;
bool debug_ekf_error_only = true; // Print only error for EKF debugging
bool print_debug =false;
bool debug_ekf = false; // Print EKF debug info
bool debug_ekf_error_only = false; // Print only error for EKF debugging
bool no_meas = false; // set to true to turn off measurements

if (print_debug) {
tf = t0 + 2 * Dt;
print_every = Dt;
}

/**********************************************
* Setup
* ********************************************/
Expand All @@ -551,8 +560,6 @@ int main() {
iparams.abstol = 1e-12;
iparams.reltol = 1e-12;

auto dyn_earth_tb = std::make_shared<CartesianTwoBodyDynamics>(
GM_EARTH); // use 2d earth dynamics to propagate GPS constellation
auto dyn_est = MakePtr<NBodyDynamics<Real>>(IntegratorType::RKF45); // Filter Dynamics
auto dyn_true = MakePtr<NBodyDynamics<double>>(IntegratorType::RKF45); // true dynamics

Expand Down Expand Up @@ -580,12 +587,6 @@ int main() {
dyn_clk_true.SetNoise(true);
dyn_clk_est.SetNoise(false);

// GPS constellation
auto channel = std::make_shared<GnssChannel>();
gps_const.SetChannel(channel);
gps_const.SetDynamics(dyn_earth_tb);
gps_const.LoadTleFile("gps"); // example gps file

// Print Time
std::string epoch_string = sp::TAItoStringUTC(epoch0, 3);
std::cout << "Initial Epoch: " << epoch_string << std::endl;
Expand Down Expand Up @@ -765,7 +766,7 @@ int main() {

// Compute Estimation
est_err = ComputeEstimationErrors(moon_sat, &ekf); // pos, vel, clkb, clkd error
PrintProgress(t.val(), est_err(0), est_err(1), est_err(2));
PrintProgress((t-t0).val(), est_err(0), est_err(1), est_err(2));
for (t = t0; t < tf; t += Dt) {
time_index += 1;
epoch += Dt; // first propagate to the next epoch
Expand All @@ -777,8 +778,10 @@ int main() {
// Get True Measurement
VecX z_true;
auto measall = receiver->GetMeasurement(epoch);

auto meas = measall.ExtractSignal("L1");
int num_sat = meas.GetTrackedSatelliteNum();

num_meas(time_index) = num_sat;

if (!no_meas) {
Expand All @@ -805,13 +808,15 @@ int main() {
error_mat.col(time_index) = est_err;

// Print progress
if (fmod(t.val(), print_every) < 1e-3) {
PrintProgress(t.val(), est_err(0), est_err(1), est_err(2));
PrintEKFDebugInfo(time_index, moon_sat, &ekf, true);
if (fmod((t-t0).val(), print_every) < 1e-3) {
PrintProgress((t-t0).val(), est_err(0), est_err(1), est_err(2));
// PrintEKFDebugInfo(time_index, moon_sat, &ekf, true);
}

// print measurement residuals
if ((print_debug) && (!no_meas)) {
std::cout << "z_true: " << z_true.transpose() << std::endl;
std::cout << "z_pred: " << ekf.z_prior_.transpose() << std::endl;
PrintEKFDebugInfo(time_index, moon_sat, &ekf, debug_ekf_error_only);
}
}
Expand Down
27 changes: 22 additions & 5 deletions include/lupnt/agents/gnss_constellation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,19 +23,37 @@
namespace lupnt {

class GnssConstellation {

private:
std::vector<Ptr<Spacecraft>> satellites_;
Ptr<IDynamics> dynamics_;
Ptr<GnssChannel> channel_;
double epoch_; // in TAI

public:
// Setters
void SetChannel(Ptr<GnssChannel> ch) { channel_ = ch; }
void SetDynamics(Ptr<IDynamics> dyn) { dynamics_ = dyn; }
void SetEpoch(double ep) { epoch_ = ep; }
void SetChannel(Ptr<GnssChannel> ch) {
channel_ = ch;
}

void LoadTleFile(std::string_view gnss_type, std::string filename);

public:
void InitializeWithTle(std::string gnss_type, std::string tle_filename,
Ptr<IDynamics> dynamics, Ptr<GnssChannel> channel);

double GetEpoch() { return epoch_; }

void SetEpoch(double ep) {
epoch_ = ep;
for (auto sat : satellites_) sat->SetEpoch(ep);
}

void SetDynamics(Ptr<IDynamics> dyn) {
dynamics_ = dyn;
// set dynamics to all satellites
for (auto sat : satellites_) sat->SetDynamics(dyn);
}

// Getters
int GetNumSatellites() { return satellites_.size(); }
Ptr<Spacecraft> GetSatellite(int i) { return satellites_[i]; }
Expand All @@ -49,7 +67,6 @@ namespace lupnt {
epoch_ = epoch;
}

void LoadTleFile(std::string_view filename);
};

} // namespace lupnt
12 changes: 10 additions & 2 deletions source/cpp/agents/gnss_constellation.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,13 @@

namespace lupnt {

void GnssConstellation::LoadTleFile(std::string_view filename) {
void GnssConstellation::InitializeWithTle(std::string gnss_type, std::string tle_filename, Ptr<IDynamics> dynamics, Ptr<GnssChannel> channel) {
SetChannel(channel);
SetDynamics(dynamics);
LoadTleFile(gnss_type, tle_filename);
}

void GnssConstellation::LoadTleFile(std::string_view gnss_type, std::string filename) {
// std::filesystem::path path = GetFilePath(filename);

bool is_first = true;
Expand All @@ -30,6 +36,8 @@ namespace lupnt {
if (is_first) {
epoch_ = tle.epoch_tai;
}
// std::cout << "Loaded satellite: " << tle.name << " PRN: " << tle.prn << std::endl;

double dt_epoch = epoch_ - tle.epoch_tai;

double T = SECS_DAY / tle.mean_motion;
Expand All @@ -56,7 +64,7 @@ namespace lupnt {
sat->SetBodyId(NaifId::EARTH);

if (channel_) {
auto transmitter = MakePtr<GnssTransmitter>(tle.name, tle.prn);
auto transmitter = MakePtr<GnssTransmitter>(std::string(gnss_type), tle.prn);
sat->AddDevice(transmitter);
transmitter->SetAgent(sat);
channel_->AddTransmitter(transmitter);
Expand Down
90 changes: 49 additions & 41 deletions source/cpp/measurements/gnss_channel.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,15 +61,24 @@ namespace lupnt {

// Occultation
std::string tx_planet = "";
std::map<std::string, bool> vis = Occultation::ComputeOccultationGnss(
std::map<std::string, bool> occult = Occultation::ComputeOccultationGnss(
rv_tx_gcrf.r().cast<double>(), rv_tx_mi.r().cast<double>(), rv_rx_gcrf.r().cast<double>(),
rv_rx_mi.r().cast<double>(), tx_planet, 10.0 * RAD);

if (vis["EARTH"] || vis["MOON"]) continue; // quit if occulted
if (occult["earth"] || occult["moon"]) {
// std::cout << "Earth or Moon occultation" << std::endl;
continue; // quit if occulted
}

// Transmitter and Receiver Antenna gain
double At = tx->GetTransmitterAntennaGain(t_tx, rv_tx_gcrf.r().cast<double>(),
rv_rx_gcrf.r().cast<double>());

if (std::isnan(At)) {
// std::cout << "Transmitter antenna gain is NaN" << std::endl;
continue;
}

double Ar = rx.GetReceiverAntennaGain(t_rx, rv_tx_gcrf.r().cast<double>(),
rv_rx_gcrf.r().cast<double>());

Expand All @@ -86,50 +95,49 @@ namespace lupnt {
- (10.0 * log10(rx.rx_param_.Tsys)) + 228.6 + rx.rx_param_.L;
trans.CN0 = At + Ar + Ad + scalars;

if (At <= -499.0 || vis["earth"] || vis["moon"] || trans.CN0 < rx.rx_param_.CN0threshold) {
if (std::isnan(At) || occult["earth"] || occult["moon"] || trans.CN0 < rx.rx_param_.CN0threshold) {
// not visible
continue;
trans.CN0 = NAN;
trans.AP = NAN;
trans.RP = NAN;
trans.vis_antenna = true;
} else {
}
else {
trans.AP = tx->P_tx + At + Ad + rx.rx_param_.Ae;
trans.RP = trans.AP + Ar + rx.rx_param_.As;
trans.vis_antenna = false;
}
trans.vis_antenna = true;

// TX
trans.t_tx = t_tx;
trans.freq = freq;
trans.freq_label = freq_name;
trans.chip_rate = tx->rc_map[freq_name];
trans.dt_tx = 0.0; // Todo: Get this from ephemeris
trans.dt_tx_dot = 0.0; // Todo: Get this from ephemeris
trans.r_tx = rv_tx_gcrf.r().cast<double>();
trans.v_tx = rv_tx_gcrf.v().cast<double>();

// Channel
trans.I_rx = 0.0;
trans.T_rx = 0.0;
trans.vis_atmos = vis["atmos"];
trans.vis_ionos = vis["ionos"];
trans.vis_earth = vis["earth"];
trans.vis_moon = vis["moon"];

trans.ID_tx = tx->GetPRN();

// RX
trans.t_rx = t_rx;
trans.dt_rx = rx.GetAgent()->GetClockState().GetValue(0).val();
trans.dt_rx_dot = rx.GetAgent()->GetClockState().GetValue(1).val();
trans.r_rx = rv_rx_gcrf.r().cast<double>();
trans.v_rx = rv_rx_gcrf.v().cast<double>();

// receiver chip param
trans.gnssr_param = rx.gnssr_param_;

received_transs.push_back(trans);
// TX
trans.t_tx = t_tx;
trans.freq = freq;
trans.freq_label = freq_name;
trans.chip_rate = tx->rc_map[freq_name];
trans.dt_tx = 0.0; // Todo: Get this from ephemeris
trans.dt_tx_dot = 0.0; // Todo: Get this from ephemeris
trans.r_tx = rv_tx_gcrf.r().cast<double>();
trans.v_tx = rv_tx_gcrf.v().cast<double>();

// Channel
trans.I_rx = 0.0;
trans.T_rx = 0.0;
trans.vis_atmos = 1 - occult["atmos"];
trans.vis_ionos = 1 - occult["ionos"];
trans.vis_earth = 1 - occult["earth"];
trans.vis_moon = 1 - occult["moon"];

trans.ID_tx = tx->GetPRN();

// RX
trans.t_rx = t_rx;
trans.dt_rx = rx.GetAgent()->GetClockState().GetValue(0).val();
trans.dt_rx_dot = rx.GetAgent()->GetClockState().GetValue(1).val();
trans.r_rx = rv_rx_gcrf.r().cast<double>();
trans.v_rx = rv_rx_gcrf.v().cast<double>();

// receiver chip param
trans.gnssr_param = rx.gnssr_param_;

// std::cout << "prn: " << tx->GetPRN() << " freq: " << freq_name << " At:" << At << " Ar:" << Ar << " C/N0:" << trans.CN0 << std::endl;

received_transs.push_back(trans);
}
}
}
return received_transs;
Expand Down
Loading

0 comments on commit 0c5c175

Please sign in to comment.