MODULE 3 — Estimation, Localization, Mapping, SLAM (Lecture 11–16)

📚 Học sâu: Lectures/ Lec11–16. File này = ôn nhanh.

z ≠ x │ ▼ belief ──predict──► phình ──update──► co │ ├── KF (1 Gauss) / PF (nhiều đỉnh) ├── Localization | Mapping | SLAM └── đô thị GPS kém: clearance phải theo bất định

Mục tiêu module.
Từ Module 1–2, hầu hết suy luận đều giả định ta biết state \(x\). Module này gỡ giả định đó: cảm biến chỉ cho observation \(z\) (nhiễu, thiếu), nên robot phải duy trì belief \(bel(x)\) — phân phối về “mình đang ở trạng thái nào”.

Đây là module then chốt nếu bạn làm RL–UAV muốn sim-to-real: nhiều môi trường huấn luyện “cheat” bằng cách đưa thẳng \(x\) cho policy. Máy thật không làm vậy.

Luồng module:

Lec 11  Ý tưởng filter (predict–update)
   ↓
Lec 12  Bayes filter (công thức chuẩn)
   ↓
Lec 13  Hai cách xấp xỉ: Kalman Filter & Particle Filter
   ↓
Lec 14  Localization   (biết map → tìm pose)
Lec 15  Mapping        (biết pose → xây map)
Lec 16  SLAM           (không biết cả hai → giải đồng thời)

Lecture 11 — Filter không xác định (Nondeterministic Filter)

0. Mục tiêu học

Hiểu vì sao phải dùng tập hợp / phân phối thay vì một điểm ước lượng; phân biệt hai bước predict và update.

1. Câu hỏi trung tâm

Khi động học và đo lường đều không chắc chắn, ta “giữ kiến thức” về state như thế nào?

2. Trực giác

Bạn đi trong phòng tối. Mỗi bước chân, vị trí có thể lệch vài chục centimet. Mỗi lần sờ tường: “à, khoảng khu vực này”. Bạn không chốt một điểm tọa độ, mà giữ một vùng nghi ngờ, rồi thu hẹp dần khi có thêm bằng chứng.

Đó chính là tinh thần của mọi bộ lọc trạng thái (state filter).

3. Cốt lõi kỹ thuật

Belief \(bel(x_t)\): mô tả độ tin về state tại thời điểm \(t\). Ở bài này, tạm hiểu như một tập hợp các state còn khả dĩ; sang Lec 12 sẽ là phân phối xác suất.
Vòng lặp hai bước, lặp lại mỗi chu kỳ cảm biến/điều khiển:

Predict (dự đoán): từ \(bel(x_{t-1})\) và lệnh đã gửi \(u_t\), suy ra belief tiên nghiệm \(bel^-(x_t)\).
Vì động học có nhiễu / mô hình không hoàn hảo → vùng belief phình ra.
Update (cập nhật đo): nhận observation \(z_t\), loại bỏ những state không giải thích được phép đo → belief hậu nghiệm \(bel(x_t)\).
Vùng belief co lại (nếu cảm biến hữu ích).

Nondeterministic filter (set-membership): biểu diễn bằng tập hợp, chưa dùng xác suất. Predict = hợp mọi chuyển tiếp khả dĩ; update = giao với tập state nhất quán với \(z_t\).

4. Sơ đồ

  bel(x_{t-1})  ──(u_t)──►  bel⁻(x_t)  ──(z_t)──►  bel(x_t)  ──► …
     ◯◯◯                    ◯◯◯◯◯◯                   ◯◯
   (gọn)                   (phình)                  (co lại)

Nếu chỉ predict mãi mà không update: uncertainty tăng không Bound → robot “quên” mình đang ở đâu.
Nếu chỉ update mà không predict: bỏ qua thông tin chuyển động, phản ứng chậm và dễ mâu thuẫn giữa các phép đo.

5. Demo ý tưởng (tập hợp 1D)

# Vùng nghi ngờ là đoạn [lo, hi]. Bước đi + đo khoảng cách tới tường tại 0.
lo, hi = 2.0, 2.0          # biết khá chắc đang ở x=2
u, process_slack = 1.0, 0.3  # muốn đi +1, nhưng lệch tới ±0.3
lo, hi = lo + u - process_slack, hi + u + process_slack   # predict: phình
print("Sau predict:", lo, hi)

# Đo: "cách tường 3.0 ± 0.4" → x ∈ [2.6, 3.4]
z_lo, z_hi = 2.6, 3.4
lo, hi = max(lo, z_lo), min(hi, z_hi)                     # update: giao
print("Sau update :", lo, hi)

6. Gắn với RL–UAV

Observation \(z\) không đồng nhất với state \(x\). Policy chỉ nhìn \(z\) (hoặc lịch sử \(z\)) → bài toán gần POMDP.
Estimator tốt (belief gọn, ít lệch) = observation “giàu” hơn = RL dễ học hơn.
Khi thiết kế env: hãy tự hỏi policy nhận \(x\) thật hay \(\hat{x}\)/\(z\)? Câu trả lời quyết định độ chân thực sim-to-real.

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

Coi \(\hat{x}\) điểm ước lượng là đủ, quên phương sai / đa đỉnh → quyết định liều khi đang rất không chắc.
Nhầm predict với update: “càng bay càng chắc vị trí” mà không có cảm biến hiệu chỉnh — thực tế ngược lại nếu chỉ có odometry.

8. Checklist

Predict làm belief phình; update làm belief co — vì sao?
Nêu một ví dụ UAV trong đó \(z\) thiếu hẳn một phần của \(x\).
Vì sao chỉ dùng một điểm \(\hat{x}\) có thể nguy hiểm?

