BÀI 61 – KALMAN FILTER CHO ENCODER + IMU

ƯỚC LƯỢNG TỐC ĐỘ, GÓC θ VÀ SENSOR FUSION TRÊN STM32F103

Đây là một bước rất quan trọng.

Từ Bài 60, chúng ta đã có:

Encoder
   ↓
Speed Estimation
   ↓
Low-pass Filter
   ↓
PI/PID
   ↓
PWM

Nhưng encoder có một nhược điểm lớn:

Encoder biết bánh xe quay bao nhiêu, nhưng không biết robot thực sự đang hướng về đâu.

Nếu bánh trái/phải có sai số nhỏ, hoặc bánh trượt:

Encoder
   ↓
Odometry
   ↓
θ
   ↓
DRIFT

Sau một thời gian, góc θ có thể sai khá nhiều.

IMU lại có đặc tính ngược lại:

Gyroscope
   ↓
ω
   ↓
tích phân
   ↓
θ

Phản ứng rất nhanh, nhưng bị drift theo thời gian.

Vì vậy:

Encoder + IMU bổ sung cho nhau.

Và Kalman Filter giúp chúng ta kết hợp chúng.

---

1. Mục tiêu của Bài 61

Sau bài này bạn phải hiểu được:

Encoder
   ↓
v, Δθ
       \
        \
         → Sensor Fusion → trạng thái tốt hơn
        /
       /
IMU
   ↓
gyro, accel

Cụ thể:

Encoder

Đo:

VL
VR

từ đó suy ra:

V
ω_encoder

IMU

Đo:

gyro Z
accelerometer

suy ra:

ω_gyro
θ_accel

Kalman Filter

Kết hợp:

encoder
+
IMU

để ước lượng:

θ
ω

---

2. Trước tiên phải hiểu một điều

Không phải cứ có IMU là robot biết chính xác góc.

Ví dụ gyroscope đo:

ω = 0.1 rad/s

Muốn có góc:

[
\theta_k

\theta_{k-1}
+
\omega_kdt
]

Nếu:

dt = 0.01 s

thì:

[
\Delta\theta

0.1\times0.01

0.001rad
]

Cộng liên tục:

ω
 ↓
integral
 ↓
θ

---

3. Vấn đề của Gyroscope

Giả sử robot đang đứng yên.

Lý tưởng:

gyro = 0

Nhưng thực tế:

gyro = 0.03 rad/s

Sau:

100 s

sai số góc:

[
\theta=0.03\times100
]

[
\boxed{\theta=3rad}
]

≈ 172°.

Robot không hề quay, nhưng hệ thống tưởng rằng nó đã quay gần nửa vòng.

Đây gọi là:

Gyro Drift

---

4. Accelerometer lại có ưu điểm khác

Khi robot không bị gia tốc quá mạnh, accelerometer có thể suy ra hướng trọng lực.

Ví dụ trong mặt phẳng:

      Z
      ↑
      │
      │
      ●──────→ X
     /
    /
   Y

Gravity cung cấp một vector tham chiếu.

Từ đó có thể ước lượng góc nghiêng.

Ví dụ:

[
\theta_{acc}

atan2(a_y,a_x)
]

Tuy nhiên:

Với robot 2 bánh chạy trên mặt phẳng, accelerometer không phải cảm biến yaw tuyệt đối.

Đây là điểm cực kỳ quan trọng.

Accelerometer giúp quan sát roll/pitch từ trọng lực, nhưng không tự cung cấp yaw tuyệt đối.

---

5. Muốn sửa yaw drift thì cần gì?

Có nhiều lựa chọn:

Gyroscope
+
Encoder

giúp ước lượng yaw tương đối tốt hơn.

Nhưng vẫn có thể drift.

Muốn có tham chiếu yaw tuyệt đối, có thể thêm:

Magnetometer

hoặc:

GPS/dual GNSS

hoặc:

camera

hoặc:

landmark

Trong Bài 61, chúng ta tập trung trước vào:

Encoder + Gyroscope

để xây nền tảng sensor fusion.

---

6. Hai nguồn dữ liệu

Giả sử robot có:

