Chương C · biết map → ước lượng pose — tracking / global / kidnapped

Khung 6 câu hỏi

🖼️ Hình nhanh: ba chế độ khó tăng dần
TRACKING GLOBAL LOC KIDNAPPED ···●··· ● · ● · ● ●···· → (teleport) → ····? (1 chùm hẹp) (nhiều giả thuyết) (mất lock — cần hạt random)

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): Mới cất cánh giữa phố với orthomosaic/map sẵn — chưa biết mình ở ngã tư nào: đó là global localization (nhiều giả thuyết).
Đang bám ổn theo tường và góc nhà thì chỉ cần tracking (một chùm hẹp).
Mất lock trong hẻm rồi “tỉnh dậy” chỗ lạ = cảm giác kidnapped: phải rải lại hạt, không giả vờ vẫn biết — dùng EKF cho cả ba mức như mang dao gọt trái cây đi chặt củi.
Chốt: cùng map cố định, tracking ≠ global ≠ kidnapped — độ khó và số mode khác hẳn; clearance theo \(P\). ../00C.

0. Mục tiêu học

1. Phát biểu bài toán localization.
2. Phân biệt tracking / global localization / kidnapped robot.
3. Hiểu MCL = Particle Filter trên pose với map cố định.
4. Biết vì sao hành lang đối xứng → hai chùm hạt; KF dễ bám nhầm.

1. 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), map cố định.

UAV ngoài trời có GPS tốt ≈ tracking dễ. Trong nhà / urban canyon / đồi chè
che khuất → gần global hoặc “semi-kidnap” mỗi lần mất lock.

  bạn + map đã in sẵn
         │ nhìn biển hiệu / laser / camera
         ▼
    "mình đang ở ô nào trên sơ đồ?"  →  posê

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

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

2.1. Ba chế độ

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

2.2. Pipeline MCL

  map cố định  +  u (odometry)  +  z (laser/camera/beacon)
                         │
                         ▼
              ┌─────────────────────┐
              │  Particle Filter    │
              │  trên pose (x,y,θ)  │
              └──────────┬──────────┘
                         ▼
                   ước lượng posê
                         │
            ┌────────────┼────────────┐
            ▼            ▼            ▼
        controller    planner      obs RL

Thành phần cần có:

• Motion model: odometry / lệnh + nhiễu (gió → \(Q\) lớn).
• Sensor model: so \(z\) với map (beam model, likelihood field, landmark).
• Resample mỗi chu kỳ; chèn hạt ngẫu nhiên chống kidnap.

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

2.3. Hành lang đối xứng (đa đỉnh)

  map:   ═══╗     ╔═══     hai góc giống nhau
            ║     ║
            ╚═════╝

  hạt:   ●●●           ●●●     hai chùm sống song song
         (giả thuyết A) (giả thuyết B)

  bay thêm + thấy chi tiết khác → một chùm chết dần
  KF chỉ 1 Gauss → dễ chọn nhầm từ đầu ★

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

2.4. Ngoài trời vs trong nhà (UAV)

3. Demo — đa đỉnh trước khi hội tụ (toy)

"""
Toy 1D 'hai phòng giống nhau': đo khoảng cách tới tường gần nhất.
Map: tường tại 0 và tại 10. Đo z=2 → khớp cả x=2 và x=8.
"""
import numpy as np

def sensor_likelihood(x, z, sigma=0.5):
    d = np.minimum(x, 10 - x)  # khoảng cách tới tường gần nhất
    return np.exp(-0.5 * ((d - z) / sigma) ** 2)

xs = np.linspace(0.5, 9.5, 19)
z = 2.0
lik = sensor_likelihood(xs, z)
bel = lik / lik.sum()
print("Các đỉnh belief (x, p):")
for x, p in zip(xs, bel):
    if p > 0.08:
        bar = "████" if p > 0.15 else "██"
        print(f"  x≈{x:.1f}  p≈{p:.3f}  {bar}")

n_modes = int(np.sum(bel > 0.08))
print(f"\nSố mode thô ≈ {n_modes} (≥2 → PF giữ được; KF 1 Gauss dễ bám nhầm)")
print("KF khởi tạo nhầm tại x̂=8 cần quan sát phân biệt mới sửa — PF đã giữ cả hai.")

🧪 Kỳ vọng demo: tin cao quanh \(x\approx 2\) và \(x\approx 8\) (đối xứng); in rõ ≥2 mode. Đây là lý do global loc thích PF hơn một Gauss duy nhất.

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

• Nhiều env RL đưa state privileged (GT). Muốn paper gần thật: train với \(\hat{x}\) nhiễu hoặc obs onboard.
• Reward phạt theo sai số pose ước lượng khác phạt theo pose thật — nhất quán với những gì robot biết lúc deploy.
• Coverage trên map: nếu còn đa đỉnh, “ô đã phủ” có thể sai — trì hoãn đánh dấu covered hoặc dùng soft coverage.
• GPS urban: đừng gọi tracking khi GPS nhảy 20 m — đó là kidnap mềm; cần recovery.
• Năng lượng: active localization (bay tới vùng đặc trưng) tốn pin nhưng cứu mission khi belief đa đỉnh.
• Gió: motion model lệch → hạt loang nhanh hơn; tăng số hạt hoặc giảm tốc độ khi \(P\)/entropy lớn.

5. Ưu / nhược

Nghĩa với năng lượng / gió / độ phủ / GPS urban: gió làm motion model lệch; GPS nhảy = kidnap; phủ phụ thuộc posê không phải GT.

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

⚠️ Lỗi hay gặp: Global localization mà dùng quá ít hạt → không cover giả thuyết.
⚠️ Không có cơ chế kidnap recovery → một lần mất lock hỏng cả nhiệm vụ.
⚠️ Đánh giá policy chỉ trên state thật rồi ngạc nhiên khi onboard lệch.
⚠️ Dùng KF khi belief còn hai chùm đối xứng.

7. Checklist

1. Tracking khác global localization ở điểm nào?
2. Vì sao cần một tỷ lệ hạt ngẫu nhiên?
3. Map trong localization đóng vai trò gì trong sensor model?
4. Task của bạn đang giả định tracking hay có thể bị kidnap (mất GPS)?

⚡ Nhớ một câu: Localization = pose trên map đã biết; khó từ tracking → global → kidnapped — đa đỉnh thì PF, đừng ép một Gauss.

🔗 Sang bài sau mang theo: Đảo bài toán — biết (gần đúng) pose, xây map (occupancy + log-odds).

  ★ Hình nhớ mãi:
  map + u + z → PF(pose) → posê
  tracking ·●· | global ●·●·● | kidnap ···?···

Trước: Lec13 · Sau: Lec15