Lecture 12 — Bayes Filtering

0. Mục tiêu học

Viết và giải thích được hai phương trình Bayes filter; phân biệt motion model và sensor model.

1. Câu hỏi trung tâm

Vòng predict–update viết chính xác bằng xác suất như thế nào?

2. Trực giác

Bayes nói: niềm tin mới tỷ lệ với (độ khớp của phép đo) nhân với (niềm tin trước đó).

Ví dụ tìm remote: bạn nghĩ “hay để ở sofa”. Nhìn sofa không thấy → giảm tin sofa, tăng tin bàn/kệ. Mỗi quan sát chỉnh lại phân phối niềm tin — không phải nhảy cóc sang một kết luận duy nhất ngay lập tức.

3. Cốt lõi kỹ thuật

Predict (Chapman–Kolmogorov):
\[ \overline{bel}(x_t) = \int p(x_t \mid x_{t-1}, u_t)\, bel(x_{t-1})\, dx_{t-1} \]

Update (Bayes):
\[ bel(x_t) = \eta\, p(z_t \mid x_t)\, \overline{bel}(x_t) \]

Trong đó:
- \(p(x_t \mid x_{t-1}, u_t)\): motion model — state tiến hóa thế nào khi có lệnh \(u_t\) (và nhiễu quá trình).
- \(p(z_t \mid x_t)\): sensor model / likelihood — nếu state là \(x_t\) thì xác suất thấy đo \(z_t\) là bao nhiêu.
- \(\eta\): hằng số chuẩn hóa để \(bel\) tổng/tích phân bằng 1.

Điểm then chốt: Kalman Filter, Extended KF, Unscented KF, Particle Filter… chỉ là các cách xấp xỉ hai tích phân/phép nhân trên khi không gian liên tục và mô hình phức tạp.

4. Sơ đồ đọc công thức

bel(x_{t-1})
    │  nhân với motion model p(x_t | x_{t-1}, u_t), rồi tích phân
    ▼
bel̄(x_t)          ← còn gọi bel⁻ hoặc prior belief tại t
    │  nhân với likelihood p(z_t | x_t), rồi chuẩn hóa η
    ▼
bel(x_t)           ← posterior belief tại t

5. Demo — Bayes rời rạc trên 3 ô

import numpy as np

# Ba ô A,B,C. Prior đều.
bel = np.array([1/3, 1/3, 1/3])

# Motion: có xu hướng sang phải (cột = từ, hàng = đến; đã đơn giản hóa biên)
T = np.array([[0.2, 0.0, 0.0],
              [0.8, 0.2, 0.0],
              [0.0, 0.8, 1.0]])
bel_bar = T @ bel

# Đo kiểu "gần tường": A và C khớp mạnh hơn B
lik = np.array([0.9, 0.1, 0.9])
bel = lik * bel_bar
bel /= bel.sum()
print("bel sau 1 chu kỳ:", np.round(bel, 3))

Hãy chạy thêm vài chu kỳ: nếu cứ đo “tường” và chuyển động sang phải, khối tin sẽ dồn về đâu? Đó là cảm giác “filter đang hội tụ”.

6. Gắn với RL–UAV

Belief là trạng thái đủ thông tin (sufficient statistic) cho POMDP trong nhiều trường hợp.
Recurrent policy / belief-state RL: mạng cố học một xấp xỉ của \(bel\), rồi quyết định từ đó.
Khi reward phụ thuộc state thật nhưng policy chỉ thấy \(z\), bạn đang tối ưu một bài khác với “full-state RL” trong sim — cần nói rõ trong paper.

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

Quên chuẩn hóa \(\eta\) → “xác suất” không còn là xác suất.
Dùng sensor model quá lạc quan (likelihood rất nhọn) → filter tự tin sai khi cảm biến outlier.
Nhầm \(p(z|x)\) với \(p(x|z)\).

8. Checklist

Viết lại 2 phương trình Bayes filter không nhìn tài liệu.
Motion model khác sensor model ở chỗ nào?
Vì sao nói “mọi filter chỉ là xấp xỉ Bayes filter”?

Lecture 13 — Kalman Filter và Particle Filter

0. Mục tiêu học

Biết khi nào dùng KF/EKF, khi nào cần PF; đọc được ý nghĩa Kalman gain; nắm 3 bước PF.

1. Câu hỏi trung tâm

Hai cách xấp xỉ \(bel(x)\) phổ biến nhất — Gaussian (KF) và đám hạt (PF) — khác nhau thế nào?

2. Trực giác

Kalman Filter (KF): tin bằng một “chuông Gauss” — chỉ cần mean và covariance. Rất nhanh, rất đẹp 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ì được trọng số cao; hạt lệch thì chết dần qua resampling. Mạnh khi belief đa đỉnh hoặc phi tuyến mạnh (ví dụ: mới vào phòng, chưa biết mình ở góc nào).

KF:   ─────╱╲─────          một đỉnh, elip bất định
PF:   · · ● · · ● · ·       đám mây; resample giữ các ● nặng

3. Cốt lõi kỹ thuật

3.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 \]
với \(w_t \sim \mathcal{N}(0,Q)\), \(v_t \sim \mathcal{N}(0,R)\).

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

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

Kalman gain \(K\):
- Tin đo nhiều / model kém (\(R\) nhỏ hơn tương đối) → \(K\) lớn → sửa mạnh theo \(z\).
- Tin model nhiều / đo xấu (\(R\) lớn) → \(K\) nhỏ → bám dự đoán.

