Skip to content

Commit

Permalink
ADD: adaptative R pvt_kf
Browse files Browse the repository at this point in the history
  • Loading branch information
@kalmancito
kalmancito committed Feb 5, 2024
1 parent 7c58a45 commit b7f6cec
Show file tree
Hide file tree
Showing 2 changed files with 67 additions and 15 deletions.
71 changes: 59 additions & 12 deletions src/algorithms/PVT/libs/pvt_kf.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
* \file pvt_kf.cc
* \brief Kalman Filter for Position and Velocity
* \author Javier Arribas, 2023. jarribas(at)cttc.es
* \author Miguel Angel Gomez Lopez, 2023. gomezlma(at)alumnos.upm.es
*
*
* -----------------------------------------------------------------------------
Expand All @@ -21,14 +22,25 @@

void Pvt_Kf::init_Kf(const arma::vec& p,
const arma::vec& v,
double update_interval_s,
const arma::vec& res_pv,
double solver_interval_s,
bool static_scenario_sd,
bool estatic_measures_sd,
double measures_ecef_pos_sd_m,
double measures_ecef_vel_sd_ms,
double system_ecef_pos_sd_m,
double system_ecef_vel_sd_ms)
{
// Kalman Filter class variables
const double Ti = update_interval_s;
const double Ti = solver_interval_s;
if (static_scenario_sd)
{
scenario_static = true;
}
else
{
scenario_static = false;
}

d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0},
{0.0, 1.0, 0.0, 0.0, Ti, 0.0},
Expand All @@ -40,13 +52,28 @@ void Pvt_Kf::init_Kf(const arma::vec& p,
d_H = arma::eye(6, 6);

// 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},
{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_R = {{res_pv[0], res_pv[3], res_pv[5], 0.0, 0.0, 0.0},
{res_pv[3], res_pv[1], res_pv[4], 0.0, 0.0, 0.0},
{res_pv[5], res_pv[4], res_pv[2], 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, res_pv[6], res_pv[9], res_pv[11]},
{0.0, 0.0, 0.0, res_pv[9], res_pv[7], res_pv[10]},
{0.0, 0.0, 0.0, res_pv[11], res_pv[10], res_pv[8]}};

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},
{0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0},
Expand Down Expand Up @@ -92,8 +119,9 @@ void Pvt_Kf::reset_Kf()
}


void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v)
void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_pv)
{
// bool static_scenario = true;
if (d_initialized)
{
// Kalman loop
Expand All @@ -104,11 +132,30 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v)
// Measurement update
try
{
arma::vec z = arma::join_cols(p, v);
if (!d_static)
{
// Measurement residuals non-static update
d_R = {{res_pv[0], res_pv[3], res_pv[5], 0.0, 0.0, 0.0},
{res_pv[3], res_pv[1], res_pv[4], 0.0, 0.0, 0.0},
{res_pv[5], res_pv[4], res_pv[2], 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, res_pv[6], res_pv[9], res_pv[11]},
{0.0, 0.0, 0.0, res_pv[9], res_pv[7], res_pv[10]},
{0.0, 0.0, 0.0, res_pv[11], res_pv[10], res_pv[8]}};
}
arma::vec z = arma::zeros(6);
if (scenario_static)
{
d_x_new_old[3] = 0;
d_x_new_old[4] = 0;
d_x_new_old[5] = 0;
}

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);
d_P_new_new = (arma::eye(6, 6) - K * d_H) * d_P_new_old;
arma::mat A = (arma::eye(6, 6) - K * d_H);
d_P_new_new = A * d_P_new_old * A.t() + K * d_R * K.t();

// prepare data for next KF epoch
d_x_old_old = d_x_new_new;
Expand Down
11 changes: 8 additions & 3 deletions src/algorithms/PVT/libs/pvt_kf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
* \file pvt_kf.h
* \brief Kalman Filter for Position and Velocity
* \author Javier Arribas, 2023. jarribas(at)cttc.es
*
* \author Miguel Angel Gomez Lopez, 2023. gomezlma(at)alumnos.upm.es
*
* -----------------------------------------------------------------------------
*
Expand Down Expand Up @@ -37,13 +37,16 @@ class Pvt_Kf
virtual ~Pvt_Kf() = default;
void init_Kf(const arma::vec& p,
const arma::vec& v,
double update_interval_s,
const arma::vec& res_pv,
double solver_interval_s,
bool static_scenario_sd,
bool estatic_measures_sd,
double measures_ecef_pos_sd_m,
double measures_ecef_vel_sd_ms,
double system_ecef_pos_sd_m,
double system_ecef_vel_sd_ms);
bool is_initialized() const;
void run_Kf(const arma::vec& p, const arma::vec& v);
void run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_pv);
void get_pv_Kf(arma::vec& p, arma::vec& v) const;
void reset_Kf();

Expand All @@ -60,6 +63,8 @@ class Pvt_Kf
arma::vec d_x_new_old;
arma::vec d_x_new_new;
bool d_initialized{false};
bool d_static{false};
bool scenario_static{false};
};


Expand Down

0 comments on commit b7f6cec

Please sign in to comment.