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

[ADD] static kalman version

Browse Source
This commit is contained in:
M.A. Gomez 2023-07-23 23:08:37 +02:00
parent db6d2427b4
commit ccf9527f54
No known key found for this signature in database
GPG Key ID: 69D837A2B262D414
5 changed files with 24 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@ -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);

1
src/algorithms/PVT/libs/pvt_conf.h
View File

@ -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;

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

@ -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);

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

@ -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};
};

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

@ -1592,6 +1592,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &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,