파티클 필터 - 지도 위의 위치 찾기

이 장에서 배우는 것

창고 로봇이 통로 한가운데서 위치 추정을 다시 시작했다고 하자. 센서로 읽은 거리는 정확해 보이지만, 지도에는 모양이 비슷한 통로가 둘 있다. 어느 통로에 있는지 아직 결정할 수 없다면 두 가능성을 함께 유지해야 한다. 앞 장에서 다룬 확장 칼만 필터는 하나의 평균과 공분산으로 상태를 표현했다. 이번에는 여러 위치 후보를 직접 유지하는 파티클 필터(particle filter)로 이 문제를 다룬다.

입자 하나는 로봇이 있을 법한 위치와 방향을 담는다. 각 입자는 이동 명령에 따라 움직이고, 센서 측정과 잘 맞는 정도에 따라 서로 다른 가중치를 얻는다. 가중치가 큰 후보를 다음 계산에 더 많이 남기면, 여러 가능성으로 시작한 추정이 관측에 맞는 위치 주변으로 모인다. 다만 입자 수가 많다는 사실만으로 다양한 가능성이 남아 있다고 판단해서는 안 된다.

• 위치와 방향의 확률분포를 입자와 가중치로 표현한다.
• 이동 불확실성을 샘플링(sampling)하여 다음 상태 후보를 만든다.
• 거리 측정의 우도로 가중치를 갱신하고 안정적으로 정규화한다.
• 유효 표본 수로 가중치 퇴화를 판단하고 재샘플링(resampling)을 수행한다.
• 가중치가 고르게 보이는 상태와 입자 위치가 다양한 상태를 구분한다.

문제 상황

실습의 창고 지도는 가로 10m, 세로 8m인 평면이다. 로봇은 지도에 위치가 등록된 표지까지의 거리를 측정한다. 표지마다 식별자가 있으므로 어떤 표지의 거리인지 이미 안다고 가정한다. 지도 작성이나 표지 대응 관계를 찾는 문제는 다루지 않는다. 이동은 기본서에서 배운 차동 구동의 선속도와 각속도로 표현한다.

현재 가능한 자세는 아래쪽 통로의 (2, 2, 0)과 위쪽 통로의 (2, 6, 0)이다. 위치 단위는 미터이고 방향 단위는 라디안이다. 두 후보의 사전 확률은 같다. 중앙선 위의 표지 (0, 4), (8, 4)까지 거리를 측정하면 두 후보가 만드는 예상 거리는 서로 같다. 이 측정만으로는 어느 통로인지 구별할 수 없다.

여기서 두 위치의 평균인 (2, 4)를 곧바로 로봇 위치로 발표하면 문제가 생긴다. 로봇이 있을 수 있다고 설정한 두 통로 사이에 평균이 놓이기 때문이다. 이번 예제는 먼저 두 후보를 그대로 보존하고, 아래쪽 표지 (2, 0)가 관측되었을 때 아래쪽 통로를 선택하는 과정을 보여 준다.

계산을 눈으로 확인하기 위해 초기 입자 400개를 두 자세에 각각 200개씩 놓는다. 창고 전체에 입자를 뿌리는 전역 위치 추정 예제는 아니다. 실제로는 초기 위치에 관한 지식에 따라 자유 공간이나 알려진 위치 주변에서 입자를 생성한다. 여기서는 분포가 두 봉우리를 갖는 상황과 가중치 변화에 집중한다.

입자로 여러 위치 가능성을 유지한다

상태는 x, y, 방향 세 값으로 구성한다. 입자 배열의 모양은 (N, 3)이고, 가중치 배열의 모양은 (N,)이다. 각 가중치는 음수가 아니며 전체 합은 1이다. 입자의 좌표는 가능한 상태를 나타내고 가중치는 해당 상태가 현재 추정에서 차지하는 비중을 나타낸다. 입자 개수와 가중치를 별개의 정보로 보아야 한다.

상태 입자: s[i] = (x[i], y[i], theta[i])
가중치:    w[i] ≥ 0
정규화:    sum(w) = 1

어떤 영역에 있을 확률의 근삿값
    = 그 영역 안에 있는 입자의 가중치 합