3.2. EKF / UKF

EKF: tuyến tính hóa \(f,h\) quanh ước lượng hiện tại, rồi chạy KF. Phổ biến trên firmware UAV.
UKF: chọn sigma point để xấp xỉ lan truyền moment — thường ổn hơn EKF khi phi tuyến vừa phải, không cần Jacobian tường minh.

3.3. Particle Filter (ba bước)

Sample: lấy hạt mới từ motion model \(x_t^{[i]} \sim p(x_t \mid x_{t-1}^{[i]}, u_t)\).
Weight: \(w^{[i]} \propto p(z_t \mid x_t^{[i]})\).
Resample: sinh tập hạt mới theo trọng số (hạt nặng được sao chép nhiều).

Nhược điểm PF: số hạt tăng nhanh theo chiều state; dễ degeneracy (một hạt chiếm gần hết trọng số) nếu không resample / không đủ hạt.

4. Demo — KF 1D theo dõi vị trí

import numpy as np

A, C, Q, R = 1.0, 1.0, 0.5, 4.0
x_true, x_hat, P = 0.0, 0.0, 10.0
rng = np.random.default_rng(0)

for t in range(8):
    x_true += 1.0 + rng.normal()*0.1
    z = x_true + rng.normal()*2.0
    # predict
    x_hat = A * x_hat
    P = A * P * A + Q
    # update
    K = P * C / (C * P * C + R)
    x_hat = x_hat + K * (z - C * x_hat)
    P = (1 - K * C) * P
    print(f"t={t}  true={x_true:5.2f}  z={z:5.2f}  hat={x_hat:5.2f}  P={P:4.2f}")

Quan sát: \(P\) thường giảm sau vài bước nếu cảm biến thực sự chứa thông tin; \(\hat{x}\) mượt hơn \(z\) thô.

5. Gắn với RL–UAV

Firmware (PX4, Crazyflie…): EKF/UKF trên IMU + baro + GPS/optical flow là chuẩn.
Policy ăn \(\hat{x}\) từ EKF khác hẳn policy ăn IMU thô: độ khó học và sim-to-real đổi hoàn toàn — hãy khai báo rõ trong MDP.
Muốn train chân thực: nhiễu hóa state, thêm độ trễ, hoặc dùng đầu ra estimator trong vòng lặp sim.

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

Dùng KF cho bài global localization đa đỉnh → filter “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 rằng EKF chỉ đảm bảo tốt cục bộ khi tuyến tính hóa còn hợp lý.

7. Checklist

Khi nào KF đủ, khi nào buộc phải PF?
Kalman gain đang “tin” đo hay tin model?
Ba bước PF là gì? Degeneracy nghĩa là gì?

Lecture 14 — Localization

0. Mục tiêu học

Phát biểu bài toán localization; phân biệt tracking / global localization / kidnapped robot; hiểu MCL.

1. Câu hỏi trung tâm

Đã có bản đồ — làm sao biết “tôi đang ở đâu trên map”?

2. Trực giác

Vào trung tâm thương mại: nhìn biển hiệu cửa hàng, đối chiếu sơ đồ, chốt “mình đang ở tầng 2 gần cầu thang”.
Localization = ước lượng pose trên map đã biết.

MCL (Monte Carlo Localization) = Particle Filter trên pose \((x,y,\theta)\) (hoặc pose 3D), với map cố định.

3. Cốt lõi kỹ thuật

Ba chế độ khó tăng dần:

Chế độ	Tình huống	Công cụ điển hình
Tracking	Đã biết gần đúng pose	EKF / ít hạt PF
Global localization	Không biết gì lúc bắt đầu	PF nhiều hạt, belief đa đỉnh
Kidnapped robot	Bị “teleport” / mất tracking	Cần thêm hạt ngẫu nhiên để phục hồi

Thành phần cần có:
- Motion model: odometry / lệnh điều khiển + nhiễu.
- Sensor model: laser beam model, likelihood field, hoặc khớp đặc trưng ảnh.
- Resampling mỗi chu kỳ; thường chèn ~1% hạt ngẫu nhiên chống kidnap.

  map cố định + u (odometry) + z (laser/camera)
                    │
                    ▼
         Particle Filter trên pose
                    │
                    ▼
              ước lượng (x, y, θ)

4. Demo ý tưởng — đa đỉnh trước khi hội tụ

Tưởng tượng hành lang có hai chỗ giống hệt nhau: belief PF có thể có hai chùm hạt. Chỉ khi đi thêm và thấy chi tiết khác biệt, một chùm chết dần.
KF chỉ giữ một Gauss → dễ bám nhầm từ đầu. Đây là lý do global localization thích PF.

5. Gắn với RL–UAV

Nhiều env RL đưa state “ privileged ” (ground truth). Muốn paper gần thật: train với \(\hat{x}\) nhiễu hoặc với obs onboard.
Nếu dùng GPS ngoài trời: localization dễ hơn trong nhà; trong nhà thường cần VIO / marker / UWB.
Reward phạt theo sai số pose ước lượng khác với phạt theo pose thật — cần nhất quán với những gì robot biết lúc deploy.

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

Global localization mà dùng quá ít hạt → không cover các giả thuyết.
Không có cơ chế kidnap recovery → một lần mất lock là hỏng cả nhiệm vụ.
Đánh giá policy chỉ trên state thật rồi ngạc nhiên khi onboard estimator lệch.

7. Checklist

Tracking khác global localization ở điểm nào?
Vì sao cần một tỷ lệ hạt ngẫu nhiên?
Map trong localization đóng vai trò gì trong sensor model?

