Stop tracking the CMake build directory.

build/ was already listed in .gitignore but remained in the index; remove those artifacts so local builds stay untracked.

.gitignore

@@ -12,3 +12,4 @@
 # Built Visual Studio Code Extensions
 *.vsix
 
+build/

CMakeLists.txt

@@ -0,0 +1,78 @@
+cmake_minimum_required(VERSION 3.14)
+project(odomety VERSION 0.1.0 LANGUAGES CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+include(GNUInstallDirs)
+
+find_package(Boost REQUIRED)
+
+add_library(odom_differential
+  src/odom_differential.cpp
+)
+target_include_directories(odom_differential
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+    ${Boost_INCLUDE_DIRS}
+)
+target_compile_options(odom_differential PRIVATE -Wall -Wextra -Wpedantic)
+
+add_executable(odomety_demo
+  src/main.cpp
+)
+target_link_libraries(odomety_demo PRIVATE odom_differential)
+target_compile_options(odomety_demo PRIVATE -Wall -Wextra -Wpedantic)
+
+enable_testing()
+
+add_executable(odomety_test
+  test/odom_differential_test.cpp
+)
+target_link_libraries(odomety_test PRIVATE odom_differential)
+target_compile_options(odomety_test PRIVATE -Wall -Wextra -Wpedantic)
+
+add_test(NAME odom_differential_test COMMAND odomety_test)
+
+# ---------------------------------------------------------------------------
+# Install
+# ---------------------------------------------------------------------------
+install(TARGETS odom_differential odomety_demo
+  EXPORT odometyTargets
+  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+  RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+  INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+)
+
+install(DIRECTORY include/
+  DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+  FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+)
+
+install(EXPORT odometyTargets
+  FILE odometyTargets.cmake
+  NAMESPACE odomety::
+  DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/odomety
+)
+
+include(CMakePackageConfigHelpers)
+write_basic_package_version_file(
+  "${CMAKE_CURRENT_BINARY_DIR}/odometyConfigVersion.cmake"
+  VERSION ${PROJECT_VERSION}
+  COMPATIBILITY SameMajorVersion
+)
+
+file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/odometyConfig.cmake"
+"include(CMakeFindDependencyMacro)
+find_dependency(Boost REQUIRED)
+include(\"\${CMAKE_CURRENT_LIST_DIR}/odometyTargets.cmake\")
+")
+
+install(FILES
+  "${CMAKE_CURRENT_BINARY_DIR}/odometyConfig.cmake"
+  "${CMAKE_CURRENT_BINARY_DIR}/odometyConfigVersion.cmake"
+  DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/odomety
+)

README.md

@@ -1,2 +1,135 @@
 # odomety
 
+Thư viện C++ ước lượng odometry cho robot **differential drive** (2 bánh chủ động).
+
+Từ vận tốc góc bánh xe, **encoder ticks**, hoặc lệnh `v`/`ω`, thư viện tích phân pose 2D `(x, y, θ)` và vận tốc `(v, ω)` theo thời gian.
+
+## Tính năng
+
+- Cập nhật từ vận tốc góc bánh trái/phải `[rad/s]`
+- Cập nhật từ cumulative encoder ticks (`Encoder_Measurement`-style)
+- Cập nhật trực tiếp từ lệnh tuyến tính/góc `[m/s]`, `[rad/s]`
+- Tích phân exact (và Runge–Kutta bậc 2 khi gần đi thẳng)
+- Làm mượt vận tốc bằng rolling mean (Boost Accumulators)
+- Demo và unit test kèm theo
+
+## Yêu cầu
+
+- CMake ≥ 3.14
+- C++17
+- Boost (header: Accumulators, Function)
+
+## Cấu trúc
+
+```
+odomety/
+├── include/odometry/odom_differential.h
+├── src/odom_differential.cpp
+├── src/main.cpp                 # demo
+├── test/odom_differential_test.cpp
+└── CMakeLists.txt
+```
+
+## Build
+
+```bash
+cd odomety
+cmake -S . -B build
+cmake --build build
+```
+
+Chạy demo:
+
+```bash
+./build/odomety_demo
+```
+
+Chạy test:
+
+```bash
+cd build && ctest --output-on-failure
+# hoặc
+./build/odomety_test
+```
+
+## Cài đặt (tùy chọn)
+
+```bash
+sudo cmake --install build --prefix /path/to/install
+```
+
+Trong project khác:
+
+```cmake
+find_package(odomety REQUIRED)
+target_link_libraries(your_target PRIVATE odomety::odom_differential)
+```
+
+## Cách dùng
+
+### Từ vận tốc bánh
+
+```cpp
+#include <odometry/odom_differential.h>
+
+differential::Odometry odom(/*velocity_rolling_window_size=*/10);
+
+odom.setWheelParams(
+    /*wheel_separation=*/0.5,   // [m]
+    /*left_wheel_radius=*/0.09, // [m]
+    /*right_wheel_radius=*/0.09 // [m]
+);
+
+odom.init(/*time_sec=*/t0);
+odom.updateWheel(left_omega, right_omega, t);  // [rad/s]
+```
+
+### Từ encoder ticks (khuyến nghị khi có đếm xung)
+
+```cpp
+odom.setWheelParams(0.5, 0.09, 0.09);
+odom.setEncoderParams(/*ticks_per_revolution=*/4096);  // phía bánh, sau gearbox
+odom.init(t0);
+
+// Cumulative tick counts (unwrap overflow về int64 monotonic trước khi gọi)
+odom.updateTicks(left_ticks, right_ticks, t);  // lần đầu chỉ seed, trả về false
+
+double x = odom.getX();
+double y = odom.getY();
+double yaw = odom.getHeading();
+double v = odom.getLinear();
+double w = odom.getAngular();
+```
+
+Công thức nội bộ mỗi bước:
+
+- `Δs_L/R = 2π · r_L/R · Δn_L/R / N_L/R`
+- `Δs = (Δs_L + Δs_R) / 2`
+- `Δθ = (Δs_R − Δs_L) / b`
+
+### Từ lệnh vận tốc (open-loop)
+
+```cpp
+odom.updateOdom(linear_mps, angular_rps, t);
+```
+
+## API chính
+
+| Hàm | Mô tả |
+|---|---|
+| `init(time)` | Reset pose/vận tốc/tick seed và gắn timestamp |
+| `setWheelParams(sep, r_l, r_r)` | Khoảng cách bánh và bán kính |
+| `setEncoderParams(N)` / `setEncoderParams(N_l, N_r)` | CPR phía bánh |
+| `updateWheel(ω_l, ω_r, time)` | Cập nhật từ vận tốc góc bánh |
+| `updateTicks(n_l, n_r, time)` | Cập nhật từ cumulative encoder ticks |
+| `updateOdom(v, ω, time)` | Cập nhật từ lệnh vận tốc |
+| `getX/Y/Heading/Linear/Angular()` | Đọc trạng thái hiện tại |
+
+## Ghi chú
+
+- `updateWheel` / `updateTicks` trả về `false` nếu `dt < 0.0001` s.
+- `updateTicks`: lần gọi đầu sau `init` chỉ lưu tick trước đó (seed), không tích phân.
+- `updateTicks` trả về `false` nếu chưa gọi `setEncoderParams` (CPR ≤ 0).
+- Tick phải là đếm **phía bánh** (sau gearbox). Overflow counter phần cứng cần unwrap về `int64` monotonic.
+- Đơn vị thời gian là giây (epoch hoặc monotonic, miễn nhất quán).
+- Pose mặc định sau `init`: `(0, 0, 0)`.

Testing/Temporary/CTestCostData.txt

@@ -0,0 +1 @@
+---

Testing/Temporary/LastTest.log

@@ -0,0 +1,3 @@
+Start testing: Jul 13 13:27 +07
+----------------------------------------------------------
+End testing: Jul 13 13:27 +07

include/odometry/odom_differential.h

@@ -0,0 +1,204 @@
+#pragma once
+
+#include <cstdint>
+#include <math.h>
+#include <boost/accumulators/accumulators.hpp>
+#include <boost/accumulators/statistics/stats.hpp>
+#include <boost/accumulators/statistics/rolling_mean.hpp>
+#include <boost/function.hpp>
+
+namespace differential
+{
+  namespace bacc = boost::accumulators;
+
+  /**
+   * \brief The Odometry class handles odometry readings
+   * (2D pose and velocity with related timestamp)
+   */
+  class Odometry
+  {
+  public:
+
+    /// Integration function, used to integrate the odometry:
+    typedef boost::function<void(double, double)> IntegrationFunction;
+
+    /**
+     * \brief Constructor
+     * Timestamp will get the current time value
+     * Value will be set to zero
+     * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean
+     */
+    Odometry(size_t velocity_rolling_window_size = 10);
+
+    /**
+     * \brief Initialize the odometry
+     * \param time Current time
+     */
+    void init(const double &time);
+
+    /**
+     * \brief Updates the odometry class with latest wheel angular velocities
+     * \param left_vel  Left  wheel angular velocity [rad/s]
+     * \param right_vel Right wheel angular velocity [rad/s]
+     * \param time      Current time
+     * \return true if the odometry is actually updated
+     */
+    bool updateWheel(double left_vel, double right_vel, const double &time);
+
+    /**
+     * \brief Updates odometry from cumulative encoder tick counts
+     *
+     * Converts tick deltas to wheel arc lengths, then integrates pose with the
+     * same exact/RK2 path as updateWheel. The first call after init() only
+     * seeds the previous tick counts and returns false.
+     *
+     * Caller should unwrap hardware counter overflow into a monotonic int64
+     * cumulative count before calling.
+     *
+     * \param left_ticks  Cumulative left  wheel ticks (after gearbox, wheel side)
+     * \param right_ticks Cumulative right wheel ticks (after gearbox, wheel side)
+     * \param time        Current time [s]
+     * \return true if the odometry is actually updated
+     */
+    bool updateTicks(std::int64_t left_ticks, std::int64_t right_ticks, const double &time);
+
+    /**
+     * \brief Updates the odometry class with latest velocity command
+     * \param linear  Linear velocity [m/s]
+     * \param angular Angular velocity [rad/s]
+     * \param time    Current time
+     */
+    void updateOdom(double linear, double angular, const double &time);
+
+    /**
+     * \brief heading getter
+     * \return heading [rad]
+     */
+    double getHeading() const
+    {
+      return heading_;
+    }
+
+    /**
+     * \brief x position getter
+     * \return x position [m]
+     */
+    double getX() const
+    {
+      return x_;
+    }
+
+    /**
+     * \brief y position getter
+     * \return y position [m]
+     */
+    double getY() const
+    {
+      return y_;
+    }
+
+    /**
+     * \brief linear velocity getter
+     * \return linear velocity [m/s]
+     */
+    double getLinear() const
+    {
+      return linear_;
+    }
+
+    /**
+     * \brief angular velocity getter
+     * \return angular velocity [rad/s]
+     */
+    double getAngular() const
+    {
+      return angular_;
+    }
+
+    /**
+     * \brief Sets the wheel parameters: radius and separation
+     * \param wheel_separation   Separation between left and right wheels [m]
+     * \param left_wheel_radius  Left wheel radius [m]
+     * \param right_wheel_radius Right wheel radius [m]
+     */
+    void setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius);
+
+    /**
+     * \brief Sets encoder resolution for both wheels (same CPR)
+     * \param ticks_per_revolution Ticks per wheel revolution (wheel side)
+     */
+    void setEncoderParams(double ticks_per_revolution);
+
+    /**
+     * \brief Sets encoder resolution per wheel
+     * \param left_ticks_per_revolution  Left  wheel ticks per revolution
+     * \param right_ticks_per_revolution Right wheel ticks per revolution
+     */
+    void setEncoderParams(double left_ticks_per_revolution, double right_ticks_per_revolution);
+
+    /**
+     * \brief Velocity rolling window size setter
+     * \param velocity_rolling_window_size Velocity rolling window size
+     */
+    void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
+
+  private:
+
+    /// Rolling mean accumulator and window:
+    typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > RollingMeanAcc;
+    typedef bacc::tag::rolling_window RollingWindow;
+
+    /**
+     * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta
+     * \param linear  Linear  displacement [m]
+     * \param angular Angular displacement [rad]
+     */
+    void integrateRungeKutta2(double linear, double angular);
+
+    /**
+     * \brief Integrates the velocities (linear and angular) using exact method
+     * \param linear  Linear  displacement [m]
+     * \param angular Angular displacement [rad]
+     */
+    void integrateExact(double linear, double angular);
+
+    /**
+     *  \brief Reset linear and angular accumulators
+     */
+    void resetAccumulators();
+
+    /// Current timestamp:
+    double timestamp_;
+
+    /// Current pose:
+    double x_;        //   [m]
+    double y_;        //   [m]
+    double heading_;  // [rad]
+
+    /// Current velocity:
+    double linear_;  //   [m/s]
+    double angular_; // [rad/s]
+
+    /// Wheel kinematic parameters [m]:
+    double wheel_separation_;
+    double left_wheel_radius_;
+    double right_wheel_radius_;
+
+    /// Encoder resolution [ticks / wheel revolution]:
+    double left_ticks_per_revolution_;
+    double right_ticks_per_revolution_;
+
+    /// Previous cumulative tick counts for delta computation:
+    bool ticks_initialized_;
+    std::int64_t left_ticks_prev_;
+    std::int64_t right_ticks_prev_;
+
+    /// Rolling mean accumulators for the linear and angular velocities:
+    size_t velocity_rolling_window_size_;
+    RollingMeanAcc linear_acc_;
+    RollingMeanAcc angular_acc_;
+
+    /// Integration function, used to integrate the odometry:
+    IntegrationFunction integrate_fun_;
+  };
+}

src/main.cpp

@@ -0,0 +1,37 @@
+#include <iostream>
+#include <odometry/odom_differential.h>
+#include <chrono>
+#include <thread>
+
+int main()
+{
+  differential::Odometry odom(10);
+  odom.setWheelParams(/*wheel_separation=*/0.5, /*left_wheel_radius=*/0.09, /*right_wheel_radius=*/0.09);
+
+  const auto now = std::chrono::system_clock::now();
+  double t = std::chrono::duration<double>(now.time_since_epoch()).count(); // seconds since epoch
+  odom.init(t);
+
+  // Constant wheel angular velocities [rad/s]
+  const double left_vel = 2.0;
+  const double right_vel = 1.0;
+
+  for (int i = 0; i < 20; ++i)
+  {
+    const auto now = std::chrono::system_clock::now();
+    t = std::chrono::duration<double>(now.time_since_epoch()).count();
+
+    odom.updateWheel(left_vel, right_vel, t);
+
+    std::cout << "t=" << t
+              << " x=" << odom.getX()
+              << " y=" << odom.getY()
+              << " heading=" << odom.getHeading()
+              << " v=" << odom.getLinear()
+              << " w=" << odom.getAngular()
+              << "\n";
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+
+  return 0;
+}

src/odom_differential.cpp

@@ -0,0 +1,199 @@
+#include <functional>
+#include <iostream>
+#include <odometry/odom_differential.h>
+
+namespace differential
+{
+  namespace bacc = boost::accumulators;
+
+  Odometry::Odometry(size_t velocity_rolling_window_size)
+    : timestamp_(0.0)
+    , x_(0.0)
+    , y_(0.0)
+    , heading_(0.0)
+    , linear_(0.0)
+    , angular_(0.0)
+    , wheel_separation_(0.0)
+    , left_wheel_radius_(0.0)
+    , right_wheel_radius_(0.0)
+    , left_ticks_per_revolution_(0.0)
+    , right_ticks_per_revolution_(0.0)
+    , ticks_initialized_(false)
+    , left_ticks_prev_(0)
+    , right_ticks_prev_(0)
+    , velocity_rolling_window_size_(velocity_rolling_window_size)
+    , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
+    , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
+  {
+  }
+
+  void Odometry::init(const double &time)
+  {
+    resetAccumulators();
+    timestamp_ = time;
+    x_ = 0.0;
+    y_ = 0.0;
+    heading_ = 0.0;
+    linear_ = 0.0;
+    angular_ = 0.0;
+    ticks_initialized_ = false;
+    left_ticks_prev_ = 0;
+    right_ticks_prev_ = 0;
+    integrate_fun_ = std::bind(&Odometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2);
+  }
+
+  bool Odometry::updateWheel(double left_vel, double right_vel, const double &time)
+  {
+    /// We cannot integrate with very small time intervals:
+    const double dt = (time - timestamp_);
+    if (dt < 0.0001)
+      return false;
+
+    /// Convert wheel angular velocity [rad/s] to linear velocity [m/s]:
+    const double left_wheel_lin_vel  = left_vel  * left_wheel_radius_;
+    const double right_wheel_lin_vel = right_vel * right_wheel_radius_;
+
+    /// Compute robot linear and angular velocity:
+    const double linear  = (right_wheel_lin_vel + left_wheel_lin_vel) * 0.5;
+    const double angular = (right_wheel_lin_vel - left_wheel_lin_vel) / wheel_separation_;
+
+    /// Integrate odometry using displacement over dt:
+    integrate_fun_(linear * dt, angular * dt);
+
+    timestamp_ = time;
+
+    /// Estimate speeds using a rolling mean to filter them out:
+    linear_acc_(linear);
+    angular_acc_(angular);
+
+    linear_ = bacc::rolling_mean(linear_acc_);
+    angular_ = bacc::rolling_mean(angular_acc_);
+    return true;
+  }
+
+  bool Odometry::updateTicks(std::int64_t left_ticks, std::int64_t right_ticks, const double &time)
+  {
+    if (!ticks_initialized_)
+    {
+      left_ticks_prev_ = left_ticks;
+      right_ticks_prev_ = right_ticks;
+      timestamp_ = time;
+      ticks_initialized_ = true;
+      return false;
+    }
+
+    if (left_ticks_per_revolution_ <= 0.0 || right_ticks_per_revolution_ <= 0.0)
+      return false;
+
+    /// We cannot integrate with very small time intervals:
+    const double dt = (time - timestamp_);
+    if (dt < 0.0001)
+      return false;
+
+    const std::int64_t left_delta  = left_ticks  - left_ticks_prev_;
+    const std::int64_t right_delta = right_ticks - right_ticks_prev_;
+    left_ticks_prev_  = left_ticks;
+    right_ticks_prev_ = right_ticks;
+
+    /// Tick delta → wheel rotation [rad] → arc length [m]:
+    const double left_wheel_phi  =
+      (2.0 * M_PI * static_cast<double>(left_delta)) / left_ticks_per_revolution_;
+    const double right_wheel_phi =
+      (2.0 * M_PI * static_cast<double>(right_delta)) / right_ticks_per_revolution_;
+
+    const double left_ds  = left_wheel_phi  * left_wheel_radius_;
+    const double right_ds = right_wheel_phi * right_wheel_radius_;
+
+    /// Differential-drive displacement:
+    const double linear_ds   = (right_ds + left_ds) * 0.5;
+    const double angular_dth = (right_ds - left_ds) / wheel_separation_;
+
+    integrate_fun_(linear_ds, angular_dth);
+
+    timestamp_ = time;
+
+    /// Estimate speeds using a rolling mean to filter them out:
+    const double linear  = linear_ds / dt;
+    const double angular = angular_dth / dt;
+    linear_acc_(linear);
+    angular_acc_(angular);
+
+    linear_ = bacc::rolling_mean(linear_acc_);
+    angular_ = bacc::rolling_mean(angular_acc_);
+    return true;
+  }
+
+  void Odometry::updateOdom(double linear, double angular, const double &time)
+  {
+    /// Save last linear and angular velocity:
+    linear_ = linear;
+    angular_ = angular;
+
+    /// Integrate odometry:
+    const double dt = (time - timestamp_);
+    timestamp_ = time;
+    integrate_fun_(linear * dt, angular * dt);
+  }
+
+  void Odometry::setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius)
+  {
+    wheel_separation_   = wheel_separation;
+    left_wheel_radius_  = left_wheel_radius;
+    right_wheel_radius_ = right_wheel_radius;
+  }
+
+  void Odometry::setEncoderParams(double ticks_per_revolution)
+  {
+    setEncoderParams(ticks_per_revolution, ticks_per_revolution);
+  }
+
+  void Odometry::setEncoderParams(double left_ticks_per_revolution, double right_ticks_per_revolution)
+  {
+    left_ticks_per_revolution_  = left_ticks_per_revolution;
+    right_ticks_per_revolution_ = right_ticks_per_revolution;
+  }
+
+  void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
+  {
+    velocity_rolling_window_size_ = velocity_rolling_window_size;
+
+    resetAccumulators();
+  }
+
+  void Odometry::integrateRungeKutta2(double linear, double angular)
+  {
+    const double direction = heading_ + angular * 0.5;
+
+    /// Runge-Kutta 2nd order integration:
+    x_       += linear * cos(direction);
+    y_       += linear * sin(direction);
+    heading_ += angular;
+  }
+
+  /**
+   * \brief Other possible integration method provided by the class
+   * \param linear
+   * \param angular
+   */
+  void Odometry::integrateExact(double linear, double angular)
+  {
+    if (fabs(angular) < 1e-6)
+      integrateRungeKutta2(linear, angular);
+    else
+    {
+      /// Exact integration (should solve problems when angular is zero):
+      const double heading_old = heading_;
+      const double r = linear/angular;
+      heading_ += angular;
+      x_       +=  r * (sin(heading_) - sin(heading_old));
+      y_       += -r * (cos(heading_) - cos(heading_old));
+    }
+  }
+
+  void Odometry::resetAccumulators()
+  {
+    linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+    angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+  }
+
+} // namespace differential

test/odom_differential_test.cpp

@@ -0,0 +1,244 @@
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
+#include <odometry/odom_differential.h>
+
+namespace
+{
+int g_failures = 0;
+
+void expect_true(bool condition, const char *expr, const char *file, int line)
+{
+  if (!condition)
+  {
+    std::cerr << "FAIL: " << file << ":" << line << " " << expr << "\n";
+    ++g_failures;
+  }
+}
+
+void expect_near(double actual, double expected, double tolerance,
+                 const char *expr, const char *file, int line)
+{
+  if (std::fabs(actual - expected) > tolerance)
+  {
+    std::cerr << "FAIL: " << file << ":" << line << " " << expr
+              << " (actual=" << actual << ", expected=" << expected
+              << ", tolerance=" << tolerance << ")\n";
+    ++g_failures;
+  }
+}
+
+#define EXPECT_TRUE(expr) expect_true((expr), #expr, __FILE__, __LINE__)
+#define EXPECT_NEAR(actual, expected, tolerance) \
+  expect_near((actual), (expected), (tolerance), #actual, __FILE__, __LINE__)
+
+void test_initial_state_after_init()
+{
+  differential::Odometry odom(10);
+  odom.setWheelParams(0.5, 0.09, 0.09);
+  odom.init(100.0);
+
+  EXPECT_NEAR(odom.getX(), 0.0, 1e-9);
+  EXPECT_NEAR(odom.getY(), 0.0, 1e-9);
+  EXPECT_NEAR(odom.getHeading(), 0.0, 1e-9);
+  EXPECT_NEAR(odom.getLinear(), 0.0, 1e-9);
+  EXPECT_NEAR(odom.getAngular(), 0.0, 1e-9);
+}
+
+void test_straight_line_motion()
+{
+  differential::Odometry odom(1);
+  const double wheel_radius = 0.09;
+  const double wheel_separation = 0.5;
+  odom.setWheelParams(wheel_separation, wheel_radius, wheel_radius);
+
+  double t = 0.0;
+  odom.init(t);
+
+  const double wheel_vel = 2.0; // [rad/s]
+  const double dt = 0.1;
+  const int steps = 10;
+
+  for (int i = 0; i < steps; ++i)
+  {
+    t += dt;
+    EXPECT_TRUE(odom.updateWheel(wheel_vel, wheel_vel, t));
+  }
+
+  const double linear_vel = wheel_radius * wheel_vel;
+  const double expected_x = linear_vel * dt * steps;
+
+  EXPECT_NEAR(odom.getY(), 0.0, 1e-6);
+  EXPECT_NEAR(odom.getHeading(), 0.0, 1e-6);
+  EXPECT_NEAR(odom.getX(), expected_x, 1e-6);
+  EXPECT_NEAR(odom.getLinear(), linear_vel, 1e-3);
+  EXPECT_NEAR(odom.getAngular(), 0.0, 1e-3);
+}
+
+void test_turn_motion()
+{
+  differential::Odometry odom(1);
+  const double wheel_radius = 0.09;
+  const double wheel_separation = 0.5;
+  odom.setWheelParams(wheel_separation, wheel_radius, wheel_radius);
+
+  double t = 0.0;
+  odom.init(t);
+
+  const double left_vel = 2.0;   // [rad/s]
+  const double right_vel = 1.0;  // [rad/s]
+  const double dt = 0.1;
+
+  t += dt;
+  EXPECT_TRUE(odom.updateWheel(left_vel, right_vel, t));
+
+  const double left_lin = wheel_radius * left_vel;
+  const double right_lin = wheel_radius * right_vel;
+  const double expected_linear = (right_lin + left_lin) * 0.5;
+  const double expected_angular = (right_lin - left_lin) / wheel_separation;
+  const double expected_heading = expected_angular * dt;
+  const double expected_x = expected_linear * dt;
+
+  EXPECT_NEAR(odom.getHeading(), expected_heading, 1e-6);
+  EXPECT_NEAR(odom.getX(), expected_x, 1e-6);
+  EXPECT_NEAR(odom.getLinear(), expected_linear, 1e-3);
+  EXPECT_NEAR(odom.getAngular(), expected_angular, 1e-3);
+}
+
+void test_update_rejects_small_dt()
+{
+  differential::Odometry odom(1);
+  odom.setWheelParams(0.5, 0.09, 0.09);
+
+  double t = 1.0;
+  odom.init(t);
+
+  const double linear_before = odom.getLinear();
+  const double angular_before = odom.getAngular();
+  EXPECT_TRUE(!odom.updateWheel(1.0, 1.0, t + 1e-5));
+  EXPECT_NEAR(odom.getLinear(), linear_before, 1e-9);
+  EXPECT_NEAR(odom.getAngular(), angular_before, 1e-9);
+  EXPECT_NEAR(odom.getX(), 0.0, 1e-9);
+}
+
+void test_update_open_loop()
+{
+  differential::Odometry odom(1);
+  odom.setWheelParams(0.5, 0.09, 0.09);
+
+  double t = 0.0;
+  odom.init(t);
+
+  const double linear = 0.2;
+  const double angular = 0.0;
+  const double dt = 0.5;
+
+  t += dt;
+  odom.updateOdom(linear, angular, t);
+
+  EXPECT_NEAR(odom.getLinear(), linear, 1e-9);
+  EXPECT_NEAR(odom.getAngular(), angular, 1e-9);
+  EXPECT_NEAR(odom.getX(), linear * dt, 1e-6);
+  EXPECT_NEAR(odom.getY(), 0.0, 1e-6);
+  EXPECT_NEAR(odom.getHeading(), 0.0, 1e-6);
+}
+
+void test_zero_velocity_keeps_pose()
+{
+  differential::Odometry odom(1);
+  odom.setWheelParams(0.5, 0.09, 0.09);
+  odom.init(0.0);
+
+  EXPECT_TRUE(odom.updateWheel(0.0, 0.0, 0.1));
+  EXPECT_NEAR(odom.getX(), 0.0, 1e-9);
+  EXPECT_NEAR(odom.getY(), 0.0, 1e-9);
+  EXPECT_NEAR(odom.getHeading(), 0.0, 1e-9);
+  EXPECT_NEAR(odom.getLinear(), 0.0, 1e-9);
+  EXPECT_NEAR(odom.getAngular(), 0.0, 1e-9);
+}
+
+void test_ticks_seed_then_straight()
+{
+  differential::Odometry odom(1);
+  const double wheel_radius = 0.09;
+  const double wheel_separation = 0.5;
+  const double ticks_per_rev = 4096.0;
+  odom.setWheelParams(wheel_separation, wheel_radius, wheel_radius);
+  odom.setEncoderParams(ticks_per_rev);
+  odom.init(0.0);
+
+  // First sample only seeds previous ticks.
+  EXPECT_TRUE(!odom.updateTicks(1000, 1000, 0.0));
+  EXPECT_NEAR(odom.getX(), 0.0, 1e-9);
+
+  // One full wheel revolution on both sides → forward 2*pi*r
+  const std::int64_t delta = static_cast<std::int64_t>(ticks_per_rev);
+  EXPECT_TRUE(odom.updateTicks(1000 + delta, 1000 + delta, 1.0));
+
+  const double expected_x = 2.0 * M_PI * wheel_radius;
+  EXPECT_NEAR(odom.getY(), 0.0, 1e-6);
+  EXPECT_NEAR(odom.getHeading(), 0.0, 1e-6);
+  EXPECT_NEAR(odom.getX(), expected_x, 1e-6);
+  EXPECT_NEAR(odom.getLinear(), expected_x / 1.0, 1e-3);
+  EXPECT_NEAR(odom.getAngular(), 0.0, 1e-3);
+}
+
+void test_ticks_turn_in_place()
+{
+  differential::Odometry odom(1);
+  const double wheel_radius = 0.09;
+  const double wheel_separation = 0.5;
+  const double ticks_per_rev = 4096.0;
+  odom.setWheelParams(wheel_separation, wheel_radius, wheel_radius);
+  odom.setEncoderParams(ticks_per_rev);
+  odom.init(0.0);
+
+  EXPECT_TRUE(!odom.updateTicks(0, 0, 0.0));
+
+  // Left backward, right forward by the same arc → pure rotation.
+  const double arc = 0.1; // [m]
+  const std::int64_t delta = static_cast<std::int64_t>(
+      (arc / (2.0 * M_PI * wheel_radius)) * ticks_per_rev);
+
+  EXPECT_TRUE(odom.updateTicks(-delta, delta, 0.2));
+
+  const double expected_heading = (2.0 * arc) / wheel_separation;
+  EXPECT_NEAR(odom.getX(), 0.0, 1e-4);
+  EXPECT_NEAR(odom.getY(), 0.0, 1e-4);
+  EXPECT_NEAR(odom.getHeading(), expected_heading, 1e-3);
+}
+
+void test_ticks_rejects_unset_encoder_params()
+{
+  differential::Odometry odom(1);
+  odom.setWheelParams(0.5, 0.09, 0.09);
+  odom.init(0.0);
+
+  EXPECT_TRUE(!odom.updateTicks(0, 0, 0.0));   // seed
+  EXPECT_TRUE(!odom.updateTicks(10, 10, 0.1)); // CPR not set
+  EXPECT_NEAR(odom.getX(), 0.0, 1e-9);
+}
+
+} // namespace
+
+int main()
+{
+  test_initial_state_after_init();
+  test_straight_line_motion();
+  test_turn_motion();
+  test_update_rejects_small_dt();
+  test_update_open_loop();
+  test_zero_velocity_keeps_pose();
+  test_ticks_seed_then_straight();
+  test_ticks_turn_in_place();
+  test_ticks_rejects_unset_encoder_params();
+
+  if (g_failures == 0)
+  {
+    std::cout << "All tests passed.\n";
+    return EXIT_SUCCESS;
+  }
+
+  std::cerr << g_failures << " test(s) failed.\n";
+  return EXIT_FAILURE;
+}