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>
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& res_p,
double solver_interval_s,
double update_interval_s,
double measures_ecef_pos_sd_m,
double measures_ecef_vel_sd_ms,
double system_ecef_pos_sd_m,
double system_ecef_vel_sd_ms)
{
// 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},
{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(3, 5) = v;
initialized = true;
d_initialized = true;
DLOG(INFO) << "Ti: " << Ti;
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)
{
// Kalman loop
// 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;
if (d_initialized)
{
// Kalman loop
// 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
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
try
{
// 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
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
// Measurement update
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
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);
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;
d_P_old_old = d_P_new_new;
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);
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;
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);
v = d_x_new_new.subvec(3, 5);
if (d_initialized)
{
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:
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& res_p,
double solver_interval_s,
double update_interval_s,
double measures_ecef_pos_sd_m,
double measures_ecef_vel_sd_ms,
double system_ecef_pos_sd_m,
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 get_pvt(arma::vec& p, arma::vec& v);
bool initialized{false};
void get_pv_Kf(arma::vec& p, arma::vec& v) const;
void reset_Kf();
private:
// Kalman Filter class variables
@ -59,9 +60,11 @@ private:
arma::vec d_x_old_old;
arma::vec d_x_new_old;
arma::vec d_x_new_new;
bool d_initialized{false};
};
/** \} */
/** \} */
#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_mat_enabled(flag_dump_to_mat)
{
this->set_averaging_flag(false);
// see freq index at src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
// function: satwavelen
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, 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;
this->set_averaging_flag(flag_averaging);
// ********************************************************************************
// ****** 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);
if (d_conf.enable_pvt_kf == true)
{
d_pvt_kf.initialized = false;
d_pvt_kf.reset_Kf();
}
}
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_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 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],
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,
res_p,
d_conf.observable_interval_ms / 1000.0,
kf_update_interval_s,
d_conf.measures_ecef_pos_sd_m,
d_conf.measures_ecef_vel_sd_ms,
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]};
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[1] = p[1]; // [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);
~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_vdop() const override;