mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-08-01 08:20:34 +00:00
Code Issues Releases Wiki Activity

[ADD] estatic_measures_sd flag

Browse Source
This commit is contained in:
M.A. Gomez 2023-07-12 17:26:45 +02:00
parent 643bf5516a
commit dd7b1f9f6a
No known key found for this signature in database
GPG Key ID: 69D837A2B262D414
5 changed files with 3111 additions and 3091 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/algorithms/PVT/adapters/rtklib_pvt.cc Normal file → Executable file
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.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);
pvt_output_parameters.system_ecef_pos_sd_m = configuration->property(role + ".kf_system_ecef_pos_sd_m", 0.01);

1
src/algorithms/PVT/libs/pvt_conf.h Normal file → Executable file
View File

@ -97,6 +97,7 @@ public:
// PVT KF parameters
bool enable_pvt_kf = false;
bool estatic_measures_sd = false;
double measures_ecef_pos_sd_m = 1.0;
double measures_ecef_vel_sd_ms = 0.1;
double system_ecef_pos_sd_m = 0.01;

19
src/algorithms/PVT/libs/pvt_kf.cc Normal file → Executable file
View File

@ -23,6 +23,7 @@ void Pvt_Kf::init_Kf(const arma::vec& p,
const arma::vec& v,
const arma::vec& res_p,
double solver_interval_s,
bool estatic_measures_sd,
double measures_ecef_pos_sd_m,
double measures_ecef_vel_sd_ms,
double system_ecef_pos_sd_m,
@ -41,6 +42,17 @@ void Pvt_Kf::init_Kf(const arma::vec& p,
d_H = arma::eye(6, 6);
// measurement matrix static covariances
if(estatic_measures_sd){
d_R = {{pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0)}};
d_static = true;
}else{
d_R = {{res_p[0], res_p[3], res_p[5], 0.0, 0.0, 0.0},
{res_p[3], res_p[1], res_p[4], 0.0, 0.0, 0.0},
{res_p[4], res_p[5], res_p[2], 0.0, 0.0, 0.0},
@ -48,6 +60,8 @@ void Pvt_Kf::init_Kf(const arma::vec& p,
{0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0)}};
d_static = false;
}
// system covariance matrix (static)
d_Q = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0},
@ -105,7 +119,8 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res
// Measurement update
try
{
// Measurement residuals
if(!d_static){
// Measurement residuals update
d_R(0, 0) = res_p[0];
d_R(0, 1) = res_p[3];
d_R(0, 2) = res_p[5];
@ -115,7 +130,7 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res
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

2
src/algorithms/PVT/libs/pvt_kf.h Normal file → Executable file
View File

@ -39,6 +39,7 @@ public:
const arma::vec& v,
const arma::vec& res_p,
double solver_interval_s,
bool estatic_measures_sd,
double measures_ecef_pos_sd_m,
double measures_ecef_vel_sd_ms,
double system_ecef_pos_sd_m,
@ -61,6 +62,7 @@ private:
arma::vec d_x_new_old;
arma::vec d_x_new_new;
bool d_initialized{false};
bool d_static{false};
};

1
src/algorithms/PVT/libs/rtklib_solver.cc Normal file → Executable file
View File

@ -1555,6 +1555,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
v,
res_p,
kf_update_interval_s,
d_conf.estatic_measures_sd,
d_conf.measures_ecef_pos_sd_m,
d_conf.measures_ecef_vel_sd_ms,
d_conf.system_ecef_pos_sd_m,