Chương C · hai cách xấp xỉ Bayes: chuông Gauss vs đám hạt

Khung 6 câu hỏi

🖼️ Hình nhanh: KF vs PF cạnh nhau
```
KF (1 Gauss): ─────╱╲───── mean + elip P
★

PF (hạt): · · ● · · ● · · ● = trọng số cao
(resample giữ ●, bỏ · nhẹ)
```

Hình minh họa (seminar-ready)

Hình gốc hoặc minh họa cho ebook — không cắt từ slide Princeton. Dùng Present: bật Slide · F toàn màn.

🇻🇳 Chuyện thật (sân B): Hai dãy cao ốc đối xứng — laser/ảnh “giống nhau hai phía” như hành lang khách sạn.
KF/EKF chỉ giữ một chuông Gauss: tuyệt khi đang tracking gần đúng chỗ; dễ bám nhầm một mode và bay tự tin sang tường.
PF thả đám hạt; khi GPS mất lock kiểu kidnapped, vài giả thuyết còn sống để đo mới cứu.
Chốt: KF/EKF = một mode, nhanh khi gần tuyến tính; PF khi belief đa đỉnh / global / kidnap. ../00C.

0. Mục tiêu học

1. Biết khi nào dùng KF/EKF, khi nào cần PF.
2. Đọc được ý nghĩa Kalman gain \(K\) (tin đo vs tin model).
3. Nắm 3 bước PF: sample → weight → resample.
4. Chạy demo KF 1D: \(\hat{x}\) mượt hơn \(z\), \(P\) hội tụ.

1. Trực giác

• Kalman Filter (KF): tin bằng một “chuông Gauss” — chỉ cần mean và covariance. Rất nhanh khi hệ gần tuyến tính và nhiễu gần Gaussian.
• Particle Filter (PF): thả \(N\) giả thuyết (hạt). Hạt nào giải thích được đo thì trọng số cao; resampling giữ các giả thuyết sống. Mạnh khi belief đa đỉnh (mới vào map, chưa biết góc nào).

Firmware PX4 / Crazyflie: EKF/UKF là xương sống. Global loc / kidnapped (Lec 14) thường cần PF/MCL.

Hình “cùng một đo, hai cách giữ tin”:

  đo z nhiễu:     ×        ×  ×     ×
  KF hợp nhất:         ───╱╲───     (một ước lượng)
  PF giữ nhiều:     ●  ●     ●  ·   (nhiều giả thuyết)

2. Cốt lõi + sơ đồ

2.1. Kalman Filter (hệ tuyến tính)

\[ x_t = A x_{t-1} + B u_t + w_t,\qquad z_t = C x_t + v_t \]
\(w\sim\mathcal{N}(0,Q),\ v\sim\mathcal{N}(0,R)\).

Predict:
\[ \hat{x}^- = A\hat{x}+Bu,\qquad P^- = APA^\top + Q \]

Update:
\[ K = P^-C^\top(CP^-C^\top+R)^{-1} \]
\[ \hat{x}=\hat{x}^-+K(z-C\hat{x}^-),\qquad P=(I-KC)P^- \]

  predict:  P ──(+Q)──► P⁻ phình nhẹ
  update:   P⁻ ─(K)─► P co  (nếu z hữu ích)

2.2. EKF / UKF

2.3. Particle Filter — 3 bước

  1. SAMPLE   x^{[i]} ~ p(x_t | x_{t-1}^{[i]}, u_t)     # motion
  2. WEIGHT   w^{[i]} ∝ p(z_t | x^{[i]})                 # sensor
  3. RESAMPLE sao chép hạt theo w  (tránh degeneracy)

Nhược: số hạt tăng theo chiều state; degeneracy nếu không resample / thiếu hạt.

2.4. Bảng chọn nhanh

3. Demo — KF 1D (+ ý PF)

"""
KF 1D: vị trí tăng ~1 mỗi bước, đo nhiễu mạnh.
Xem hat mượt hơn z; P giảm khi hội tụ.
"""
import numpy as np

