# 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)`.