Fix for UnscentedKalmannFilter

ct_optcon/examples/KalmanFiltering.cpp

@@ -105,7 +105,7 @@ int main(int argc, char** argv)
         new ct::optcon::CTSystemModel<state_dim, control_dim>(oscillator_observer_model, sensApprox, dFdv));
 
     // set up the measurement model
-    ct::core::OutputStateMatrix<output_dim, state_dim> dHdw;
+    ct::core::OutputMatrix<output_dim> dHdw;
     dHdw.setIdentity();
     std::shared_ptr<ct::optcon::LinearMeasurementModel<output_dim, state_dim>> measModel(
         new ct::optcon::LTIMeasurementModel<output_dim, state_dim>(C, dHdw));

ct_optcon/examples/nlocSolver_ObstacleConstraint.info

@@ -37,5 +37,6 @@ ilqr
     {
         lqoc_debug_print        false
         num_lqoc_iterations  	50
+        hpipm_mode_str          SPEED
     }
 }

ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter-impl.h

@@ -62,6 +62,7 @@ UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::UnscentedKalm
     const ct::core::StateMatrix<STATE_DIM, SCALAR>& P0)
     : Base(f, h, x0), alpha_(alpha), beta_(beta), kappa_(kappa), P_(P0)
 {
+    computeWeights();
 }
 
 template <size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR>
@@ -75,6 +76,7 @@ UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::UnscentedKalm
       kappa_(ukf_settings.kappa),
       P_(ukf_settings.P0)
 {
+    computeWeights();
 }
 
 template <size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR>
@@ -263,8 +265,9 @@ void UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::computeS
 
 template <size_t STATE_DIM, size_t CONTROL_DIM, size_t OUTPUT_DIM, typename SCALAR>
 template <size_t DIM>
-auto UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::computePredictionFromSigmaPoints(
-    const SigmaPoints<DIM>& sigmaPoints) -> state_vector_t
+Eigen::Matrix<SCALAR, DIM, 1>
+UnscentedKalmanFilter<STATE_DIM, CONTROL_DIM, OUTPUT_DIM, SCALAR>::computePredictionFromSigmaPoints(
+    const SigmaPoints<DIM>& sigmaPoints)
 {
     // Use efficient matrix x vector computation
     return sigmaPoints * sigmaWeights_m_;

ct_optcon/include/ct/optcon/filter/UnscentedKalmanFilter.h

@@ -147,9 +147,9 @@ class UnscentedKalmanFilter final : public EstimatorBase<STATE_DIM, CONTROL_DIM,
     //! Predict measurements of sigma points.
     void computeSigmaPointMeasurements(SigmaPoints<OUTPUT_DIM>& 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 <size_t DIM>
-    state_vector_t computePredictionFromSigmaPoints(const SigmaPoints<DIM>& sigmaPoints);
+    Eigen::Matrix<SCALAR, DIM, 1> computePredictionFromSigmaPoints(const SigmaPoints<DIM>& sigmaPoints);
 
 private:
     state_matrix_t P_;                                          //! Covariance matrix.

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<std::string>(ns + ".hpipm_mode_str");
+        } catch (...)
+        {
+        }
     }
 };
 

ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp

@@ -139,6 +139,16 @@ void HPIPMInterface<STATE_DIM, CONTROL_DIM>::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_);
 

ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp

@@ -255,8 +255,11 @@ class HPIPMInterface : public LQOCSolver<STATE_DIM, CONTROL_DIM>
     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<std::string, ::hpipm_mode> stringToHpipmMode = {{"SPEED_ABS", ::hpipm_mode::SPEED_ABS},
+        {"SPEED", ::hpipm_mode::SPEED}, {"ROBUST", ::hpipm_mode::ROBUST}, {"BALANCE", ::hpipm_mode::BALANCE}};
 };
 
 }  // namespace optcon