Wheel Base = W

Encoder cho:

VL
VR

Ta tính:

[
\omega_{enc}

\frac{V_R-V_L}{W}
]

IMU cho:

gyro_z

Sau khi chuyển sang rad/s:

ωgyro

Bây giờ chúng ta có:

ωenc
ωgyro

---

7. Tại sao hai cảm biến lại bổ sung?

Encoder:

+ không drift nhanh
+ biết chuyển động bánh
- bị ảnh hưởng bởi trượt
- sai số cơ khí

Gyroscope:

+ phản ứng cực nhanh
+ đo trực tiếp tốc độ quay
- bias
- drift

Do đó:

Encoder ──────┐
              ├──→ Estimate ω
Gyro ─────────┘

---

8. Kalman Filter thực chất làm gì?

Nó không phải “phép thuật”.

Nó liên tục thực hiện:

PREDICT
   ↓
MEASURE
   ↓
CORRECT
   ↓
PREDICT
   ↓
MEASURE
   ↓
CORRECT

---

9. Tư duy cực kỳ quan trọng

Giả sử Kalman Filter dự đoán:

θ = 1.00 rad

Encoder/IMU đưa ra:

θ_measure = 1.10 rad

Kalman không nhất thiết chọn:

1.00

hoặc:

1.10

mà tạo một giá trị trung gian dựa trên:

độ tin cậy của prediction
+
độ tin cậy của measurement

Ví dụ:

Estimate = 1.06 rad

---

10. Một Kalman Filter 1D cực kỳ đơn giản

Trước tiên chưa cần ma trận.

Ta dùng:

typedef struct
{
    float x;
    float P;

    float Q;
    float R;

} Kalman1D_t;

Trong đó:

x = giá trị ước lượng
P = covariance
Q = process noise
R = measurement noise

---

11. Prediction

P = P + Q;

Ý nghĩa:

Khi thời gian trôi qua, chúng ta trở nên ít chắc chắn hơn về trạng thái.

---

12. Measurement update

Kalman Gain:

[
K=\frac{P}{P+R}
]

Sau đó:

[
x=x+K(z-x)
]

và:

[
P=(1-K)P
]

---

13. Code Kalman 1D

float Kalman1D_Update(
    Kalman1D_t *kf,
    float measurement)
{
    /*
     * Predict
     */

    kf->P += kf->Q;

    /*
     * Kalman Gain
     */

    float K =
        kf->P /
        (kf->P + kf->R);

    /*
     * Correct
     */

    kf->x +=
        K *
        (measurement -
         kf->x);

    /*
     * Update covariance
     */

    kf->P =
        (1.0f - K) *
        kf->P;

    return kf->x;
}

---

14. Ví dụ

Giả sử:

Estimate = 10
Measurement = 12

và:

K = 0.25

thì:

[
x=10+0.25(12-10)
]

[
x=10.5
]

Filter không nhảy ngay lên 12.

---

15. Nhưng đây chưa phải Kalman tốt cho robot

Vì robot của chúng ta có:

θ
ω
gyro bias

Do đó chúng ta cần một model tốt hơn.

Một mô hình rất hay cho Bài 61 là:

[
x=
\begin{bmatrix}
\theta\
b
\end{bmatrix}
]

Trong đó:

θ = góc
b = gyro bias

---

16. Tại sao phải ước lượng gyro bias?

Gyro thực tế:

[
gyro_{measured}

gyro_{true}
+
bias
+
noise
]

Do đó:

[
gyro_{true}

gyro_{measured}-bias
]

Nếu chúng ta biết bias, việc tích phân gyro sẽ tốt hơn rất nhiều.

---

17. State của Kalman

Ta định nghĩa:

x0 = θ
x1 = gyro_bias

Vector:

[
x=
\begin{bmatrix}
\theta\
b
\end{bmatrix}
]

Đây là Kalman 2 trạng thái.

---

18. Input của hệ thống

Gyroscope cho:

[
u=\omega_{gyro}
]

Mô hình:

[
\theta_k

\theta_{k-1}
+
(\omega_{gyro}-b)dt
]

Bias gần như thay đổi chậm:

