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

Merge branch 'next' of github.com:gnss-sdr/gnss-sdr into pps_lime

This commit is contained in:
Javier Arribas 2023-07-04 17:30:02 +02:00
parent 692d12d7ec 4c448251fa
commit 2f2ed7938e
2 changed files with 3 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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