Lecture 15 — Mapping

0. Mục tiêu học

Hiểu occupancy grid và cập nhật log-odds; biết map dùng vào planning/reward thế nào.

1. Câu hỏi trung tâm

Đã biết (gần đúng) mình đang ở đâu — làm sao xây bản đồ thế giới?

2. Trực giác

Robot hút bụi lần đầu chạy quanh nhà: vừa đi vừa tô tường, ghế, cửa. Lần sau nó đi khôn hơn vì đã có map.
Mapping = ước lượng bản đồ \(m\) khi pose được xem là biết (hoặc đủ tốt).

3. Cốt lõi kỹ thuật — Occupancy Grid

Chia không gian thành ô. Mỗi ô \(i\) giữ xác suất đang bị chiếm \(p(m_i=1)\).
Làm việc trên log-odds để cập nhật ổn định số:
\[ l_{t,i} = l_{t-1,i} + \underbrace{\text{inverse\_sensor\_model}(z_t, x_t, i)}_{\text{bằng chứng mới}} - l_0 \]
Tia cảm biến:
ô bị “trúng” → tăng occupied;
các ô trên đoạn từ sensor tới điểm trúng → tăng free;
ngoài tầm nhìn → giữ nguyên.

  ....####....     .  free
  ..##....##..     #  occupied
  ##........##     (ô chưa quan sát: prior / unknown)

Map còn có dạng khác: đám landmark thưa, mesh, semantic map… — occupancy grid là điểm vào chuẩn mực.

4. Demo ý tưởng — log-odds một ô

import numpy as np

def prob_to_logodds(p):
    return np.log(p / (1 - p))

def logodds_to_prob(l):
    return 1 - 1 / (1 + np.exp(l))

l = prob_to_logodds(0.5)          # chưa biết
for evidence in [0.7, 0.8, 0.75]: # vài lần “thấy occupied”
    l += prob_to_logodds(evidence) - prob_to_logodds(0.5)
    print("p(occupied) ≈", round(logodds_to_prob(l), 3))

5. Gắn với RL–UAV

Occupancy / cost map → input cho planner (Module 2) và cho reward (phạt gần vật cản).
Map cập nhật online khiến môi trường non-stationary với RL: policy phụ thuộc map đang đổi. Thực dụng: dùng local map trượt, hoặc đóng băng mapper khi đánh giá policy.
Sai map (ảo ảnh tường) có thể làm policy “né bóng ma” hoặc đâm lỗ trống — cần robust.

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

Cộng xác suất trực tiếp thay vì log-odds → bão hòa số / mất đối xứng.
Coi mọi ô độc lập hoàn toàn (giả định occupancy grid) rồi kỳ vọng bản đồ mượt như thật — đây chỉ là xấp xỉ.
Xây map khi pose đang rất lệch → map “bôi nhòe”; lúc đó cần SLAM (Lec 16), không phải mapping thuần.

7. Checklist

Tia laser xuyên không khí / chạm vật: log-odds các ô đổi ra sao?
Vì sao dùng log-odds thay vì nhân xác suất thô?
Map online tạo khó khăn gì cho huấn luyện RL?

Lecture 16 — SLAM

0. Mục tiêu học

Phát biểu bài toán SLAM; hiểu vòng lặp gà–trứng và vai trò loop closure; nắm các họ thuật toán ở mức “đủ để đọc paper”.

1. Câu hỏi trung tâm

Không biết pose và cũng không biết map — giải đồng thời thế nào?

2. Trực giác

Lạc trong rừng không GPS: bạn vừa đánh dấu cây lạ, vừa đoán mình đã đi bao xa.
Pose sai → map vẽ sai → dùng map sai lại làm pose sai hơn. Đó là vòng gà–trứng.
SLAM (Simultaneous Localization and Mapping) siết hai thứ cùng lúc.

Khi loop closure (nhận ra “chỗ này mình đã từng qua”): có ràng buộc mạnh để sửa cả một đoạn quỹ đạo quá khứ — map hết bị trôi một phía.

Không có loop:   quỹ đạo và map trôi theo nhiễu odometry tích lũy
Có loop:         khép vòng  ●═══●  → tối ưu lại cả chuỗi pose

3. Cốt lõi kỹ thuật

Full SLAM: ước lượng cả quỹ đạo và map
\(p(x_{1:t}, m \mid z_{1:t}, u_{1:t})\).
Online SLAM: chủ yếu quan tâm \(p(x_t, m \mid \ldots)\) để chạy realtime.

Họ thuật toán (chỉ cần nhớ ý tưởng):

Họ	Ý tưởng	Điểm mạnh / yếu
EKF-SLAM	State = pose + toàn bộ landmark	Cổ điển; nặng \(O(n^2)\) theo số landmark
FastSLAM	PF trên quỹ đạo; mỗi hạt một map riêng	Xử lý đa giả thuyết tốt hơn EKF thuần
Graph-SLAM / pose-graph	Nút = pose; cạnh = odometry / loop; tối ưu least-squares	Chuẩn thực tế; rõ ràng khi có loop closure
Visual SLAM / VIO	Ảnh (+ IMU): ORB-SLAM, VINS, …	Chuẩn UAV trong nhà / GPS-denied

Với UAV: VIO (visual–inertial odometry) thường là lớp nền; “full SLAM có map tái sử dụng” là tầng trên khi cần quay lại vùng cũ.

4. Sơ đồ pipeline thực dụng trên drone

IMU + camera
    │
    ▼
  VIO / Visual SLAM  →  pose ĥ(t), map/local map
    │
    ├──────────────► controller / tracker
    └──────────────► planner hoặc obs cho RL policy

