mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-07-05 11:32:56 +00:00
Code Issues Releases Wiki Activity

[ADD] velocity covariance for kf

Browse Source
This commit is contained in:
M.A. Gomez 2023-07-12 17:56:00 +02:00
parent accd9f7c34
commit 0b139fa42b
No known key found for this signature in database
GPG Key ID: 69D837A2B262D414
4 changed files with 34 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,7 +21,7 @@
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_pv,
double solver_interval_s, double solver_interval_s,
bool estatic_measures_sd, bool estatic_measures_sd,
double measures_ecef_pos_sd_m, double measures_ecef_pos_sd_m,
@ -55,12 +55,12 @@ void Pvt_Kf::init_Kf(const arma::vec& p,
} }
else else
{ {
d_R = {{res_p[0], res_p[3], res_p[5], 0.0, 0.0, 0.0}, d_R = {{res_pv[0], res_pv[3], res_pv[5], 0.0, 0.0, 0.0},
{res_p[3], res_p[1], res_p[4], 0.0, 0.0, 0.0}, {res_pv[3], res_pv[1], res_pv[4], 0.0, 0.0, 0.0},
{res_p[4], res_p[5], res_p[2], 0.0, 0.0, 0.0}, {res_pv[5], res_pv[4], res_pv[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, res_pv[6], res_pv[9], res_pv[11]},
{0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0}, {0.0, 0.0, 0.0, res_pv[9], res_pv[7], res_pv[10]},
{0.0, 0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0)}}; {0.0, 0.0, 0.0, res_pv[11], res_pv[10], res_pv[8]}};
d_static = false; d_static = false;
} }
@ -109,7 +109,7 @@ void Pvt_Kf::reset_Kf()
} }
void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p) void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_pv)
{ {
if (d_initialized) if (d_initialized)
{ {
@ -124,17 +124,14 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res
if (!d_static) if (!d_static)
{ {
// Measurement residuals update // Measurement residuals update
d_R(0, 0) = res_p[0]; d_R = {{res_pv[0], res_pv[3], res_pv[5], 0.0, 0.0, 0.0},
d_R(0, 1) = res_p[3]; {res_pv[3], res_pv[1], res_pv[4], 0.0, 0.0, 0.0},
d_R(0, 2) = res_p[5]; {res_pv[5], res_pv[4], res_pv[2], 0.0, 0.0, 0.0},
d_R(1, 0) = res_p[3]; {0.0, 0.0, 0.0, res_pv[6], res_pv[9], res_pv[11]},
d_R(1, 1) = res_p[1]; {0.0, 0.0, 0.0, res_pv[9], res_pv[7], res_pv[10]},
d_R(1, 2) = res_p[4]; {0.0, 0.0, 0.0, res_pv[11], res_pv[10], res_pv[8]}};
d_R(2, 0) = res_p[5];
d_R(2, 1) = res_p[4]; } // Measurement update
d_R(2, 2) = res_p[2];
}
// 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

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

@ -37,7 +37,7 @@ public:
virtual ~Pvt_Kf() = default; virtual ~Pvt_Kf() = default;
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_pv,
double solver_interval_s, double solver_interval_s,
bool estatic_measures_sd, bool estatic_measures_sd,
double measures_ecef_pos_sd_m, double measures_ecef_pos_sd_m,
@ -45,7 +45,7 @@ public:
double system_ecef_pos_sd_m, double system_ecef_pos_sd_m,
double system_ecef_vel_sd_ms); double system_ecef_vel_sd_ms);
bool is_initialized() const; bool is_initialized() const;
void run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p); void run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_pv);
void get_pv_Kf(arma::vec& p, arma::vec& v) const; void get_pv_Kf(arma::vec& p, arma::vec& v) const;
void reset_Kf(); void reset_Kf();

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

@ -1548,12 +1548,13 @@ 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 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 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], arma::vec res_pv = {pvt_sol.qr[0], pvt_sol.qr[1], pvt_sol.qr[2],
pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5]}; pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5], pvt_sol.qvr[0], pvt_sol.qvr[1], pvt_sol.qvr[2],
pvt_sol.qvr[3], pvt_sol.qvr[4], pvt_sol.qvr[5]};
d_pvt_kf.init_Kf(p, d_pvt_kf.init_Kf(p,
v, v,
res_p, res_pv,
kf_update_interval_s, kf_update_interval_s,
d_conf.estatic_measures_sd, d_conf.estatic_measures_sd,
d_conf.measures_ecef_pos_sd_m, d_conf.measures_ecef_pos_sd_m,
@ -1565,10 +1566,11 @@ 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 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 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], arma::vec res_pv = {pvt_sol.qr[0], pvt_sol.qr[1], pvt_sol.qr[2],
pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5]}; pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5], pvt_sol.qvr[0], pvt_sol.qvr[1], pvt_sol.qvr[2],
pvt_sol.qvr[3], pvt_sol.qvr[4], pvt_sol.qvr[5]};
d_pvt_kf.run_Kf(p, v, res_p); d_pvt_kf.run_Kf(p, v, res_pv);
d_pvt_kf.get_pv_Kf(p, v); d_pvt_kf.get_pv_Kf(p, v);
pvt_sol.rr[0] = p[0]; // [m] pvt_sol.rr[0] = p[0]; // [m]
pvt_sol.rr[1] = p[1]; // [m] pvt_sol.rr[1] = p[1]; // [m]

8
src/algorithms/libs/rtklib/rtklib_pntpos.cc
View File

@ -1061,6 +1061,14 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
sol->rr[i + 3] = x[i]; sol->rr[i + 3] = x[i];
} }
sol->dtr[5] = x[3]; sol->dtr[5] = x[3];
for (j = 0; j < 3; j++)
{
sol->qvr[j] = static_cast<float>(Q[j + j * 4]);
}
sol->qvr[3] = static_cast<float>(Q[1]); /* cov xy */
sol->qvr[4] = static_cast<float>(Q[2 + 4]); /* cov yz */
sol->qvr[5] = static_cast<float>(Q[2]); /* cov zx */
break; break;
} }
} }