diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml
index 3193d2aae..397766cbd 100644
--- a/.github/workflows/build.yaml
+++ b/.github/workflows/build.yaml
@@ -8,12 +8,12 @@ on:
   pull_request:
   workflow_dispatch:
 jobs:
-  build:
+  test:
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v2
       - name: Install dependencies
-        run: sudo apt install libeigen3-dev libgtest-dev libginac-dev
+        run: sudo apt install libeigen3-dev libgtest-dev
       - name: Install Python dependencies
         run: |
           python -m pip install --upgrade pip
@@ -24,8 +24,26 @@ jobs:
         working-directory: ${{github.workspace}}
         run: |
           JOBS=$(($(grep cpu.cores /proc/cpuinfo | sort -u | sed 's/[^0-9]//g') + 1))
-          cmake -S . -B build -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON -DBUILD_BENCHMARKS=ON
+          cmake -S . -B build -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=ON -DBUILD_BENCHMARKS=OFF
           cmake --build build -j${JOBS}
           cd build
           ctest
-      
+
+  example:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+      - name: Install dependencies
+        run: sudo apt install libeigen3-dev libgtest-dev
+      - name: Install Python dependencies
+        run: |
+          python -m pip install --upgrade pip
+          pip install pipenv
+          pip install numpy
+          pip install matplotlib
+      - name: Build & Test
+        working-directory: ${{github.workspace}}
+        run: |
+          JOBS=$(($(grep cpu.cores /proc/cpuinfo | sort -u | sed 's/[^0-9]//g') + 1))
+          cmake -S . -B build -DBUILD_EXAMPLES=ON -DBUILD_TESTS=OFF -DBUILD_BENCHMARKS=OFF
+          cmake --build build -j${JOBS}
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4c9ee4de5..3bedec2ae 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -79,11 +79,11 @@ if(BUILD_TESTS)
 find_package(GTest REQUIRED)
 enable_testing()
 set(test_files
-  common/lib_test
+  # common/lib_test
   utility/singleton_test
+  utility/random_test
 
   algorithm/poly_regression_test
-  algorithm/random_test
   algorithm/k_means_test
   algorithm/auto_diff_test
   algorithm/icp_test
@@ -129,14 +129,17 @@ if(BUILD_BENCHMARKS)
 
 find_package(Python3 COMPONENTS Development NumPy)
 
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftime-report")
+
 set(bench_test_files
+  build_time
   autodiff
   eigen_arg_ret
 )
 
 foreach(bench_test_file ${bench_test_files})
-get_filename_component(bench_test_file benckmark/${bench_test_file}.cpp NAME_WE)
-add_executable(bench_${bench_test_file} benckmark/${bench_test_file}.cpp)
+get_filename_component(bench_test_file benchmark/${bench_test_file}.cpp NAME_WE)
+add_executable(bench_${bench_test_file} benchmark/${bench_test_file}.cpp)
 target_include_directories(bench_${bench_test_file} PRIVATE include)
 target_link_libraries(bench_${bench_test_file} PRIVATE Eigen3::Eigen Python3::Python Python3::NumPy)
 endforeach()
diff --git a/benckmark/autodiff.cpp b/benchmark/autodiff.cpp
similarity index 100%
rename from benckmark/autodiff.cpp
rename to benchmark/autodiff.cpp
diff --git a/benchmark/build_time.cpp b/benchmark/build_time.cpp
new file mode 100644
index 000000000..450737e12
--- /dev/null
+++ b/benchmark/build_time.cpp
@@ -0,0 +1,15 @@
+// utility core
+#include "cpp_robotics/utility.hpp" // 1.15
+#include "cpp_robotics/vector.hpp" // 0.26
+#include "cpp_robotics/unit.hpp" // 0.29
+
+#include "cpp_robotics/system.hpp" // 2.4
+#include "cpp_robotics/controller.hpp" // 3.0
+#include "cpp_robotics/filter.hpp" // 2.0
+
+#include "cpp_robotics/chassis.hpp" // 1.9
+#include "cpp_robotics/motor.hpp" // 2.0
+
+int main() {
+  return 0;
+}
\ No newline at end of file
diff --git a/benckmark/eigen_arg_ret.cpp b/benchmark/eigen_arg_ret.cpp
similarity index 100%
rename from benckmark/eigen_arg_ret.cpp
rename to benchmark/eigen_arg_ret.cpp
diff --git a/example/algorithm/icp_sample.cpp b/example/algorithm/icp_sample.cpp
index 5d85359a0..564b91e12 100644
--- a/example/algorithm/icp_sample.cpp
+++ b/example/algorithm/icp_sample.cpp
@@ -1,9 +1,9 @@
 #define CR_USE_MATPLOTLIB
 #include <iostream>
 #include <iomanip>
-#include <cpp_robotics/algorithm.hpp>
+#include <cpp_robotics/matplotlibcpp.hpp>
+#include <cpp_robotics/algorithm/icp.hpp>
 #include <cpp_robotics/vector/vector2.hpp>
-#include <cpp_robotics/third_party/matplotlib-cpp/matplotlibcpp.h>
 #include <Eigen/Geometry> 
 
 int main()
diff --git a/example/algorithm/random_example.cpp b/example/algorithm/random_example.cpp
index da63070a7..2f92705a2 100644
--- a/example/algorithm/random_example.cpp
+++ b/example/algorithm/random_example.cpp
@@ -1,5 +1,5 @@
 #include <iostream>
-#include <cpp_robotics/algorithm.hpp>
+#include <cpp_robotics/utility.hpp>
 
 double random_std()
 {
diff --git a/example/controller/canonicalize.cpp b/example/controller/canonicalize.cpp
index 609fb524c..ad55808ea 100644
--- a/example/controller/canonicalize.cpp
+++ b/example/controller/canonicalize.cpp
@@ -2,6 +2,7 @@
 #include <iomanip>
 #include <cpp_robotics/system.hpp>
 #include <cpp_robotics/controller.hpp>
+#include <cpp_robotics/controller/modern_control.hpp>
 
 #define PRINT_MAT(MAT) \
     std::cout << #MAT << std::endl;\
diff --git a/example/controller/linear_mpc.cpp b/example/controller/linear_mpc.cpp
index f4e0bd00a..857f6e341 100644
--- a/example/controller/linear_mpc.cpp
+++ b/example/controller/linear_mpc.cpp
@@ -1,10 +1,9 @@
 #define CR_USE_MATPLOTLIB
 #include <iostream>
-
-#include <cpp_robotics/core.hpp>
+#include <cpp_robotics/utility.hpp>
 #include <cpp_robotics/controller/linear_mpc.hpp>
 #include <cpp_robotics/system/state_space_system.hpp>
-#include <limits>
+#include <cpp_robotics/matplotlibcpp.hpp>
 
 
 int main()
diff --git a/example/controller/lqr.cpp b/example/controller/lqr.cpp
index 26fbdeec2..3fd3079cf 100644
--- a/example/controller/lqr.cpp
+++ b/example/controller/lqr.cpp
@@ -1,7 +1,8 @@
 #define CR_USE_MATPLOTLIB
 #include <iostream>
-#include <cpp_robotics/core.hpp>
-
+#include <cpp_robotics/system.hpp>
+#include <cpp_robotics/controller/lqr.hpp>
+#include <cpp_robotics/matplotlibcpp.hpp>
 
 int main()
 {
diff --git a/example/controller/motor_pid_control.cpp b/example/controller/motor_pid_control.cpp
index 54545a3e7..b1416180d 100644
--- a/example/controller/motor_pid_control.cpp
+++ b/example/controller/motor_pid_control.cpp
@@ -1,8 +1,8 @@
 #define CR_USE_MATPLOTLIB
 #include <iostream>
-#include <cpp_robotics/core.hpp>
-#include <cpp_robotics/motor/dc_motor_list.hpp>
-#include <cpp_robotics/motor/motor_tf.hpp>
+#include <cpp_robotics/utility.hpp>
+#include <cpp_robotics/system.hpp>
+#include <cpp_robotics/motor.hpp>
 
 static std::tuple<std::vector<double>, std::vector<double>> ref_trajectory(double time, double dt)
 {
diff --git a/example/controller/nctf_example.cpp b/example/controller/nctf_example.cpp
index 654215093..1c855e78b 100644
--- a/example/controller/nctf_example.cpp
+++ b/example/controller/nctf_example.cpp
@@ -1,8 +1,9 @@
 #define CR_USE_MATPLOTLIB
 #include <iostream>
-#include <cpp_robotics/core.hpp>
-#include <cpp_robotics/motor/dc_motor_list.hpp>
-#include <cpp_robotics/motor/motor_tf.hpp>
+#include <cpp_robotics/utility.hpp>
+#include <cpp_robotics/motor.hpp>
+#include <cpp_robotics/controller.hpp>
+#include <cpp_robotics/system.hpp>
 
 static std::tuple<std::vector<double>, std::vector<double>> ref_trajectory(double time, double dt)
 {
diff --git a/example/controller/pid_vs_nctf.cpp b/example/controller/pid_vs_nctf.cpp
index bfa7262f8..e1a3af556 100644
--- a/example/controller/pid_vs_nctf.cpp
+++ b/example/controller/pid_vs_nctf.cpp
@@ -1,8 +1,9 @@
 #define CR_USE_MATPLOTLIB
 #include <iostream>
-#include <cpp_robotics/core.hpp>
-#include <cpp_robotics/motor/dc_motor_list.hpp>
-#include <cpp_robotics/motor/motor_tf.hpp>
+#include <cpp_robotics/utility.hpp>
+#include <cpp_robotics/system.hpp>
+#include <cpp_robotics/controller.hpp>
+#include <cpp_robotics/motor.hpp>
 
 static std::tuple<std::vector<double>, std::vector<double>> ref_trajectory(double time, double dt)
 {
diff --git a/example/controller/place.cpp b/example/controller/place.cpp
index 6892e8d1d..3e6f87b00 100644
--- a/example/controller/place.cpp
+++ b/example/controller/place.cpp
@@ -3,6 +3,7 @@
 #include <iomanip>
 #include <cpp_robotics/system.hpp>
 #include <cpp_robotics/controller.hpp>
+#include <cpp_robotics/controller/modern_control.hpp>
 
 #define PRINT_MAT(MAT) \
     std::cout << #MAT << std::endl;\
diff --git a/example/controller/vrft_pid.cpp b/example/controller/vrft_pid.cpp
index 8fb7d32b7..ea9413458 100644
--- a/example/controller/vrft_pid.cpp
+++ b/example/controller/vrft_pid.cpp
@@ -1,10 +1,11 @@
 #define CR_USE_MATPLOTLIB
 #include <cpp_robotics/system.hpp>
 #include <cpp_robotics/controller.hpp>
-#include <cpp_robotics/motor/dc_motor_list.hpp>
-#include <cpp_robotics/motor/motor_tf.hpp>
+#include <cpp_robotics/controller/vrft.hpp>
+#include <cpp_robotics/optimize/derivative.hpp>
+#include <cpp_robotics/optimize/quasi_newton_method.hpp>
+#include <cpp_robotics/motor.hpp>
 #include <cpp_robotics/filter.hpp>
-#include <cpp_robotics/optimize.hpp>
 #include <string>
 #include <iomanip>
 
@@ -173,370 +174,9 @@ void vrft_pid_with_closed_loop()
     plt::show();
 }
 
-using namespace cpp_robotics;
-namespace plt = matplotlibcpp;
-Eigen::VectorXd vrft_nctf(const Eigen::VectorXd &u, const Eigen::VectorXd &y, const double Ts, std::function<double(double)> nct, PID::pid_param_t base_param, TransferFunction ref_model, std::optional<TransferFunction> prefilter, double Td = 0.01)
-{
-    // PID::pid_param_t p_param =
-    // {
-    //     .Ts = base_param.Ts,
-    //     .gpd = base_param.gpd,
-    //     .Kp = 1,
-    //     .Ki = 0,
-    //     .Kd = 0,
-    //     .output_limit = std::nullopt
-    // };
-    // PID::pid_param_t i_param =
-    // {
-    //     .Ts = base_param.Ts,
-    //     .gpd = base_param.gpd,
-    //     .Kp = 0,
-    //     .Ki = 1,
-    //     .Kd = 0,
-    //     .output_limit = std::nullopt
-    // };
-    // std::vector<NctfController> control_base = {
-    //     NctfController(nct, p_param, Ts),
-    //     NctfController(nct, i_param, Ts)
-    // };
-
-    // (void) Ts;
-
-    // Eigen::VectorXd uf, ef;
-    // if(prefilter)
-    // {
-    //     uf = lsim_y(prefilter.value(), u);
-    //     ef = lsim_y(prefilter.value()*(ref_model.inv()-1.0), y);
-    // }
-    // else
-    // {
-    //     uf = u;
-    //     ef = lsim_y((ref_model.inv()-1.0), y);;
-    // }
-
-    // Eigen::MatrixXd phi(ef.size(), control_base.size());
-    // for(size_t i = 0; i < control_base.size(); ++i)
-    // {
-    //     phi.col(i) = lsim_y([&](double e){ return control_base[i].control(e); }, Ts, ef);
-    // }
-
-    // plt::figure();
-    // eigen_named_plot("ef", ef);
-    // eigen_named_plot("y", y);
-    // eigen_named_plot("p0", phi.col(0));
-    // eigen_named_plot("p1", phi.col(1));
-    // plt::legend();
-    // plt::show(false);
-    // return leastsq(phi, uf);
-
-
-    std::vector<TransferFunction> control_base = {
-        TransferFunction({1.0}, {1.0}, Ts),
-        TransferFunction({1.0}, {1.0, 0.0}, Ts)
-    };
-
-    Differentiator vel_diff(base_param.gpd, Ts);
-    Eigen::VectorXd vel = lsim_y(vel_diff, y);
-
-    plt::figure();
-    eigen_named_plot("Vel", vel);
-    
-
-    Eigen::VectorXd uf, ef;
-    // prefilter.reset();
-    if(prefilter)
-    {
-        uf = lsim_y(prefilter.value(), u);
-        ef = lsim_y(prefilter.value()*(ref_model.inv()-1.0), vel);
-    }
-    else
-    {
-        uf = u;
-        ef = lsim_y((ref_model.inv()-1.0), vel);
-    }
-    ef = y[y.size()-1]*Eigen::VectorXd::Ones(ef.size()) - y;
-
-    // ud = -NCT(u)+ u*sを求める
-    Differentiator diff(base_param.gpd, Ts);
-    diff.reset_test(0, ef[0]);
-    Eigen::VectorXd ed = lsim_y(diff,ef,true);
-    for(int i = 0; i < y.size(); i++)
-    {
-        ed[i] -= nct(ef[i]);
-    }
-
-    eigen_named_plot("ef", ef);
-    eigen_named_plot("ed", ed);
-    eigen_named_plot("y", y);
-    plt::legend();
-    plt::show(false);
-
-    Eigen::MatrixXd phi(ef.size(), control_base.size());
-    for(size_t i = 0; i < control_base.size(); ++i)
-    {
-        phi.col(i) = lsim_y(control_base[i], ed);
-    }
-
-    // ActiveSetMethod asmethod;
-    // ActiveSetMethod::Problem p;
-    // std::tie(p.Q, p.c) = lsi2qp(phi, -uf);
-    // // p.A = -Eigen::Matrix2d::Identity();
-    // // p.b = Eigen::Vector2d::Zero();
-    // // p.constraint_size = 2;
-    // auto result = asmethod.solve(p, Eigen::Vector2d::Ones());
-    // return result.x_opt;
-
-    return leastsq(phi, uf);
-
-    // return vrft_pi(ed, vel, Ts, ref_model, ref_model);
-}
-
-// まずは評価関数に落とし込むところから考える
-Eigen::VectorXd vrft_nctf2(const Eigen::VectorXd &u, const Eigen::VectorXd &y, const double Ts, std::function<double(double)> nct, PID::pid_param_t base_param, TransferFunction ref_model, std::optional<TransferFunction> prefilter)
-{
-    namespace cr = cpp_robotics;
-    std::cout << "vrft_nctf2" << std::endl;
-    // PID::pid_param_t p_param =
-    // {
-    //     .Ts = base_param.Ts,
-    //     .gpd = base_param.gpd,
-    //     .Kp = 1,
-    //     .Ki = 0,
-    //     .Kd = 0,
-    //     .output_limit = std::nullopt
-    // };
-    // PID::pid_param_t i_param =
-    // {
-    //     .Ts = base_param.Ts,
-    //     .gpd = base_param.gpd,
-    //     .Kp = 0,
-    //     .Ki = 1,
-    //     .Kd = 0,
-    //     .output_limit = std::nullopt
-    // };
-    // std::vector<NctfController> control_base = {
-    //     NctfController(nct, p_param, Ts),
-    //     NctfController(nct, i_param, Ts)
-    // };
-
-    // std::vector<TransferFunction> control_base = {
-    //     TransferFunction({1.0}, {1.0}, Ts),
-    //     TransferFunction({1.0}, {1.0, 0.0}, Ts)
-    // };
-
-    Differentiator vel_diff(base_param.gpd, Ts);
-    Eigen::VectorXd vel = lsim_y(vel_diff, y);
-
-    Eigen::VectorXd uf, ef;
-    // prefilter.reset();
-    Eigen::VectorXd vref;
-
-    prefilter = cr::TransferFunction({1.0}, {0.001, 1.0}, base_param.Ts);
-    if(prefilter)
-    {
-        uf = lsim_y(prefilter.value(), u);
-        ef = lsim_y(prefilter.value()*(ref_model.inv()-1.0), y);
-        vref = lsim_y(prefilter.value()*(ref_model.inv()), y);
-    }
-    else
-    {
-        uf = u;
-        ef = lsim_y((ref_model.inv()), y);
-    }
-
-    plt::figure();
-    eigen_named_plot("ef", ef);
-    eigen_named_plot("vref", vref);
-    // eigen_named_plot("ed", ed);
-    // eigen_named_plot("vel", vel);
-    eigen_named_plot("y", y);
-    plt::legend();
-    plt::show(false);
-
-    // 最小二乗法で解こうとしてるのが間違い？
-    // Eigen::MatrixXd phi(vf.size(), control_base.size());
-    // for(size_t i = 0; i < control_base.size(); ++i)
-    // {
-    //     phi.col(i) = lsim_y([&](double e){ return control_base[i].control(e); }, base_param.Ts, vf);
-    // }
-
-    // return leastsq(phi, uf);
-
-    auto cost_f = [&](Eigen::VectorXd K)
-    {
-        PID::pid_param_t nctf_param =
-        {
-            .Ts = base_param.Ts,
-            .gpd = base_param.gpd,
-            .Kp = K[0],
-            .Ki = 3,
-            .Kd = 0,
-            .output_limit = base_param.output_limit
-        };
-        NctfController nctf(nct, nctf_param, Ts);
-        Eigen::VectorXd uc = lsim_y([&](double e){ return nctf.control(e); }, base_param.Ts, ef);
-        std::cout << "uc = " << uc.squaredNorm() << std::endl;
-        return (uf-uc).squaredNorm();
-    };
-    auto cost_grad = [&](const Eigen::VectorXd &K)
-    {
-        return cr::derivative(cost_f, K);
-    };
-    auto [ret, rx, rcnt] = cr::quasi_newton_method(cost_f, cost_grad, Eigen::VectorXd::Zero(1));
-    if(ret)
-    {
-        std::cout << "quasi_newton_method : sucsess" << std::endl;
-        return rx;
-    }
-    else
-    {
-        std::cout << "quasi_newton_method : failed" << std::endl;
-        return Eigen::VectorXd::Zero(2);
-    }
-}
-
-void check_nct(Eigen::VectorXd y, double Ts, std::function<double(double)> nct)
-{
-
-    namespace plt = matplotlibcpp;
-    Differentiator diff(600, Ts);
-    diff.reset_test(0,y[0]);
-    Eigen::VectorXd vel = lsim_y(diff, y,true);
-
-    // Eigen::VectorXd y2(y.size()*2), vel2(vel.size()*2);
-    // y2 << Eigen::VectorXd(-y.array().reverse()), y;
-    // vel2 << Eigen::VectorXd(vel.array().reverse()), -vel;
-
-    Eigen::VectorXd nct_out(y.size());
-    for(int i = 0; i < y.size(); i++)
-    {
-        nct_out[i] = nct(y[i]);
-    }
-
-    plt::figure();
-    eigen_named_plot("sys", y, (vel).eval());
-    eigen_named_plot("NCT", y, nct_out);
-    plt::xlim(-1.5, 1.5);
-    plt::ylim(-20, 5);
-    plt::legend();
-    plt::show(false);
-}
-
-void vrft_nctf_with_open_loop()
-{
-    namespace cr = cpp_robotics;
-    namespace plt = matplotlibcpp;
-    
-    const double Ts = 0.001; // サンプリング周期
-    const double sim_time = 2.0;
-    const double exp_sim_time = 0.5;
-    // 制御対象
-    const double omega = 50.0;
-    const double zeta = 1.0;
-    const double max_input = 50;
-    cr::TransferFunction tf = cr::TransferFunction::make_second_order_system(omega, zeta, Ts) * cr::TransferFunction({0.3}, {1,0}, Ts); 
-
-    // 4秒分のステップ応答を計算
-    Eigen::VectorXd u = max_input * cr::stepspace<Eigen::VectorXd>(exp_sim_time, Ts);
-    for(int i = 2.0*u.size()/4; i < u.size(); ++i)
-    {
-        u[i] = 0;
-    }
-    for(int i = u.size()/2; i < u.size(); ++i)
-    {
-        const int size = u.size()/2;
-        // u[i] *= (double)i/(u.size()/2);
-        // u[i] = -max_input + std::sin((i-u.size()/2) *0.5*M_PI/size)*max_input;
-    }
-    auto [t1, y] = cr::lsim(tf, u);
-
-    const double nct_max_velocity = max_input*0.3;
-    const double nct_slope = 0.6/0.015;
-    check_nct(Eigen::VectorXd::Constant(y.size(), y[y.size()-1]) - y, Ts, NctfController::make_simple_nct(nct_max_velocity, nct_slope));
-
-    // 規範モデル
-    cr::TransferFunction ref_model = cr::TransferFunction({1.0}, {0.01, 1.0}, Ts);
-
-    // VRFTでPIDゲインを求める
-    cr::PID::pid_param_t nctf_pid_param = 
-    {
-        .Ts = Ts,
-        .gpd = 100*2*M_PI,
-        .Kp = 0,
-        .Ki = 0,
-        .Kd = 0,
-        .output_limit = std::pair(-max_input, max_input)
-    };
-
-    auto K = vrft_nctf2(u, y, Ts, NctfController::make_simple_nct(nct_max_velocity, nct_slope), nctf_pid_param, ref_model, ref_model);
-    // auto K = cr::vrft_pi(u,y,Ts,ref_model,ref_model);
-    // Eigen::Vector2d K(5.0, 0.0);
-    std::cout << "PI gain =" << std::endl;
-    std::cout << K << std::endl;
-    nctf_pid_param.Kp = K[0];
-    nctf_pid_param.Ki = 0.0;
-    // nctf_pid_param.Kd = K[2];
-
-
-    cr::NctfController nctf(nct_max_velocity, nct_slope, nctf_pid_param, Ts);
-    // cr::PID pid(nctf_pid_param);
-    
-    // cr::PID pid(pid_param);
-    // // PIDコントローラとtfからなるフィードバック制御系
-    cr::SisoFeedbackSystem siso_sys = cr::make_feedback_system(nctf, tf); 
-
-    // VRFTでチューニングしたPIDゲインを用いて閉ループのステップ応答をシミュレーション
-    const double target_pos = 5.0;
-    Eigen::VectorXd test_y_ref = Eigen::VectorXd(target_pos * cr::stepspace<Eigen::VectorXd>(sim_time, Ts));
-    size_t i = 0;
-    for(; i < 100; i++)
-    {
-        test_y_ref[i] *= 0.02;
-    }
-    for(; i < 300; i++)
-    {
-        test_y_ref[i] *= 0.1;
-    }
-    for(; i < 700; i++)
-    {
-        test_y_ref[i] *= 0.5;
-    }
-    auto [t2, reu, responce] = cr::lsim(siso_sys, test_y_ref);
-
-    check_nct(Eigen::VectorXd::Constant(responce.size(), responce[responce.size()-1])-responce, Ts, NctfController::make_simple_nct(nct_max_velocity, nct_slope));
-
-    // 開ループのステップ応答のグラフ表示
-    plt::figure_size(1020, 500);
-    plt::subplot(1, 2, 1);
-    eigen_named_plot("Input", t1, u);
-    eigen_named_plot("Responce", t1, y);
-    plt::xlabel("Time [s]");
-    plt::title("Open loop responce");
-    // plt::ylim(-0.1, 1.5);
-    plt::legend();
-    plt::grid(true);
-
-    // チューニング後の結果のグラフ表示
-    plt::subplot(1, 2, 2);
-    eigen_named_plot("Target", t2, test_y_ref);
-    eigen_named_plot("PID feedback responce", t2, responce);
-    plt::xlabel("Time [s]");
-    std::stringstream ss;
-    ss << std::fixed << std::setprecision(3);
-    // ss << "(Kp = " << K[0] << ", Ki = " << K[1] << ")";
-    plt::title("Closed loop responce\n" + ss.str());
-    // plt::ylim(-0.1, 1.5);
-    plt::legend();
-    plt::grid(true);
-    plt::tight_layout();
-    plt::show();
-    
-}
-
 int main()
 {
-    // vrft_pid_with_open_loop();
-    // vrft_pid_with_closed_loop();
-    vrft_nctf_with_open_loop();
+    vrft_pid_with_open_loop();
+    vrft_pid_with_closed_loop();
     return 0;
 }
\ No newline at end of file
diff --git a/example/hello_world.cpp b/example/hello_world.cpp
index 1a6d7b5b2..0d240bb4b 100644
--- a/example/hello_world.cpp
+++ b/example/hello_world.cpp
@@ -1,7 +1,10 @@
 #define CR_USE_MATPLOTLIB
 #include <iostream>
 #include <Eigen/Dense>
-#include <cpp_robotics/cpp_robotics.hpp>
+#include <cpp_robotics/vector.hpp>
+#include <cpp_robotics/controller/lqr.hpp>
+#include <cpp_robotics/system.hpp>
+#include <cpp_robotics/matplotlibcpp.hpp>
 
 int main()
 {
diff --git a/example/optimize/active_set_method_example.cpp b/example/optimize/active_set_method_example.cpp
index 45ff34ee0..acb847e10 100644
--- a/example/optimize/active_set_method_example.cpp
+++ b/example/optimize/active_set_method_example.cpp
@@ -1,5 +1,4 @@
 #include <iostream>
-#include <cpp_robotics/algorithm.hpp>
 #include <cpp_robotics/optimize/active_set_method.hpp>
 
 int main()
diff --git a/example/optimize/optimize_rosenbrock.cpp b/example/optimize/optimize_rosenbrock.cpp
index c320816e4..9448589df 100644
--- a/example/optimize/optimize_rosenbrock.cpp
+++ b/example/optimize/optimize_rosenbrock.cpp
@@ -1,6 +1,6 @@
 #include <iostream>
 #include <Eigen/Dense>
-#include <cpp_robotics/optimize.hpp>
+#include <cpp_robotics/optimize/no_constraint_method.hpp>
 
 int main()
 {
diff --git a/example/optimize/optimize_scalar.cpp b/example/optimize/optimize_scalar.cpp
index 3c025bdc0..1c2aa4229 100644
--- a/example/optimize/optimize_scalar.cpp
+++ b/example/optimize/optimize_scalar.cpp
@@ -1,6 +1,6 @@
 #include <iostream>
 #include <Eigen/Dense>
-#include <cpp_robotics/optimize.hpp>
+#include <cpp_robotics/optimize/no_constraint_method.hpp>
 
 int main()
 {
diff --git a/example/optimize/optimize_with_constraint.cpp b/example/optimize/optimize_with_constraint.cpp
index 6585ac0fe..9c202be3f 100644
--- a/example/optimize/optimize_with_constraint.cpp
+++ b/example/optimize/optimize_with_constraint.cpp
@@ -1,6 +1,7 @@
 #include <iostream>
 #include <Eigen/Dense>
-#include <cpp_robotics/optimize.hpp>
+#include <cpp_robotics/optimize/barrier_method.hpp>
+#include <cpp_robotics/optimize/penalty_method.hpp>
 
 int main()
 {
diff --git a/example/optimize/optimize_without_constraint.cpp b/example/optimize/optimize_without_constraint.cpp
index 5e3d0ab36..b6ad0556a 100644
--- a/example/optimize/optimize_without_constraint.cpp
+++ b/example/optimize/optimize_without_constraint.cpp
@@ -1,6 +1,6 @@
 #include <iostream>
 #include <Eigen/Dense>
-#include <cpp_robotics/optimize.hpp>
+#include <cpp_robotics/optimize/no_constraint_method.hpp>
 
 int main()
 {
diff --git a/example/optimize/sqp_example.cpp b/example/optimize/sqp_example.cpp
index 811fd6ad8..872c6c2fc 100644
--- a/example/optimize/sqp_example.cpp
+++ b/example/optimize/sqp_example.cpp
@@ -1,6 +1,5 @@
 #define CR_USE_MATPLOTLIB
 #include <iostream>
-#include <cpp_robotics/algorithm.hpp>
 #include <cpp_robotics/optimize/sqp.hpp>
 #include "cpp_robotics/third_party/matplotlib-cpp/matplotlibcpp.h"
 
diff --git a/example/path_planning/dubins_path.cpp b/example/path_planning/dubins_path.cpp
index 4456284b9..d9d340d6a 100644
--- a/example/path_planning/dubins_path.cpp
+++ b/example/path_planning/dubins_path.cpp
@@ -1,6 +1,7 @@
 #define CR_USE_MATPLOTLIB
 #include <iostream>
-#include "cpp_robotics/cpp_robotics.hpp"
+#include <cpp_robotics/matplotlibcpp.hpp>
+#include "cpp_robotics/path_planning/dubins_path.hpp"
 
 int main()
 {
diff --git a/example/system/state_space_system.cpp b/example/system/state_space_system.cpp
index a899a0573..0a9784422 100644
--- a/example/system/state_space_system.cpp
+++ b/example/system/state_space_system.cpp
@@ -1,5 +1,7 @@
+#include <iostream>
 #include <cpp_robotics/system.hpp>
 #include <cpp_robotics/controller.hpp>
+#include <cpp_robotics/controller/modern_control.hpp>
 
 int main()
 {
diff --git a/include/cpp_robotics/algorithm.hpp b/include/cpp_robotics/algorithm.hpp
deleted file mode 100644
index f31fef756..000000000
--- a/include/cpp_robotics/algorithm.hpp
+++ /dev/null
@@ -1,10 +0,0 @@
-#pragma once
-
-#include "algorithm/kdtree.hpp"
-#include "algorithm/icp.hpp"
-#include "algorithm/ndt.hpp"
-#include "algorithm/poly_regression.hpp"
-#include "algorithm/state_machine.hpp"
-#include "algorithm/k_means_method.hpp"
-#include "algorithm/mahalanobis.hpp"
-#include "algorithm/random.hpp"
\ No newline at end of file
diff --git a/include/cpp_robotics/algorithm/poly_regression.hpp b/include/cpp_robotics/algorithm/poly_regression.hpp
index 5f49f92d5..20d28310e 100644
--- a/include/cpp_robotics/algorithm/poly_regression.hpp
+++ b/include/cpp_robotics/algorithm/poly_regression.hpp
@@ -1,7 +1,5 @@
 #pragma once
 
-#include <array>
-#include <vector>
 #include <Eigen/Dense>
 #include "cpp_robotics/system/polynomial.hpp"
 
diff --git a/include/cpp_robotics/arm_ik.hpp b/include/cpp_robotics/arm_ik.hpp
deleted file mode 100644
index 7b9637ef9..000000000
--- a/include/cpp_robotics/arm_ik.hpp
+++ /dev/null
@@ -1 +0,0 @@
-#pragma once
\ No newline at end of file
diff --git a/include/cpp_robotics/controller.hpp b/include/cpp_robotics/controller.hpp
index c8e160eed..88f5bc9a4 100644
--- a/include/cpp_robotics/controller.hpp
+++ b/include/cpp_robotics/controller.hpp
@@ -2,9 +2,19 @@
 
 #include "controller/pid.hpp"
 #include "controller/pid2.hpp"
-#include "controller/lqr.hpp"
-#include "controller/pure_pursuit.hpp"
+#include "controller/psmc.hpp"
 #include "controller/nctf.hpp"
-#include "controller/modern_control.hpp"
-#include "controller/pfc.hpp"
-#include "controller/vrft.hpp"
\ No newline at end of file
+
+// #include "controller/pfc.hpp"
+// #include "controller/pure_pursuit.hpp"
+
+// // ここから時間かかる
+// #include "controller/lqr.hpp" // + 7.0s
+// #include "controller/modern_control.hpp" // +1.7s
+// #include "controller/vrft.hpp" // +4.0s
+// #include "controller/linear_mpc.hpp" // +2.6s
+// #include "controller/linear_regulator_mpc.hpp" // +2.6s
+// #include "controller/ocp_dynamics.hpp"
+// #include "controller/ocp_cost.hpp"
+// #include "controller/ocp_constraint.hpp"
+// #include "controller/optimal_control_problem.hpp"
diff --git a/include/cpp_robotics/core.hpp b/include/cpp_robotics/core.hpp
deleted file mode 100644
index 5f726ea04..000000000
--- a/include/cpp_robotics/core.hpp
+++ /dev/null
@@ -1,11 +0,0 @@
-#pragma once
-
-// utility core
-#include "utility.hpp"
-#include "vector.hpp"
-#include "unit.hpp"
-
-// control core
-#include "controller.hpp"
-#include "filter.hpp"
-#include "system.hpp"
\ No newline at end of file
diff --git a/include/cpp_robotics/cpp_robotics.hpp b/include/cpp_robotics/cpp_robotics.hpp
deleted file mode 100644
index 53172c9cd..000000000
--- a/include/cpp_robotics/cpp_robotics.hpp
+++ /dev/null
@@ -1,14 +0,0 @@
-#pragma once
-
-#include "core.hpp"
-
-#include "path_planning.hpp"
-#include "filter.hpp"
-#include "chassis.hpp"
-#include "algorithm.hpp"
-#include "motor.hpp"
-#include "optimize.hpp"
-
-#ifdef CR_USE_MATPLOTLIB
-#include "third_party/matplotlib-cpp/matplotlibcpp.h"
-#endif
\ No newline at end of file
diff --git a/include/cpp_robotics/filter.hpp b/include/cpp_robotics/filter.hpp
index 1880f084b..1b147e959 100644
--- a/include/cpp_robotics/filter.hpp
+++ b/include/cpp_robotics/filter.hpp
@@ -11,5 +11,7 @@
 #include "filter/velocity_limit_filter.hpp"
 #include "filter/acceleration_limit_filter.hpp"
 #include "filter/filter_state_holder.hpp"
-#include "filter/kalman_filter.hpp"
-#include "filter/extended_kalman_filter.hpp"
+// #include "filter/kalman_filter.hpp"
+
+// // 時間かかる
+// #include "filter/extended_kalman_filter.hpp" // +2.3s
diff --git a/include/cpp_robotics/matplotlibcpp.hpp b/include/cpp_robotics/matplotlibcpp.hpp
new file mode 100644
index 000000000..f2a6ee977
--- /dev/null
+++ b/include/cpp_robotics/matplotlibcpp.hpp
@@ -0,0 +1 @@
+#include <cpp_robotics/third_party/matplotlib-cpp/matplotlibcpp.h>
\ No newline at end of file
diff --git a/include/cpp_robotics/optimize.hpp b/include/cpp_robotics/optimize.hpp
deleted file mode 100644
index 21a92d555..000000000
--- a/include/cpp_robotics/optimize.hpp
+++ /dev/null
@@ -1,14 +0,0 @@
-#include "optimize/constraint.hpp"
-#include "optimize/derivative.hpp"
-#include "optimize/bracketing_serach.hpp"
-#include "optimize/golden_serach.hpp"
-#include "optimize/steepest_descent_method.hpp"
-#include "optimize/newton_method.hpp"
-#include "optimize/quasi_newton_method.hpp"
-#include "optimize/penalty_method.hpp"
-#include "optimize/barrier_method.hpp"
-#include "optimize/linprog.hpp"
-#include "optimize/quadprog.hpp"
-#include "optimize/sqp.hpp"
-#include "optimize/interior_point_method.hpp"
-#include "optimize/active_set_method.hpp"
\ No newline at end of file
diff --git a/include/cpp_robotics/optimize/no_constraint_method.hpp b/include/cpp_robotics/optimize/no_constraint_method.hpp
new file mode 100644
index 000000000..0a2aebaa9
--- /dev/null
+++ b/include/cpp_robotics/optimize/no_constraint_method.hpp
@@ -0,0 +1,5 @@
+#include <cpp_robotics/optimize/golden_serach.hpp>
+#include <cpp_robotics/optimize/derivative.hpp>
+#include <cpp_robotics/optimize/steepest_descent_method.hpp>
+#include <cpp_robotics/optimize/newton_method.hpp>
+#include <cpp_robotics/optimize/quasi_newton_method.hpp>
\ No newline at end of file
diff --git a/include/cpp_robotics/path_planning.hpp b/include/cpp_robotics/path_planning.hpp
deleted file mode 100644
index dd128c121..000000000
--- a/include/cpp_robotics/path_planning.hpp
+++ /dev/null
@@ -1,7 +0,0 @@
-#include "path_planning/grid_path_planning_utils.hpp"
-#include "path_planning/a_star.hpp"
-#include "path_planning/wave_propagation.hpp"
-#include "path_planning/dubins_path.hpp"
-#include "path_planning/frenet_frame.hpp"
-#include "path_planning/dwa.hpp"
-#include "path_planning/spline.hpp"
\ No newline at end of file
diff --git a/include/cpp_robotics/path_planning/dubins_path.hpp b/include/cpp_robotics/path_planning/dubins_path.hpp
index 3c839f922..24272d371 100644
--- a/include/cpp_robotics/path_planning/dubins_path.hpp
+++ b/include/cpp_robotics/path_planning/dubins_path.hpp
@@ -1,8 +1,9 @@
 #pragma once
 
-#include "../vector/transform.hpp"
+#include "cpp_robotics/utility.hpp"
+#include "cpp_robotics/vector/transform.hpp"
+#include <optional>
 #include <functional>
-#include "../utility/math_utils.hpp"
 
 namespace cpp_robotics
 {
diff --git a/include/cpp_robotics/system.hpp b/include/cpp_robotics/system.hpp
index 387f26cc7..360a4646b 100644
--- a/include/cpp_robotics/system.hpp
+++ b/include/cpp_robotics/system.hpp
@@ -7,4 +7,6 @@
 #include "system/bode.hpp"
 #include "system/nyquist.hpp"
 #include "system/time_responce.hpp"
-#include "system/siso_system.hpp"
\ No newline at end of file
+#include "system/siso_system.hpp"
+// #include "system/dynamics_integrate.hpp"
+// #include "system/system_transformation.hpp" // +4.0s
\ No newline at end of file
diff --git a/include/cpp_robotics/system/polynomial.hpp b/include/cpp_robotics/system/polynomial.hpp
index d950c2603..ec9ca9eb9 100644
--- a/include/cpp_robotics/system/polynomial.hpp
+++ b/include/cpp_robotics/system/polynomial.hpp
@@ -1,9 +1,8 @@
 #pragma once
 
 #include <vector>
-// #include <initializer_list>
 #include <cassert>
-#include "../utility/cpp_support.hpp"
+#include <cpp_robotics/utility/cpp_support.hpp>
 
 namespace cpp_robotics
 {
diff --git a/include/cpp_robotics/system/transfer_function.hpp b/include/cpp_robotics/system/transfer_function.hpp
index 0a8c03a8e..ffa5eefbc 100644
--- a/include/cpp_robotics/system/transfer_function.hpp
+++ b/include/cpp_robotics/system/transfer_function.hpp
@@ -1,6 +1,5 @@
 #pragma once
 
-#include <Eigen/Dense>
 #include <cpp_robotics/system/discret.hpp>
 #include <cpp_robotics/system/polynomial.hpp>
 
diff --git a/include/cpp_robotics/utility.hpp b/include/cpp_robotics/utility.hpp
index 4e1dbf6cc..77f85a3ac 100644
--- a/include/cpp_robotics/utility.hpp
+++ b/include/cpp_robotics/utility.hpp
@@ -1,7 +1,12 @@
 #pragma once
 
+#include "utility/cpp_support.hpp"
 #include "utility/math_utils.hpp"
 #include "utility/angle_range.hpp"
 #include "utility/space.hpp"
 #include "utility/singleton.hpp"
-#include "utility/cpp_support.hpp"
\ No newline at end of file
+#include "utility/random.hpp"
+
+#ifdef CR_USE_MATPLOTLIB
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#endif
\ No newline at end of file
diff --git a/include/cpp_robotics/utility/cpp_support.hpp b/include/cpp_robotics/utility/cpp_support.hpp
index ea539eb5a..52aee758e 100644
--- a/include/cpp_robotics/utility/cpp_support.hpp
+++ b/include/cpp_robotics/utility/cpp_support.hpp
@@ -3,7 +3,6 @@
 #include <string>
 #include <cstdio>
 #include <vector>
-#include <iostream>
 
 // platform
 #if defined(linux) || defined(__linux) || defined(__linux__)
@@ -123,17 +122,4 @@ std::string c_format(const std::string &format, Args const&... args)
 #pragma GCC diagnostic warning "-Wformat-security"
 }
 
-/**
- * @brief std::coutにprintfの形式でフォーマットされた文字列を出力する
- * 
- * @tparam Args 
- * @param format 
- * @param args 
- */
-template <typename... Args>
-void print(const std::string &format, Args const&... args)
-{
-    std::cout << c_format(format, args...);
-}
-
 }
\ No newline at end of file
diff --git a/include/cpp_robotics/utility/eigen_utils.hpp b/include/cpp_robotics/utility/eigen_utils.hpp
index 49976f74f..f7e07faa3 100755
--- a/include/cpp_robotics/utility/eigen_utils.hpp
+++ b/include/cpp_robotics/utility/eigen_utils.hpp
@@ -5,10 +5,12 @@
 namespace cpp_robotics
 {
 
-bool isPositiveDefinite(const Eigen::MatrixXd& matrix) {
-    Eigen::LLT<Eigen::MatrixXd> llt(matrix);  // Compute the Cholesky decomposition
+// 行列が正定値かどうか
+template<typename MatrixType>
+bool isPositiveDefinite(const MatrixType& matrix) {
+    Eigen::LLT<MatrixType> llt(matrix);
     if(llt.info() == Eigen::NumericalIssue) {
-        return false;  // Matrix is not positive definite
+        return false;
     }
     return true;
 }
diff --git a/include/cpp_robotics/algorithm/random.hpp b/include/cpp_robotics/utility/random.hpp
similarity index 99%
rename from include/cpp_robotics/algorithm/random.hpp
rename to include/cpp_robotics/utility/random.hpp
index 4890fee21..2974ae267 100644
--- a/include/cpp_robotics/algorithm/random.hpp
+++ b/include/cpp_robotics/utility/random.hpp
@@ -2,7 +2,7 @@
 
 #include <cmath>
 #include <random>
-#include "../utility/singleton.hpp"
+#include "cpp_robotics/utility/singleton.hpp"
 
 namespace cpp_robotics
 {
diff --git a/test/algorithm/icp_test.cpp b/test/algorithm/icp_test.cpp
index efc5fc85f..2e191dd1c 100644
--- a/test/algorithm/icp_test.cpp
+++ b/test/algorithm/icp_test.cpp
@@ -2,7 +2,7 @@
 
 
 #include <iostream>
-#include <cpp_robotics/algorithm.hpp>
+#include <cpp_robotics/algorithm/icp.hpp>
 #include <cpp_robotics/vector/vector2.hpp>
 // #include <cpp_robotics/third_party/matplotlib-cpp/matplotlibcpp.h>
 
diff --git a/test/common/lib_test.cpp b/test/common/lib_test.cpp
deleted file mode 100644
index a0db38a4c..000000000
--- a/test/common/lib_test.cpp
+++ /dev/null
@@ -1,9 +0,0 @@
-#include <gtest/gtest.h>
-
-// 一番大きいヘッダをincludeしてビルドが通るかどうかテスト
-#include <cpp_robotics/cpp_robotics.hpp>
-
-TEST(lib_test, test) {
-    namespace cr = cpp_robotics;
-    cr::print("hello world: %d, %.3lf\n", 100, 3.141);
-}
\ No newline at end of file
diff --git a/test/optimize/sqp_test.cpp b/test/optimize/sqp_test.cpp
index b8288cb74..7bd1136c4 100644
--- a/test/optimize/sqp_test.cpp
+++ b/test/optimize/sqp_test.cpp
@@ -1,8 +1,6 @@
 #include <gtest/gtest.h>
 
 #include <iostream>
-#include <limits>
-#include <cpp_robotics/algorithm.hpp>
 #include <cpp_robotics/optimize/sqp.hpp>
 
 
diff --git a/test/algorithm/random_test.cpp b/test/utility/random_test.cpp
similarity index 98%
rename from test/algorithm/random_test.cpp
rename to test/utility/random_test.cpp
index 86c3af3aa..f337fc8cb 100644
--- a/test/algorithm/random_test.cpp
+++ b/test/utility/random_test.cpp
@@ -1,7 +1,7 @@
 #include <gtest/gtest.h>
 
 #include <iostream>
-#include <cpp_robotics/algorithm/random.hpp>
+#include <cpp_robotics/utility/random.hpp>
 #include <array>
 
 TEST(random, constructor) {
diff --git a/unsupported/arm_ik/arm_ik.hpp b/unsupported/arm_ik/arm_ik.hpp
new file mode 100644
index 000000000..bf292945b
--- /dev/null
+++ b/unsupported/arm_ik/arm_ik.hpp
@@ -0,0 +1,3 @@
+#pragma once
+
+#include "arm_ik/delta_robot.hpp"
\ No newline at end of file
diff --git a/include/cpp_robotics/arm_ik/delta_robot.hpp b/unsupported/arm_ik/delta_robot.hpp
similarity index 100%
rename from include/cpp_robotics/arm_ik/delta_robot.hpp
rename to unsupported/arm_ik/delta_robot.hpp
diff --git a/unsupported/xy_2deg_robot/xy_2deg_robot.hpp b/unsupported/arm_ik/xy_2deg_robot/xy_2deg_robot.hpp
similarity index 100%
rename from unsupported/xy_2deg_robot/xy_2deg_robot.hpp
rename to unsupported/arm_ik/xy_2deg_robot/xy_2deg_robot.hpp
diff --git a/unsupported/xy_2deg_robot/xy_2deg_robot_test.cpp b/unsupported/arm_ik/xy_2deg_robot/xy_2deg_robot_test.cpp
similarity index 100%
rename from unsupported/xy_2deg_robot/xy_2deg_robot_test.cpp
rename to unsupported/arm_ik/xy_2deg_robot/xy_2deg_robot_test.cpp
diff --git a/include/cpp_robotics/filter/filter_connector.hpp b/unsupported/filter_connector.hpp
similarity index 100%
rename from include/cpp_robotics/filter/filter_connector.hpp
rename to unsupported/filter_connector.hpp
diff --git a/include/cpp_robotics/utility/ginac_utils.hpp b/unsupported/ginac_utils.hpp
similarity index 100%
rename from include/cpp_robotics/utility/ginac_utils.hpp
rename to unsupported/ginac_utils.hpp
diff --git a/include/cpp_robotics/algorithm/misc.hpp b/unsupported/misc.hpp
similarity index 100%
rename from include/cpp_robotics/algorithm/misc.hpp
rename to unsupported/misc.hpp
diff --git a/include/cpp_robotics/algorithm/ndt.hpp b/unsupported/ndt.hpp
similarity index 100%
rename from include/cpp_robotics/algorithm/ndt.hpp
rename to unsupported/ndt.hpp
diff --git a/unsupported/fmin_search_test.cpp b/unsupported/optimize/fmin_search_test.cpp
similarity index 100%
rename from unsupported/fmin_search_test.cpp
rename to unsupported/optimize/fmin_search_test.cpp
diff --git a/include/cpp_robotics/optimize/interior_point_method.hpp b/unsupported/optimize/interior_point_method.hpp
similarity index 100%
rename from include/cpp_robotics/optimize/interior_point_method.hpp
rename to unsupported/optimize/interior_point_method.hpp
diff --git a/unsupported/ipopt_example.cpp b/unsupported/optimize/ipopt_example.cpp
similarity index 100%
rename from unsupported/ipopt_example.cpp
rename to unsupported/optimize/ipopt_example.cpp
diff --git a/include/cpp_robotics/optimize/lesi_doc.hpp b/unsupported/optimize/lesi_doc.hpp
similarity index 100%
rename from include/cpp_robotics/optimize/lesi_doc.hpp
rename to unsupported/optimize/lesi_doc.hpp
diff --git a/include/cpp_robotics/optimize/linprog.hpp b/unsupported/optimize/linprog.hpp
similarity index 100%
rename from include/cpp_robotics/optimize/linprog.hpp
rename to unsupported/optimize/linprog.hpp
diff --git a/include/cpp_robotics/path_planning/dwa.hpp b/unsupported/path_planning/dwa.hpp
similarity index 100%
rename from include/cpp_robotics/path_planning/dwa.hpp
rename to unsupported/path_planning/dwa.hpp
diff --git a/include/cpp_robotics/path_planning/frenet_frame.hpp b/unsupported/path_planning/frenet_frame.hpp
similarity index 100%
rename from include/cpp_robotics/path_planning/frenet_frame.hpp
rename to unsupported/path_planning/frenet_frame.hpp
diff --git a/unsupported/print.hpp b/unsupported/print.hpp
new file mode 100644
index 000000000..5c1e7a3cf
--- /dev/null
+++ b/unsupported/print.hpp
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <iostream>
+#include <string>
+#include <cpp_robotics/utility/cpp_support.hpp>
+
+/**
+ * @brief std::coutにprintfの形式でフォーマットされた文字列を出力する
+ * 
+ * @tparam Args 
+ * @param format 
+ * @param args 
+ */
+template <typename... Args>
+void print(const std::string &format, Args const&... args)
+{
+    std::cout << c_format(format, args...);
+}
\ No newline at end of file
diff --git a/include/cpp_robotics/algorithm/state_machine.hpp b/unsupported/state_machine.hpp
similarity index 100%
rename from include/cpp_robotics/algorithm/state_machine.hpp
rename to unsupported/state_machine.hpp