From c9ebddf618fff6e47e9b640b310c043b9ca3372b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?C=C3=A9dric=20Perottet?= Date: Tue, 15 Jun 2021 05:21:54 +0200 Subject: [PATCH 1/2] Fix for UnscentedKalmannFilter --- ct_optcon/examples/KalmanFiltering.cpp | 2 +- .../include/ct/optcon/filter/UnscentedKalmanFilter-impl.h | 7 +++++-- ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter.h | 4 ++-- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/ct_optcon/examples/KalmanFiltering.cpp b/ct_optcon/examples/KalmanFiltering.cpp index d3f8a7dba..fef7a0989 100644 --- a/ct_optcon/examples/KalmanFiltering.cpp +++ b/ct_optcon/examples/KalmanFiltering.cpp @@ -105,7 +105,7 @@ int main(int argc, char** argv) new ct::optcon::CTSystemModel(oscillator_observer_model, sensApprox, dFdv)); // set up the measurement model - ct::core::OutputStateMatrix dHdw; + ct::core::OutputMatrix dHdw; dHdw.setIdentity(); std::shared_ptr> measModel( new ct::optcon::LTIMeasurementModel(C, dHdw)); diff --git a/ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter-impl.h b/ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter-impl.h index 2e5dfb5c1..57d30e4b4 100644 --- a/ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter-impl.h +++ b/ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter-impl.h @@ -62,6 +62,7 @@ UnscentedKalmanFilter::UnscentedKalm const ct::core::StateMatrix& P0) : Base(f, h, x0), alpha_(alpha), beta_(beta), kappa_(kappa), P_(P0) { + computeWeights(); } template @@ -75,6 +76,7 @@ UnscentedKalmanFilter::UnscentedKalm kappa_(ukf_settings.kappa), P_(ukf_settings.P0) { + computeWeights(); } template @@ -263,8 +265,9 @@ void UnscentedKalmanFilter::computeS template template -auto UnscentedKalmanFilter::computePredictionFromSigmaPoints( - const SigmaPoints& sigmaPoints) -> state_vector_t +Eigen::Matrix +UnscentedKalmanFilter::computePredictionFromSigmaPoints( + const SigmaPoints& sigmaPoints) { // Use efficient matrix x vector computation return sigmaPoints * sigmaWeights_m_; diff --git a/ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter.h b/ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter.h index bffac4c60..36f160d96 100644 --- a/ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter.h +++ b/ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter.h @@ -147,9 +147,9 @@ class UnscentedKalmanFilter final : public EstimatorBase& sigmaMeasurementPoints, const ct::core::Time& t = 0); - //! Make a prediction based on sigma points. + //! Make a prediction based on sigma points. Used for both state and output prediction. template - state_vector_t computePredictionFromSigmaPoints(const SigmaPoints& sigmaPoints); + Eigen::Matrix computePredictionFromSigmaPoints(const SigmaPoints& sigmaPoints); private: state_matrix_t P_; //! Covariance matrix. From ef096b9e21a1682bb19019e32dc158d1fd4583e0 Mon Sep 17 00:00:00 2001 From: cedric Date: Mon, 28 Mar 2022 15:49:29 +0000 Subject: [PATCH 2/2] hpipm_mode can be defined in configuration file --- .../nlocSolver_ObstacleConstraint.info | 1 + .../ct/optcon/solver/NLOptConSettings.hpp | 20 ++++++++++++++----- .../optcon/solver/lqp/HPIPMInterface-impl.hpp | 10 ++++++++++ .../ct/optcon/solver/lqp/HPIPMInterface.hpp | 7 +++++-- 4 files changed, 31 insertions(+), 7 deletions(-) diff --git a/ct_optcon/examples/nlocSolver_ObstacleConstraint.info b/ct_optcon/examples/nlocSolver_ObstacleConstraint.info index c0a635180..3d28fcc6d 100644 --- a/ct_optcon/examples/nlocSolver_ObstacleConstraint.info +++ b/ct_optcon/examples/nlocSolver_ObstacleConstraint.info @@ -37,5 +37,6 @@ ilqr { lqoc_debug_print false num_lqoc_iterations 50 + hpipm_mode_str SPEED } } \ No newline at end of file diff --git a/ct_optcon/include/ct/optcon/solver/NLOptConSettings.hpp b/ct_optcon/include/ct/optcon/solver/NLOptConSettings.hpp index fd6918b9f..c7457b3d1 100644 --- a/ct_optcon/include/ct/optcon/solver/NLOptConSettings.hpp +++ b/ct_optcon/include/ct/optcon/solver/NLOptConSettings.hpp @@ -24,9 +24,9 @@ struct LineSearchSettings //! types of backtracking line-search enum TYPE { - NONE = 0, //! take full-step updates - SIMPLE, //! simple backtracking using cost or merit function - ARMIJO, //! backtracking including riccati matrix measure + NONE = 0, //! take full-step updates + SIMPLE, //! simple backtracking using cost or merit function + ARMIJO, //! backtracking including riccati matrix measure GOLDSTEIN, //! backtracking including riccati matrix measure and defects NUM_TYPES }; @@ -154,16 +154,20 @@ struct LineSearchSettings struct LQOCSolverSettings { public: - LQOCSolverSettings() : lqoc_debug_print(false), num_lqoc_iterations(10) {} + LQOCSolverSettings() : lqoc_debug_print(false), num_lqoc_iterations(10), hpipm_mode_str("SPEED") {} bool lqoc_debug_print; - int num_lqoc_iterations; //! number of allowed sub-iterations of LQOC solver per NLOC main iteration + int num_lqoc_iterations; //! number of allowed sub-iterations of LQOC solver per NLOC main iteration + std::string hpipm_mode_str; // HPIPM specific : SPEED_ABS, SPEED, ROBUST or BALANCED. + // Stored as string and not as hpipm_mode enum because HPIPM is not a compulsory dependency. + //(see https://github.com/giaf/hpipm/blob/f38a216c5a9f916682cd543f8992a594323ad202/ocp_qp/x_ocp_qp_ipm.c#L69) void print() const { std::cout << "======================= LQOCSolverSettings =====================" << std::endl; std::cout << "num_lqoc_iterations: \t" << num_lqoc_iterations << std::endl; std::cout << "lqoc_debug_print: \t" << lqoc_debug_print << std::endl; + std::cout << "hpipm_mode_str: \t" << hpipm_mode_str << std::endl; } void load(const std::string& filename, bool verbose = true, const std::string& ns = "lqoc_solver_settings") @@ -186,6 +190,12 @@ struct LQOCSolverSettings } catch (...) { } + try + { + hpipm_mode_str = pt.get(ns + ".hpipm_mode_str"); + } catch (...) + { + } } }; diff --git a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp index ea11a2a77..b6216b4e7 100644 --- a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp +++ b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp @@ -139,6 +139,16 @@ void HPIPMInterface::initializeAndAllocate() ipm_arg_mem_ = malloc(ipm_arg_size); ::d_ocp_qp_ipm_arg_create(&dim_, &arg_, ipm_arg_mem_); + if (stringToHpipmMode.find(settings_.lqoc_solver_settings.hpipm_mode_str) != stringToHpipmMode.end()) + mode_ = stringToHpipmMode[settings_.lqoc_solver_settings.hpipm_mode_str]; + else + { + std::cout << "Invalid hpipm_mode_str specified in config, should be one of the following:" << std::endl; + for (auto it = stringToHpipmMode.begin(); it != stringToHpipmMode.end(); it++) + std::cout << it->first << std::endl; + exit(-1); + } + ::d_ocp_qp_ipm_arg_set_default(mode_, &arg_); ::d_ocp_qp_ipm_arg_set_iter_max(&settings_.lqoc_solver_settings.num_lqoc_iterations, &arg_); diff --git a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp index 8ed22ffe1..c71dda2f4 100644 --- a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp +++ b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp @@ -255,8 +255,11 @@ class HPIPMInterface : public LQOCSolver struct d_ocp_qp_ipm_ws workspace_; int hpipm_status_; // status code after solving - // todo make this a setting - ::hpipm_mode mode_ = ::hpipm_mode::SPEED; // ROBUST/BALANCED; see also hpipm_common.h + // hpipm mode SPEED_ABS, SPEED, ROBUST or BALANCED + ::hpipm_mode mode_; // see also hpipm_common.h + + std::map stringToHpipmMode = {{"SPEED_ABS", ::hpipm_mode::SPEED_ABS}, + {"SPEED", ::hpipm_mode::SPEED}, {"ROBUST", ::hpipm_mode::ROBUST}, {"BALANCE", ::hpipm_mode::BALANCE}}; }; } // namespace optcon