[
b_k\approx b_{k-1}
]

---

19. Measurement

Encoder có thể cung cấp:

[
\omega_{enc}
]

Nhưng để đo trực tiếp θ, encoder cần được tích phân:

[
\theta_{enc}

\theta_{enc}
+
\omega_{enc}dt
]

Do đó:

Encoder
 ↓
ωenc
 ↓
integral
 ↓
θenc

Đây là một measurement tương đối.

---

20. Nhưng có một vấn đề

Nếu:

Encoder trượt

thì:

θenc

cũng drift.

Vì vậy Kalman không thể biến encoder thành cảm biến tuyệt đối.

Nó chỉ giúp kết hợp hai nguồn thông tin có sai số khác nhau.

---

21. Một hệ thống tốt hơn

Nếu có IMU 6DOF như:

MPU6050

thì:

Gyro Z

→ yaw rate.

Encoder:

VL
VR

→ yaw rate.

Hai tín hiệu:

ωgyro
ωencoder

có thể được fusion.

---

22. Fusion tốc độ góc trước

Đây là cách dễ hiểu nhất.

Ta có:

[
\omega_{enc}

\frac{V_R-V_L}{W}
]

và:

[
\omega_{gyro}

gyro_z-b
]

Ta có thể dùng Kalman 1D để ước lượng:

[
\boxed{\omega}
]

---

23. Sau đó tích phân

[
\theta_k

\theta_{k-1}
+
\omega_kdt
]

Như vậy:

Encoder ──────┐
              │
Gyro ─────────┤
              ▼
          Kalman ω
              │
              ▼
           Integrate
              │
              ▼
              θ

Đây là kiến trúc rất dễ triển khai trên STM32F103.

---

24. Nhưng có thể làm tốt hơn

Thay vì Kalman:

ω

ta có thể đưa trực tiếp:

θ
bias

vào state.

Một dạng phổ biến là:

state:
θ
bias

input:

gyro

measurement:

θ_encoder

Đây chính là mô hình Kalman 2-state cho angle + gyro bias.

---

25. Kalman 2-state

Cấu trúc:

typedef struct
{
    float angle;
    float bias;

    float P[2][2];

    float Q_angle;
    float Q_bias;

    float R_measure;

} KalmanAngle_t;

---

26. Khởi tạo

void KalmanAngle_Init(
    KalmanAngle_t *kf)
{
    kf->angle = 0.0f;
    kf->bias = 0.0f;

    kf->P[0][0] = 0.0f;
    kf->P[0][1] = 0.0f;
    kf->P[1][0] = 0.0f;
    kf->P[1][1] = 0.0f;

    kf->Q_angle =
        0.001f;

    kf->Q_bias =
        0.003f;

    kf->R_measure =
        0.03f;
}

Các giá trị này chỉ là giá trị khởi đầu minh họa, phải tuning theo IMU và encoder thực tế.

---

27. Prediction

Gyroscope:

rate =
    gyro -
    bias;

Sau đó:

angle +=
    dt * rate;

---

28. Covariance prediction

Với:

[
F=
\begin{bmatrix}
1&-dt\
0&1
\end{bmatrix}
]

ta cập nhật covariance.

Code:

kf->P[0][0] +=
    dt *
    (dt * kf->P[1][1]
     - kf->P[0][1]
     - kf->P[1][0]
     + kf->Q_angle);

kf->P[0][1] -=
    dt * kf->P[1][1];

kf->P[1][0] -=
    dt * kf->P[1][1];

kf->P[1][1] +=
    kf->Q_bias * dt;

---

29. Measurement update

Measurement:

z = θ_encoder

Sai số:

[
y=z-\theta
]

Innovation covariance:

[
S=P_{00}+R
]

Kalman gain:

[
K_0=\frac{P_{00}}{S}
]

[
K_1=\frac{P_{10}}{S}
]

---

30. Update state

float y =
    measurement -
    kf->angle;

float S =
    kf->P[0][0] +
    kf->R_measure;

float K0 =
    kf->P[0][0] / S;

float K1 =
    kf->P[1][0] / S;

kf->angle +=
    K0 * y;

