diff

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0d8ec8c..c18359f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -5,6 +5,6 @@ set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 add_subdirectory(Core)
-add_subdirectory(Mecanum4)
-add_subdirectory(Steering4)
-add_subdirectory(Controller)
\ No newline at end of file
+add_subdirectory(Chassis)
+add_subdirectory(Controller)
+add_subdirectory(Localization)
\ No newline at end of file
diff --git a/Chassis/CMakeLists.txt b/Chassis/CMakeLists.txt
new file mode 100644
index 0000000..5afdd16
--- /dev/null
+++ b/Chassis/CMakeLists.txt
@@ -0,0 +1,2 @@
+add_subdirectory(Mecanum4)
+add_subdirectory(Steering4)
\ No newline at end of file
diff --git a/Mecanum4/CMakeLists.txt b/Chassis/Mecanum4/CMakeLists.txt
similarity index 82%
rename from Mecanum4/CMakeLists.txt
rename to Chassis/Mecanum4/CMakeLists.txt
index be25ed5..3b4227b 100644
--- a/Mecanum4/CMakeLists.txt
+++ b/Chassis/Mecanum4/CMakeLists.txt
@@ -1,10 +1,10 @@
 add_library(ChassisMecanum4 STATIC
-    "./Mecanum4.cpp"
+        "Mecanum4.cpp"
 )
 
 target_include_directories(ChassisMecanum4
-    PUBLIC
-    ${CMAKE_CURRENT_SOURCE_DIR}
+        PUBLIC
+        .
 )
 
 # link dependencies if any
diff --git a/Mecanum4/Mecanum4.cpp b/Chassis/Mecanum4/Mecanum4.cpp
similarity index 69%
rename from Mecanum4/Mecanum4.cpp
rename to Chassis/Mecanum4/Mecanum4.cpp
index 61d15cf..6186c73 100644
--- a/Mecanum4/Mecanum4.cpp
+++ b/Chassis/Mecanum4/Mecanum4.cpp
@@ -11,7 +11,7 @@
  */
 #define RPS2RPM(__RPS__) ((__RPS__) * 60.0f / (2 * 3.14159265358979323846f))
 
-#define DEG2RAD(__DEG__) ((__DEG__) * (float)3.14159265358979323846f / 180.0f)
+#define DEG2RAD(__DEG__) ((__DEG__) * (float) 3.14159265358979323846f / 180.0f)
 
 #define RPM2DPS(__RPM__) ((__RPM__) / 60.0f * 360.0f)
 
@@ -22,9 +22,8 @@ static constexpr size_t idx(Mecanum4::WheelType w)
     return static_cast<size_t>(w);
 }
 
-Mecanum4::Mecanum4(const Config& driver_cfg, const IChassis::Config& base_cfg) :
-    IChassis(base_cfg), type_(driver_cfg.chassis_type),
-    wheel_radius_(driver_cfg.wheel_radius * 1e-3f)
+Mecanum4::Mecanum4(chassis_loc::ILoc& loc, const Config& driver_cfg) :
+    IChassis(loc), type_(driver_cfg.chassis_type), wheel_radius_(driver_cfg.wheel_radius * 1e-3f)
 {
     if (type_ == ChassisType::OType)
         k_omega_ = driver_cfg.wheel_distance_x * 1e-3f * 0.5f +
@@ -123,6 +122,45 @@ void Mecanum4::velocityControllerUpdate()
         wheel->update();
 }
 
+IChassis::Velocity Mecanum4::forwardGetVelocity()
+{
+    Velocity vel{};
+    vel.vx = RPM2DPS(wheel_radius_ * 0.25f *
+                     DEG2RAD(wheel_[idx(WheelType::FrontRight)]->getMotor()->getVelocity() +
+                             wheel_[idx(WheelType::FrontLeft)]->getMotor()->getVelocity() +
+                             wheel_[idx(WheelType::RearRight)]->getMotor()->getVelocity() +
+                             wheel_[idx(WheelType::RearLeft)]->getMotor()->getVelocity()));
+
+    if (type_ == ChassisType::OType)
+    {
+        vel.wz = RPM2DPS(wheel_radius_ / (4 * k_omega_) *
+                         (wheel_[idx(WheelType::FrontRight)]->getMotor()->getVelocity() -
+                          wheel_[idx(WheelType::FrontLeft)]->getMotor()->getVelocity() +
+                          wheel_[idx(WheelType::RearRight)]->getMotor()->getVelocity() -
+                          wheel_[idx(WheelType::RearLeft)]->getMotor()->getVelocity()));
+        vel.vy = RPM2DPS(wheel_radius_ * 0.25f *
+                         DEG2RAD(wheel_[idx(WheelType::FrontRight)]->getMotor()->getVelocity() -
+                                 wheel_[idx(WheelType::FrontLeft)]->getMotor()->getVelocity() -
+                                 wheel_[idx(WheelType::RearRight)]->getMotor()->getVelocity() +
+                                 wheel_[idx(WheelType::RearLeft)]->getMotor()->getVelocity()));
+    }
+    else if (type_ == ChassisType::XType)
+    {
+        vel.wz = RPM2DPS(wheel_radius_ / (4 * k_omega_) *
+                         (wheel_[idx(WheelType::FrontRight)]->getMotor()->getVelocity() -
+                          wheel_[idx(WheelType::FrontLeft)]->getMotor()->getVelocity() -
+                          wheel_[idx(WheelType::RearRight)]->getMotor()->getVelocity() +
+                          wheel_[idx(WheelType::RearLeft)]->getMotor()->getVelocity()));
+        vel.vy = RPM2DPS(wheel_radius_ / (4 * k_omega_) *
+                         DEG2RAD(-wheel_[idx(WheelType::FrontRight)]->getMotor()->getVelocity() +
+                                 wheel_[idx(WheelType::FrontLeft)]->getMotor()->getVelocity() +
+                                 wheel_[idx(WheelType::RearRight)]->getMotor()->getVelocity() -
+                                 wheel_[idx(WheelType::RearLeft)]->getMotor()->getVelocity()));
+    }
+    return vel;
+}
+
+/* 以下函数将不再使用
 float Mecanum4::forwardGetYaw()
 {
     if (type_ == ChassisType::OType)
@@ -169,52 +207,6 @@ float Mecanum4::forwardGetY()
 
     return 0.0f;
 }
-
-float Mecanum4::forwardGetWz()
-{
-    if (type_ == ChassisType::OType)
-        return RPM2DPS(wheel_radius_ / (4 * k_omega_) *
-                       (wheel_[idx(WheelType::FrontRight)]->getMotor()->getVelocity() -
-                        wheel_[idx(WheelType::FrontLeft)]->getMotor()->getVelocity() +
-                        wheel_[idx(WheelType::RearRight)]->getMotor()->getVelocity() -
-                        wheel_[idx(WheelType::RearLeft)]->getMotor()->getVelocity()));
-
-    if (type_ == ChassisType::XType)
-        return RPM2DPS(wheel_radius_ / (4 * k_omega_) *
-                       (wheel_[idx(WheelType::FrontRight)]->getMotor()->getVelocity() -
-                        wheel_[idx(WheelType::FrontLeft)]->getMotor()->getVelocity() -
-                        wheel_[idx(WheelType::RearRight)]->getMotor()->getVelocity() +
-                        wheel_[idx(WheelType::RearLeft)]->getMotor()->getVelocity()));
-
-    return 0.0f;
-}
-
-float Mecanum4::forwardGetVx()
-{
-    return RPM2DPS(wheel_radius_ * 0.25f *
-                   DEG2RAD(wheel_[idx(WheelType::FrontRight)]->getMotor()->getVelocity() +
-                           wheel_[idx(WheelType::FrontLeft)]->getMotor()->getVelocity() +
-                           wheel_[idx(WheelType::RearRight)]->getMotor()->getVelocity() +
-                           wheel_[idx(WheelType::RearLeft)]->getMotor()->getVelocity()));
-}
-
-float Mecanum4::forwardGetVy()
-{
-    if (type_ == ChassisType::OType)
-        return RPM2DPS(wheel_radius_ * 0.25f *
-                       DEG2RAD(wheel_[idx(WheelType::FrontRight)]->getMotor()->getVelocity() -
-                               wheel_[idx(WheelType::FrontLeft)]->getMotor()->getVelocity() -
-                               wheel_[idx(WheelType::RearRight)]->getMotor()->getVelocity() +
-                               wheel_[idx(WheelType::RearLeft)]->getMotor()->getVelocity()));
-
-    if (type_ == ChassisType::XType)
-        return RPM2DPS(wheel_radius_ / (4 * k_omega_) *
-                       DEG2RAD(-wheel_[idx(WheelType::FrontRight)]->getMotor()->getVelocity() +
-                               wheel_[idx(WheelType::FrontLeft)]->getMotor()->getVelocity() +
-                               wheel_[idx(WheelType::RearRight)]->getMotor()->getVelocity() -
-                               wheel_[idx(WheelType::RearLeft)]->getMotor()->getVelocity()));
-
-    return 0.0f;
-}
+*/
 
 } // namespace chassis