확률분포를 입자로 표현하면 평균 하나로 합쳐지지 않는 후보들을 유지할 수 있다. 아래쪽 통로에 있는 입자의 가중치 합과 위쪽 통로에 있는 입자의 가중치 합을 따로 계산하면 두 통로의 가능성을 비교할 수 있다. 반면 입자가 한 개도 없는 영역은 이 표현에서 확률이 0이다. 입자를 적게 배치해 중요한 영역을 놓쳤다면 측정식만 개선해도 그 영역이 저절로 나타나지는 않는다.

예측은 모든 입자를 따로 움직이는 과정이다

로봇에 전진 명령을 보내도 실제 이동량은 명령값과 조금 다를 수 있다. 따라서 모든 입자를 동일하게 평행 이동하지 않고, 입자마다 다른 이동 오차를 뽑는다. 이번 코드에서는 한 예측 구간의 이동 거리 오차를 −0.02m부터 0.02m까지, 회전량 오차를 −0.02rad부터 0.02rad까지의 균등분포로 설정한다.

ds = v * dt + 거리 오차
dtheta = omega * dt + 회전량 오차
theta_mid = theta + dtheta / 2

x_new = x + ds * cos(theta_mid)
y_new = y + ds * sin(theta_mid)
theta_new = wrap(theta + dtheta)

이 식은 구간 중간의 방향으로 이동을 근사한다. 구간 내 회전량이 작을 때 사용하기 편하며, 큰 회전이 포함된 이동을 그대로 처리하는 정밀한 적분식은 아니다. 실제 시스템에서는 시간 간격을 줄이거나 운동 모델을 바꾸어야 한다. 방향은 계산 후 −π 이상 π 미만으로 접어 같은 방향이 서로 다른 수치로 계속 커지지 않도록 한다.

여기서 지정한 오차 폭은 한 번의 예측 호출에 해당한다. 호출 주기를 바꾸면서 같은 오차 폭을 그대로 쓰면 단위 시간 동안 누적되는 불확실성도 달라진다. 센서 기록으로 이동량과 시간 간격에 맞는 오차 모델을 정하는 일이 필요하다. 정지 상태에서 얼마만큼 퍼져야 하는지도 그 모델의 일부다.

이번 필터는 운동 모델에서 직접 다음 입자를 생성한다. 따라서 예측 단계에서는 기존 가중치를 유지하고, 이어지는 측정 단계에서 관측의 우도를 곱한다. 다른 분포에서 후보를 생성하는 방식이라면 그에 맞는 추가 보정이 필요하지만, 이 예제에는 해당하지 않는다.

입자 필터에서 각 배열과 단계가 담당하는 정보

항목	표현	역할
자세 후보	(N, 3) 배열	위치와 방향의 가능한 값
가중치	(N,) 배열	각 후보가 차지하는 확률 질량
예측	운동 모델과 이동 오차	이동 후 상태 후보 생성
측정 갱신	예상 거리와 실제 거리의 차이	관측과 일치하는 후보의 비중 증가

거리 측정을 가중치로 바꾼다

입자의 위치가 (x, y)이고 표지 위치가 (lx, ly)이면 예상 거리는 두 점 사이의 유클리드 거리다. 센서가 준 거리와 예상 거리의 차이를 잔차라고 한다. 잔차가 작을수록 측정이 해당 입자와 잘 맞는다. 이번 측정 모델은 표지별 거리 오차가 서로 독립이고, 모두 표준편차 sigma인 정규분포를 따른다고 가정한다.

예상 거리 r[i, j] = sqrt((x[i] - lx[j])² + (y[i] - ly[j])²)
잔차 e[i, j] = z[j] - r[i, j]

log_likelihood[i] = -0.5 * sum((e[i, j] / sigma)²)
새 가중치 ∝ 기존 가중치 * exp(log_likelihood)

정규분포의 정규화 상수는 모든 입자에 공통이므로 생략한다. 최종 가중치를 합이 1이 되도록 나누면 공통 인자가 사라지기 때문이다. 입자마다 서로 다른 측정 분산을 사용한다면 이 상수를 같은 방식으로 생략할 수는 없다.

