mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

[ADD] velocity covariance for kf

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,
const arma::vec& v,
const arma::vec& res_p,
const arma::vec& res_pv,
double solver_interval_s,
bool estatic_measures_sd,
double measures_ecef_pos_sd_m,
@ -55,12 +55,12 @@ void Pvt_Kf::init_Kf(const arma::vec& p,
}
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_R = {{res_pv[0], res_pv[3], res_pv[5], 0.0, 0.0, 0.0},
{res_pv[3], res_pv[1], res_pv[4], 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, res_pv[6], res_pv[9], res_pv[11]},
{0.0, 0.0, 0.0, res_pv[9], res_pv[7], res_pv[10]},
{0.0, 0.0, 0.0, res_pv[11], res_pv[10], res_pv[8]}};
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)
{
@ -124,17 +124,14 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res
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
d_R = {{res_pv[0], res_pv[3], res_pv[5], 0.0, 0.0, 0.0},
{res_pv[3], res_pv[1], res_pv[4], 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, res_pv[6], res_pv[9], res_pv[11]},
{0.0, 0.0, 0.0, res_pv[9], res_pv[7], res_pv[10]},
{0.0, 0.0, 0.0, res_pv[11], res_pv[10], res_pv[8]}};
} // 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

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

@ -37,7 +37,7 @@ public:
virtual ~Pvt_Kf() = default;
void init_Kf(const arma::vec& p,
const arma::vec& v,
const arma::vec& res_p,
const arma::vec& res_pv,
double solver_interval_s,
bool estatic_measures_sd,
double measures_ecef_pos_sd_m,
@ -45,7 +45,7 @@ public:
double system_ecef_pos_sd_m,
double system_ecef_vel_sd_ms);
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 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 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]};
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.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,
v,
res_p,
res_pv,
kf_update_interval_s,
d_conf.estatic_measures_sd,
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 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]};
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.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);
pvt_sol.rr[0] = p[0]; // [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->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;
}
}