5. Gắn với RL–UAV

Active SLAM: dùng RL để chọn hành vi vừa khám phá vừa giữ localization — đây là hướng nghiên cứu thật, không chỉ bài tập.
Policy phụ thuộc map đang xây → non-stationarity; thực dụng nên tách rõ: mapper cập nhật chậm, policy nhìn local cost map.
Sim-to-real: drift VIO, trượt feature khi thiếu texture, rung camera — đó là reality gap kiểu estimation, không phải chỉ dynamics.
Paper cần nêu: policy có được pose ground-truth lúc train không? Lúc test onboard dùng gì?

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

Gọi mọi thứ có camera là “SLAM” — odometry thuần không có loop closure thì map sẽ trôi.
Đánh giá RL trong sim không có drift, rồi đổ lỗi “policy kém” khi lên máy thật.
Tối ưu graph sai thông số nhiễu cạnh (covariance) → loop closure kéo map vỡ.

7. Checklist

Vì sao loop closure quan trọng hơn chỉ “odometry tốt”?
EKF-SLAM và pose-graph khác nhau ở cách biểu diễn bài toán như thế nào?
Với task RL–UAV của bạn, observation cuối cùng lấy từ VIO hay từ state giả lập?

Tổng kết Module 3

Bayes filter là xương sống. KF/PF là hai cách xấp xỉ chính.
Localization = ước lượng pose khi đã có map. Mapping = ước lượng map khi đã có pose.
SLAM = làm đồng thời cả hai, và sống nhờ loop closure.
Với RL–UAV: đừng giả vờ \(z = x\) nếu máy thật không cho phép — hãy đưa assumption này vào MDP và vào phần thí nghiệm.

Bài tiếp: 04_Vision_Learning.md — khi observation chính là ảnh.

Ebook Robotics · RL–UAV

MODULE 3 — Estimation, Localization, Mapping, SLAM (Lecture 11–16)

Bản ôn nhanh theo module

Chế độ Slide · phím ← → · F toàn màn · Esc về đọc · S mở slide

📚 Học sâu: Lectures/ Lec11–16. File này = ôn nhanh.

z ≠ x │ ▼ belief ──predict──► phình ──update──► co │ ├── KF (1 Gauss) / PF (nhiều đỉnh) ├── Localization | Mapping | SLAM └── đô thị GPS kém: clearance phải theo bất định

Luồng module:

Lec 11  Ý tưởng filter (predict–update)
   ↓
Lec 12  Bayes filter (công thức chuẩn)
   ↓
Lec 13  Hai cách xấp xỉ: Kalman Filter & Particle Filter
   ↓
Lec 14  Localization   (biết map → tìm pose)
Lec 15  Mapping        (biết pose → xây map)
Lec 16  SLAM           (không biết cả hai → giải đồng thời)

Lecture 11 — Filter không xác định (Nondeterministic Filter)

0. Mục tiêu học

Hiểu vì sao phải dùng tập hợp / phân phối thay vì một điểm ước lượng; phân biệt hai bước predict và update.

1. Câu hỏi trung tâm

Khi động học và đo lường đều không chắc chắn, ta “giữ kiến thức” về state như thế nào?

2. Trực giác

Đó chính là tinh thần của mọi bộ lọc trạng thái (state filter).

3. Cốt lõi kỹ thuật

Belief \(bel(x_t)\): mô tả độ tin về state tại thời điểm \(t\). Ở bài này, tạm hiểu như một tập hợp các state còn khả dĩ; sang Lec 12 sẽ là phân phối xác suất.
Vòng lặp hai bước, lặp lại mỗi chu kỳ cảm biến/điều khiển:

Predict (dự đoán): từ \(bel(x_{t-1})\) và lệnh đã gửi \(u_t\), suy ra belief tiên nghiệm \(bel^-(x_t)\).
Vì động học có nhiễu / mô hình không hoàn hảo → vùng belief phình ra.
Update (cập nhật đo): nhận observation \(z_t\), loại bỏ những state không giải thích được phép đo → belief hậu nghiệm \(bel(x_t)\).
Vùng belief co lại (nếu cảm biến hữu ích).

Nondeterministic filter (set-membership): biểu diễn bằng tập hợp, chưa dùng xác suất. Predict = hợp mọi chuyển tiếp khả dĩ; update = giao với tập state nhất quán với \(z_t\).

4. Sơ đồ

  bel(x_{t-1})  ──(u_t)──►  bel⁻(x_t)  ──(z_t)──►  bel(x_t)  ──► …
     ◯◯◯                    ◯◯◯◯◯◯                   ◯◯
   (gọn)                   (phình)                  (co lại)

5. Demo ý tưởng (tập hợp 1D)

# Vùng nghi ngờ là đoạn [lo, hi]. Bước đi + đo khoảng cách tới tường tại 0.
lo, hi = 2.0, 2.0          # biết khá chắc đang ở x=2
u, process_slack = 1.0, 0.3  # muốn đi +1, nhưng lệch tới ±0.3
lo, hi = lo + u - process_slack, hi + u + process_slack   # predict: phình
print("Sau predict:", lo, hi)

# Đo: "cách tường 3.0 ± 0.4" → x ∈ [2.6, 3.4]
z_lo, z_hi = 2.6, 3.4
lo, hi = max(lo, z_lo), min(hi, z_hi)                     # update: giao
print("Sau update :", lo, hi)

6. Gắn với RL–UAV