kf->bias +=
    K1 * y;

Sau đó covariance được cập nhật.

---

31. Hàm hoàn chỉnh

float KalmanAngle_Update(
    KalmanAngle_t *kf,
    float measurement,
    float gyro,
    float dt)
{
    /*
     * Remove gyro bias
     */

    float rate =
        gyro -
        kf->bias;

    /*
     * Predict angle
     */

    kf->angle +=
        dt * rate;

    /*
     * Predict covariance
     */

    kf->P[0][0] +=
        dt *
        (dt * kf->P[1][1]
        - kf->P[0][1]
        - kf->P[1][0]
        + kf->Q_angle);

    kf->P[0][1] -=
        dt * kf->P[1][1];

    kf->P[1][0] -=
        dt * kf->P[1][1];

    kf->P[1][1] +=
        kf->Q_bias * dt;

    /*
     * Measurement update
     */

    float y =
        measurement -
        kf->angle;

    float S =
        kf->P[0][0] +
        kf->R_measure;

    float K0 =
        kf->P[0][0] / S;

    float K1 =
        kf->P[1][0] / S;

    /*
     * Correct
     */

    kf->angle +=
        K0 * y;

    kf->bias +=
        K1 * y;

    /*
     * Covariance update
     */

    float P00 =
        kf->P[0][0];

    float P01 =
        kf->P[0][1];

    kf->P[0][0] -=
        K0 * P00;

    kf->P[0][1] -=
        K0 * P01;

    kf->P[1][0] -=
        K1 * P00;

    kf->P[1][1] -=
        K1 * P01;

    return kf->angle;
}

Đây là một bộ lọc Kalman 2 trạng thái kinh điển cho bài toán angle + gyro bias.

---

32. Tuy nhiên cần hiểu giới hạn của ví dụ này

Trong ví dụ trên:

measurement = θ_encoder

không phải yaw tuyệt đối.

Nếu encoder bị trượt lâu dài:

θ_encoder

vẫn drift.

Do đó Kalman chỉ giảm nhiễu và kết hợp động học, chứ không tạo ra thông tin mà cảm biến không có.

Đây là nguyên tắc rất quan trọng trong sensor fusion.

---

33. Nếu dùng MPU6050

Kiến trúc:

MPU6050
 ├── Accelerometer
 └── Gyroscope

Accelerometer:

Ax
Ay
Az

Gyroscope:

Gx
Gy
Gz

Đối với robot chạy trên mặt phẳng:

Gz

thường là thành phần quan trọng để đo yaw rate.

---

34. Nhưng accelerometer dùng để làm gì?

Nó rất hữu ích cho:

Roll
Pitch

Ví dụ:

[
roll=
atan2(A_y,A_z)
]

và:

[
pitch=
atan2(-A_x,\sqrt{A_y^2+A_z^2})
]

Nhưng:

Không dùng accelerometer đơn độc để lấy yaw.

Đây là một lỗi rất phổ biến khi mới học IMU.

---

35. Nếu muốn yaw tuyệt đối

Có thể thêm:

Magnetometer

Ví dụ:

MPU9250
ICM20948
BNO055

Khi đó:

Gyro
   ↓
yaw rate

Magnetometer
   ↓
yaw reference

và Kalman/EKF có thể fusion chúng.

---

36. Encoder + IMU thực tế

Robot 2 bánh:

Encoder L ──→ VL
Encoder R ──→ VR
                 │
                 ▼
              ωencoder
                 │
                 │
Gyro Z ──────────┤
                 ▼
             SENSOR FUSION
                 │
                 ▼
                ω
                 │
                 ▼
                θ

Sau đó:

θ
+
V

đưa vào:

Odometry

để tính:

X
Y
θ

---

37. Tại sao fusion giúp robot tốt hơn?

Giả sử:

Encoder:
ω = 0.52 rad/s

Gyro:
ω = 0.58 rad/s

Không nên đơn giản:

ω = (0.52+0.58)/2

vì hai cảm biến có độ tin cậy khác nhau.

Kalman sử dụng:

Q
R
P

để quyết định nên tin nguồn nào nhiều hơn.