\ No newline at end of file
diff --git a/Mecanum4/Mecanum4.hpp b/Chassis/Mecanum4/Mecanum4.hpp
similarity index 73%
rename from Mecanum4/Mecanum4.hpp
rename to Chassis/Mecanum4/Mecanum4.hpp
index b46992e..dea8ac4 100644
--- a/Mecanum4/Mecanum4.hpp
+++ b/Chassis/Mecanum4/Mecanum4.hpp
@@ -42,27 +42,21 @@ public:
         controllers::MotorVelController* wheel_rear_right;  ///< 右后方
     };
 
-    Mecanum4(const Config& driver_cfg, const IChassis::Config& base_cfg);
+    Mecanum4(chassis_loc::ILoc& loc, const Config& driver_cfg);
 
     bool enable() override;
     void disable() override;
 
-    [[nodiscard]] bool enabled() const override { return enabled_; }
+    [[nodiscard]] bool enabled() const override
+    {
+        return enabled_;
+    }
 
 protected:
-    void  applyVelocity(const Velocity& velocity) override;
-    void  velocityControllerUpdate() override;
-    float forwardGetX() override;
-    float forwardGetY() override;
-    float forwardGetYaw() override;
-    float forwardGetVx() override;
-    float forwardGetVy() override;
-    float forwardGetWz() override;
+    void applyVelocity(const Velocity& velocity) override;
+    void velocityControllerUpdate() override;
 
-    [[nodiscard]] WheeledKinematicsType kinematicsType() const override
-    {
-        return WheeledKinematicsType::RollingConstrained;
-    }
+    Velocity forwardGetVelocity() override;
 
 private:
     bool        enabled_{ false };
diff --git a/Mecanum4/cpkg.toml b/Chassis/Mecanum4/cpkg.toml
similarity index 100%
rename from Mecanum4/cpkg.toml
rename to Chassis/Mecanum4/cpkg.toml
diff --git a/Steering4/CMakeLists.txt b/Chassis/Steering4/CMakeLists.txt
similarity index 80%
rename from Steering4/CMakeLists.txt
rename to Chassis/Steering4/CMakeLists.txt
index 17363a5..e5424f3 100644
--- a/Steering4/CMakeLists.txt
+++ b/Chassis/Steering4/CMakeLists.txt
@@ -1,11 +1,11 @@
 add_library(ChassisSteering4 STATIC
-    "./SteeringWheel.cpp"
-    "./Steering4.cpp"
+        "SteeringWheel.cpp"
+        "Steering4.cpp"
 )
 
 target_include_directories(ChassisSteering4
-    PUBLIC
-    ${CMAKE_CURRENT_SOURCE_DIR}
+        PUBLIC
+        .
 )
 
 # link dependencies if any
diff --git a/Steering4/Steering4.cpp b/Chassis/Steering4/Steering4.cpp
similarity index 95%
rename from Steering4/Steering4.cpp
rename to Chassis/Steering4/Steering4.cpp
index fcdfce9..5e988db 100644
--- a/Steering4/Steering4.cpp
+++ b/Chassis/Steering4/Steering4.cpp
@@ -16,9 +16,9 @@
 
 namespace chassis
 {
-Steering4::Steering4(Config cfg, const IChassis::Config& base_cfg) :
-    IChassis(base_cfg), enable_calib_(cfg.enable_calibration),
-    wheel_radius_(1e-3f * cfg.radius), // mm to m
+Steering4::Steering4(chassis_loc::ILoc& loc, const Config& cfg) :
+    IChassis(loc), enable_calib_(cfg.enable_calibration), //
+    wheel_radius_(1e-3f * cfg.radius),                    // mm to m
     half_distance_x(0.5f * cfg.distance_x), half_distance_y(0.5f * cfg.distance_y),
     inv_l2_(4.0f / (cfg.distance_x * cfg.distance_x + cfg.distance_y * cfg.distance_y)),
     spd2rpm_(1.0f / (wheel_radius_ * 3.14159265358979323846f * 2) * 60.0f), wheel_{
diff --git a/Steering4/Steering4.hpp b/Chassis/Steering4/Steering4.hpp
similarity index 72%
rename from Steering4/Steering4.hpp
rename to Chassis/Steering4/Steering4.hpp
index e7246ec..28c812a 100644
--- a/Steering4/Steering4.hpp
+++ b/Chassis/Steering4/Steering4.hpp
@@ -44,7 +44,7 @@ public:
         Wheel wheel_rear_right;  ///< 右后方
     };
 
-    Steering4(Config cfg, const IChassis::Config& base_cfg);
+    Steering4(chassis_loc::ILoc& loc, const Config& cfg);
     [[nodiscard]] bool enable() override
     {
         if (enabled_)
@@ -75,30 +75,13 @@ public:
     }
 
 protected:
-    void applyVelocity(const Velocity& velocity) override;
-    void velocityControllerUpdate() override;
-
-    float forwardGetVx() override
-    {
-        return velocity_.vx;
-    }
-    float forwardGetVy() override
-    {
-        return velocity_.vy;
-    }
-    float forwardGetWz() override
+    void     applyVelocity(const Velocity& velocity) override;
+    void     velocityControllerUpdate() override;
+    Velocity forwardGetVelocity() override
     {
-        return velocity_.wz;
+        return velocity_;
     }
 
-    [[nodiscard]] WheeledKinematicsType kinematicsType() const override
-    {
-        return WheeledKinematicsType::VelocityIntegrated;
-    }
-    float forwardGetX() override   = 0;
-    float forwardGetY() override   = 0;
-    float forwardGetYaw() override = 0;
-
 private:
     bool enabled_{ false };
     bool enable_calib_;
@@ -110,10 +93,7 @@ private:
     float inv_l2_;
     float spd2rpm_;
 
-    struct
-    {
-        float vx, vy, wz;
-    } velocity_{}; // 反馈速度
+    Velocity velocity_{}; // 反馈速度
 
     steering::SteeringWheel wheel_[static_cast<size_t>(WheelType::Max)];
 
@@ -122,7 +102,7 @@ private:
         float x, y;
     };
 
-    WheelPosition getWheelPosition(WheelType wheel) const;
+    [[nodiscard]] WheelPosition getWheelPosition(WheelType wheel) const;
 };
 
 } // namespace chassis
