diff --git a/CMakeLists.txt b/CMakeLists.txt
index 22f79752cb61df76fa737a4a9e58fd10032e2d69..8f8178eab4936624e5ee07b5aacd307d6b1126b8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -99,80 +99,31 @@ ENDIF()
 # Configure config.h
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
 
-# ============ HEADERS ============ 
-SET(HDRS_MATH
-  include/${PROJECT_NAME}/math/force_torque_delta_tools.h
-  )
-SET(HDRS_CAPTURE
-  include/${PROJECT_NAME}/capture/capture_force_torque_preint.h
-  include/${PROJECT_NAME}/capture/capture_inertial_kinematics.h
-  include/${PROJECT_NAME}/capture/capture_leg_odom.h
-  include/${PROJECT_NAME}/capture/capture_point_feet_nomove.h
-  )
-SET(HDRS_COMMON
-  include/${PROJECT_NAME}/common/bodydynamics.h
-  )
-SET(HDRS_FACTOR
-  include/${PROJECT_NAME}/factor/factor_force_torque.h
-  include/${PROJECT_NAME}/factor/factor_force_torque_preint.h
-  include/${PROJECT_NAME}/factor/factor_inertial_kinematics.h
-  include/${PROJECT_NAME}/factor/factor_point_feet_nomove.h
-  include/${PROJECT_NAME}/factor/factor_point_feet_altitude.h
-  )
-SET(HDRS_FEATURE
-  include/${PROJECT_NAME}/feature/feature_force_torque.h
-  include/${PROJECT_NAME}/feature/feature_force_torque_preint.h
-  include/${PROJECT_NAME}/feature/feature_inertial_kinematics.h
-  )
-SET(HDRS_PROCESSOR
-  include/${PROJECT_NAME}/processor/processor_force_torque_preint.h
-  include/${PROJECT_NAME}/processor/processor_inertial_kinematics.h
-  include/${PROJECT_NAME}/processor/processor_point_feet_nomove.h
-  )
-SET(HDRS_SENSOR
-  include/${PROJECT_NAME}/sensor/sensor_force_torque.h
-  include/${PROJECT_NAME}/sensor/sensor_inertial_kinematics.h
-  include/${PROJECT_NAME}/sensor/sensor_point_feet_nomove.h
-  )
-SET(HDRS_UTILS
-  include/${PROJECT_NAME}/utils/load_bodydynamics.h
-  )
-
 # ============ SOURCES ============ 
-SET(SRCS_CAPTURE
+SET(SRCS
+  # capture
   src/capture/capture_force_torque_preint.cpp
   src/capture/capture_inertial_kinematics.cpp
   src/capture/capture_leg_odom.cpp
   src/capture/capture_point_feet_nomove.cpp
-)
-SET(SRCS_FEATURE
+  # feature
   src/feature/feature_force_torque.cpp
   src/feature/feature_force_torque_preint.cpp
   src/feature/feature_inertial_kinematics.cpp
-  )
-SET(SRCS_PROCESSOR
+  # processor
   src/processor/processor_force_torque_preint.cpp
   src/processor/processor_inertial_kinematics.cpp
   src/processor/processor_point_feet_nomove.cpp
-)
-SET(SRCS_SENSOR
+  # sensor
   src/sensor/sensor_force_torque.cpp
   src/sensor/sensor_inertial_kinematics.cpp
   src/sensor/sensor_point_feet_nomove.cpp
-  )
-SET(SRCS_UTILS
+  # utils
   src/utils/load_bodydynamics.cpp
-  )
+)
 
 # create the shared library
-ADD_LIBRARY(${PLUGIN_NAME}
-  SHARED
-  ${SRCS_CAPTURE}
-  ${SRCS_FEATURE}
-  ${SRCS_PROCESSOR}
-  ${SRCS_SENSOR}
-  ${SRCS_UTILS}
-  )
+ADD_LIBRARY(${PLUGIN_NAME} SHARED ${SRCS})
 
 # Set compiler options
 # ====================
@@ -236,20 +187,8 @@ target_include_directories(${PLUGIN_NAME} PUBLIC
 )
 
 #install headers
-INSTALL(FILES ${HDRS_CAPTURE}
-   DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/capture)
-INSTALL(FILES ${HDRS_FACTOR}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/factor)
-INSTALL(FILES ${HDRS_FEATURE}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/feature)
-INSTALL(FILES ${HDRS_MATH}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/math)
-INSTALL(FILES ${HDRS_PROCESSOR}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/processor)
-INSTALL(FILES ${HDRS_SENSOR}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/sensor)
-INSTALL(FILES ${HDRS_UTILS}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/utils)
+INSTALL(DIRECTORY include/${PROJECT_NAME}
+  DESTINATION ${INCLUDE_INSTALL_DIR})
 
 #install config.h
 INSTALL(FILES ${WOLF_CONFIG_DIR}/config.h