Observation \(z\) không đồng nhất với state \(x\). Policy chỉ nhìn \(z\) (hoặc lịch sử \(z\)) → bài toán gần POMDP.
Estimator tốt (belief gọn, ít lệch) = observation “giàu” hơn = RL dễ học hơn.
Khi thiết kế env: hãy tự hỏi policy nhận \(x\) thật hay \(\hat{x}\)/\(z\)? Câu trả lời quyết định độ chân thực sim-to-real.

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

Coi \(\hat{x}\) điểm ước lượng là đủ, quên phương sai / đa đỉnh → quyết định liều khi đang rất không chắc.
Nhầm predict với update: “càng bay càng chắc vị trí” mà không có cảm biến hiệu chỉnh — thực tế ngược lại nếu chỉ có odometry.

8. Checklist

Predict làm belief phình; update làm belief co — vì sao?
Nêu một ví dụ UAV trong đó \(z\) thiếu hẳn một phần của \(x\).
Vì sao chỉ dùng một điểm \(\hat{x}\) có thể nguy hiểm?

Lecture 12 — Bayes Filtering

0. Mục tiêu học

Viết và giải thích được hai phương trình Bayes filter; phân biệt motion model và sensor model.

1. Câu hỏi trung tâm

Vòng predict–update viết chính xác bằng xác suất như thế nào?

2. Trực giác

Bayes nói: niềm tin mới tỷ lệ với (độ khớp của phép đo) nhân với (niềm tin trước đó).

3. Cốt lõi kỹ thuật

Predict (Chapman–Kolmogorov):
\[ \overline{bel}(x_t) = \int p(x_t \mid x_{t-1}, u_t)\, bel(x_{t-1})\, dx_{t-1} \]

Update (Bayes):
\[ bel(x_t) = \eta\, p(z_t \mid x_t)\, \overline{bel}(x_t) \]

4. Sơ đồ đọc công thức

bel(x_{t-1})
    │  nhân với motion model p(x_t | x_{t-1}, u_t), rồi tích phân
    ▼
bel̄(x_t)          ← còn gọi bel⁻ hoặc prior belief tại t
    │  nhân với likelihood p(z_t | x_t), rồi chuẩn hóa η
    ▼
bel(x_t)           ← posterior belief tại t

5. Demo — Bayes rời rạc trên 3 ô

import numpy as np

# Ba ô A,B,C. Prior đều.
bel = np.array([1/3, 1/3, 1/3])

# Motion: có xu hướng sang phải (cột = từ, hàng = đến; đã đơn giản hóa biên)
T = np.array([[0.2, 0.0, 0.0],
              [0.8, 0.2, 0.0],
              [0.0, 0.8, 1.0]])
bel_bar = T @ bel

# Đo kiểu "gần tường": A và C khớp mạnh hơn B
lik = np.array([0.9, 0.1, 0.9])
bel = lik * bel_bar
bel /= bel.sum()
print("bel sau 1 chu kỳ:", np.round(bel, 3))

Hãy chạy thêm vài chu kỳ: nếu cứ đo “tường” và chuyển động sang phải, khối tin sẽ dồn về đâu? Đó là cảm giác “filter đang hội tụ”.

6. Gắn với RL–UAV

Belief là trạng thái đủ thông tin (sufficient statistic) cho POMDP trong nhiều trường hợp.
Recurrent policy / belief-state RL: mạng cố học một xấp xỉ của \(bel\), rồi quyết định từ đó.
Khi reward phụ thuộc state thật nhưng policy chỉ thấy \(z\), bạn đang tối ưu một bài khác với “full-state RL” trong sim — cần nói rõ trong paper.

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

Quên chuẩn hóa \(\eta\) → “xác suất” không còn là xác suất.
Dùng sensor model quá lạc quan (likelihood rất nhọn) → filter tự tin sai khi cảm biến outlier.
Nhầm \(p(z|x)\) với \(p(x|z)\).

8. Checklist

Viết lại 2 phương trình Bayes filter không nhìn tài liệu.
Motion model khác sensor model ở chỗ nào?
Vì sao nói “mọi filter chỉ là xấp xỉ Bayes filter”?

Lecture 13 — Kalman Filter và Particle Filter

0. Mục tiêu học

Biết khi nào dùng KF/EKF, khi nào cần PF; đọc được ý nghĩa Kalman gain; nắm 3 bước PF.

1. Câu hỏi trung tâm

Hai cách xấp xỉ \(bel(x)\) phổ biến nhất — Gaussian (KF) và đám hạt (PF) — khác nhau thế nào?

2. Trực giác

Kalman Filter (KF): tin bằng một “chuông Gauss” — chỉ cần mean và covariance. Rất nhanh, rất đẹp 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ì được trọng số cao; hạt lệch thì chết dần qua resampling. Mạnh khi belief đa đỉnh hoặc phi tuyến mạnh (ví dụ: mới vào phòng, chưa biết mình ở góc nào).

KF:   ─────╱╲─────          một đỉnh, elip bất định
PF:   · · ● · · ● · ·       đám mây; resample giữ các ● nặng

3. Cốt lõi kỹ thuật

3.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 \]
với \(w_t \sim \mathcal{N}(0,Q)\), \(v_t \sim \mathcal{N}(0,R)\).

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

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

3.2. EKF / UKF

EKF: tuyến tính hóa \(f,h\) quanh ước lượng hiện tại, rồi chạy KF. Phổ biến trên firmware UAV.
UKF: chọn sigma point để xấp xỉ lan truyền moment — thường ổn hơn EKF khi phi tuyến vừa phải, không cần Jacobian tường minh.

3.3. Particle Filter (ba bước)