이번 표지는 거리만 제공한다. 따라서 한 번의 측정은 같은 위치에 있는 서로 다른 방향을 직접 구별하지 못한다. 방향은 이동 후의 위치 변화와 여러 시점의 거리 관측이 결합되어 간접적으로 제한된다. 위치 측정이 들어왔다는 이유만으로 방향까지 충분히 알려졌다고 생각해서는 안 된다.

작은 수를 계속 곱하지 않는다

표지를 여러 개 관측하면 우도를 곱해야 한다. 잔차가 큰 입자에서는 우도가 매우 작아지고, 이를 반복해서 곱하면 컴퓨터가 표현할 수 있는 범위 아래로 내려갈 수 있다. 모든 가중치가 0이 된 뒤 합으로 나누면 정상적인 확률을 얻을 수 없다.

코드는 기존 가중치의 로그와 로그 우도를 더한 뒤, 가장 큰 값을 빼고 지수 함수를 적용한다. 그러면 가장 유리한 입자의 변환값은 1이 된다. 따라서 유효한 기존 가중치와 유한한 측정 계산이 있는 한 정규화에 사용할 양수 항이 남는다.

log_weight[i] = log(w[i]) + log_likelihood[i]
shifted[i] = log_weight[i] - max(log_weight)
scaled[i] = exp(shifted[i])
w_new[i] = scaled[i] / sum(scaled)

매우 불리한 입자의 scaled 값은 여전히 0으로 내려갈 수 있다. 이는 모든 항이 동시에 사라지는 문제와 다르다. 이번 코드에서는 그 지수 계산에 한해서 언더플로 경고를 억제한다. 잘못된 값이나 모든 후보의 소실까지 숨기는 방식은 쓰지 않는다. 기존 가중치가 0인 항은 로그를 직접 계산하지 않고 음의 무한대로 둔다.

첫 관측에서는 두 후보가 모두 중앙선의 표지와 정확히 맞으므로 가중치가 달라지지 않는다. 이어 아래쪽 표지까지의 거리 2m를 관측한다. 아래쪽 후보의 예상 거리는 2m이고 위쪽 후보의 예상 거리는 6m다. sigma를 0.1m로 두었으므로 위쪽 후보의 상대 로그 우도는 −800이다. 일반적인 NumPy의 64비트 부동소수점 계산에서는 그 지수값이 0이 된다. 이 실험에서 위쪽 후보가 제거되는 이유가 분명하게 드러난다.

퇴화를 감시하고 입자를 다시 뽑는다

관측을 반복하면 일부 입자에 대부분의 가중치가 몰릴 수 있다. 나머지 입자는 계산 시간을 쓰지만 추정에 거의 기여하지 못한다. 이런 가중치 퇴화를 살피는 대표적인 수치가 유효 표본 수(effective sample size)다. 정규화된 가중치에 대해 다음과 같이 계산한다.

N_eff = 1 / sum(w[i]²)

N개 입자의 가중치가 모두 1/N이면 유효 표본 수는 N이다. 한 입자의 가중치만 1이고 나머지가 0이면 값은 1이다. 이 수치는 가중치가 얼마나 집중되었는지를 나타낸다. 입자 위치의 독립성이나 공간적 다양성을 직접 측정하는 값은 아니다.

예제에서는 유효 표본 수가 입자 수의 75%보다 작아지면 재샘플링한다. 이 임계값은 과정을 드러내기 위한 선택이며 모든 로봇에 적용할 고정 규칙은 아니다. 너무 자주 뽑으면 같은 조상의 복제가 늘고, 너무 늦게 뽑으면 유용한 입자가 적은 상태로 계산을 이어가게 된다.

누적 가중치에서 일정한 간격으로 선택한다

코드는 체계적 재샘플링(systematic resampling)을 사용한다. 먼저 가중치의 누적합으로 0부터 1까지의 구간을 만든다. 0 이상 1/N 미만에서 시작점을 한 번 뽑고, 1/N 간격으로 N개의 선택점을 놓는다. 각 선택점이 속한 누적 구간의 입자를 복사한다. 가중치가 큰 입자는 더 넓은 구간을 가지므로 여러 번 선택될 수 있다.

