mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-08-01 08:20:34 +00:00
[ADD] estatic_measures_sd flag
This commit is contained in:
parent
643bf5516a
commit
dd7b1f9f6a
1
src/algorithms/PVT/adapters/rtklib_pvt.cc
Normal file → Executable file
1
src/algorithms/PVT/adapters/rtklib_pvt.cc
Normal file → Executable 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
1
src/algorithms/PVT/libs/pvt_conf.h
Normal file → Executable 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;
|
||||
|
|
39
src/algorithms/PVT/libs/pvt_kf.cc
Normal file → Executable file
39
src/algorithms/PVT/libs/pvt_kf.cc
Normal file → Executable 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,13 +42,26 @@ void Pvt_Kf::init_Kf(const arma::vec& p,
|
|||
d_H = arma::eye(6, 6);
|
||||
|
||||
// measurement matrix static covariances
|
||||
d_R = {{res_p[0], res_p[3], res_p[5], 0.0, 0.0, 0.0},
|
||||
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},
|
||||
{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 = 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,17 +119,18 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res
|
|||
// 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];
|
||||
|
||||
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];
|
||||
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
|
||||
|
|
2
src/algorithms/PVT/libs/pvt_kf.h
Normal file → Executable file
2
src/algorithms/PVT/libs/pvt_kf.h
Normal file → Executable 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
1
src/algorithms/PVT/libs/rtklib_solver.cc
Normal file → Executable 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,
|
||||
|
|
Loading…
Reference in New Issue
Block a user