mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-07-06 20:12:55 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr-vtl into next

Browse Source
This commit is contained in:
M.A. Gomez 2023-07-10 15:17:04 +02:00
commit f6420a2bcb
No known key found for this signature in database
GPG Key ID: 69D837A2B262D414
3 changed files with 11 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -22,14 +22,14 @@
void Pvt_Kf::init_Kf(const arma::vec& p, 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 update_interval_s, double solver_interval_s,
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,
double system_ecef_vel_sd_ms) double system_ecef_vel_sd_ms)
{ {
// Kalman Filter class variables // Kalman Filter class variables
const double Ti = update_interval_s; const double Ti = solver_interval_s;
d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0}, d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0},
{0.0, 1.0, 0.0, 0.0, Ti, 0.0}, {0.0, 1.0, 0.0, 0.0, Ti, 0.0},
@ -110,6 +110,11 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res
d_R(1,0) = res_p[3]; d_R(1,1) = res_p[1]; d_R(1,2) = res_p[4]; 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]; d_R(2,0) = res_p[5]; d_R(2,1) = res_p[4]; d_R(2,2) = res_p[2];
// 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];
// 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
@ -117,6 +122,7 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res
d_x_new_new = d_x_new_old + K * (z - d_H * d_x_new_old); d_x_new_new = d_x_new_old + K * (z - d_H * d_x_new_old);
arma::mat A = (arma::eye(6, 6) - K * d_H); arma::mat A = (arma::eye(6, 6) - K * d_H);
d_P_new_new = A * d_P_new_old * A.t() + K * d_R * K.t(); d_P_new_new = A * d_P_new_old * A.t() + K * d_R * K.t();
// prepare data for next KF epoch // prepare data for next KF epoch
d_x_old_old = d_x_new_new; d_x_old_old = d_x_new_new;
d_P_old_old = d_P_new_new; d_P_old_old = d_P_new_new;

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

@ -38,7 +38,7 @@ public:
void init_Kf(const arma::vec& p, void 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 update_interval_s, double solver_interval_s,
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,

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

@ -898,6 +898,7 @@ void Rtklib_Solver::get_current_has_obs_correction(const std::string &signal, ui
} }
bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, double kf_update_interval_s)
bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, double kf_update_interval_s) bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_map, double kf_update_interval_s)
{ {
std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter; std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
@ -1554,7 +1555,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
d_pvt_kf.init_Kf(p, d_pvt_kf.init_Kf(p,
v, v,
res_p, res_p,
kf_update_interval_s, kf_update_interval_s,
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,