mirror of https://github.com/gnss-sdr/gnss-sdr
Code cleaning
This commit is contained in:
parent
36e709dda6
commit
4c448251fa
|
@ -95,7 +95,7 @@ public:
|
||||||
bool use_has_corrections = true;
|
bool use_has_corrections = true;
|
||||||
bool use_unhealthy_sats = false;
|
bool use_unhealthy_sats = false;
|
||||||
|
|
||||||
//PVT KF parameters
|
// PVT KF parameters
|
||||||
bool enable_pvt_kf = false;
|
bool enable_pvt_kf = false;
|
||||||
double measures_ecef_pos_sd_m = 1.0;
|
double measures_ecef_pos_sd_m = 1.0;
|
||||||
double measures_ecef_vel_sd_ms = 0.1;
|
double measures_ecef_vel_sd_ms = 0.1;
|
||||||
|
|
|
@ -38,7 +38,7 @@ void Pvt_Kf::init_kf(arma::vec p, arma::vec v,
|
||||||
|
|
||||||
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 = {{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, 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, 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)
|
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
|
// Kalman loop
|
||||||
// Prediction
|
// Prediction
|
||||||
// std::cout << "d_x_old_old=" << d_x_old_old << "\n";
|
|
||||||
d_x_new_old = d_F * d_x_old_old;
|
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;
|
d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q;
|
||||||
|
|
||||||
// Measurement update
|
// 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);
|
|
||||||
// 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;
|
d_P_new_new = (arma::eye(6, 6) - K * d_H) * d_P_new_old;
|
||||||
|
|
||||||
// prepare data for next KF epoch
|
// prepare data for next KF epoch
|
||||||
|
|
Loading…
Reference in New Issue