mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-16 06:44:57 +00:00
Code Issues Releases Wiki Activity

Make clang-format happy

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

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

@ -42,25 +42,27 @@ void Pvt_Kf::init_Kf(const arma::vec& p,
d_H = arma::eye(6, 6);
// measurement matrix static covariances
if(estatic_measures_sd){
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},
{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)}};
if (estatic_measures_sd)
{
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},
{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_static = true;
}else{
d_static = true;
}
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)}};
{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_static = false;
d_static = false;
}
// system covariance matrix (static)
d_Q = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0},
@ -119,18 +121,19 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res
// Measurement update
try
{
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];
}
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
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