---

38. Q là gì?

Q:

Process Noise

Nó thể hiện:

“Mô hình dự đoán của tôi không hoàn hảo đến mức nào?”

Nếu:

Q lớn

→ ít tin prediction.

Nếu:

Q nhỏ

→ tin prediction nhiều hơn.

---

39. R là gì?

R:

Measurement Noise

Nó thể hiện:

“Cảm biến của tôi nhiễu đến mức nào?”

Nếu:

R lớn

→ ít tin measurement.

Nếu:

R nhỏ

→ tin measurement nhiều.

---

40. Ví dụ tư duy

Nếu gyro rất sạch:

Rgyro nhỏ

→ tin gyro nhiều.

Nếu encoder bị trượt:

Rencoder lớn

→ giảm trọng số encoder.

Ngược lại:

gyro drift mạnh

→ tăng mức không tin gyro prediction.

---

41. Đây chính là sức mạnh của Kalman

Không phải:

Sensor A + Sensor B / 2

mà là:

Sensor A
    ↓
độ tin cậy

Sensor B
    ↓
độ tin cậy

       ↓

Optimal Estimate

theo mô hình xác suất của hệ thống.

---

42. Kalman Filter chạy ở đâu?

Trên STM32F103 hoàn toàn có thể chạy realtime.

Ví dụ:

IMU:
100 Hz

Encoder:
100 Hz

Kalman:
100 Hz

PID:
100 Hz

STM32F103 có thể xử lý mức này khá thoải mái nếu code được tổ chức hợp lý.

---

43. Kiến trúc firmware

Tôi khuyên chia thành:

main.c
│
├── encoder.c
├── motor.c
├── pid.c
├── imu.c
├── kalman.c
├── odometry.c
└── robot.c

Trong đó:

encoder.c

read encoder
calculate wheel speed

imu.c

read gyro
read accelerometer

kalman.c

sensor fusion

odometry.c

X,Y,θ

---

44. Chu kỳ điều khiển

Ví dụ Timer 1 ms:

volatile uint32_t tick_ms;

Trong interrupt:

tick_ms++;

Main loop:

if (tick_ms - last_imu >= 10)
{
    last_imu += 10;

    IMU_Update();
}

Sau đó:

if (tick_ms - last_encoder >= 10)
{
    last_encoder += 10;

    Encoder_Update();
}

và:

if (tick_ms - last_kalman >= 10)
{
    last_kalman += 10;

    Kalman_Update();
}

---

45. Không nên làm thế này

while(1)
{
    read_imu();

    delay_ms(10);

    read_encoder();

    delay_ms(10);

    kalman();

    delay_ms(10);
}

Đây là hệ thống blocking.

Tốt hơn:

Timer
 ↓
scheduler
 ↓
tasks

---

46. UART Debug

Trong giai đoạn tuning, hãy in:

gyro_z
encoder_omega
fused_omega

gyro_bias

theta_encoder
theta_kalman

X
Y
theta

Ví dụ:

Gyro:       0.523
Encoder:    0.491
Fused:      0.507

Bias:       0.018

ThetaEnc:   1.250
ThetaKF:    1.238

Đây là cách rất tốt để nhìn thấy Kalman đang làm gì.

---

47. Thí nghiệm 1 – Robot đứng yên

Đặt robot trên bàn.

Không chạm vào robot.

Theo dõi:

gyro_z

Bạn sẽ thấy nó không hoàn toàn bằng:

0

Ví dụ:

0.012
0.018
0.015
0.021
...

Đây là:

Gyro Bias + Noise

---

48. Thí nghiệm 2 – Calibration Bias

Khi robot đứng yên:

N = 1000 samples

tính:

[
bias=
\frac1N
\sum gyro_i
]

Sau đó:

gyro_corrected =
    gyro_raw -
    gyro_bias;

Đây là bước bắt buộc trước khi dùng gyro tốt.

---

49. Thí nghiệm 3 – Quay robot 90°

Quay robot bằng tay khoảng:

90°

So sánh:

Encoder θ
Gyro θ
Kalman θ

Bạn sẽ bắt đầu thấy rõ sự khác biệt.

