SLAM และ sensor fusion

ตัวอย่างที่ 1 หลอมรวม IMU กับ UWB

โดรนบินตามทางเดินด้วยความเร็วจริง 1.0 m/s เริ่มที่ความเร็วประมาณ 0 (ไม่รู้) UWB วัดตำแหน่งทุก 0.5 s ส่วนเบี่ยงเบนมาตรฐาน 0.10 m ความคลาดของค่าวัดใช้ชุดค่าตายตัวเพื่อให้ผลซ้ำได้

DT, SIGMA_A, SIGMA_Z = 0.1, 0.5, 0.10
uwb_noise = [0.08, -0.12, 0.05, 0.10, -0.06, -0.09]      # m, ค่าตายตัวแทนสัญญาณรบกวน

x = [0.0, 0.0]                                            # ตำแหน่ง ความเร็ว (ประมาณ)
P = [[0.5 ** 2, 0.0], [0.0, 1.0 ** 2]]
q = SIGMA_A ** 2
Q = [[q * DT ** 4 / 4, q * DT ** 3 / 2], [q * DT ** 3 / 2, q * DT ** 2]]

for step in range(1, 31):
    x = [x[0] + DT * x[1], x[1]]                           # ทำนาย (IMU วัดความเร่ง 0)
    P = [[P[0][0] + DT * (P[1][0] + P[0][1]) + DT * DT * P[1][1] + Q[0][0], P[0][1] + DT * P[1][1] + Q[0][1]],
         [P[1][0] + DT * P[1][1] + Q[1][0], P[1][1] + Q[1][1]]]
    if step % 5 == 0:                                      # ปรับแก้ด้วย UWB
        z = 1.0 * step * DT + uwb_noise[step // 5 - 1]
        s = P[0][0] + SIGMA_Z ** 2
        k = [P[0][0] / s, P[1][0] / s]
        innov = z - x[0]
        x = [x[0] + k[0] * innov, x[1] + k[1] * innov]
        P = [[(1 - k[0]) * P[0][0], (1 - k[0]) * P[0][1]], [P[1][0] - k[1] * P[0][0], P[1][1] - k[1] * P[0][1]]]
        print(f"t {step * DT:.1f} s: p {x[0]:.2f} (true {step * DT:.2f}), v {x[1]:.2f}, "
              f"sigma_p {P[0][0] ** 0.5:.3f} m, sigma_v {P[1][1] ** 0.5:.3f} m/s")

t 0.5 s: p 0.57 (true 0.50), v 0.57, sigma_p 0.099 m, sigma_v 0.719 m/s
t 1.0 s: p 0.88 (true 1.00), v 0.61, sigma_p 0.097 m, sigma_v 0.262 m/s
t 1.5 s: p 1.48 (true 1.50), v 0.98, sigma_p 0.090 m, sigma_v 0.164 m/s
t 2.0 s: p 2.06 (true 2.00), v 1.07, sigma_p 0.085 m, sigma_v 0.141 m/s
t 2.5 s: p 2.49 (true 2.50), v 0.97, sigma_p 0.082 m, sigma_v 0.137 m/s
t 3.0 s: p 2.93 (true 3.00), v 0.92, sigma_p 0.081 m, sigma_v 0.136 m/s

ความเร็วที่ไม่เคยวัดตรงถูกประมาณจากตำแหน่งที่วัดต่อเนื่อง และเข้าใกล้ 1.0 m/s ภายในไม่กี่วินาที ส่วนเบี่ยงเบนมาตรฐานของตำแหน่งลดลงจนต่ำกว่าความคลาดของ UWB เอง เพราะตัวกรองรวมข้อมูลหลายครั้งเข้าด้วยกัน

ตัวอย่างที่ 2 กระจายความคลาดปิดวงตามระยะทาง

วิธีอย่างง่ายนี้กระจายความคลาดตามสัดส่วนระยะทางสะสม ระบบจริงแก้ด้วยการปรับกราฟทั้งหมดที่คิดน้ำหนักความไม่แน่นอนของแต่ละเส้นเชื่อม

import math

odom = [(0.0, 0.0), (20.3, 0.2), (20.6, 20.4), (0.8, 20.9), (1.2, 0.8)]   # กลับที่เริ่มจริง
err = (odom[-1][0] - odom[0][0], odom[-1][1] - odom[0][1])
seg = [math.dist(a, b) for a, b in zip(odom, odom[1:])]
cum = [0.0]
for s in seg:
    cum.append(cum[-1] + s)
total = cum[-1]
print(f"loop closure error ({err[0]:.1f}, {err[1]:.1f}) m over {total:.1f} m = {math.hypot(*err) / total:.1%}")
for (x, y), s in zip(odom, cum):
    f = s / total
    print(f"({x:5.1f}, {y:5.1f}) -> ({x - f * err[0]:5.2f}, {y - f * err[1]:5.2f})")

loop closure error (1.2, 0.8) m over 80.4 m = 1.8%
(  0.0,   0.0) -> ( 0.00,  0.00)
( 20.3,   0.2) -> (20.00, -0.00)
( 20.6,  20.4) -> (20.00, 20.00)
(  0.8,  20.9) -> (-0.10, 20.30)
(  1.2,   0.8) -> ( 0.00,  0.00)