A, C = 1.0, 1.0
Q, R = 0.5, 4.0          # đo xấu hơn quá trình (R > Q)
x_true = 0.0
x_hat, P = 0.0, 10.0     # ban đầu rất không chắc
rng = np.random.default_rng(0)

print("t   true     z      hat     P      K")
for t in range(8):
    x_true = x_true + 1.0 + 0.1 * rng.normal()
    z = x_true + 2.0 * rng.normal()

    # PREDICT
    x_hat = A * x_hat
    P = A * P * A + Q

    # UPDATE
    S = C * P * C + R
    K = (P * C) / S
    x_hat = x_hat + K * (z - C * x_hat)
    P = (1 - K * C) * P
    print(f"{t}  {x_true:6.2f}  {z:6.2f}  {x_hat:6.2f}  {P:5.2f}  {K:4.2f}")

# Mini PF: trọng số 5 hạt quanh đo z=3.0
particles = np.array([1.0, 2.5, 3.0, 3.2, 6.0])
z_pf, sigma = 3.0, 0.8
w = np.exp(-0.5 * ((particles - z_pf) / sigma) ** 2)
w /= w.sum()
print("PF weights", np.round(w, 3), "→ hạt gần z nặng; resample nhân bản chúng")

🧪 Kỳ vọng demo: cột hat bám true tốt hơn z thô; P thường giảm từ ~10 xuống vài đơn vị; K phản ánh mức tin đo. Dòng PF: trọng số cao quanh 3.0 / 3.2, thấp ở 1.0 và 6.0.

4. Gắn RL–UAV / đa mục tiêu

• Policy ăn \(\hat{x}\) từ EKF khác hẳn policy ăn IMU thô — khai báo rõ trong MDP.
• Train chân thực: nhiễu hóa state, thêm độ trễ, hoặc đưa đầu ra estimator vào vòng sim.
• GPS urban: tăng \(R\) khi DOP xấu / reject innovation lớn (gating) — tránh KF “tin” multipath.
• Gió: tăng \(Q\) khi gió mạnh (motion model kém) → \(K\) mở ra nghe đo hơn (nếu đo còn tốt).
• Đa mục tiêu: dùng \(P\) (hoặc entropy PF) để nới khoảng cách an toàn khi lập path coverage.
• Năng lượng: filter ổn → ít phải bay “re-observe”; \(P\) lớn gần vật cản → chậm lại / đổi path (tốn pin đổi an toàn).

5. Ưu / nhược

Nghĩa với năng lượng / gió / GPS: chỉnh \(Q,R\) theo điều kiện = chỉnh “tin ai”; đừng chỉnh tùy hứng không đơn vị.

6. Lỗi thường gặp

⚠️ Lỗi hay gặp: Dùng KF cho global localization đa đỉnh → bám nhầm một đỉnh.
⚠️ \(Q,R\) chỉnh tùy hứng không gắn đơn vị vật lý → \(K\) vô nghĩa.
⚠️ Quên EKF chỉ tốt cục bộ khi tuyến tính hóa còn hợp lý.
⚠️ PF không resample → một hạt chiếm gần hết trọng số (degeneracy).

7. Checklist

1. Khi nào KF đủ, khi nào buộc phải PF?
2. Kalman gain đang “tin” đo hay tin model?
3. Ba bước PF là gì? Degeneracy nghĩa là gì?
4. Trong firmware bạn dùng, estimator là EKF hay UKF — \(z\) gồm những kênh nào?

⚡ Nhớ một câu: KF = một chuông Gauss cập nhật bằng \(K\); PF = đám hạt sống chết theo likelihood — cùng xấp xỉ Bayes, khác hình dạng belief.

🔗 Sang bài sau mang theo: Đặt PF/EKF lên map cố định → localization: tracking vs global vs kidnapped.

  ★ Hình nhớ mãi:
  KF ────╱╲────     PF ··●··●··
       tin K              resample

Trước: Lec12 · Sau: Lec14