Sample: lấy hạt mới từ motion model \(x_t^{[i]} \sim p(x_t \mid x_{t-1}^{[i]}, u_t)\).
Weight: \(w^{[i]} \propto p(z_t \mid x_t^{[i]})\).
Resample: sinh tập hạt mới theo trọng số (hạt nặng được sao chép nhiều).

Nhược điểm PF: số hạt tăng nhanh theo chiều state; dễ degeneracy (một hạt chiếm gần hết trọng số) nếu không resample / không đủ hạt.

4. Demo — KF 1D theo dõi vị trí

import numpy as np

A, C, Q, R = 1.0, 1.0, 0.5, 4.0
x_true, x_hat, P = 0.0, 0.0, 10.0
rng = np.random.default_rng(0)

for t in range(8):
    x_true += 1.0 + rng.normal()*0.1
    z = x_true + rng.normal()*2.0
    # predict
    x_hat = A * x_hat
    P = A * P * A + Q
    # update
    K = P * C / (C * P * C + R)
    x_hat = x_hat + K * (z - C * x_hat)
    P = (1 - K * C) * P
    print(f"t={t}  true={x_true:5.2f}  z={z:5.2f}  hat={x_hat:5.2f}  P={P:4.2f}")

Quan sát: \(P\) thường giảm sau vài bước nếu cảm biến thực sự chứa thông tin; \(\hat{x}\) mượt hơn \(z\) thô.

5. Gắn với RL–UAV

Firmware (PX4, Crazyflie…): EKF/UKF trên IMU + baro + GPS/optical flow là chuẩn.
Policy ăn \(\hat{x}\) từ EKF khác hẳn policy ăn IMU thô: độ khó học và sim-to-real đổi hoàn toàn — hãy khai báo rõ trong MDP.
Muốn train chân thực: nhiễu hóa state, thêm độ trễ, hoặc dùng đầu ra estimator trong vòng lặp sim.

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

Dùng KF cho bài global localization đa đỉnh → filter “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 rằng EKF chỉ đảm bảo tốt cục bộ khi tuyến tính hóa còn hợp lý.

7. Checklist

Khi nào KF đủ, khi nào buộc phải PF?
Kalman gain đang “tin” đo hay tin model?
Ba bước PF là gì? Degeneracy nghĩa là gì?

Lecture 14 — Localization

0. Mục tiêu học

Phát biểu bài toán localization; phân biệt tracking / global localization / kidnapped robot; hiểu MCL.

1. Câu hỏi trung tâm

Đã có bản đồ — làm sao biết “tôi đang ở đâu trên map”?

2. Trực giác

MCL (Monte Carlo Localization) = Particle Filter trên pose \((x,y,\theta)\) (hoặc pose 3D), với map cố định.

3. Cốt lõi kỹ thuật

Ba chế độ khó tăng dần:

Chế độ	Tình huống	Công cụ điển hình
Tracking	Đã biết gần đúng pose	EKF / ít hạt PF
Global localization	Không biết gì lúc bắt đầu	PF nhiều hạt, belief đa đỉnh
Kidnapped robot	Bị “teleport” / mất tracking	Cần thêm hạt ngẫu nhiên để phục hồi

  map cố định + u (odometry) + z (laser/camera)
                    │
                    ▼
         Particle Filter trên pose
                    │
                    ▼
              ước lượng (x, y, θ)

4. Demo ý tưởng — đa đỉnh trước khi hội tụ

5. Gắn với RL–UAV

Nhiều env RL đưa state “ privileged ” (ground truth). Muốn paper gần thật: train với \(\hat{x}\) nhiễu hoặc với obs onboard.
Nếu dùng GPS ngoài trời: localization dễ hơn trong nhà; trong nhà thường cần VIO / marker / UWB.
Reward phạt theo sai số pose ước lượng khác với phạt theo pose thật — cần nhất quán với những gì robot biết lúc deploy.

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

Global localization mà dùng quá ít hạt → không cover các giả thuyết.
Không có cơ chế kidnap recovery → một lần mất lock là hỏng cả nhiệm vụ.
Đánh giá policy chỉ trên state thật rồi ngạc nhiên khi onboard estimator lệch.

7. Checklist

Tracking khác global localization ở điểm nào?
Vì sao cần một tỷ lệ hạt ngẫu nhiên?
Map trong localization đóng vai trò gì trong sensor model?

Lecture 15 — Mapping

0. Mục tiêu học

Hiểu occupancy grid và cập nhật log-odds; biết map dùng vào planning/reward thế nào.

1. Câu hỏi trung tâm

Đã biết (gần đúng) mình đang ở đâu — làm sao xây bản đồ thế giới?

2. Trực giác

3. Cốt lõi kỹ thuật — Occupancy Grid

Chia không gian thành ô. Mỗi ô \(i\) giữ xác suất đang bị chiếm \(p(m_i=1)\).
Làm việc trên log-odds để cập nhật ổn định số:
\[ l_{t,i} = l_{t-1,i} + \underbrace{\text{inverse\_sensor\_model}(z_t, x_t, i)}_{\text{bằng chứng mới}} - l_0 \]
Tia cảm biến:
ô bị “trúng” → tăng occupied;
các ô trên đoạn từ sensor tới điểm trúng → tăng free;
ngoài tầm nhìn → giữ nguyên.

  ....####....     .  free
  ..##....##..     #  occupied
  ##........##     (ô chưa quan sát: prior / unknown)

Map còn có dạng khác: đám landmark thưa, mesh, semantic map… — occupancy grid là điểm vào chuẩn mực.