---

50. Thí nghiệm 4 – Quay rồi dừng

Đây là test quan trọng nhất.

START
 ↓
quay 90°
 ↓
STOP
 ↓
đứng yên

Gyro:

ω
 ↓
0

nhưng nếu bias chưa được xử lý tốt:

θ vẫn tiếp tục trôi

Encoder:

θ

cũng có thể sai nếu bánh trượt.

Kalman giúp giảm ảnh hưởng của các sai số này, tùy vào mô hình và tuning.

---

51. Điều rất quan trọng về yaw

Nếu bạn chỉ có:

2 encoder
+
1 gyro
+
1 accelerometer

thì bạn không có yaw tuyệt đối lâu dài.

Bạn có:

relative heading

chứ không phải:

absolute heading

Muốn robot biết:

“Tôi đang hướng đúng 37.2° so với Bắc”

thì cần thêm một nguồn tham chiếu như:

Magnetometer
GPS/dual GNSS
vision
landmark

---

52. Từ đây robot tiến sang State Estimation

Bây giờ ta có:

Sensors
   ↓
State Estimator
   ↓
X
Y
θ
V
ω

Đây chính là:

STATE ESTIMATION

Một khái niệm cực kỳ quan trọng trong robotics.

---

53. Và đây là bước chuyển từ robot cơ bản sang robot tự hành

Chúng ta đã đi:

B45
Encoder Mode
      ↓
B46
Motor Speed
      ↓
B48
Position
      ↓
B53
Odometry
      ↓
B54
Inverse Kinematics
      ↓
B55
Trajectory Tracking
      ↓
B57
Trajectory Generation
      ↓
B58
Feedforward + Feedback
      ↓
B59
Cascade Control
      ↓
B60
Speed Filtering + PID
      ↓
⭐ B61
Sensor Fusion

Từ đây kiến trúc robot đã bắt đầu giống một robot di động thực tế.

---

54. Bài tập cuối Bài 61

Hãy xây sơ đồ:

ENCODER LEFT
     ↓
    VL
     │
     ├──────────────┐
     │              │
ENCODER RIGHT       │
     ↓              │
    VR              │
     │              │
     └→ ωencoder ←──┘
            │
            │
         ┌──┴──┐
         │     │
      GYRO Z   │
         │     │
         ▼     │
      ωgyro    │
         │     │
         └──┬──┘
            ▼
       KALMAN FILTER
            │
            ▼
       ω_fused
            │
            ▼
      θ = ∫ωdt
            │
            ▼
       ODOMETRY
            │
            ▼
          X,Y,θ

Sau đó đưa:

X,Y,θ

vào controller của Bài 59.

Khi làm được điều này, vòng kín sẽ trở thành:

             TRAJECTORY
                  ↓
          Xd,Yd,θd,Vd,ωd
                  ↓
       POSITION CONTROLLER
                  ↓
                V,ω
                  ↓
        INVERSE KINEMATICS
             ↙          ↘
           VLd          VRd
            ↓            ↓
         SPEED PID    SPEED PID
            ↓            ↓
          PWM L        PWM R
            ↓            ↓
         MOTOR L      MOTOR R
            ↓            ↓
        ENCODER L    ENCODER R
             ↘          ↙
              ODOMETRY
                  ↑
                  │
             KALMAN θ
                  ↑
                  │
               GYRO Z

Đây là kiến trúc nền tảng của một robot 2 bánh có state estimation.

---

Bài tiếp theo

Sau Bài 61, bài hợp lý nhất là:

BÀI 62 – EKF (EXTENDED KALMAN FILTER) CHO ROBOT 2 BÁNH: FUSION ENCODER + IMU + ƯỚC LƯỢNG X, Y, θ, V, ω

Bài 62 sẽ khác Bài 61 ở một điểm rất quan trọng: thay vì chỉ lọc một góc θ, chúng ta xây state vector của cả robot:

[
\boxed{
x=
[X,;Y,;\theta,;V,;\omega]^T
}
]

và bắt đầu đi vào cách mà robot tự hành thực sự ước lượng trạng thái của chính nó.