diff --git a/src/mvp_control/mvp_control.cpp b/src/mvp_control/mvp_control.cpp
index 273a72a..b08300b 100644
--- a/src/mvp_control/mvp_control.cpp
+++ b/src/mvp_control/mvp_control.cpp
@@ -35,7 +35,7 @@ MvpControl::MvpControl() {
      * Initialize the error state
      */
     m_error_state = Eigen::VectorXd(CONTROLLABLE_DOF_LENGTH);
-
+    m_error_state.setZero();
     /**
      * Create the multiple input multiple output PID controller
      */
diff --git a/src/mvp_control/mvp_control_ros.cpp b/src/mvp_control/mvp_control_ros.cpp
index ab869bb..61a6daf 100644
--- a/src/mvp_control/mvp_control_ros.cpp
+++ b/src/mvp_control/mvp_control_ros.cpp
@@ -25,7 +25,7 @@
 #include "exception.hpp"
 #include "tf2_eigen/tf2_eigen.h"
 
-#include "mvp_control/dictionary.h"
+
 
 #include "boost/regex.hpp"
 
diff --git a/src/mvp_control/mvp_control_ros.h b/src/mvp_control/mvp_control_ros.h
index 8a9a554..83cdad7 100644
--- a/src/mvp_control/mvp_control_ros.h
+++ b/src/mvp_control/mvp_control_ros.h
@@ -55,7 +55,7 @@
 #include "mvp_msgs/GetControlMode.h"
 #include "mvp_msgs/GetControlModes.h"
 #include "mvp_msgs/SetControlPoint.h"
-
+#include "mvp_control/dictionary.h"
 /*******************************************************************************
  * MVP
  */