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

[merge] next branches

Browse Source
This commit is contained in:
miguekf 2023-07-10 15:08:35 +02:00
parent fd57c34d5d
commit 636f618fba
3 changed files with 37 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,7 +21,8 @@
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,
double update_interval_s, const arma::vec& res_p,
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,
@ -40,9 +41,9 @@ 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 = {{pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0}, d_R = {{res_p[0], res_p[3], res_p[5], 0.0, 0.0, 0.0},
{0.0, pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0}, {res_p[3], res_p[1], res_p[4], 0.0, 0.0, 0.0},
{0.0, 0.0, pow(measures_ecef_pos_sd_m, 2.0), 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)}};
@ -92,7 +93,7 @@ void Pvt_Kf::reset_Kf()
} }
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)
{ {
if (d_initialized) if (d_initialized)
{ {
@ -104,11 +105,30 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v)
// Measurement update // Measurement update
try try
{ {
// 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::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
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);
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();
// 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);
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 // prepare data for next KF epoch
d_x_old_old = d_x_new_new; d_x_old_old = d_x_new_new;

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

@ -37,13 +37,14 @@ 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,
double update_interval_s, const arma::vec& res_p,
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);
bool is_initialized() const; bool is_initialized() const;
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_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();

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

@ -1548,9 +1548,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 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],
pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5]};
d_pvt_kf.init_Kf(p, d_pvt_kf.init_Kf(p,
v, v,
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,
@ -1561,7 +1564,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 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]};
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_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]