diff --git a/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h b/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h index 0271253..6c24bad 100644 --- a/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h +++ b/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h @@ -41,47 +41,48 @@ class AltGeneric : public AltitudeEstimator { typedef mrs_lib::DynamicReconfigureMgr drmgr_t; - using lkf_t = mrs_lib::LKF; - 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 rep_lkf_t; + using lkf_t = mrs_lib::LKF; + using varstep_lkf_t = mrs_lib::varstepLKF; + 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 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_; - std::unique_ptr lkf_rep_; - std::vector> 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_; + std::unique_ptr lkf_rep_; + std::vector> models_; + mutable std::mutex mutex_lkf_; + statecov_t sc_; + mutable std::mutex mutex_sc_; std::unique_ptr 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; @@ -143,6 +144,8 @@ class AltGeneric : public AltitudeEstimator { void setDt(const double &dt); void setInputCoeff(const double &input_coeff); + void generateRepredictorModels(const double input_coeff); + void generateA(); void generateB(); diff --git a/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h b/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h index 7b94bec..c5972e6 100644 --- a/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h +++ b/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h @@ -39,38 +39,39 @@ class HdgGeneric : public HeadingEstimator { typedef mrs_lib::DynamicReconfigureMgr drmgr_t; - using lkf_t = mrs_lib::LKF; - 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 rep_lkf_t; + using lkf_t = mrs_lib::LKF; + using varstep_lkf_t = mrs_lib::varstepLKF; + 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 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_; - std::unique_ptr lkf_rep_; - std::vector> 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_; + std::unique_ptr lkf_rep_; + std::vector> models_; + mutable std::mutex mutex_lkf_; + statecov_t sc_; + mutable std::mutex mutex_sc_; std::unique_ptr drmgr_; void callbackReconfigure(HeadingEstimatorConfig &config, [[maybe_unused]] uint32_t level); @@ -143,6 +144,8 @@ class HdgGeneric : public HeadingEstimator { void setDt(const double &dt); void setInputCoeff(const double &input_coeff); + void generateRepredictorModels(const double input_coeff); + void generateA(); void generateB(); diff --git a/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h b/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h index 8b934d7..ea1844b 100644 --- a/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h +++ b/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h @@ -38,37 +38,38 @@ class LatGeneric : public LateralEstimator { typedef mrs_lib::DynamicReconfigureMgr drmgr_t; - using lkf_t = mrs_lib::LKF; - 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 rep_lkf_t; + using lkf_t = mrs_lib::LKF; + using varstep_lkf_t = mrs_lib::varstepLKF; + 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 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_; - std::unique_ptr lkf_rep_; - std::vector> 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_; + std::unique_ptr lkf_rep_; + std::vector> models_; + mutable std::mutex mutex_lkf_; + statecov_t sc_; + mutable std::mutex mutex_sc_; std::unique_ptr drmgr_; void callbackReconfigure(LateralEstimatorConfig &config, [[maybe_unused]] uint32_t level); @@ -76,9 +77,9 @@ class LatGeneric : public LateralEstimator { 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; @@ -116,7 +117,10 @@ class LatGeneric : public LateralEstimator { 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()> fun_get_hdg) - : LateralEstimator(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(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) { @@ -148,6 +152,8 @@ class LatGeneric : public LateralEstimator { void setDt(const double &dt); void setInputCoeff(const double &input_coeff); + void generateRepredictorModels(const double input_coeff); + void generateA(); void generateB(); diff --git a/src/estimators/altitude/alt_generic.cpp b/src/estimators/altitude/alt_generic.cpp index 5cb41dd..6a4915a 100644 --- a/src/estimators/altitude/alt_generic.cpp +++ b/src/estimators/altitude/alt_generic.cpp @@ -101,15 +101,11 @@ void AltGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr(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(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>(x0, P0, u0, Q_, t0, lkf_, rep_buffer_size_); + lkf_rep_ = std::make_unique>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_); setDt(1.0 / ch_->desired_uav_state_rate); } @@ -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>(x0, P0, u0, Q_, t0, lkf_, rep_buffer_size_); + lkf_rep_ = std::make_unique>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_); } ROS_INFO("[%s]: Estimator reset", getPrintName().c_str()); @@ -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); } @@ -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(); } @@ -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_); @@ -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(lambda_generateA, lambda_generateB, H)); + } } /*//}*/ diff --git a/src/estimators/heading/hdg_generic.cpp b/src/estimators/heading/hdg_generic.cpp index 4cf9a31..cedb4ce 100644 --- a/src/estimators/heading/hdg_generic.cpp +++ b/src/estimators/heading/hdg_generic.cpp @@ -96,15 +96,11 @@ void HdgGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr(A_, B_, H_); if (is_repredictor_enabled_) { - for (int i = 0; i < hdg_generic::n_states; i++) { - H_t H = H_t::Zero(); - H(i) = 1; - models_.push_back(std::make_shared(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>(x0, P0, u0, Q_, t0, lkf_, rep_buffer_size_); + lkf_rep_ = std::make_unique>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_); setDt(1.0 / ch_->desired_uav_state_rate); } @@ -197,7 +193,7 @@ bool HdgGeneric::reset(void) { const u_t u0 = u_t::Zero(); const ros::Time t0 = ros::Time(0); - lkf_rep_ = std::make_unique>(x0, P0, u0, Q_, t0, lkf_, rep_buffer_size_); + lkf_rep_ = std::make_unique>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_); } ROS_INFO("[%s]: Estimator reset", getPrintName().c_str()); @@ -310,7 +306,7 @@ void HdgGeneric::timerUpdate(const ros::TimerEvent &event) { return; } - if (!is_repredictor_enabled_) { // repredictor requires constant dt + if (!is_repredictor_enabled_) { // repredictor calculates dt on its own setDt(dt); } @@ -328,11 +324,15 @@ void HdgGeneric::timerUpdate(const ros::TimerEvent &event) { ros::Time input_stamp; if (is_input_ready_) { input_stamp = sh_control_input_.getMsg()->header.stamp; - setInputCoeff(default_input_coeff_); + if (input_coeff_ != default_input_coeff_){ + setInputCoeff(default_input_coeff_); + } u(0) = sh_control_input_.getMsg()->control_hdg_rate; } else { input_stamp = ros::Time::now(); - setInputCoeff(0); + if (input_coeff_ != 0){ + setInputCoeff(0); + } u = u_t::Zero(); } @@ -342,7 +342,7 @@ void HdgGeneric::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_); @@ -614,6 +614,43 @@ void HdgGeneric::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 HdgGeneric::generateRepredictorModels(const double input_coeff) { + + for (int i = 0; i < hdg_generic::n_states; i++) { + + auto lambda_generateA = [input_coeff](const double dt) { + A_t A; + // clang-format off + A << + 1, dt, + 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, + (input_coeff * dt); + // clang-format on + return B; + }; + + H_t H = H_t::Zero(); + H(i) = 1; + models_.push_back(std::make_shared(lambda_generateA, lambda_generateB, H)); + } } /*//}*/ diff --git a/src/estimators/lateral/lat_generic.cpp b/src/estimators/lateral/lat_generic.cpp index df560a2..843af49 100644 --- a/src/estimators/lateral/lat_generic.cpp +++ b/src/estimators/lateral/lat_generic.cpp @@ -105,16 +105,11 @@ void LatGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr(A_, B_, H_); if (is_repredictor_enabled_) { - for (int i = 0; i < lat_generic::n_states; i++) { - H_t H = H_t::Zero(); - H(AXIS_X, i * 2) = 1; - H(AXIS_Y, i * 2 + 1) = 1; - models_.push_back(std::make_shared(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>(x0, P0, u0, Q_, t0, lkf_, rep_buffer_size_); + lkf_rep_ = std::make_unique>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_); setDt(1.0 / ch_->desired_uav_state_rate); } @@ -209,7 +204,7 @@ bool LatGeneric::reset(void) { const u_t u0 = u_t::Zero(); const ros::Time t0 = ros::Time(0); - lkf_rep_ = std::make_unique>(x0, P0, u0, Q_, t0, lkf_, rep_buffer_size_); + lkf_rep_ = std::make_unique>(x0, P0, u0, Q_, t0, models_[0], rep_buffer_size_); } ROS_INFO("[%s]: Estimator reset", getPrintName().c_str()); @@ -349,7 +344,7 @@ void LatGeneric::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); } @@ -365,14 +360,18 @@ void LatGeneric::timerUpdate(const ros::TimerEvent &event) { const tf2::Vector3 des_acc_global = getAccGlobal(sh_control_input_.getMsg(), res.value()); input_stamp = sh_control_input_.getMsg()->header.stamp; - setInputCoeff(default_input_coeff_); + if (input_coeff_ != default_input_coeff_){ + setInputCoeff(default_input_coeff_); + } u(0) = des_acc_global.getX(); u(1) = des_acc_global.getY(); } else { // this is ok before the controller starts controlling but bad during actual flight (causes delayed estimated acceleration and velocity) 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(); } @@ -394,7 +393,7 @@ void LatGeneric::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_); @@ -693,6 +692,52 @@ void LatGeneric::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 LatGeneric::generateRepredictorModels(const double input_coeff) { + for (int i = 0; (int)(i < lat_generic::n_states / 2); i++) { + + auto lambda_generateA = [input_coeff](const double dt) { + A_t A; + // clang-format off + A << + 1, 0, dt, 0, 0.5 * dt * dt, 0, + 0, 1, 0, dt, 0, 0.5 * dt * dt, + 0, 0, 1, 0, dt, 0, + 0, 0, 0, 1, 0, dt, + 0, 0, 0, 0, 1-(input_coeff * dt), 0, + 0, 0, 0, 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, + 0, 0, + 0, 0, + 0, 0, + input_coeff * dt, 0, + 0, input_coeff * dt; + // clang-format on + + return B; + }; + + H_t H = H.Zero(); + H(AXIS_X, i * 2) = 1; + H(AXIS_Y, i * 2 + 1) = 1; + models_.push_back(std::make_shared(lambda_generateA, lambda_generateB, H)); + } } /*//}*/