diff --git a/Steering4/SteeringWheel.cpp b/Chassis/Steering4/SteeringWheel.cpp
similarity index 100%
rename from Steering4/SteeringWheel.cpp
rename to Chassis/Steering4/SteeringWheel.cpp
diff --git a/Steering4/SteeringWheel.hpp b/Chassis/Steering4/SteeringWheel.hpp
similarity index 100%
rename from Steering4/SteeringWheel.hpp
rename to Chassis/Steering4/SteeringWheel.hpp
diff --git a/Steering4/cpkg.toml b/Chassis/Steering4/cpkg.toml
similarity index 100%
rename from Steering4/cpkg.toml
rename to Chassis/Steering4/cpkg.toml
diff --git a/Controller/CMakeLists.txt b/Controller/CMakeLists.txt
index e68fa92..853d945 100644
--- a/Controller/CMakeLists.txt
+++ b/Controller/CMakeLists.txt
@@ -1,13 +1,2 @@
-add_library(__Chassis_Controller INTERFACE)
-
-target_include_directories(__Chassis_Controller INTERFACE
-    ${CMAKE_CURRENT_SOURCE_DIR}
-)
-
-# link dependencies if any
-target_link_libraries(__Chassis_Controller INTERFACE Chassis::Core)
-target_link_libraries(__Chassis_Controller INTERFACE libs::RingBuffer)
-target_link_libraries(__Chassis_Controller INTERFACE VelocityProfile::SCurve)
-
-# alias for external use
-add_library(Chassis::Controller ALIAS __Chassis_Controller)
+add_subdirectory(Master)
+add_subdirectory(Slave)
\ No newline at end of file
diff --git a/Controller/Master.hpp b/Controller/Master.hpp
deleted file mode 100644
index d93511a..0000000
--- a/Controller/Master.hpp
+++ /dev/null
@@ -1,351 +0,0 @@
-/**
- * @file    Master.hpp
- * @author  syhanjin
- * @date    2026-02-24
- * @brief   Brief description of the file
- */
-#pragma once
-#include "IChassis.hpp"
-#include "isr_lock.h"
-#include "s_curve.hpp"
-#include "mit_pd.hpp"
-#include "cmsis_os2.h"
-#include <algorithm>
-#include <cmath>
-#include <type_traits>
-
-namespace chassis::controller
-{
-
-template <typename Chassis> class Master : public Chassis
-{
-    static_assert(std::is_base_of_v<IChassis, Chassis>);
-
-public:
-    using AxisLimit = velocity_profile::SCurveProfile::Config;
-    using Velocity  = IChassis::Velocity;
-    using Posture   = IChassis::Posture;
-
-    struct Config
-    {
-        struct
-        {
-            MITPD::Config vx; ///< x 速度 PD 控制器
-            MITPD::Config vy; ///< y 速度 PD 控制器
-            MITPD::Config wz; ///< 角速度 PD 控制器
-        } posture_error_pd_cfg;
-
-        struct
-        {
-            AxisLimit x, y, yaw;
-        } limit{};
-    };
-
-    enum class CtrlMode
-    {
-        Stopped,
-        Velocity,
-        Posture,
-    };
-
-    Master(Chassis&& chassis, const Config& cfg) :
-        Chassis(std::move(chassis)), lock_(osMutexNew(nullptr)),                       //
-        limit_x_{ cfg.limit.x }, limit_y_{ cfg.limit.y }, limit_yaw_{ cfg.limit.yaw }, //
-        posture_trajectory_{ .pd    = { MITPD(cfg.posture_error_pd_cfg.vx),
-                                        MITPD(cfg.posture_error_pd_cfg.vy),
-                                        MITPD(cfg.posture_error_pd_cfg.wz) },
-                             .curve = { velocity_profile::SCurveProfile(cfg.limit.x, 0, 0, 0, 0),
-                                        velocity_profile::SCurveProfile(cfg.limit.y, 0, 0, 0, 0),
-                                        velocity_profile::SCurveProfile(cfg.limit.yaw, 0, 0, 0, 0) }
-
-        }
-    {
-    }
-
-    bool setTargetPostureInWorld(const Posture& absolute_target);
-    bool setTargetPostureInBody(const Posture& relative_target);
-
-    [[nodiscard]] bool isTrajectoryFinished() const;
-    void               waitTrajectoryFinish() const;
-
-    void setVelocityInWorld(const Velocity& world_velocity, bool target_in_world);
-    void setVelocityInBody(const Velocity& body_velocity, bool target_in_world);
-
-    void stop();
-
-    void profileUpdate(float dt);
-    void errorUpdate();
-    void controllerUpdate();
-
-    bool enable()
-    {
-        stop();
-        return Chassis::enable();
-    }
-
-    void setWorldFromCurrent()
-    {
-        if (this->isOpsEnabled())
-            return;
-        osMutexAcquire(lock_, osWaitForever);
-        const auto saved = isr_lock();
-        this->applySetWorldFromCurrent();
-        isr_unlock(saved);
-        osMutexRelease(lock_);
-    }
-
-private:
-    osMutexId_t lock_;
-
-    CtrlMode ctrl_mode_{ CtrlMode::Stopped }; ///< 当前控制模式
-
-    AxisLimit limit_x_;
-    AxisLimit limit_y_;
-    AxisLimit limit_yaw_;
-
-    struct
-    {
-        volatile bool target_in_world; ///< 速度是否相对于世界坐标系不变
-        Velocity      in_world;        ///< 世界坐标系下速度
-        Velocity      in_body;         ///< 车体坐标系下速度
-    } velocity_ref_;
-
-    struct
-    {
-        float now{};        ///< 当前执行时间
-        float total_time{}; ///< 总执行时间
-
-        struct
-        {
-            MITPD vx; ///< x 速度 PD 控制器
-            MITPD vy; ///< y 速度 PD 控制器
-            MITPD wz; ///< 角速度 PD 控制器
-        } pd;
-
-        struct
-        {
-            velocity_profile::SCurveProfile x;
-            velocity_profile::SCurveProfile y;
-            velocity_profile::SCurveProfile yaw;
-        } curve;
-
-        Posture  p_ref_curr_{};
-        Velocity v_ref_curr_{};
-    } posture_trajectory_;
-
-private:
-    void update_velocity_control();
-    void apply_position_velocity();
-};
-template <typename Chassis>
-bool Master<Chassis>::setTargetPostureInWorld(const Posture& absolute_target)
-{
-    osMutexAcquire(lock_, osWaitForever);
-
-    // copy 当前位置和速度
-    const auto [x, y, yaw]  = this->posture().in_world;
-    const auto [vx, vy, wz] = this->velocity().in_world;
-
-    float ax = 0, ay = 0, ayaw = 0;
-    if (ctrl_mode_ == CtrlMode::Posture)
-    {
-        ax   = posture_trajectory_.curve.x.CalcA(posture_trajectory_.now);
-        ay   = posture_trajectory_.curve.y.CalcA(posture_trajectory_.now);
-        ayaw = posture_trajectory_.curve.yaw.CalcA(posture_trajectory_.now);
-    }
-    // 初始化 S 型曲线
-    // 衔接当前位置，速度，如果之前是位置控制还会衔接加速度
-    const velocity_profile::SCurveProfile //
-            curve_x(limit_x_, x, vx, ax, absolute_target.x),
-            curve_y(limit_y_, y, vy, ay, absolute_target.y),
-            curve_yaw(limit_yaw_, yaw, wz, ayaw, absolute_target.yaw);
-
-    if (!curve_x.success() || !curve_y.success() || !curve_yaw.success())
-        return false;
-
-    float total_time = std::fmaxf(curve_x.getTotalTime(),
-                                  std::fmaxf(curve_y.getTotalTime(), curve_yaw.getTotalTime()));
-
-    const uint32_t saved = isr_lock(); // 写入过程加中断锁
-
-    posture_trajectory_.now        = 0;
-    posture_trajectory_.total_time = total_time;
-
-    posture_trajectory_.curve.x   = curve_x;
-    posture_trajectory_.curve.y   = curve_y;
-    posture_trajectory_.curve.yaw = curve_yaw;
-
-    ctrl_mode_ = CtrlMode::Posture;
-
-    isr_unlock(saved);
-
-    osMutexRelease(lock_);
-    return true;
-}
-template <typename Chassis>
-bool Master<Chassis>::setTargetPostureInBody(const Posture& relative_target)
-{
-    osMutexAcquire(lock_, osWaitForever);
-    const auto absolute_target = this->BodyPosture2WorldPosture(relative_target);
-    osMutexRelease(lock_);
-
-    return setTargetPostureInWorld(absolute_target);
-}
-template <typename Chassis> bool Master<Chassis>::isTrajectoryFinished() const
-{
-    return posture_trajectory_.now >= posture_trajectory_.total_time;
-}
-
-template <typename Chassis> void Master<Chassis>::waitTrajectoryFinish() const
-{
-    while (!isTrajectoryFinished())
-        osDelay(1);
-}
-
-template <typename Chassis>
-void Master<Chassis>::setVelocityInWorld(const Velocity& world_velocity, bool target_in_world)
-{
-    osMutexAcquire(lock_, osWaitForever);
-    const auto [vx, vy, wz] = this->WorldVelocity2BodyVelocity(world_velocity);
-
-    const uint32_t saved = isr_lock(); // 写入过程加中断锁
-
-    velocity_ref_.target_in_world = target_in_world;
-    velocity_ref_.in_world.vx     = world_velocity.vx;
-    velocity_ref_.in_world.vy     = world_velocity.vy;
-    velocity_ref_.in_world.wz     = world_velocity.wz;
-    velocity_ref_.in_body.vx      = vx;
-    velocity_ref_.in_body.vy      = vy;
-    velocity_ref_.in_body.wz      = wz;
-
-    ctrl_mode_ = CtrlMode::Velocity;
-
-    isr_unlock(saved);
-
-    osMutexRelease(lock_);
-}
-template <typename Chassis>
-void Master<Chassis>::setVelocityInBody(const Velocity& body_velocity, bool target_in_world)
-{
-    osMutexAcquire(lock_, osWaitForever);
-    const auto [vx, vy, wz] = this->BodyVelocity2WorldVelocity(body_velocity);
-
-    const uint32_t saved = isr_lock(); // 写入过程加中断锁
-
-    velocity_ref_.target_in_world = target_in_world;
-    velocity_ref_.in_body.vx      = body_velocity.vx;
-    velocity_ref_.in_body.vy      = body_velocity.vy;
-    velocity_ref_.in_body.wz      = body_velocity.wz;
-    velocity_ref_.in_world.vx     = vx;
-    velocity_ref_.in_world.vy     = vy;
-    velocity_ref_.in_world.wz     = wz;
-
-    ctrl_mode_ = CtrlMode::Velocity;
-
-    isr_unlock(saved);
-    osMutexRelease(lock_);
-}
-template <typename Chassis> void Master<Chassis>::stop()
-{
-    osMutexAcquire(lock_, osWaitForever);
-    const uint32_t saved = isr_lock();
-
-    ctrl_mode_ = CtrlMode::Stopped;
-
-    posture_trajectory_.p_ref_curr_ = this->posture().in_world;
-    posture_trajectory_.v_ref_curr_ = { 0, 0, 0 };
-
-    isr_unlock(saved);
-    osMutexRelease(lock_);
-}
-template <typename Chassis> void Master<Chassis>::profileUpdate(float dt)
-{
-    if (!this->enabled() || ctrl_mode_ != CtrlMode::Posture)
-        return;
-
-    // 推进曲线
-    const float now               = this->posture_trajectory_.now + dt;
-    this->posture_trajectory_.now = now;
-
-    // 计算前馈速度
-    this->posture_trajectory_.v_ref_curr_ = { .vx = posture_trajectory_.curve.x.CalcV(now),
-                                              .vy = posture_trajectory_.curve.y.CalcV(now),
-                                              .wz = posture_trajectory_.curve.yaw.CalcV(now) };
-
-    // 计算当前目标
-    this->posture_trajectory_.p_ref_curr_ = { .x   = posture_trajectory_.curve.x.CalcX(now),
-                                              .y   = posture_trajectory_.curve.y.CalcX(now),
-                                              .yaw = posture_trajectory_.curve.yaw.CalcX(now) };
-
-    apply_position_velocity();
-}
-template <typename Chassis> void Master<Chassis>::errorUpdate()
-{
-    if (!this->enabled() || !(ctrl_mode_ == CtrlMode::Posture || ctrl_mode_ == CtrlMode::Stopped))
-        return;
-
-    // 使用 pd 控制器跟随当前目标
-    posture_trajectory_.pd.vx.calc(posture_trajectory_.p_ref_curr_.x,
-                                   this->posture().in_world.x,
-                                   posture_trajectory_.v_ref_curr_.vx,
-                                   this->velocity().in_world.vx);
-    posture_trajectory_.pd.vy.calc(posture_trajectory_.p_ref_curr_.y,
-                                   this->posture().in_world.y,
-                                   posture_trajectory_.v_ref_curr_.vy,
-                                   this->velocity().in_world.vy);
-    posture_trajectory_.pd.wz.calc(posture_trajectory_.p_ref_curr_.yaw,
-                                   this->posture().in_world.yaw,
-                                   posture_trajectory_.v_ref_curr_.wz,
-                                   this->velocity().in_world.wz);
-    apply_position_velocity();
-}
-template <typename Chassis> void Master<Chassis>::controllerUpdate()
-{
-    if (!this->enabled())
-        return;
-
-    if (ctrl_mode_ == CtrlMode::Velocity)
-        update_velocity_control();
-    this->velocityControllerUpdate();
-}
-template <typename Chassis> void Master<Chassis>::update_velocity_control()
-{
-    if (velocity_ref_.target_in_world)
-    { // 如果基于世界坐标计算速度，则需要转为车身坐标系，并应用到底盘驱动器
-        velocity_ref_.in_body = this->WorldVelocity2BodyVelocity(velocity_ref_.in_world);
-        // 进行修正 1e-3f 为更新间隔，此处发现前馈并没有什么精度优化，暂时不做前馈
-        // const float              beta     = DEG2RAD(0.5f * velocity_.in_body.wz * 1e-3f);
-        // const float              cot_beta = 1.0f / tanf(beta);
-        // const Chassis_Velocity_t temp_velocity = {
-        //     .vx = beta * (velocity_.in_body.vx * cot_beta +
-        //     velocity_.in_body.vy), .vy = beta * (velocity_.in_body.vy * cot_beta
-        //     - velocity_.in_body.vx), .wz = velocity_.in_body.wz
-        // };
-        // ChassisDriver_ApplyVelocity(&driver_,
-        //                             temp_velocity.vx,
-        //                             temp_velocity.vy,
-        //                             temp_velocity.wz);
-    }
-    else
-    {
-        // 直接应用速度
-    }
-    this->applyVelocity(velocity_ref_.in_body);
-}
-template <typename Chassis> void Master<Chassis>::apply_position_velocity()
-{
-    // 叠加前馈和 pd 输出
-    const Velocity velocity_in_world = {
-        posture_trajectory_.v_ref_curr_.vx + posture_trajectory_.pd.vx.getOutput(),
-        posture_trajectory_.v_ref_curr_.vy + posture_trajectory_.pd.vy.getOutput(),
-        posture_trajectory_.v_ref_curr_.wz + posture_trajectory_.pd.wz.getOutput(),
-    };
-
-    // 将世界坐标系速度转换为底盘坐标系速度
-    const Velocity body_velocity = this->WorldVelocity2BodyVelocity(velocity_in_world);
-
-    // 应用速度
-    this->applyVelocity(body_velocity);
-}
-
-} // namespace chassis::controller
diff --git a/Controller/Master/CMakeLists.txt b/Controller/Master/CMakeLists.txt
new file mode 100644
index 0000000..6902e66
--- /dev/null
+++ b/Controller/Master/CMakeLists.txt
@@ -0,0 +1,12 @@
+add_library(__Chassis_ControllerMaster INTERFACE)
+
+target_include_directories(__Chassis_ControllerMaster INTERFACE
+    ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+# link dependencies if any
+target_link_libraries(__Chassis_ControllerMaster INTERFACE Chassis::Core)
+target_link_libraries(__Chassis_ControllerMaster INTERFACE VelocityProfile::SCurve)
+
+# alias for external use
+add_library(Chassis::ControllerMaster ALIAS __Chassis_ControllerMaster)
diff --git a/Controller/Master/Master.hpp b/Controller/Master/Master.hpp
new file mode 100644
index 0000000..535efa5
--- /dev/null
+++ b/Controller/Master/Master.hpp
@@ -0,0 +1,340 @@
+/**
+ * @file    Master.hpp
+ * @author  syhanjin
+ * @date    2026-02-24
+ * @brief   Brief description of the file
+ */
+#pragma once
+#include "IChassis.hpp"
+#include "isr_lock.h"
+#include "s_curve.hpp"
+#include "mit_pd.hpp"
+#include "cmsis_os2.h"
+#include <algorithm>
+#include <cmath>
+#include <type_traits>
+
+namespace chassis::controller
+{
+
+template <typename Chassis> class Master : public Chassis
+{
+    static_assert(std::is_base_of_v<IChassis, Chassis>);
+
+public:
+    using AxisLimit = velocity_profile::SCurveProfile::Config;
+    using Velocity  = IChassis::Velocity;
+    using Posture   = IChassis::Posture;
+
+    struct Config
+    {
+        struct
+        {
+            MITPD::Config vx; ///< x 速度 PD 控制器
+            MITPD::Config vy; ///< y 速度 PD 控制器
+            MITPD::Config wz; ///< 角速度 PD 控制器
+        } posture_error_pd_cfg;
+
+        struct
+        {
+            AxisLimit x, y, yaw;
+        } limit{};
+    };
+
+    enum class CtrlMode
+    {
+        Stopped,
+        Velocity,
+        Posture,
+    };
+
+    Master(Chassis&& chassis, const Config& cfg) :
+        Chassis(std::move(chassis)), lock_(osMutexNew(nullptr)),                       //
+        limit_x_{ cfg.limit.x }, limit_y_{ cfg.limit.y }, limit_yaw_{ cfg.limit.yaw }, //
+        posture_trajectory_{ .pd    = { MITPD(cfg.posture_error_pd_cfg.vx),
+                                        MITPD(cfg.posture_error_pd_cfg.vy),
+                                        MITPD(cfg.posture_error_pd_cfg.wz) },
+                             .curve = { velocity_profile::SCurveProfile(cfg.limit.x, 0, 0, 0, 0),
+                                        velocity_profile::SCurveProfile(cfg.limit.y, 0, 0, 0, 0),
+                                        velocity_profile::SCurveProfile(cfg.limit.yaw, 0, 0, 0, 0) }
+
+        }
+    {
+    }
+
+    bool setTargetPostureInWorld(const Posture& absolute_target)
+    {
+        osMutexAcquire(lock_, osWaitForever);
+
+        // copy 当前位置和速度
+        const auto [x, y, yaw]  = this->posture().in_world;
+        const auto [vx, vy, wz] = this->velocity().in_world;
+
+        float ax = 0, ay = 0, ayaw = 0;
+        if (ctrl_mode_ == CtrlMode::Posture)
+        {
+            ax   = posture_trajectory_.curve.x.CalcA(posture_trajectory_.now);
+            ay   = posture_trajectory_.curve.y.CalcA(posture_trajectory_.now);
+            ayaw = posture_trajectory_.curve.yaw.CalcA(posture_trajectory_.now);
+        }
+        // 初始化 S 型曲线
+        // 衔接当前位置，速度，如果之前是位置控制还会衔接加速度
+        const velocity_profile::SCurveProfile //
+                curve_x(limit_x_, x, vx, ax, absolute_target.x),
+                curve_y(limit_y_, y, vy, ay, absolute_target.y),
+                curve_yaw(limit_yaw_, yaw, wz, ayaw, absolute_target.yaw);
+
+        if (!curve_x.success() || !curve_y.success() || !curve_yaw.success())
+            return false;
+
+        float total_time = std::fmaxf(curve_x.getTotalTime(),
+                                      std::fmaxf(curve_y.getTotalTime(), curve_yaw.getTotalTime()));
+
+        const uint32_t saved = isr_lock(); // 写入过程加中断锁
+
+        posture_trajectory_.now        = 0;
+        posture_trajectory_.total_time = total_time;
+
+        posture_trajectory_.curve.x   = curve_x;
+        posture_trajectory_.curve.y   = curve_y;
+        posture_trajectory_.curve.yaw = curve_yaw;
+
+        ctrl_mode_ = CtrlMode::Posture;
+
+        isr_unlock(saved);
+
+        osMutexRelease(lock_);
+        return true;
+    }
+
+    bool setTargetPostureInBody(const Posture& relative_target)
+    {
+        osMutexAcquire(lock_, osWaitForever);
+        const auto absolute_target = this->loc().BodyPosture2WorldPosture(relative_target);
+        osMutexRelease(lock_);
+
+        return setTargetPostureInWorld(absolute_target);
+    }
+
+    [[nodiscard]] bool isTrajectoryFinished() const
+    {
+        return posture_trajectory_.now >= posture_trajectory_.total_time;
+    }
+
+    void waitTrajectoryFinish() const
+    {
+        while (!isTrajectoryFinished())
+            osDelay(1);
+    }
+
+    void setVelocityInWorld(const Velocity& world_velocity, bool target_in_world)
+    {
+        osMutexAcquire(lock_, osWaitForever);
+        const auto [vx, vy, wz] = this->WorldVelocity2BodyVelocity(world_velocity);
+
+        const uint32_t saved = isr_lock(); // 写入过程加中断锁
+
+        velocity_ref_.target_in_world = target_in_world;
+        velocity_ref_.in_world.vx     = world_velocity.vx;
+        velocity_ref_.in_world.vy     = world_velocity.vy;
+        velocity_ref_.in_world.wz     = world_velocity.wz;
+        velocity_ref_.in_body.vx      = vx;
+        velocity_ref_.in_body.vy      = vy;
+        velocity_ref_.in_body.wz      = wz;
+
+        ctrl_mode_ = CtrlMode::Velocity;
+
+        isr_unlock(saved);
+
+        osMutexRelease(lock_);
+    }
+
+    void setVelocityInBody(const Velocity& body_velocity, bool target_in_world)
+    {
+        osMutexAcquire(lock_, osWaitForever);
+        const auto [vx, vy, wz] = this->BodyVelocity2WorldVelocity(body_velocity);
+
+        const uint32_t saved = isr_lock(); // 写入过程加中断锁
+
+        velocity_ref_.target_in_world = target_in_world;
+        velocity_ref_.in_body.vx      = body_velocity.vx;
+        velocity_ref_.in_body.vy      = body_velocity.vy;
+        velocity_ref_.in_body.wz      = body_velocity.wz;
+        velocity_ref_.in_world.vx     = vx;
+        velocity_ref_.in_world.vy     = vy;
+        velocity_ref_.in_world.wz     = wz;
+
+        ctrl_mode_ = CtrlMode::Velocity;
+
+        isr_unlock(saved);
+        osMutexRelease(lock_);
+    }
+
+    void stop()
+    {
+        osMutexAcquire(lock_, osWaitForever);
+        const uint32_t saved = isr_lock();
+
+        ctrl_mode_ = CtrlMode::Stopped;
+
+        posture_trajectory_.p_ref_curr_ = this->posture().in_world;
+        posture_trajectory_.v_ref_curr_ = { 0, 0, 0 };
+
+        isr_unlock(saved);
+        osMutexRelease(lock_);
+    }
+
+    void profileUpdate(float dt)
+    {
+        if (!this->enabled() || ctrl_mode_ != CtrlMode::Posture)
+            return;
+
+        // 推进曲线
+        const float now               = this->posture_trajectory_.now + dt;
+        this->posture_trajectory_.now = now;
+
+        // 计算前馈速度
+        this->posture_trajectory_.v_ref_curr_ = { .vx = posture_trajectory_.curve.x.CalcV(now),
+                                                  .vy = posture_trajectory_.curve.y.CalcV(now),
+                                                  .wz = posture_trajectory_.curve.yaw.CalcV(now) };
+
+        // 计算当前目标
+        this->posture_trajectory_.p_ref_curr_ = { .x   = posture_trajectory_.curve.x.CalcX(now),
+                                                  .y   = posture_trajectory_.curve.y.CalcX(now),
+                                                  .yaw = posture_trajectory_.curve.yaw.CalcX(now) };
+
+        apply_position_velocity();
+    }
+
+    void errorUpdate()
+    {
+        if (!this->enabled() ||
+            !(ctrl_mode_ == CtrlMode::Posture || ctrl_mode_ == CtrlMode::Stopped))
+            return;
+
+        // 使用 pd 控制器跟随当前目标
+        posture_trajectory_.pd.vx.calc(posture_trajectory_.p_ref_curr_.x,
+                                       this->posture().in_world.x,
+                                       posture_trajectory_.v_ref_curr_.vx,
+                                       this->velocity().in_world.vx);
+        posture_trajectory_.pd.vy.calc(posture_trajectory_.p_ref_curr_.y,
+                                       this->posture().in_world.y,
+                                       posture_trajectory_.v_ref_curr_.vy,
+                                       this->velocity().in_world.vy);
+        posture_trajectory_.pd.wz.calc(posture_trajectory_.p_ref_curr_.yaw,
+                                       this->posture().in_world.yaw,
+                                       posture_trajectory_.v_ref_curr_.wz,
+                                       this->velocity().in_world.wz);
+        apply_position_velocity();
+    }
+
+    void controllerUpdate()
+    {
+        if (!this->enabled())
+            return;
+
+        if (ctrl_mode_ == CtrlMode::Velocity)
+            update_velocity_control();
+        this->velocityControllerUpdate();
+    }
+
+    bool enable()
+    {
+        stop();
+        return Chassis::enable();
+    }
+
+    void setWorldFromCurrent()
+    {
+        if (this->isOpsEnabled())
+            return;
+        osMutexAcquire(lock_, osWaitForever);
+        const auto saved = isr_lock();
+        this->applySetWorldFromCurrent();
+        isr_unlock(saved);
+        osMutexRelease(lock_);
+    }
+
+private:
+    osMutexId_t lock_;
+
+    CtrlMode ctrl_mode_{ CtrlMode::Stopped }; ///< 当前控制模式
+
+    AxisLimit limit_x_;
+    AxisLimit limit_y_;
+    AxisLimit limit_yaw_;
+
+    struct
+    {
+        volatile bool target_in_world; ///< 速度是否相对于世界坐标系不变
+        Velocity      in_world;        ///< 世界坐标系下速度
+        Velocity      in_body;         ///< 车体坐标系下速度
+    } velocity_ref_;
+
+    struct
+    {
+        float now{};        ///< 当前执行时间
+        float total_time{}; ///< 总执行时间
+
+        struct
+        {
+            MITPD vx; ///< x 速度 PD 控制器
+            MITPD vy; ///< y 速度 PD 控制器
+            MITPD wz; ///< 角速度 PD 控制器
+        } pd;
+
+        struct
+        {
+            velocity_profile::SCurveProfile x;
+            velocity_profile::SCurveProfile y;
+            velocity_profile::SCurveProfile yaw;
+        } curve;
+
+        Posture  p_ref_curr_{};
+        Velocity v_ref_curr_{};
+    } posture_trajectory_;
+
+private:
+    void update_velocity_control()
+    {
+        if (velocity_ref_.target_in_world)
+        { // 如果基于世界坐标计算速度，则需要转为车身坐标系，并应用到底盘驱动器
+            velocity_ref_.in_body = this->loc().WorldVelocity2BodyVelocity(velocity_ref_.in_world);
+            // 进行修正 1e-3f 为更新间隔，此处发现前馈并没有什么精度优化，暂时不做前馈
+            // const float              beta     = DEG2RAD(0.5f * velocity_.in_body.wz * 1e-3f);
+            // const float              cot_beta = 1.0f / tanf(beta);
+            // const Chassis_Velocity_t temp_velocity = {
+            //     .vx = beta * (velocity_.in_body.vx * cot_beta +
+            //     velocity_.in_body.vy), .vy = beta * (velocity_.in_body.vy * cot_beta
+            //     - velocity_.in_body.vx), .wz = velocity_.in_body.wz
+            // };
+            // ChassisDriver_ApplyVelocity(&driver_,
+            //                             temp_velocity.vx,
+            //                             temp_velocity.vy,
+            //                             temp_velocity.wz);
+        }
+        else
+        {
+            // 直接应用速度
+        }
+        this->applyVelocity(velocity_ref_.in_body);
+    }
+
+    void apply_position_velocity()
+    {
+        // 叠加前馈和 pd 输出
+        const Velocity velocity_in_world = {
+            posture_trajectory_.v_ref_curr_.vx + posture_trajectory_.pd.vx.getOutput(),
+            posture_trajectory_.v_ref_curr_.vy + posture_trajectory_.pd.vy.getOutput(),
+            posture_trajectory_.v_ref_curr_.wz + posture_trajectory_.pd.wz.getOutput(),
+        };
+
+        // 将世界坐标系速度转换为底盘坐标系速度
+        const Velocity body_velocity = this->loc().WorldVelocity2BodyVelocity(velocity_in_world);
+
+        // 应用速度
+        this->applyVelocity(body_velocity);
+    }
+};
+
+} // namespace chassis::controller
diff --git a/Controller/Master/cpkg.toml b/Controller/Master/cpkg.toml
new file mode 100644
index 0000000..787a8c4
--- /dev/null
+++ b/Controller/Master/cpkg.toml
@@ -0,0 +1,4 @@
+name = "ControllerMaster"
+pkgname = "Chassis::ControllerMaster"
+version = "0.1.0"
+dependencies = ["Chassis::Core", "VelocityProfile::SCurve"]
diff --git a/Controller/Slave/CMakeLists.txt b/Controller/Slave/CMakeLists.txt
new file mode 100644
index 0000000..de625d2
--- /dev/null
+++ b/Controller/Slave/CMakeLists.txt
@@ -0,0 +1,12 @@
+add_library(__Chassis_ControllerSlave INTERFACE)
+
+target_include_directories(__Chassis_ControllerSlave INTERFACE
+    ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+# link dependencies if any
+target_link_libraries(__Chassis_ControllerSlave INTERFACE Chassis::Core)
+target_link_libraries(__Chassis_ControllerSlave INTERFACE libs::RingBuffer)
+
+# alias for external use
+add_library(Chassis::ControllerSlave ALIAS __Chassis_ControllerSlave)
diff --git a/Controller/Slave.hpp b/Controller/Slave/Slave.hpp
similarity index 96%
rename from Controller/Slave.hpp
rename to Controller/Slave/Slave.hpp
index 1f165a7..763af2b 100644
--- a/Controller/Slave.hpp
+++ b/Controller/Slave/Slave.hpp
@@ -102,8 +102,8 @@ void Slave<Chassis, BufferCapacity>::errorUpdate()
     if (!this->enabled())
         return;
 
-    const auto& [x, y, yaw]  = this->WorldPosture2BodyPosture(p_ref_);
-    const auto& [vx, vy, wz] = this->WorldVelocity2BodyVelocity(v_ref_);
+    const auto& [x, y, yaw]  = this->loc().WorldPosture2BodyPosture(p_ref_);
+    const auto& [vx, vy, wz] = this->loc().WorldVelocity2BodyVelocity(v_ref_);
     pd_vx_.calc(x, 0, vx, this->velocity().in_body.vx);
     pd_vy_.calc(y, 0, vy, this->velocity().in_body.vy);
     pd_wz_.calc(yaw, 0, wz, this->velocity().in_body.wz);
diff --git a/Controller/Slave/cpkg.toml b/Controller/Slave/cpkg.toml
new file mode 100644
index 0000000..6a5c908
--- /dev/null
+++ b/Controller/Slave/cpkg.toml
@@ -0,0 +1,4 @@
+name = "ControllerSlave"
+pkgname = "Chassis::ControllerSlave"
+version = "0.1.0"
+dependencies = ["Chassis::Core", "libs::RingBuffer"]
diff --git a/Controller/cpkg.toml b/Controller/cpkg.toml
deleted file mode 100644
index de59263..0000000
--- a/Controller/cpkg.toml
+++ /dev/null
@@ -1,4 +0,0 @@
-name = "Controller"
-pkgname = "Chassis::Controller"
-version = "0.1.0"
-dependencies = ["Chassis::Core", "libs::RingBuffer", "VelocityProfile::SCurve"]
diff --git a/Core/CMakeLists.txt b/Core/CMakeLists.txt
index b83281a..aab9d39 100644
--- a/Core/CMakeLists.txt
+++ b/Core/CMakeLists.txt
@@ -1,10 +1,11 @@
 add_library(ChassisCore STATIC
-    "./IChassis.cpp"
+        "./IChassis.cpp"
+        "ILoc.cpp"
 )
 
 target_include_directories(ChassisCore
-    PUBLIC
-    ${CMAKE_CURRENT_SOURCE_DIR}
+        PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}
 )
 
 # link dependencies if any
diff --git a/Core/IChassis.cpp b/Core/IChassis.cpp
index 4ccfc81..a8cfe9b 100644
--- a/Core/IChassis.cpp
+++ b/Core/IChassis.cpp
@@ -7,159 +7,9 @@
 #include <cassert>
 #include <cmath>
 
-#define DEG2RAD(__DEG__) ((__DEG__) * (float)3.14159265358979323846f / 180.0f)
+#define DEG2RAD(__DEG__) ((__DEG__) * (float) 3.14159265358979323846f / 180.0f)
 
 namespace chassis
 {
-IChassis::Velocity IChassis::WorldVelocity2BodyVelocity(const Velocity& velocity_in_world) const
-{
-    const float _sin_yaw = sinf(DEG2RAD(-posture_.in_world.yaw)),
-                _cos_yaw = cosf(DEG2RAD(-posture_.in_world.yaw));
-
-    const Velocity velocity_in_body = {
-        .vx = velocity_in_world.vx * _cos_yaw - velocity_in_world.vy * _sin_yaw,
-        .vy = velocity_in_world.vx * _sin_yaw + velocity_in_world.vy * _cos_yaw,
-        .wz = velocity_in_world.wz
-    };
-
-    return velocity_in_body;
-}
-
-IChassis::Velocity IChassis::BodyVelocity2WorldVelocity(const Velocity& velocity_in_body) const
-{
-    const float sin_yaw = sinf(DEG2RAD(posture_.in_world.yaw)),
-                cos_yaw = cosf(DEG2RAD(posture_.in_world.yaw));
-
-    const Velocity velocity_in_world = {
-        .vx = velocity_in_body.vx * cos_yaw - velocity_in_body.vy * sin_yaw,
-        .vy = velocity_in_body.vx * sin_yaw + velocity_in_body.vy * cos_yaw,
-        .wz = velocity_in_body.wz,
-    };
-
-    return velocity_in_world;
-}
-
-IChassis::Posture IChassis::WorldPosture2BodyPosture(const Posture& posture_in_world) const
-{
-    const float _sin_yaw = sinf(DEG2RAD(-posture_.in_world.yaw)),
-                _cos_yaw = cosf(DEG2RAD(-posture_.in_world.yaw));
-
-    const float tx = posture_in_world.x - posture_.in_world.x;
-    const float ty = posture_in_world.y - posture_.in_world.y;
-
-    const Posture posture_in_body = {
-        .x   = tx * _cos_yaw - ty * _sin_yaw,
-        .y   = tx * _sin_yaw + ty * _cos_yaw,
-        .yaw = posture_in_world.yaw - posture_.in_world.yaw,
-    };
-
-    return posture_in_body;
-}
-
-IChassis::Posture IChassis::BodyPosture2WorldPosture(const Posture& posture_in_body) const
-{
-    const float sin_yaw            = sinf(DEG2RAD(posture_.in_world.yaw)),
-                cos_yaw            = cosf(DEG2RAD(posture_.in_world.yaw));
-    const Posture posture_in_world = {
-        .x   = posture_in_body.x * cos_yaw - posture_in_body.y * sin_yaw + posture_.in_world.x,
-        .y   = posture_in_body.x * sin_yaw + posture_in_body.y * cos_yaw + posture_.in_world.y,
-        .yaw = posture_in_body.yaw + posture_.in_world.yaw,
-    };
-
-    return posture_in_world;
-}
-
-/**
- * 更新底盘反馈
- * @param dt 更新间隔 (unit: s)
- */
-void IChassis::feedbackUpdate(const float dt)
-{
-    if (!enabled() || dt <= 0)
-        return;
-
-    if (isOpsEnabled())
-    {
-        assert(feedback_.x && feedback_.y && feedback_.yaw);
-        // 直接读取 OPS
-        posture_.in_world.x   = *feedback_.x;
-        posture_.in_world.y   = *feedback_.y;
-        posture_.in_world.yaw = *feedback_.yaw;
-        return;
-    }
-    // 通过里程计或者运动学解算计算位置
-    // 通过速度积分还是直接读取底盘
-    const bool use_vel = kinematicsType() == WheeledKinematicsType::VelocityIntegrated;
-    // 速度反馈
-    const float vx = feedback_.vx != nullptr ? *feedback_.vx : forwardGetVx();
-    const float vy = feedback_.vy != nullptr ? *feedback_.vy : forwardGetVy();
-    const float wz = feedback_.wz != nullptr ? *feedback_.wz : forwardGetWz();
-    // 计算里程增量
-    float dx, dy;
-    if (feedback_.sx || !use_vel)
-    {
-        // use measured or estimated position
-        const float sx    = feedback_.sx ? *feedback_.sx : forwardGetX();
-        dx                = sx - last_feedback_.sx;
-        last_feedback_.sx = sx;
-    }
-    else
-    {
-        // fallback: integrate velocity
-        dx = 0.5f * (vx + last_feedback_.vx) * dt;
-        last_feedback_.sx += dx;
-    }
-    if (feedback_.sy || !use_vel)
-    {
-        // use measured or estimated position
-        const float sy    = feedback_.sy ? *feedback_.sy : forwardGetY();
-        dy                = sy - last_feedback_.sy;
-        last_feedback_.sy = sy;
-    }
-    else
-    {
-        // fallback: integrate velocity
-        dy = 0.5f * (vy + last_feedback_.vy) * dt;
-        last_feedback_.sy += dy;
-    }
-    // 计算平均 yaw，作为积分方向
-    const float& yaw_prev = last_feedback_.yaw;
-
-    const float yaw = feedback_.yaw ? *feedback_.yaw                              // 直接读取陀螺仪
-                      : use_vel ? yaw_prev + 0.5f * (wz + last_feedback_.wz) * dt // 用平均速度积分
-                                : forwardGetYaw(); // 直接读取底盘解算
-
-    const float ave_yaw              = (yaw + yaw_prev) * 0.5f;
-    const float ave_yaw_in_world_rad = DEG2RAD(ave_yaw - world_.posture.yaw);
-    last_feedback_.yaw               = yaw;
-
-    // 积分
-    posture_.in_world.x += dx * cosf(ave_yaw_in_world_rad) - dy * sinf(ave_yaw_in_world_rad);
-    posture_.in_world.y += dx * sinf(ave_yaw_in_world_rad) + dy * cosf(ave_yaw_in_world_rad);
-    posture_.in_world.yaw = yaw - world_.posture.yaw;
-    // 更新速度反馈
-    last_feedback_.vx = vx, last_feedback_.vy = vy, last_feedback_.wz = wz;
-
-    // 更新底盘速度
-    velocity_.in_body.vx = vx;
-    velocity_.in_body.vy = vy;
-    velocity_.in_body.wz = wz;
-    velocity_.in_world   = BodyVelocity2WorldVelocity(velocity_.in_body);
-}
-
-void IChassis::applySetWorldFromCurrent()
-{
-    // 启用 OPS 的情况下 world 由 OPS 管理
-    if (isOpsEnabled())
-        return;
-    world_.posture.x += posture_.in_world.x;
-    world_.posture.y += posture_.in_world.y;
-    world_.posture.yaw += posture_.in_world.yaw;
-    posture_.in_world.x   = 0.0f;
-    posture_.in_world.y   = 0.0f;
-    posture_.in_world.yaw = 0.0f;
-    velocity_.in_world    = velocity_.in_body;
-}
 
-IChassis::IChassis(const Config& cfg) : feedback_(cfg.feedback_source) {}
 } // namespace chassis
\ No newline at end of file
diff --git a/Core/IChassis.hpp b/Core/IChassis.hpp
index bb2801a..367cf5d 100644
--- a/Core/IChassis.hpp
+++ b/Core/IChassis.hpp
@@ -4,143 +4,53 @@
  * @date    2026-01-31
  */
 #pragma once
+#include "ILoc.hpp"
 namespace chassis
 {
 
 class IChassis
 {
 public:
-    virtual ~IChassis() = default;
+    using Velocity = chassis_loc::ILoc::Velocity;
+    using Posture  = chassis_loc::ILoc::Posture;
 
-    struct Velocity
+    virtual ~IChassis()                  = default;
+    [[nodiscard]] virtual bool enable()  = 0;
+    virtual void               disable() = 0;
+    [[nodiscard]] virtual bool enabled() const
     {
-        float vx; ///< 指向车体前方 (unit: m/s)
-        float vy; ///< 指向车体左侧 (unit: m/s)
-        float wz; ///< 向上（逆时针）为正 (unit: deg/s)
-    };
-
-    struct Posture
+        return false;
+    }
+    [[nodiscard]] const chassis_loc::ILoc& loc() const
     {
-        float x;   ///< 指向车体前方 (unit: m)
-        float y;   ///< 指向车体左侧 (unit: m)
-        float yaw; ///< 向上（逆时针）为正 (unit: deg)
-    };
+        return loc_;
+    }
 
-    /**
-     * 车体运动反馈，用于闭环控制
-     *
-     * 指针指向反馈来源。为 NULL 表示没有数据来源，在必要情况下会通过运动学解算开环控制
-     */
-    struct Feedback
+    void encoderUpdate(const float dt)
     {
-        // 相对于车体，该数据一般来源于 IMU
-        const float* vx = nullptr; ///< 指向车体前方 (unit: m/s)
-        const float* vy = nullptr; ///< 指向车体左侧 (unit: m/s)
-        const float* wz = nullptr; ///< 车体角速度 (unit: deg/s)
-
-        // 该数据一般来源于里程计，因为是计算增量，所以没有相对一说
-        const float* sx = nullptr; ///< x 方向里程计读数 (unit: m)
-        const float* sy = nullptr; ///< y 方向里程计读数 (unit: m)
-
-        // 该数据一般来源于全向定位平台（OPS），相对于 OPS World。这组数据的优先级高于 sx, sy
-        // 如果要使用 OPS，世界坐标系的位置由 OPS 管理，机体不再执行换算。
-        // 此时 Chassis_SetWorldFromCurrent 将不起作用
-        // 当 x, y, yaw 都不为 nullptr 时，启用 OPS，请保证三个量都来自 OPS
-        const float* x = nullptr; ///< x 方向位置 (unit: m)
-        const float* y = nullptr; ///< y 方向位置 (unit: m)
-        // 该数据一般来源于陀螺仪（或码盘），底盘不需要管相对于谁
-        const float* yaw = nullptr; ///< 车体角度 (unit: deg)
-    };
+        loc_.updateEncoder(forwardGetVelocity(), dt);
+    }
 
