From accd9f7c34624154fc597e562b5391f11384c7c2 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 12 Jul 2023 17:27:58 +0200 Subject: [PATCH] Make clang-format happy --- src/algorithms/PVT/libs/pvt_kf.cc | 59 ++++++++++++++++--------------- 1 file changed, 31 insertions(+), 28 deletions(-) diff --git a/src/algorithms/PVT/libs/pvt_kf.cc b/src/algorithms/PVT/libs/pvt_kf.cc index 17336e0fa..ecd7e7bce 100755 --- a/src/algorithms/PVT/libs/pvt_kf.cc +++ b/src/algorithms/PVT/libs/pvt_kf.cc @@ -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