From ccf9527f5498546134fcedd8b0357ad52f842508 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Sun, 23 Jul 2023 23:08:37 +0200 Subject: [PATCH] [ADD] static kalman version --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 1 + src/algorithms/PVT/libs/pvt_conf.h | 1 + src/algorithms/PVT/libs/pvt_kf.cc | 20 +++++++++++++++++++- src/algorithms/PVT/libs/pvt_kf.h | 2 ++ src/algorithms/PVT/libs/rtklib_solver.cc | 1 + 5 files changed, 24 insertions(+), 1 deletion(-) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index ad967d953..5ce4cc924 100755 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -76,6 +76,7 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, // PVT KF settings pvt_output_parameters.enable_pvt_kf = configuration->property(role + ".enable_pvt_kf", false); + pvt_output_parameters.static_scenario_sd = configuration->property(role + ".static_scenario_sd", false); pvt_output_parameters.estatic_measures_sd = configuration->property(role + ".estatic_measures_sd", false); pvt_output_parameters.measures_ecef_pos_sd_m = configuration->property(role + ".kf_measures_ecef_pos_sd_m", 1.0); pvt_output_parameters.measures_ecef_vel_sd_ms = configuration->property(role + ".kf_measures_ecef_vel_sd_ms", 0.1); diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index f194c0596..39bc03e36 100755 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -97,6 +97,7 @@ public: // PVT KF parameters bool enable_pvt_kf = false; + bool static_scenario_sd = false; bool estatic_measures_sd = false; double measures_ecef_pos_sd_m = 1.0; double measures_ecef_vel_sd_ms = 0.1; diff --git a/src/algorithms/PVT/libs/pvt_kf.cc b/src/algorithms/PVT/libs/pvt_kf.cc index f59cdb92f..aef09efe9 100755 --- a/src/algorithms/PVT/libs/pvt_kf.cc +++ b/src/algorithms/PVT/libs/pvt_kf.cc @@ -24,6 +24,7 @@ void Pvt_Kf::init_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_pv, double solver_interval_s, + bool static_scenario_sd, bool estatic_measures_sd, double measures_ecef_pos_sd_m, double measures_ecef_vel_sd_ms, @@ -32,6 +33,14 @@ void Pvt_Kf::init_Kf(const arma::vec& p, { // Kalman Filter class variables const double Ti = solver_interval_s; + if (static_scenario_sd) + { + scenario_static = true; + } + else + { + scenario_static = false; + } d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0}, {0.0, 1.0, 0.0, 0.0, Ti, 0.0}, @@ -112,6 +121,7 @@ void Pvt_Kf::reset_Kf() void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_pv) { + // bool static_scenario = true; if (d_initialized) { // Kalman loop @@ -132,7 +142,15 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res {0.0, 0.0, 0.0, res_pv[9], res_pv[7], res_pv[10]}, {0.0, 0.0, 0.0, res_pv[11], res_pv[10], res_pv[8]}}; } - arma::vec z = arma::join_cols(p, v); + arma::vec z = arma::zeros(6); + if (scenario_static) + { + d_x_new_old[3] = 0; + d_x_new_old[4] = 0; + d_x_new_old[5] = 0; + } + + 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); diff --git a/src/algorithms/PVT/libs/pvt_kf.h b/src/algorithms/PVT/libs/pvt_kf.h index 000bbd2c4..995d19931 100755 --- a/src/algorithms/PVT/libs/pvt_kf.h +++ b/src/algorithms/PVT/libs/pvt_kf.h @@ -39,6 +39,7 @@ public: const arma::vec& v, const arma::vec& res_pv, double solver_interval_s, + bool static_scenario_sd, bool estatic_measures_sd, double measures_ecef_pos_sd_m, double measures_ecef_vel_sd_ms, @@ -63,6 +64,7 @@ private: arma::vec d_x_new_new; bool d_initialized{false}; bool d_static{false}; + bool scenario_static{false}; }; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index eac9395d0..596bc5fbd 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1592,6 +1592,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ v, res_pv, kf_update_interval_s, + d_conf.static_scenario_sd, d_conf.estatic_measures_sd, d_conf.measures_ecef_pos_sd_m, d_conf.measures_ecef_vel_sd_ms,