-    struct Config
+    [[nodiscard]] const auto& posture() const
     {
-        Feedback feedback_source;
-    };
-
-    [[nodiscard]] Velocity WorldVelocity2BodyVelocity(const Velocity& velocity_in_world) const;
-    [[nodiscard]] Velocity BodyVelocity2WorldVelocity(const Velocity& velocity_in_body) const;
-    [[nodiscard]] Posture  WorldPosture2BodyPosture(const Posture& posture_in_world) const;
-    [[nodiscard]] Posture  BodyPosture2WorldPosture(const Posture& posture_in_body) const;
-
-    void feedbackUpdate(float dt);
-
-    [[nodiscard]] bool isOpsEnabled() const
+        return loc().posture();
+    }
+    [[nodiscard]] const auto& velocity() const
     {
-        return feedback_.x != nullptr && feedback_.y != nullptr && feedback_.yaw != nullptr;
+        return loc().velocity();
     }
 
-    [[nodiscard]] virtual bool enable() = 0;
-
-    virtual void disable() = 0;
-
-    [[nodiscard]] virtual bool enabled() const { return false; }
-
-    [[nodiscard]] const auto& velocity() const { return velocity_; }
-    [[nodiscard]] const auto& posture() const { return posture_; }
-
 protected:
-    explicit IChassis(const Config& cfg);
-
-    enum class WheeledKinematicsType
-    {
-        /**
-         * nonholonomic rolling-without-slipping constraint
-         * 直接用轮子位移就能计算出总位移的底盘类型，如 Mecanum4, Omni4
-         */
-        RollingConstrained,
-        /**
-         * pose via time integration of velocity
-         * 只能由速度量积分得到总位移的底盘类型，如 Steering4
-         */
-        VelocityIntegrated
-    };
-
-    [[nodiscard]] virtual WheeledKinematicsType kinematicsType() const = 0;
+    friend chassis_loc::ILoc;
+    explicit IChassis(chassis_loc::ILoc& loc) : loc_(loc) {}
 
     virtual void applyVelocity(const Velocity& velocity) = 0;
