Skip to content

Commit

Permalink
Merge pull request #2 from ctu-mrs/repredictor_dt_fix
Browse files Browse the repository at this point in the history 
fix for repredictor to be able to use variable dt
  • Loading branch information
@petrlmat
petrlmat authored Feb 16, 2024
2 parents 3622064 + ef5d29e commit 79362d8
Show file tree
Hide file tree
Showing 6 changed files with 249 additions and 116 deletions.
59 changes: 31 additions & 28 deletions include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,47 +41,48 @@ class AltGeneric : public AltitudeEstimator<alt_generic::n_states> {

typedef mrs_lib::DynamicReconfigureMgr<AltitudeEstimatorConfig> drmgr_t;

using lkf_t = mrs_lib::LKF<alt_generic::n_states, alt_generic::n_inputs, alt_generic::n_measurements>;
using A_t = lkf_t::A_t;
using B_t = lkf_t::B_t;
using H_t = lkf_t::H_t;
using Q_t = lkf_t::Q_t;
using x_t = lkf_t::x_t;
using P_t = lkf_t::P_t;
using u_t = lkf_t::u_t;
using z_t = lkf_t::z_t;
using R_t = lkf_t::R_t;
using statecov_t = lkf_t::statecov_t;

typedef mrs_lib::Repredictor<lkf_t> rep_lkf_t;
using lkf_t = mrs_lib::LKF<alt_generic::n_states, alt_generic::n_inputs, alt_generic::n_measurements>;
using varstep_lkf_t = mrs_lib::varstepLKF<alt_generic::n_states, alt_generic::n_inputs, alt_generic::n_measurements>;
using A_t = lkf_t::A_t;
using B_t = lkf_t::B_t;
using H_t = lkf_t::H_t;
using Q_t = lkf_t::Q_t;
using x_t = lkf_t::x_t;
using P_t = lkf_t::P_t;
using u_t = lkf_t::u_t;
using z_t = lkf_t::z_t;
using R_t = lkf_t::R_t;
using statecov_t = lkf_t::statecov_t;

typedef mrs_lib::Repredictor<varstep_lkf_t> rep_lkf_t;

using StateId_t = mrs_uav_managers::estimation_manager::StateId_t;

private:
std::string parent_state_est_name_;

double dt_;
double input_coeff_, default_input_coeff_;
A_t A_;
B_t B_;
H_t H_;
Q_t Q_;
std::shared_ptr<lkf_t> lkf_;
std::unique_ptr<rep_lkf_t> lkf_rep_;
std::vector<std::shared_ptr<lkf_t>> models_;
mutable std::mutex mutex_lkf_;
statecov_t sc_;
mutable std::mutex mutex_sc_;
double dt_;
double input_coeff_, default_input_coeff_;
A_t A_;
B_t B_;
H_t H_;
Q_t Q_;
std::shared_ptr<lkf_t> lkf_;
std::unique_ptr<rep_lkf_t> lkf_rep_;
std::vector<std::shared_ptr<varstep_lkf_t>> models_;
mutable std::mutex mutex_lkf_;
statecov_t sc_;
mutable std::mutex mutex_sc_;

std::unique_ptr<drmgr_t> drmgr_;
void callbackReconfigure(AltitudeEstimatorConfig& config, [[maybe_unused]] uint32_t level);
void callbackReconfigure(AltitudeEstimatorConfig &config, [[maybe_unused]] uint32_t level);

z_t innovation_;
mutable std::mutex mtx_innovation_;

bool is_error_state_first_time_ = true;
bool is_error_state_first_time_ = true;
ros::Duration error_state_duration_;
ros::Time prev_time_in_error_state_;
ros::Time prev_time_in_error_state_;

bool is_repredictor_enabled_;
int rep_buffer_size_ = 200;
Expand Down Expand Up @@ -143,6 +144,8 @@ class AltGeneric : public AltitudeEstimator<alt_generic::n_states> {
void setDt(const double &dt);
void setInputCoeff(const double &input_coeff);

void generateRepredictorModels(const double input_coeff);

void generateA();
void generateB();

Expand Down
55 changes: 29 additions & 26 deletions include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,38 +39,39 @@ class HdgGeneric : public HeadingEstimator<hdg_generic::n_states> {

typedef mrs_lib::DynamicReconfigureMgr<HeadingEstimatorConfig> drmgr_t;

using lkf_t = mrs_lib::LKF<hdg_generic::n_states, hdg_generic::n_inputs, hdg_generic::n_measurements>;
using A_t = lkf_t::A_t;
using B_t = lkf_t::B_t;
using H_t = lkf_t::H_t;
using Q_t = lkf_t::Q_t;
using x_t = lkf_t::x_t;
using P_t = lkf_t::P_t;
using u_t = lkf_t::u_t;
using z_t = lkf_t::z_t;
using R_t = lkf_t::R_t;
using statecov_t = lkf_t::statecov_t;

typedef mrs_lib::Repredictor<lkf_t> rep_lkf_t;
using lkf_t = mrs_lib::LKF<hdg_generic::n_states, hdg_generic::n_inputs, hdg_generic::n_measurements>;
using varstep_lkf_t = mrs_lib::varstepLKF<hdg_generic::n_states, hdg_generic::n_inputs, hdg_generic::n_measurements>;
using A_t = lkf_t::A_t;
using B_t = lkf_t::B_t;
using H_t = lkf_t::H_t;
using Q_t = lkf_t::Q_t;
using x_t = lkf_t::x_t;
using P_t = lkf_t::P_t;
using u_t = lkf_t::u_t;
using z_t = lkf_t::z_t;
using R_t = lkf_t::R_t;
using statecov_t = lkf_t::statecov_t;

typedef mrs_lib::Repredictor<varstep_lkf_t> rep_lkf_t;

using StateId_t = mrs_uav_managers::estimation_manager::StateId_t;

private:
std::string parent_state_est_name_;

double dt_;
double input_coeff_;
double default_input_coeff_;
A_t A_;
B_t B_;
H_t H_;
Q_t Q_;
std::shared_ptr<lkf_t> lkf_;
std::unique_ptr<rep_lkf_t> lkf_rep_;
std::vector<std::shared_ptr<lkf_t>> models_;
mutable std::mutex mutex_lkf_;
statecov_t sc_;
mutable std::mutex mutex_sc_;
double dt_;
double input_coeff_;
double default_input_coeff_;
A_t A_;
B_t B_;
H_t H_;
Q_t Q_;
std::shared_ptr<lkf_t> lkf_;
std::unique_ptr<rep_lkf_t> lkf_rep_;
std::vector<std::shared_ptr<varstep_lkf_t>> models_;
mutable std::mutex mutex_lkf_;
statecov_t sc_;
mutable std::mutex mutex_sc_;

std::unique_ptr<drmgr_t> drmgr_;
void callbackReconfigure(HeadingEstimatorConfig &config, [[maybe_unused]] uint32_t level);
Expand Down Expand Up @@ -143,6 +144,8 @@ class HdgGeneric : public HeadingEstimator<hdg_generic::n_states> {
void setDt(const double &dt);
void setInputCoeff(const double &input_coeff);

void generateRepredictorModels(const double input_coeff);

void generateA();
void generateB();

Expand Down
62 changes: 34 additions & 28 deletions include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,47 +38,48 @@ class LatGeneric : public LateralEstimator<lat_generic::n_states> {

typedef mrs_lib::DynamicReconfigureMgr<LateralEstimatorConfig> drmgr_t;

using lkf_t = mrs_lib::LKF<lat_generic::n_states, lat_generic::n_inputs, lat_generic::n_measurements>;
using A_t = lkf_t::A_t;
using B_t = lkf_t::B_t;
using H_t = lkf_t::H_t;
using Q_t = lkf_t::Q_t;
using x_t = lkf_t::x_t;
using P_t = lkf_t::P_t;
using u_t = lkf_t::u_t;
using z_t = lkf_t::z_t;
using R_t = lkf_t::R_t;
using statecov_t = lkf_t::statecov_t;

typedef mrs_lib::Repredictor<lkf_t> rep_lkf_t;
using lkf_t = mrs_lib::LKF<lat_generic::n_states, lat_generic::n_inputs, lat_generic::n_measurements>;
using varstep_lkf_t = mrs_lib::varstepLKF<lat_generic::n_states, lat_generic::n_inputs, lat_generic::n_measurements>;
using A_t = lkf_t::A_t;
using B_t = lkf_t::B_t;
using H_t = lkf_t::H_t;
using Q_t = lkf_t::Q_t;
using x_t = lkf_t::x_t;
using P_t = lkf_t::P_t;
using u_t = lkf_t::u_t;
using z_t = lkf_t::z_t;
using R_t = lkf_t::R_t;
using statecov_t = lkf_t::statecov_t;

typedef mrs_lib::Repredictor<varstep_lkf_t> rep_lkf_t;

private:
const std::string package_name_ = "mrs_uav_state_estimators";

std::string parent_state_est_name_;

double dt_;
double input_coeff_, default_input_coeff_;
A_t A_;
B_t B_;
H_t H_;
Q_t Q_;
std::shared_ptr<lkf_t> lkf_;
std::unique_ptr<rep_lkf_t> lkf_rep_;
std::vector<std::shared_ptr<lkf_t>> models_;
mutable std::mutex mutex_lkf_;
statecov_t sc_;
mutable std::mutex mutex_sc_;
double dt_;
double input_coeff_, default_input_coeff_;
A_t A_;
B_t B_;
H_t H_;
Q_t Q_;
std::shared_ptr<lkf_t> lkf_;
std::unique_ptr<rep_lkf_t> lkf_rep_;
std::vector<std::shared_ptr<varstep_lkf_t>> models_;
mutable std::mutex mutex_lkf_;
statecov_t sc_;
mutable std::mutex mutex_sc_;

std::unique_ptr<drmgr_t> drmgr_;
void callbackReconfigure(LateralEstimatorConfig &config, [[maybe_unused]] uint32_t level);

z_t innovation_;
mutable std::mutex mtx_innovation_;

bool is_error_state_first_time_ = true;
bool is_error_state_first_time_ = true;
ros::Duration error_state_duration_;
ros::Time prev_time_in_error_state_;
ros::Time prev_time_in_error_state_;

bool is_repredictor_enabled_;
int rep_buffer_size_ = 200;
Expand Down Expand Up @@ -116,7 +117,10 @@ class LatGeneric : public LateralEstimator<lat_generic::n_states> {
public:
LatGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin,
std::function<std::optional<double>()> fun_get_hdg)
: LateralEstimator<lat_generic::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin), fun_get_hdg_(fun_get_hdg) {
: LateralEstimator<lat_generic::n_states>(name, ns_frame_id),
parent_state_est_name_(parent_state_est_name),
is_core_plugin_(is_core_plugin),
fun_get_hdg_(fun_get_hdg) {
}

~LatGeneric(void) {
Expand Down Expand Up @@ -148,6 +152,8 @@ class LatGeneric : public LateralEstimator<lat_generic::n_states> {
void setDt(const double &dt);
void setInputCoeff(const double &input_coeff);

void generateRepredictorModels(const double input_coeff);

void generateA();
void generateB();

Expand Down
61 changes: 50 additions & 11 deletions src/estimators/altitude/alt_generic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,15 +101,11 @@ void AltGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHan
lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
if (is_repredictor_enabled_) {

for (int i = 0; i < alt_generic::n_states; i++) {
H_t H = H_t::Zero();
H(i) = 1;
models_.push_back(std::make_shared<lkf_t>(A_, B_, H));
}
generateRepredictorModels(input_coeff_);

const u_t u0 = u_t::Zero();
const ros::Time t0 = ros::Time::now();
lkf_rep_ = std::make_unique<mrs_lib::Repredictor<lkf_t>>(x0, P0, u0, Q_, t0, lkf_, rep_buffer_size_);
lkf_rep_ = std::make_unique<mrs_lib::Repredictor<varstep_lkf_t>>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_);

setDt(1.0 / ch_->desired_uav_state_rate);
}
Expand Down Expand Up @@ -202,7 +198,7 @@ bool AltGeneric::reset(void) {

const u_t u0 = u_t::Zero();
const ros::Time t0 = ros::Time(0);
lkf_rep_ = std::make_unique<mrs_lib::Repredictor<lkf_t>>(x0, P0, u0, Q_, t0, lkf_, rep_buffer_size_);
lkf_rep_ = std::make_unique<mrs_lib::Repredictor<varstep_lkf_t>>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_);
}

ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
Expand Down Expand Up @@ -337,7 +333,7 @@ void AltGeneric::timerUpdate(const ros::TimerEvent &event) {
return;
}

if (!is_repredictor_enabled_) { // repredictor requires constant dt TODO: how to handle repredictor + variable rate?
if (!is_repredictor_enabled_) { // repredictor calculates dt on its own
setDt(dt);
}

Expand All @@ -348,12 +344,16 @@ void AltGeneric::timerUpdate(const ros::TimerEvent &event) {
mrs_msgs::EstimatorInputConstPtr msg = sh_control_input_.getMsg();
const tf2::Vector3 des_acc_global = getAccGlobal(msg, 0); // we don't care about heading
input_stamp = msg->header.stamp;
setInputCoeff(default_input_coeff_);
if (input_coeff_ != default_input_coeff_){
setInputCoeff(default_input_coeff_);
}
u(0) = des_acc_global.getZ();
} else {
ROS_DEBUG_THROTTLE(1.0, "[%s]: not receiving control input, estimation suboptimal, potentially unstable", getPrintName().c_str());
input_stamp = ros::Time::now();
setInputCoeff(0);
if (input_coeff_ != 0){
setInputCoeff(0);
}
u = u_t::Zero();
}

Expand All @@ -373,7 +373,7 @@ void AltGeneric::timerUpdate(const ros::TimerEvent &event) {
// Apply the prediction step
std::scoped_lock lock(mutex_lkf_);
if (is_repredictor_enabled_) {
lkf_rep_->addInputChangeWithNoise(u, Q, input_stamp, lkf_);
lkf_rep_->addInputChangeWithNoise(u, Q, input_stamp, models_[0]);
sc = lkf_rep_->predictTo(ros::Time::now());
} else {
sc = lkf_->predict(sc, u, Q, dt_);
Expand Down Expand Up @@ -660,6 +660,45 @@ void AltGeneric::setInputCoeff(const double &input_coeff) {
std::scoped_lock lock(mutex_lkf_);
lkf_->A = A_;
lkf_->B = B_;

if (is_repredictor_enabled_) {
models_.clear();
generateRepredictorModels(input_coeff_);
}
}
/*//}*/

/*//{ generateRepredictorModels() */
void AltGeneric::generateRepredictorModels(const double input_coeff) {

for (int i = 0; i < alt_generic::n_states; i++) {

auto lambda_generateA = [input_coeff](const double dt) {
A_t A;
// clang-format off
A <<
1, dt, 0.5 * dt * dt,
0, 1, dt,
0, 0, 1-(input_coeff * dt);
// clang-format on
return A;
};

auto lambda_generateB = [input_coeff]([[maybe_unused]] const double dt) {
B_t B = B.Zero();
// clang-format off
B <<
0,
0,
(input_coeff * dt);
// clang-format on
return B;
};

H_t H = H_t::Zero();
H(i) = 1;
models_.push_back(std::make_shared<varstep_lkf_t>(lambda_generateA, lambda_generateB, H));
}
}
/*//}*/

Expand Down
Loading

0 comments on commit 79362d8

Please sign in to comment.