mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-04-02 00:37:03 +00:00
Code Issues Releases Wiki Activity

Code cleaning

Browse Source
This commit is contained in:
Javier Arribas 2023-07-04 17:19:17 +02:00
parent 36e709dda6
commit 4c448251fa
2 changed files with 3 additions and 25 deletions
src/algorithms/PVT/libs
pvt_conf.hpvt_kf.cc

2
src/algorithms/PVT/libs/pvt_conf.h

@ -95,7 +95,7 @@ public:
bool use_has_corrections = true;
bool use_unhealthy_sats = false;
//PVT KF parameters
// PVT KF parameters
bool enable_pvt_kf = false;
double measures_ecef_pos_sd_m = 1.0;
double measures_ecef_vel_sd_ms = 0.1;

26
src/algorithms/PVT/libs/pvt_kf.cc

@ -38,7 +38,7 @@ void Pvt_Kf::init_kf(arma::vec p, arma::vec v,
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},
{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},
@ -81,38 +81,16 @@ void Pvt_Kf::init_kf(arma::vec p, arma::vec v,
void Pvt_Kf::run_Kf(arma::vec p, arma::vec v)
{
//
// Pkp{1,i}=F*Pk{1,i-1}*F'+Q;
// Xkp{1,i}=F*Xk{1,i-1}; %nuevo estado a partir del modelo de transición
//
// %% Ganancia de Kalman
//
// K=Pkp{1,i}*A'*inv(A*Pkp{1,i}*A'+R); %en base a lo que acaba de predecir
//
// %% Corrección de la predicción y la covarianza, usando la info de las
// %% observaciones
//
// Xk{1,i}=Xkp{1,i}+K*(Z{1,i}-A*Xkp{1,i}); %correccion de lo predicho en base a la observación Z
//
// Pk{1,i}=(eye(4)-K*A)*Pkp{1,i}; % Corrección de la covarianza
// %pause(1)
//
// Kalman loop
// Prediction
// std::cout << "d_x_old_old=" << d_x_old_old << "\n";
d_x_new_old = d_F * d_x_old_old;
// std::cout << "d_x_new_old=" << d_x_new_old << "\n";
d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q;
// 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);
// std::cout << "z=" << z << "\n";
// std::cout << "K=" << K << "\n";
// std::cout << "d_x_new_new=" << d_x_new_new << "\n";
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;
// prepare data for next KF epoch