mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-10-05 02:10:50 +00:00
Code Issues Releases Wiki Activity

[ADD]: rtklib pos res to pvt_kf

Browse Source
This commit is contained in:
M.A. Gomez 2023-07-09 14:39:05 +02:00
parent 7d475f9ffe
commit 1513b31a3d
3 changed files with 22 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,6 +21,7 @@
void Pvt_Kf::init_kf(const arma::vec& p,
const arma::vec& v,
const arma::vec& res_p,
double solver_interval_s,
double measures_ecef_pos_sd_m,
double measures_ecef_vel_sd_ms,
@ -40,9 +41,9 @@ void Pvt_Kf::init_kf(const arma::vec& p,
d_H = arma::eye(6, 6);
// measurement matrix static covariances
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},
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)}};
@ -80,19 +81,26 @@ void Pvt_Kf::init_kf(const arma::vec& p,
}
void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v)
void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p)
{
// Kalman loop
// Prediction
d_x_new_old = d_F * d_x_old_old;
d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q;
// 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
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
d_x_new_new = d_x_new_old + K * (z - d_H * d_x_new_old);
d_P_new_new = (arma::eye(6, 6) - K * d_H) * d_P_new_old;
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();
// prepare data for next KF epoch
d_x_old_old = d_x_new_new;

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

@ -37,12 +37,13 @@ public:
virtual ~Pvt_Kf() = default;
void init_kf(const arma::vec& p,
const arma::vec& v,
const arma::vec& res_p,
double solver_interval_s,
double measures_ecef_pos_sd_m,
double measures_ecef_vel_sd_ms,
double system_ecef_pos_sd_m,
double system_ecef_vel_sd_ms);
void run_Kf(const arma::vec& p, const arma::vec& v);
void run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p);
void get_pvt(arma::vec& p, arma::vec& v);
bool initialized{false};

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

@ -1552,9 +1552,12 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
{
arma::vec p = {pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]};
arma::vec v = {pvt_sol.rr[3], pvt_sol.rr[4], pvt_sol.rr[5]};
arma::vec res_p = {pvt_sol.qr[0], pvt_sol.qr[1], pvt_sol.qr[2],
pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5]};
d_pvt_kf.init_kf(p,
v,
res_p,
d_conf.observable_interval_ms / 1000.0,
d_conf.measures_ecef_pos_sd_m,
d_conf.measures_ecef_vel_sd_ms,
@ -1565,7 +1568,10 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
{
arma::vec p = {pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]};
arma::vec v = {pvt_sol.rr[3], pvt_sol.rr[4], pvt_sol.rr[5]};
d_pvt_kf.run_Kf(p, v);
arma::vec res_p = {pvt_sol.qr[0], pvt_sol.qr[1], pvt_sol.qr[2],
pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5]};
d_pvt_kf.run_Kf(p, v, res_p);
d_pvt_kf.get_pvt(p, v);
pvt_sol.rr[0] = p[0]; // [m]
pvt_sol.rr[1] = p[1]; // [m]