+    virtual void velocityControllerUpdate()              = 0;
 
-    virtual void velocityControllerUpdate() = 0;
-
-    virtual float forwardGetX()   = 0;
-    virtual float forwardGetY()   = 0;
-    virtual float forwardGetYaw() = 0;
-    virtual float forwardGetVx()  = 0;
-    virtual float forwardGetVy()  = 0;
-    virtual float forwardGetWz()  = 0;
-
-    void applySetWorldFromCurrent();
+    virtual Velocity forwardGetVelocity() = 0;
 
 private:
-    struct
-    {
-        Velocity in_world;
-        Velocity in_body;
-    } velocity_{};
-
-    struct
-    {
-        /**
-         * feedback_yaw - world_yaw = body_yaw
-         */
-        Posture in_world{}; ///< 车身位置（世界坐标系与车身坐标系的变换关系）、
-    } posture_;
-
-    struct
-    {
-        Posture posture; ///< 世界坐标系位置（相对于反馈）
-    } world_{};
-
-    Feedback feedback_;
-
-    struct
-    {
-        float vx;
-        float vy;
-        float wz;
-        float sx;
-        float sy;
-        float yaw;
-    } last_feedback_{};
+    chassis_loc::ILoc& loc_;
 };
 
 } // namespace chassis
diff --git a/Core/ILoc.cpp b/Core/ILoc.cpp
new file mode 100644
index 0000000..4c2eafe
--- /dev/null
+++ b/Core/ILoc.cpp
@@ -0,0 +1,76 @@
+/**
+ * @file    ILoc.cpp
+ * @author  syhanjin
+ * @date    2026-03-07
+ * @brief   Brief description of the file
+ *
+ * Detailed description (optional).
+ *
+ */
+#include "ILoc.hpp"
+#include <cmath>
+
+#define DEG2RAD(__DEG__) ((__DEG__) * (float) 3.14159265358979323846f / 180.0f)
+
+namespace chassis_loc
+{
+
+ILoc::Velocity ILoc::WorldVelocity2BodyVelocity(const Velocity& velocity_in_world) const
+{
+    const float _sin_yaw = sinf(DEG2RAD(-posture_.in_world.yaw)),
+                _cos_yaw = cosf(DEG2RAD(-posture_.in_world.yaw));
+
+    const Velocity velocity_in_body = {
+        .vx = velocity_in_world.vx * _cos_yaw - velocity_in_world.vy * _sin_yaw,
+        .vy = velocity_in_world.vx * _sin_yaw + velocity_in_world.vy * _cos_yaw,
+        .wz = velocity_in_world.wz
+    };
+
+    return velocity_in_body;
+}
+
+ILoc::Velocity ILoc::BodyVelocity2WorldVelocity(const Velocity& velocity_in_body) const
+{
+    const float sin_yaw = sinf(DEG2RAD(posture_.in_world.yaw)),
+                cos_yaw = cosf(DEG2RAD(posture_.in_world.yaw));
+
+    const Velocity velocity_in_world = {
+        .vx = velocity_in_body.vx * cos_yaw - velocity_in_body.vy * sin_yaw,
+        .vy = velocity_in_body.vx * sin_yaw + velocity_in_body.vy * cos_yaw,
+        .wz = velocity_in_body.wz,
+    };
+
+    return velocity_in_world;
+}
+
+ILoc::Posture ILoc::WorldPosture2BodyPosture(const Posture& posture_in_world) const
+{
+    const float _sin_yaw = sinf(DEG2RAD(-posture_.in_world.yaw)),
+                _cos_yaw = cosf(DEG2RAD(-posture_.in_world.yaw));
+
+    const float tx = posture_in_world.x - posture_.in_world.x;
+    const float ty = posture_in_world.y - posture_.in_world.y;
+
+    const Posture posture_in_body = {
+        .x   = tx * _cos_yaw - ty * _sin_yaw,
+        .y   = tx * _sin_yaw + ty * _cos_yaw,
+        .yaw = posture_in_world.yaw - posture_.in_world.yaw,
+    };
+
+    return posture_in_body;
+}
+
+ILoc::Posture ILoc::BodyPosture2WorldPosture(const Posture& posture_in_body) const
+{
+    const float sin_yaw            = sinf(DEG2RAD(posture_.in_world.yaw)),
+                cos_yaw            = cosf(DEG2RAD(posture_.in_world.yaw));
+    const Posture posture_in_world = {
+        .x   = posture_in_body.x * cos_yaw - posture_in_body.y * sin_yaw + posture_.in_world.x,
+        .y   = posture_in_body.x * sin_yaw + posture_in_body.y * cos_yaw + posture_.in_world.y,
+        .yaw = posture_in_body.yaw + posture_.in_world.yaw,
+    };
+
+    return posture_in_world;
+}
+
+} // namespace chassis_loc
\ No newline at end of file
diff --git a/Core/ILoc.hpp b/Core/ILoc.hpp
new file mode 100644
index 0000000..fc075cd
--- /dev/null
+++ b/Core/ILoc.hpp
@@ -0,0 +1,65 @@
+/**
+ * @file    ILoc.hpp
+ * @author  syhanjin
+ * @date    2026-03-07
+ */
+#pragma once
+
+namespace chassis_loc
+{
+
+class ILoc
+{
+public:
+    struct Velocity
+    {
+        float vx; ///< 指向车体前方 (unit: m/s)
+        float vy; ///< 指向车体左侧 (unit: m/s)
+        float wz; ///< 向上（逆时针）为正 (unit: deg/s)
+    };
+
+    struct Posture
+    {
+        float x;   ///< 指向车体前方 (unit: m)
+        float y;   ///< 指向车体左侧 (unit: m)
+        float yaw; ///< 向上（逆时针）为正 (unit: deg)
+    };
+
+    explicit ILoc() = default;
+    virtual ~ILoc() = default;
+
+    [[nodiscard]] const auto& velocity() const
+    {
+        return velocity_;
+    }
+
+    [[nodiscard]] const auto& posture() const
+    {
+        return posture_;
+    }
+
+    [[nodiscard]] Velocity WorldVelocity2BodyVelocity(const Velocity& velocity_in_world) const;
+    [[nodiscard]] Velocity BodyVelocity2WorldVelocity(const Velocity& velocity_in_body) const;
+    [[nodiscard]] Posture  WorldPosture2BodyPosture(const Posture& posture_in_world) const;
+    [[nodiscard]] Posture  BodyPosture2WorldPosture(const Posture& posture_in_body) const;
+
+    virtual void updateEncoder(const Velocity& velocity, const float dt)
+    {
+        (void) velocity;
+        (void) dt;
+    }
+
+protected:
+    struct
+    {
+        Velocity in_body{};
+        Velocity in_world{};
+    } velocity_{};
+
+    struct
+    {
+        Posture in_world{};
+    } posture_{};
+};
+
+} // namespace chassis_loc
diff --git a/Localization/CMakeLists.txt b/Localization/CMakeLists.txt
new file mode 100644
index 0000000..5c47e6d
--- /dev/null
+++ b/Localization/CMakeLists.txt
@@ -0,0 +1 @@
+add_subdirectory(JustEncoder)
\ No newline at end of file
diff --git a/Localization/JustEncoder/CMakeLists.txt b/Localization/JustEncoder/CMakeLists.txt
new file mode 100644
index 0000000..85d52ae
--- /dev/null
+++ b/Localization/JustEncoder/CMakeLists.txt
@@ -0,0 +1,14 @@
+add_library(ChassisLocalizationJustEncoder STATIC
+        "JustEncoder.cpp"
+)
+
+target_include_directories(ChassisLocalizationJustEncoder
+        PUBLIC
+        ..
+)
+
+# link dependencies if any
+target_link_libraries(ChassisLocalizationJustEncoder PUBLIC Chassis::Core)
+
+# alias for external use
+add_library(ChassisLocalization::JustEncoder ALIAS ChassisLocalizationJustEncoder)
diff --git a/Localization/JustEncoder/JustEncoder.cpp b/Localization/JustEncoder/JustEncoder.cpp
new file mode 100644
index 0000000..f61d5d8
--- /dev/null
+++ b/Localization/JustEncoder/JustEncoder.cpp
@@ -0,0 +1,32 @@
+/**
+ * @file    JustEncoder.cpp
+ * @author  syhanjin
+ * @date    2026-03-07
+ */
+#include "JustEncoder.hpp"
+#include <cmath>
+
+#define DEG2RAD(__DEG__) ((__DEG__) * (float) 3.14159265358979323846f / 180.0f)
+
+namespace chassis_loc
+{
+void JustEncoder::updateEncoder(const Velocity& velocity, const float dt)
+{
+    // 速度反馈
+    const auto [vx, vy, wz] = velocity;
+    // 速度积分
+    const float dx = 0.5f * (vx + velocity_.in_body.vx) * dt;
+    const float dy = 0.5f * (vy + velocity_.in_body.vy) * dt;
+    // 计算平均 yaw，作为积分方向
+    const float& yaw_prev    = posture_.in_world.yaw;
+    const float  yaw         = yaw_prev + 0.5f * (wz + velocity_.in_body.wz) * dt; // 用平均速度积分
+    const float  ave_yaw_rad = DEG2RAD((yaw + yaw_prev) * 0.5f);
+    // 积分
+    posture_.in_world.x += dx * cosf(ave_yaw_rad) - dy * sinf(ave_yaw_rad);
+    posture_.in_world.y += dx * sinf(ave_yaw_rad) + dy * cosf(ave_yaw_rad);
+    posture_.in_world.yaw = yaw;
+    // 更新速度反馈
+    velocity_.in_body  = velocity;
+    velocity_.in_world = BodyVelocity2WorldVelocity(velocity);
+}
+} // namespace chassis_loc
\ No newline at end of file
diff --git a/Localization/JustEncoder/JustEncoder.hpp b/Localization/JustEncoder/JustEncoder.hpp
new file mode 100644
index 0000000..472dd0e
--- /dev/null
+++ b/Localization/JustEncoder/JustEncoder.hpp
@@ -0,0 +1,20 @@
+/**
+ * @file    JustEncoder.hpp
+ * @author  syhanjin
+ * @date    2026-03-07
+ */
+#pragma once
+
+#include "ILoc.hpp"
+
+namespace chassis_loc
+{
+
+class JustEncoder final : public ILoc
+{
+public:
+    explicit JustEncoder() : ILoc() {}
+    void updateEncoder(const Velocity& velocity, float dt) override;
+};
+
+} // namespace chassis_loc
diff --git a/Localization/JustEncoder/cpkg.toml b/Localization/JustEncoder/cpkg.toml
new file mode 100644
index 0000000..b31dcac
--- /dev/null
+++ b/Localization/JustEncoder/cpkg.toml
@@ -0,0 +1,4 @@
+name = "JustEncoder"
+pkgname = "ChassisLocalization::JustEncoder"
+version = "0.1.0"
+dependencies = ["Chassis::Core"]