mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-16 06:44:57 +00:00
Code Issues Releases Wiki Activity

[Merge] branches

Browse Source
This commit is contained in:
M.A. Gomez 2023-07-10 14:38:39 +02:00
parent 1513b31a3d
commit c0d484f80d
No known key found for this signature in database
GPG Key ID: 69D837A2B262D414
4 changed files with 65 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
src/algorithms/PVT/libs/pvt_kf.cc
View File

@ -19,17 +19,17 @@
#include <glog/logging.h> #include <glog/logging.h>
void Pvt_Kf::init_kf(const arma::vec& p, void Pvt_Kf::init_Kf(const arma::vec& p,
const arma::vec& v, const arma::vec& v,
const arma::vec& res_p, const arma::vec& res_p,
double solver_interval_s, double update_interval_s,
double measures_ecef_pos_sd_m, double measures_ecef_pos_sd_m,
double measures_ecef_vel_sd_ms, double measures_ecef_vel_sd_ms,
double system_ecef_pos_sd_m, double system_ecef_pos_sd_m,
double system_ecef_vel_sd_ms) double system_ecef_vel_sd_ms)
{ {
// Kalman Filter class variables // Kalman Filter class variables
const double Ti = solver_interval_s; const double Ti = update_interval_s;
d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0}, d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0},
{0.0, 1.0, 0.0, 0.0, Ti, 0.0}, {0.0, 1.0, 0.0, 0.0, Ti, 0.0},
@ -69,7 +69,7 @@ void Pvt_Kf::init_kf(const arma::vec& p,
d_x_old_old.subvec(0, 2) = p; d_x_old_old.subvec(0, 2) = p;
d_x_old_old.subvec(3, 5) = v; d_x_old_old.subvec(3, 5) = v;
initialized = true; d_initialized = true;
DLOG(INFO) << "Ti: " << Ti; DLOG(INFO) << "Ti: " << Ti;
DLOG(INFO) << "F: " << d_F; DLOG(INFO) << "F: " << d_F;
@ -81,35 +81,60 @@ void Pvt_Kf::init_kf(const arma::vec& p,
} }
bool Pvt_Kf::is_initialized() const
{
return d_initialized;
}
void Pvt_Kf::reset_Kf()
{
d_initialized = false;
}
void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p) void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p)
{ {
// Kalman loop if (d_initialized)
// Prediction {
d_x_new_old = d_F * d_x_old_old; // Kalman loop
d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q; // Prediction
d_x_new_old = d_F * d_x_old_old;
d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q;
// Measurement residuals // Measurement update
d_R(0,0) = res_p[0]; d_R(0,1) = res_p[3]; d_R(0,2) = res_p[5]; try
d_R(1,0) = res_p[3]; d_R(1,1) = res_p[1]; d_R(1,2) = res_p[4]; {
d_R(2,0) = res_p[5]; d_R(2,1) = res_p[4]; d_R(2,2) = res_p[2]; // Measurement residuals
d_R(0,0) = res_p[0]; d_R(0,1) = res_p[3]; d_R(0,2) = res_p[5];
d_R(1,0) = res_p[3]; d_R(1,1) = res_p[1]; d_R(1,2) = res_p[4];
d_R(2,0) = res_p[5]; d_R(2,1) = res_p[4]; d_R(2,2) = res_p[2];
// Measurement update // Measurement update
arma::vec z = arma::join_cols(p, v); arma::vec z = arma::join_cols(p, v);
arma::mat K = d_P_new_old * d_H.t() * arma::inv(d_H * d_P_new_old * d_H.t() + d_R); // Kalman gain arma::mat K = d_P_new_old * d_H.t() * arma::inv(d_H * d_P_new_old * d_H.t() + d_R); // Kalman gain
d_x_new_new = d_x_new_old + K * (z - d_H * d_x_new_old); d_x_new_new = d_x_new_old + K * (z - d_H * d_x_new_old);
arma::mat A = (arma::eye(6, 6) - K * d_H); arma::mat A = (arma::eye(6, 6) - K * d_H);
d_P_new_new = A * d_P_new_old * A.t() + K * d_R * K.t(); d_P_new_new = A * d_P_new_old * A.t() + K * d_R * K.t();
// prepare data for next KF epoch
d_x_old_old = d_x_new_new;
// prepare data for next KF epoch d_P_old_old = d_P_new_new;
d_x_old_old = d_x_new_new; }
d_P_old_old = d_P_new_new; catch (...)
{
d_x_new_new = d_x_new_old;
this->reset_Kf();
}
}
} }
void Pvt_Kf::get_pvt(arma::vec& p, arma::vec& v) void Pvt_Kf::get_pv_Kf(arma::vec& p, arma::vec& v) const
{ {
p = d_x_new_new.subvec(0, 2); if (d_initialized)
v = d_x_new_new.subvec(3, 5); {
p = d_x_new_new.subvec(0, 2);
v = d_x_new_new.subvec(3, 5);
}
} }

11
src/algorithms/PVT/libs/pvt_kf.h
View File