재샘플링이 끝나면 모든 가중치를 1/N으로 설정한다. 이전의 비중은 이미 복제 횟수에 반영되었다. 복제된 입자에 이전 가중치를 다시 붙이면 관측의 영향이 중복 반영된다. 또한 뽑은 좌표는 복사해서 새 배열로 만들어야 한다. 이후 예측에서 원본 후보를 의도치 않게 공유하며 수정하지 않도록 하기 위해서다.

가중치 회복과 다양성 회복은 다르다

이번 실험에서는 아래쪽 후보에 있던 200개 입자만 살아남는다. 재샘플링하면 이 위치의 복사본 400개가 생기고 유효 표본 수는 다시 400이 된다. 그러나 서로 다른 자세는 하나뿐이다. 이를 통해 유효 표본 수만 보고 추정의 정보량이 충분하다고 판단할 수 없음을 확인한다.

같은 후보의 복제가 늘면서 다양한 상태 표현을 잃는 현상을 표본 빈곤(sample impoverishment)이라고 한다. 다음 예측에서 이동 오차를 입자마다 따로 뽑으면 복사본들이 조금씩 다른 자세로 퍼진다. 이것은 설정한 운동 불확실성을 반영하는 과정이다. 이미 제거된 먼 통로의 가능성을 다시 탐색한다는 뜻은 아니다.

로봇이 갑자기 옮겨졌거나 지도와 측정이 맞지 않는 상황에서는 국소적인 이동 오차만으로 복구하기 어렵다. 그런 시스템에는 일부 입자의 전역 재초기화나 별도의 위치 상실 판단이 필요할 수 있다. 여기서는 해당 복구 정책을 구현하지 않고, 현재 입자들이 표현하는 후보 안에서 관측을 반영하는 데 범위를 둔다.

완성 코드

다음 내용을 particle_filter.py로 저장한다. Python 3와 NumPy를 사용한다. 임의의 자세 후보를 만드는 필터 난수와 마지막 모의 관측에 사용하는 센서 난수를 분리하고 각각 시드를 고정한다. 초기 두 측정은 대칭과 후보 제거를 확인하기 위해 잡음을 더하지 않은 시험값이다. 마지막 측정에만 작은 모의 센서 오차를 더한다.

이 프로그램은 초기 관측 두 번, 재샘플링 한 번, 이동 예측 한 번, 이동 후 관측 한 번을 수행한다. 마지막 자세 추정은 재샘플링 전의 가중치로 계산한다. 출력의 위치는 소수 첫째 자리까지 표시하며, 출력이 곧 정밀도 평가를 뜻하지는 않는다.

import numpy as np


def wrap_angle(angle):
    return (angle + np.pi) % (2.0 * np.pi) - np.pi


def predict(particles, v, omega, dt, rng):
    n = len(particles)
    ds = v * dt + rng.uniform(-0.02, 0.02, size=n)
    dtheta = omega * dt + rng.uniform(-0.02, 0.02, size=n)
    middle = particles[:, 2] + 0.5 * dtheta

    result = particles.copy()
    result[:, 0] += ds * np.cos(middle)
    result[:, 1] += ds * np.sin(middle)
    result[:, 2] = wrap_angle(particles[:, 2] + dtheta)
    return result


def ranges_from(particles, landmarks):
    delta = particles[:, None, :2] - landmarks[None, :, :]
    return np.sqrt(np.sum(delta * delta, axis=2))


def update_weights(particles, weights, landmarks, observed, sigma):
    if not np.isfinite(sigma) or sigma <= 0.0:
        raise ValueError("sigma must be finite and positive")

    expected = ranges_from(particles, landmarks)
    residual = (observed[None, :] - expected) / sigma
    log_likelihood = -0.5 * np.sum(residual * residual, axis=1)

    log_prior = np.full(weights.shape, -np.inf)
    positive = weights > 0.0
    log_prior[positive] = np.log(weights[positive])
    log_weight = log_prior + log_likelihood
    peak = np.max(log_weight)
    if not np.isfinite(peak):
        raise ValueError("no finite particle weight")

    with np.errstate(under="ignore"):
        scaled = np.exp(log_weight - peak)
    total = np.sum(scaled)
    if not np.isfinite(total) or total <= 0.0:
        raise ValueError("invalid weight sum")
    return scaled / total


