diff --git a/CMakeLists.txt b/CMakeLists.txt
index 255893152bccd7d369cd537afb2c1199fc442668..48bdd8c66ad9eb7b5967c109a24f40a57442d1ae 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,7 @@
 cmake_minimum_required(VERSION 3.1)
 
+set (CMAKE_CXX_STANDARD 11)
+
 # Project properties
 set(PROJECT_NAMESPACE gepetto)
 set(PROJECT_NAME example-adder)
@@ -77,6 +79,9 @@ endif()
 # target_compile_options(${PROJECT_NAME} PUBLIC PRINTING)
 # target_compile_definitions(${PROJECT_NAME} PUBLIC PRINTING)
 
+target_compile_options(${PROJECT_NAME} PUBLIC -DNDEBUG -O3)
+
+
 # Main Executable
 add_executable(${PROJECT_NAMESPACE}-${PROJECT_NAME} src/main.cpp)
 target_link_libraries(${PROJECT_NAMESPACE}-${PROJECT_NAME} ${PROJECT_NAME})
diff --git a/src/MPC.cpp b/src/MPC.cpp
index b67fec7bc4dc1a6ba32ee1a07d3d6aab0a37ca6f..0ac6563f8911732a52072e335f4ad59b65d85c2a 100644
--- a/src/MPC.cpp
+++ b/src/MPC.cpp
@@ -267,7 +267,7 @@ int MPC::create_ML() {
     double s = sin(xref(5, k));
     Eigen::Matrix<double, 3, 3> R;
     R << c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0;
-    Eigen::Matrix<double, 3, 3> R_gI = R * gI;
+    Eigen::Matrix<double, 3, 3> R_gI = R.transpose() * gI * R;
     Eigen::Matrix<double, 3, 3> I_inv = R_gI.inverse();
 
     // Get skew-symetric matrix for each foothold
@@ -420,7 +420,7 @@ int MPC::create_weight_matrices() {
 
   // Define weights for the x-x_ref components of the optimization vector
   // Hand-tuning of parameters if you want to give more weight to specific components
-  double w[12] = {0.5f, 0.5f, 2.0f, 0.11f, 0.11f, 0.11f};
+  double w[12] = {0.5f, 0.5f, 2.0f, 0.55f, 0.55f, 0.11f};
   w[6] = 2.0f * sqrt(w[0]);
   w[7] = 2.0f * sqrt(w[1]);
   w[8] = 2.0f * sqrt(w[2]);
@@ -522,7 +522,7 @@ int MPC::update_ML(Eigen::MatrixXd fsteps) {
       double s = sin(xref(5, k));
       Eigen::Matrix<double, 3, 3> R;
       R << c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0;
-      Eigen::Matrix<double, 3, 3> R_gI = R * gI;
+      Eigen::Matrix<double, 3, 3> R_gI = R.transpose() * gI * R;
       Eigen::Matrix<double, 3, 3> I_inv = R_gI.inverse();
 
       // Get skew-symetric matrix for each foothold