@ -35,17 +35,18 @@ class Pvt_Kf
public: public:
Pvt_Kf() = default; Pvt_Kf() = default;
virtual ~Pvt_Kf() = default; virtual ~Pvt_Kf() = default;
void init_kf(const arma::vec& p, void init_Kf(const arma::vec& p,
const arma::vec& v, const arma::vec& v,
const arma::vec& res_p, const arma::vec& res_p,
double solver_interval_s, double update_interval_s,
double measures_ecef_pos_sd_m, double measures_ecef_pos_sd_m,
double measures_ecef_vel_sd_ms, double measures_ecef_vel_sd_ms,
double system_ecef_pos_sd_m, double system_ecef_pos_sd_m,
double system_ecef_vel_sd_ms); double system_ecef_vel_sd_ms);
bool is_initialized() const;
void run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p); void run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p);
void get_pvt(arma::vec& p, arma::vec& v); void get_pv_Kf(arma::vec& p, arma::vec& v) const;
bool initialized{false}; void reset_Kf();
private: private:
// Kalman Filter class variables // Kalman Filter class variables
@ -59,9 +60,11 @@ private:
arma::vec d_x_old_old; arma::vec d_x_old_old;
arma::vec d_x_new_old; arma::vec d_x_new_old;
arma::vec d_x_new_new; arma::vec d_x_new_new;
bool d_initialized{false};
}; };
/** \} */ /** \} */
/** \} */ /** \} */
#endif // GNSS_SDR_Pvt_Kf_H #endif // GNSS_SDR_Pvt_Kf_H

16
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -56,8 +56,6 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk,
d_flag_dump_enabled(flag_dump_to_file), d_flag_dump_enabled(flag_dump_to_file),
d_flag_dump_mat_enabled(flag_dump_to_mat) d_flag_dump_mat_enabled(flag_dump_to_mat)
{ {
this->set_averaging_flag(false);
// see freq index at src/algorithms/libs/rtklib/rtklib_rtkcmn.cc // see freq index at src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
// function: satwavelen // function: satwavelen
d_rtklib_freq_index[0] = 0; d_rtklib_freq_index[0] = 0;
@ -900,7 +898,7 @@ void Rtklib_Solver::get_current_has_obs_correction(const std::string &signal, ui
} }
bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, bool flag_averaging) bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, double kf_update_interval_s)
{ {
std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter; std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
std::map<int, Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter; std::map<int, Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter;
@ -911,8 +909,6 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
const Glonass_Gnav_Utc_Model &gnav_utc = this->glonass_gnav_utc_model; const Glonass_Gnav_Utc_Model &gnav_utc = this->glonass_gnav_utc_model;
this->set_averaging_flag(flag_averaging);
// ******************************************************************************** // ********************************************************************************
// ****** PREPARE THE DATA (SV EPHEMERIS AND OBSERVATIONS) ************************ // ****** PREPARE THE DATA (SV EPHEMERIS AND OBSERVATIONS) ************************
// ******************************************************************************** // ********************************************************************************
@ -1509,7 +1505,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
this->set_num_valid_observations(0); this->set_num_valid_observations(0);
if (d_conf.enable_pvt_kf == true) if (d_conf.enable_pvt_kf == true)
{ {
d_pvt_kf.initialized = false; d_pvt_kf.reset_Kf();
} }
} }
else else
@ -1548,17 +1544,17 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
if (d_conf.enable_pvt_kf == true) if (d_conf.enable_pvt_kf == true)
{ {
if (d_pvt_kf.initialized == false) if (d_pvt_kf.is_initialized() == false)
{ {
arma::vec p = {pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]}; arma::vec p = {pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]};
arma::vec v = {pvt_sol.rr[3], pvt_sol.rr[4], pvt_sol.rr[5]}; arma::vec v = {pvt_sol.rr[3], pvt_sol.rr[4], pvt_sol.rr[5]};
arma::vec res_p = {pvt_sol.qr[0], pvt_sol.qr[1], pvt_sol.qr[2], arma::vec res_p = {pvt_sol.qr[0], pvt_sol.qr[1], pvt_sol.qr[2],
pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5]}; pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5]};
d_pvt_kf.init_kf(p, d_pvt_kf.init_Kf(p,
v, v,
res_p, res_p,
d_conf.observable_interval_ms / 1000.0, kf_update_interval_s,
d_conf.measures_ecef_pos_sd_m, d_conf.measures_ecef_pos_sd_m,
d_conf.measures_ecef_vel_sd_ms, d_conf.measures_ecef_vel_sd_ms,
d_conf.system_ecef_pos_sd_m, d_conf.system_ecef_pos_sd_m,
@ -1572,7 +1568,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5]}; pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5]};
d_pvt_kf.run_Kf(p, v, res_p); d_pvt_kf.run_Kf(p, v, res_p);
d_pvt_kf.get_pvt(p, v); d_pvt_kf.get_pv_Kf(p, v);
pvt_sol.rr[0] = p[0]; // [m] pvt_sol.rr[0] = p[0]; // [m]
pvt_sol.rr[1] = p[1]; // [m] pvt_sol.rr[1] = p[1]; // [m]
pvt_sol.rr[2] = p[2]; // [m] pvt_sol.rr[2] = p[2]; // [m]

2
src/algorithms/PVT/libs/rtklib_solver.h
View File

@ -89,7 +89,7 @@ public:
Pvt_Conf conf); Pvt_Conf conf);
~Rtklib_Solver(); ~Rtklib_Solver();
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging); bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double kf_update_interval_s);
double get_hdop() const override; double get_hdop() const override;
double get_vdop() const override; double get_vdop() const override;