def effective_size(weights):
    return 1.0 / np.sum(weights * weights)


def systematic_resample(particles, weights, rng):
    n = len(weights)
    cumulative = np.cumsum(weights)
    cumulative[-1] = 1.0
    positions = (rng.random() + np.arange(n)) / n
    indices = np.searchsorted(cumulative, positions, side="right")
    return particles[indices].copy(), np.full(n, 1.0 / n)


def estimate_pose(particles, weights):
    xy = np.sum(particles[:, :2] * weights[:, None], axis=0)
    sine = np.sum(weights * np.sin(particles[:, 2]))
    cosine = np.sum(weights * np.cos(particles[:, 2]))
    if np.hypot(sine, cosine) < 1e-12:
        raise ValueError("mean heading is undefined")
    heading = np.arctan2(sine, cosine)
    return np.array([xy[0], xy[1], heading])


def main():
    rng = np.random.default_rng(20260404)
    sensor_rng = np.random.default_rng(20260405)
    n = 400

    candidates = np.array([[2.0, 2.0, 0.0], [2.0, 6.0, 0.0]])
    particles = np.repeat(candidates, n // 2, axis=0)
    weights = np.full(n, 1.0 / n)
    print(f"초기 입자 수: {n}")
    print(f"초기 유효 표본 수: {effective_size(weights):.1f}")

    center_marks = np.array([[0.0, 4.0], [8.0, 4.0]])
    true_pose = np.array([[2.0, 2.0, 0.0]])
    observed = ranges_from(true_pose, center_marks)[0]
    weights = update_weights(
        particles, weights, center_marks, observed, sigma=0.1
    )
    print(f"대칭 관측 후 유효 표본 수: {effective_size(weights):.1f}")

    lower_mark = np.array([[2.0, 0.0]])
    observed = np.array([2.0])
    weights = update_weights(
        particles, weights, lower_mark, observed, sigma=0.1
    )
    neff = effective_size(weights)
    lower_mass = np.sum(weights[particles[:, 1] < 4.0])
    print(f"추가 관측 후 아래쪽 확률: {lower_mass:.6f}")
    print(f"추가 관측 후 유효 표본 수: {neff:.1f}")

    did_resample = neff < 0.75 * n
    if did_resample:
        particles, weights = systematic_resample(particles, weights, rng)
    print(f"재샘플링 수행: {did_resample}")
    print(f"재샘플링 후 유효 표본 수: {effective_size(weights):.1f}")
    distinct = np.unique(particles, axis=0).shape[0]
    print(f"재샘플링 후 서로 다른 자세 수: {distinct}")

    particles = predict(particles, v=0.5, omega=0.0, dt=1.0, rng=rng)
    true_pose = np.array([[2.5, 2.0, 0.0]])
    all_marks = np.vstack((center_marks, lower_mark))
    observed = ranges_from(true_pose, all_marks)[0]
    observed += sensor_rng.uniform(-0.01, 0.01, size=len(all_marks))
    weights = update_weights(
        particles, weights, all_marks, observed, sigma=0.1
    )
    estimate = estimate_pose(particles, weights)

    assert particles.shape == (n, 3)
    assert np.all(np.isfinite(particles))
    assert np.all(np.isfinite(weights))
    assert np.all(weights >= 0.0)
    assert np.isclose(np.sum(weights), 1.0)
    print(f"이동 후 추정 위치: ({estimate[0]:.1f}, {estimate[1]:.1f})")
    print(f"마지막 가중치 합: {np.sum(weights):.6f}")


if __name__ == "__main__":
    main()

줄별 해설

상태 예측과 거리 배열

wrap_angle의 나머지 연산은 방향을 한 바퀴 범위로 되돌린다. predict에서 size=n은 입자마다 별도의 오차를 뽑으라는 뜻이다. 오차를 하나만 뽑아 모든 입자에 더하면 입자들이 함께 이동할 뿐, 같은 위치에 있던 복사본 사이에 운동 불확실성이 표현되지 않는다.

middle은 각 입자의 이동 전 방향에 회전량의 절반을 더한 값이다. result를 복사한 뒤 x, y를 갱신하므로 함수 호출 전 배열은 바뀌지 않는다. 마지막 열에는 이동 후 방향을 넣는다. 위치와 방향을 함께 갱신하지만, 거리 오차와 회전량 오차는 독립이라는 모델을 사용한다.

ranges_from의 particles[:, None, :2]는 입자 위치를 (N, 1, 2) 모양으로 만들고, landmarks[None, :, :]는 표지를 (1, M, 2) 모양으로 만든다. 두 배열을 빼면 모든 입자와 모든 표지의 좌표 차이가 (N, M, 2) 배열로 계산된다. 마지막 축을 제곱합한 결과는 (N, M) 거리 배열이다.

로그 가중치와 유효 표본 수

update_weights는 입력 가중치가 음수가 아니고 합이 1이며, 배열의 모양과 관측값이 유효하다는 내부 사용 계약을 따른다. 외부 센서 입력을 직접 받는 함수라면 관측의 유한성, 표지 개수, 가중치의 부호도 경계에서 검증해야 한다. 여기서는 잘못된 sigma와 정규화 실패를 명시적으로 검사한다.

residual은 잔차를 표준편차로 나누어 단위를 없앤 값이다. axis=1로 제곱합하면 표지별 기여가 합쳐져 입자마다 로그 우도 하나가 남는다. log_prior를 먼저 음의 무한대로 채우는 이유는 기존 가중치 0을 유지하면서 log(0)을 호출하지 않기 위해서다.

peak를 빼는 연산은 입자 사이의 상대적인 비율을 바꾸지 않는다. peak가 유한하지 않으면 정상적으로 정규화할 후보가 없다고 판단해 예외를 발생시킨다. scaled의 합으로 나눈 결과가 새 가중치이며, effective_size는 이 정규화된 배열을 사용한다. 정규화되지 않은 점수를 그대로 넣으면 유효 표본 수의 의미가 사라진다.

재샘플링과 자세 요약

cumulative[-1]을 1로 맞추는 코드는 부동소수점 누적합의 작은 오차를 처리한다. positions는 시작점만 무작위이고 간격은 일정하다. searchsorted의 side="right"는 선택점과 경계가 같을 때 오른쪽 구간을 고른다. 덕분에 누적값이 0으로 반복되는 앞쪽의 가중치 0인 항을 선택점 0이 잘못 고르는 일을 피한다.

estimate_pose의 위치는 가중 평균이다. 방향은 각도 자체를 평균하지 않고 사인과 코사인의 가중합으로 계산한다. 두 합이 거의 0이면 대표 방향을 정하기 어려우므로 예외를 낸다. 위치의 가중 평균도 여러 통로가 살아 있는 동안에는 부적절한 대표값일 수 있다. 이 함수는 그런 다봉 분포를 자동으로 판별하지 않으므로 호출 시점을 판단해야 한다.

실험 순서와 점검 항목

np.repeat는 두 후보를 같은 개수로 복제한다. 중앙선 표지의 관측은 두 집단의 가중치를 그대로 유지하고, 아래쪽 표지의 관측은 두 집단을 구별한다. lower_mass는 y가 4보다 작은 입자의 가중치를 더한 값이다. 입자 개수의 비율이 아니라 가중치 합으로 통로의 확률을 계산한다는 점이 중요하다.

재샘플링 직후 np.unique로 서로 다른 자세 수를 센다. 이는 동일한 자세를 의도적으로 복제한 이번 실험에 적합한 점검이다. 일반적인 연속 상태에서는 아주 작은 수치 차이도 서로 다른 행으로 세어지므로 다양성 평가에는 공간 구획별 입자 수나 분산 등을 함께 사용해야 한다.

마지막 모의 관측의 균등 오차 폭은 ±0.01m이고 필터가 가정한 정규분포의 표준편차는 0.1m다. 이는 모델과 정확히 일치하는 센서 성능 시험이 아니라 작은 측정 변화가 있는 계산 예제다. 마지막 단언문은 배열 크기와 확률의 기본 조건을 확인하며, 위치 추정의 통계적 정확도를 보증하지 않는다.

실행 결과

NumPy가 설치된 환경에서 다음 명령을 실행한다. 첫 명령은 경고를 오류로 취급하면서 문법을 컴파일하고, 성공하면 별도 출력 없이 끝난다. 두 번째 명령은 같은 설정으로 프로그램을 실행한다.

python3 -W error -m py_compile particle_filter.py
python3 -W error particle_filter.py

예상 출력은 다음과 같다.

초기 입자 수: 400
초기 유효 표본 수: 400.0
대칭 관측 후 유효 표본 수: 400.0
추가 관측 후 아래쪽 확률: 1.000000
추가 관측 후 유효 표본 수: 200.0
재샘플링 수행: True
재샘플링 후 유효 표본 수: 400.0
재샘플링 후 서로 다른 자세 수: 1
이동 후 추정 위치: (2.5, 2.0)
마지막 가중치 합: 1.000000

대칭 관측 뒤 유효 표본 수가 줄지 않는 것은 필터가 동작하지 않아서가 아니다. 주어진 관측에 두 후보를 구별할 정보가 없기 때문이다. 추가 관측 뒤에는 살아 있는 200개 입자가 같은 가중치를 가져 유효 표본 수도 200이 된다.

재샘플링 뒤의 400과 서로 다른 자세 수 1을 함께 읽어야 한다. 가중치는 고르게 되었지만 공간적 후보는 하나다. 이동 예측이 각 입자를 조금씩 퍼뜨린 뒤 거리 관측이 그 비중을 조정한다. 최종 위치의 한 자리 출력은 모든 예측 후보가 (2.5, 2.0) 주변의 작은 범위에 있기 때문에 같게 표시된다. 이 결과를 센티미터 단위 정확도의 근거로 사용할 수는 없다.

시드 고정은 같은 환경에서 실험을 반복하기 위한 장치다. 더 많은 자릿수의 내부 값이나 긴 실험 기록을 비교하려면 NumPy 버전도 함께 기록하는 편이 좋다. 여기서는 난수의 세부값에 민감한 수치를 예상 출력으로 제시하지 않는다.

실무에서 자주 틀리는 것

기존 가중치를 버리고 새 우도만 저장한다

매번 재샘플링하지 않는 필터에서 다음 코드는 이전 관측으로 얻은 비중을 없앤다.

# 틀린 코드
weights = likelihood / np.sum(likelihood)

기존 가중치와 새 우도를 함께 반영해야 한다. 완성 코드의 로그 계산을 호출하면 작은 우도의 반복 곱셈도 피할 수 있다.

# 고친 코드
weights = update_weights(
    particles, weights, landmarks, observed, sigma=0.1
)

재샘플링한 입자에 이전 가중치를 다시 붙인다

가중치가 큰 입자는 이미 여러 번 선택될 가능성이 높다. 선택된 가중치를 다시 정규화하면 같은 관측이 복제 횟수와 가중치에 중복 반영된다.

# 틀린 코드: indices는 가중치에 따라 뽑은 인덱스다
particles = particles[indices].copy()
weights = weights[indices]
weights /= np.sum(weights)

새 입자들은 동일한 가중치로 시작한다.

# 고친 코드
particles = particles[indices].copy()
weights = np.full(len(indices), 1.0 / len(indices))

방향을 일반 숫자처럼 평균한다

179도와 −179도는 모두 뒤쪽을 가리킨다. 두 수를 그냥 평균하면 0도가 되어 앞쪽 방향을 얻는다.

# 틀린 코드
heading = np.sum(weights * particles[:, 2])

완성 코드의 함수는 사인과 코사인을 평균하고, 평균 방향을 정하기 어려운 경우도 검사한다.

# 고친 코드
heading = estimate_pose(particles, weights)[2]

유효 표본 수가 크면 서로 다른 후보도 많다고 본다

유효 표본 수는 가중치 집중도를 측정한다. 다음 변수에는 자세의 종류 수가 들어가지 않는다.

# 틀린 코드
distinct_pose_count = int(round(effective_size(weights)))

동일한 행의 복제 여부를 조사하는 이번 실험에서는 다음처럼 두 정보를 따로 기록한다. 실제 운용에서는 유효 표본 수와 함께 공간적 분포도 살펴야 한다.

# 고친 코드
neff = effective_size(weights)
distinct_pose_count = np.unique(particles, axis=0).shape[0]

한눈에 보기

각 단계에서 바뀌는 정보와 확인해야 할 조건

단계	수행하는 계산	확인할 조건	주의점
초기화	사전 지식에 따른 후보 배치	입자 수와 가중치 합	배치하지 않은 영역은 표현하지 못함
예측	입자별 이동 오차 생성	단위와 시간 간격	모든 입자에 같은 오차를 더하지 않음
측정 갱신	기존 비중에 거리 우도 반영	유한한 값과 정규화	로그 계산으로 작은 수의 곱셈 처리
퇴화 판단	가중치 제곱합의 역수	설정한 임계값과 비교	공간적 다양성의 측정값은 아님
재샘플링	누적 가중치에서 후보 복제	입자 수 유지와 균등 가중치	반복 복제로 후보 다양성이 줄 수 있음
대표 자세	위치 평균과 방향의 원형 평균	분포 형태와 방향의 정의 가능성	두 통로의 평균은 유효한 후보가 아닐 수 있음

이번 실습에서는 지도를 알고 있는 상태에서 로봇 위치를 추정했다. 다음 장에서는 센서가 관측한 공간을 점유 격자 지도에 기록한다. 위치 추정과 지도 표현은 연결되어 있지만, 이번 필터의 표지 지도는 실행 도중 바뀌지 않는다는 점을 구분해 두어야 한다.

연습 문제

1. 가중치가 [0.5, 0.25, 0.25, 0.0]인 네 입자의 유효 표본 수를 구하라. 임계값이 전체 입자 수의 75%이고 비교 연산이 미만일 때 재샘플링 여부를 판단하라.
2. 완성 코드에서 아래쪽 표지의 측정 갱신을 생략하라. 이동 전 아래쪽 통로의 확률, 유효 표본 수, 위치의 가중 평균을 구하고 평균을 대표 위치로 발표할 때의 문제를 설명하라.
3. 재샘플링 직후 predict의 거리 오차와 회전량 오차를 모두 0으로 바꾸라. 동일한 명령으로 한 번 이동한 뒤 서로 다른 자세 수가 얼마인지 설명하라.
4. 예제의 추가 관측에서 sigma를 0.1 대신 2.0으로 바꾸라. 아래쪽 통로의 확률을 식으로 구하고, 더 큰 측정 불확실성이 두 후보의 비중에 어떤 영향을 주는지 설명하라.

정답과 해설

1. 가중치 제곱합은 0.25 + 0.0625 + 0.0625 = 0.375다. 유효 표본 수는 1/0.375, 약 2.667이다. 임계값은 4 × 0.75 = 3이며 2.667이 3보다 작으므로 재샘플링한다. 값이 정확히 3이었다면 미만 조건에서는 수행하지 않는다.

2. 대칭 관측만으로 두 후보의 비중은 달라지지 않는다. 아래쪽 확률은 0.5, 유효 표본 수는 400, 위치 평균은 (2, 4)다. 초기 후보는 (2, 2)와 (2, 6)이므로 평균 위치에 입자가 존재하지 않는다. 후보별 확률을 유지하거나 통로별로 분포를 나누어 요약해야 한다. 판단할 근거 없이 평균을 실제 위치처럼 사용하면 안 된다.

3. 서로 다른 자세 수는 계속 1이다. 같은 자세에 같은 이동을 적용하므로 복사본들은 모두 같은 결과를 갖는다. 유효 표본 수는 가중치가 균등하면 여전히 400일 수 있다. 움직임에 따른 퍼짐이 필요하다면 물리적인 이동 오차 모델이 있어야 하며, 입자 수만 늘리는 것으로 그 역할을 대신할 수 없다.

4. 아래쪽 후보의 잔차는 0이고 위쪽 후보의 잔차 크기는 4m다. 위쪽의 상대 우도는 exp(−0.5 × (4/2)²) = exp(−2)다. 두 집단의 사전 비중이 같으므로 아래쪽 확률은 1/(1 + exp(−2)), 약 0.881이다. sigma가 작을 때보다 위쪽 후보를 더 많이 남긴다. 측정을 덜 정밀하다고 보는 모델이므로 두 후보를 구별하는 힘도 약해진다.