mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-09-08 21:46:54 +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 KF settings
|
||||||
pvt_output_parameters.enable_pvt_kf = configuration->property(role + ".enable_pvt_kf", false);
|
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_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.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);
|
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
|
// PVT KF parameters
|
||||||
bool enable_pvt_kf = false;
|
bool enable_pvt_kf = false;
|
||||||
|
bool estatic_measures_sd = false;
|
||||||
double measures_ecef_pos_sd_m = 1.0;
|
double measures_ecef_pos_sd_m = 1.0;
|
||||||
double measures_ecef_vel_sd_ms = 0.1;
|
double measures_ecef_vel_sd_ms = 0.1;
|
||||||
double system_ecef_pos_sd_m = 0.01;
|
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& v,
|
||||||
const arma::vec& res_p,
|
const arma::vec& res_p,
|
||||||
double solver_interval_s,
|
double solver_interval_s,
|
||||||
|
bool estatic_measures_sd,
|
||||||
double measures_ecef_pos_sd_m,
|
double measures_ecef_pos_sd_m,
|
||||||
double measures_ecef_vel_sd_ms,
|
double measures_ecef_vel_sd_ms,
|
||||||
double system_ecef_pos_sd_m,
|
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);
|
d_H = arma::eye(6, 6);
|
||||||
|
|
||||||
// measurement matrix static covariances
|
// 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[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},
|
{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, 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, 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, pow(measures_ecef_vel_sd_ms, 2.0)}};
|
||||||
|
|
||||||
|
d_static = false;
|
||||||
|
}
|
||||||
// system covariance matrix (static)
|
// system covariance matrix (static)
|
||||||
d_Q = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0},
|
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},
|
{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
|
// Measurement update
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
// Measurement residuals
|
if(!d_static){
|
||||||
d_R(0, 0) = res_p[0];
|
// Measurement residuals update
|
||||||
d_R(0, 1) = res_p[3];
|
d_R(0, 0) = res_p[0];
|
||||||
d_R(0, 2) = res_p[5];
|
d_R(0, 1) = res_p[3];
|
||||||
d_R(1, 0) = res_p[3];
|
d_R(0, 2) = res_p[5];
|
||||||
d_R(1, 1) = res_p[1];
|
d_R(1, 0) = res_p[3];
|
||||||
d_R(1, 2) = res_p[4];
|
d_R(1, 1) = res_p[1];
|
||||||
d_R(2, 0) = res_p[5];
|
d_R(1, 2) = res_p[4];
|
||||||
d_R(2, 1) = res_p[4];
|
d_R(2, 0) = res_p[5];
|
||||||
d_R(2, 2) = res_p[2];
|
d_R(2, 1) = res_p[4];
|
||||||
|
d_R(2, 2) = res_p[2];
|
||||||
|
}
|
||||||
// Measurement update
|
// Measurement update
|
||||||
arma::vec z = arma::join_cols(p, v);
|
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
|
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& v,
|
||||||
const arma::vec& res_p,
|
const arma::vec& res_p,
|
||||||
double solver_interval_s,
|
double solver_interval_s,
|
||||||
|
bool estatic_measures_sd,
|
||||||
double measures_ecef_pos_sd_m,
|
double measures_ecef_pos_sd_m,
|
||||||
double measures_ecef_vel_sd_ms,
|
double measures_ecef_vel_sd_ms,
|
||||||
double system_ecef_pos_sd_m,
|
double system_ecef_pos_sd_m,
|
||||||
@ -61,6 +62,7 @@ private:
|
|||||||
arma::vec d_x_new_old;
|
arma::vec d_x_new_old;
|
||||||
arma::vec d_x_new_new;
|
arma::vec d_x_new_new;
|
||||||
bool d_initialized{false};
|
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,
|
v,
|
||||||
res_p,
|
res_p,
|
||||||
kf_update_interval_s,
|
kf_update_interval_s,
|
||||||
|
d_conf.estatic_measures_sd,
|
||||||
d_conf.measures_ecef_pos_sd_m,
|
d_conf.measures_ecef_pos_sd_m,
|
||||||
d_conf.measures_ecef_vel_sd_ms,
|
d_conf.measures_ecef_vel_sd_ms,
|
||||||
d_conf.system_ecef_pos_sd_m,
|
d_conf.system_ecef_pos_sd_m,
|
||||||
|
Loading…
Reference in New Issue
Block a user