4. Demo ý tưởng — log-odds một ô

import numpy as np

def prob_to_logodds(p):
    return np.log(p / (1 - p))

def logodds_to_prob(l):
    return 1 - 1 / (1 + np.exp(l))

l = prob_to_logodds(0.5)          # chưa biết
for evidence in [0.7, 0.8, 0.75]: # vài lần “thấy occupied”
    l += prob_to_logodds(evidence) - prob_to_logodds(0.5)
    print("p(occupied) ≈", round(logodds_to_prob(l), 3))

5. Gắn với RL–UAV

Occupancy / cost map → input cho planner (Module 2) và cho reward (phạt gần vật cản).
Map cập nhật online khiến môi trường non-stationary với RL: policy phụ thuộc map đang đổi. Thực dụng: dùng local map trượt, hoặc đóng băng mapper khi đánh giá policy.
Sai map (ảo ảnh tường) có thể làm policy “né bóng ma” hoặc đâm lỗ trống — cần robust.

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

Cộng xác suất trực tiếp thay vì log-odds → bão hòa số / mất đối xứng.
Coi mọi ô độc lập hoàn toàn (giả định occupancy grid) rồi kỳ vọng bản đồ mượt như thật — đây chỉ là xấp xỉ.
Xây map khi pose đang rất lệch → map “bôi nhòe”; lúc đó cần SLAM (Lec 16), không phải mapping thuần.

7. Checklist

Tia laser xuyên không khí / chạm vật: log-odds các ô đổi ra sao?
Vì sao dùng log-odds thay vì nhân xác suất thô?
Map online tạo khó khăn gì cho huấn luyện RL?

Lecture 16 — SLAM

0. Mục tiêu học

Phát biểu bài toán SLAM; hiểu vòng lặp gà–trứng và vai trò loop closure; nắm các họ thuật toán ở mức “đủ để đọc paper”.

1. Câu hỏi trung tâm

Không biết pose và cũng không biết map — giải đồng thời thế nào?

2. Trực giác

Khi loop closure (nhận ra “chỗ này mình đã từng qua”): có ràng buộc mạnh để sửa cả một đoạn quỹ đạo quá khứ — map hết bị trôi một phía.

Không có loop:   quỹ đạo và map trôi theo nhiễu odometry tích lũy
Có loop:         khép vòng  ●═══●  → tối ưu lại cả chuỗi pose

3. Cốt lõi kỹ thuật

Full SLAM: ước lượng cả quỹ đạo và map
\(p(x_{1:t}, m \mid z_{1:t}, u_{1:t})\).
Online SLAM: chủ yếu quan tâm \(p(x_t, m \mid \ldots)\) để chạy realtime.

Họ thuật toán (chỉ cần nhớ ý tưởng):

Họ	Ý tưởng	Điểm mạnh / yếu
EKF-SLAM	State = pose + toàn bộ landmark	Cổ điển; nặng \(O(n^2)\) theo số landmark
FastSLAM	PF trên quỹ đạo; mỗi hạt một map riêng	Xử lý đa giả thuyết tốt hơn EKF thuần
Graph-SLAM / pose-graph	Nút = pose; cạnh = odometry / loop; tối ưu least-squares	Chuẩn thực tế; rõ ràng khi có loop closure
Visual SLAM / VIO	Ảnh (+ IMU): ORB-SLAM, VINS, …	Chuẩn UAV trong nhà / GPS-denied

Với UAV: VIO (visual–inertial odometry) thường là lớp nền; “full SLAM có map tái sử dụng” là tầng trên khi cần quay lại vùng cũ.

4. Sơ đồ pipeline thực dụng trên drone

IMU + camera
    │
    ▼
  VIO / Visual SLAM  →  pose ĥ(t), map/local map
    │
    ├──────────────► controller / tracker
    └──────────────► planner hoặc obs cho RL policy

5. Gắn với RL–UAV

Active SLAM: dùng RL để chọn hành vi vừa khám phá vừa giữ localization — đây là hướng nghiên cứu thật, không chỉ bài tập.
Policy phụ thuộc map đang xây → non-stationarity; thực dụng nên tách rõ: mapper cập nhật chậm, policy nhìn local cost map.
Sim-to-real: drift VIO, trượt feature khi thiếu texture, rung camera — đó là reality gap kiểu estimation, không phải chỉ dynamics.
Paper cần nêu: policy có được pose ground-truth lúc train không? Lúc test onboard dùng gì?

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

Gọi mọi thứ có camera là “SLAM” — odometry thuần không có loop closure thì map sẽ trôi.
Đánh giá RL trong sim không có drift, rồi đổ lỗi “policy kém” khi lên máy thật.
Tối ưu graph sai thông số nhiễu cạnh (covariance) → loop closure kéo map vỡ.

7. Checklist

Vì sao loop closure quan trọng hơn chỉ “odometry tốt”?
EKF-SLAM và pose-graph khác nhau ở cách biểu diễn bài toán như thế nào?
Với task RL–UAV của bạn, observation cuối cùng lấy từ VIO hay từ state giả lập?

Tổng kết Module 3

Bayes filter là xương sống. KF/PF là hai cách xấp xỉ chính.
Localization = ước lượng pose khi đã có map. Mapping = ước lượng map khi đã có pose.
SLAM = làm đồng thời cả hai, và sống nhờ loop closure.
Với RL–UAV: đừng giả vờ \(z = x\) nếu máy thật không cho phép — hãy đưa assumption này vào MDP và vào phần thí nghiệm.

Bài tiếp: 04_Vision_Learning.md — khi observation chính là ảnh.