diff --git a/src/algorithms/PVT/libs/pvt_kf.cc b/src/algorithms/PVT/libs/pvt_kf.cc index 72429f160..9608890c6 100644 --- a/src/algorithms/PVT/libs/pvt_kf.cc +++ b/src/algorithms/PVT/libs/pvt_kf.cc @@ -19,17 +19,17 @@ #include -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); + } } diff --git a/src/algorithms/PVT/libs/pvt_kf.h b/src/algorithms/PVT/libs/pvt_kf.h index dc5b088ff..22640f3ff 100644 --- a/src/algorithms/PVT/libs/pvt_kf.h +++ b/src/algorithms/PVT/libs/pvt_kf.h @@ -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 + diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 23ffa062d..e1a003a46 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -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 &gnss_observables_map, bool flag_averaging) +bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, double kf_update_interval_s) { std::map::const_iterator gnss_observables_iter; std::map::const_iterator galileo_ephemeris_iter; @@ -911,8 +909,6 @@ bool Rtklib_Solver::get_PVT(const std::map &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 &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 &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 &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] diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 7bffa042d..f24a37b2a 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -89,7 +89,7 @@ public: Pvt_Conf conf); ~Rtklib_Solver(); - bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging); + bool get_PVT(const std::map& gnss_observables_map, double kf_update_interval_s); double get_hdop() const override; double get_vdop() const override;