확장 칼만 필터 - 비선형 로봇 위치 추정

이 장에서 배우는 것

앞 장에서 다룬 칼만 필터는 상태와 관측 사이의 관계를 행렬로 표현했다. 창고 로봇의 위치를 추정할 때는 이 관계에 삼각함수와 제곱근이 들어간다. 같은 바퀴 이동량이라도 로봇이 어느 방향을 보고 있는지에 따라 지도 위의 위치 변화가 달라지기 때문이다. 랜드마크까지의 거리와 방향 역시 위치에 대해 선형으로 변하지 않는다.

이 장에서는 확장 칼만 필터(extended Kalman filter, EKF)로 이 문제를 다룬다. 비선형 함수로 평균을 이동시키고, 현재 추정값 주변의 기울기로 공분산을 계산한다. 지도에 위치가 알려진 표식을 두고, 바퀴에서 얻은 이동량과 표식 관측을 결합하는 프로그램을 만든다.

• 위치와 방향으로 이루어진 상태에 비선형 이동 모델을 세운다.
• 야코비안(Jacobian)을 구하고 수치 미분으로 구현을 확인한다.
• 이동량 잡음과 거리·방향 관측 잡음을 서로 다른 좌표에서 다룬다.
• 각도 정규화와 안정적인 공분산 갱신을 포함한 필터를 구현한다.
• 고정된 난수 시드로 시뮬레이션을 반복하고 계산의 기본 조건을 검사한다.

문제 상황

창고 로봇이 선반 사이에서 상자를 운반한다고 하자. 바퀴 회전으로 계산한 오도메트리(odometry)는 자주 들어오지만, 바닥의 작은 미끄러짐까지 알려 주지는 않는다. 회전량에 작은 오차가 있으면 다음 이동의 방향도 어긋난다. 바퀴 정보만 누적한 위치는 실제 위치와 점차 달라질 수 있다.

창고에는 지도 좌표가 알려진 랜드마크(landmark)를 설치했다. 센서는 각 표식의 식별자, 로봇에서 표식까지의 거리, 로봇의 정면을 기준으로 한 방향각을 반환한다. 표식 위치는 정확히 알려져 있다고 가정한다. 표식 위치까지 함께 추정하는 문제는 여기서 다루지 않는다.

추정할 상태는 지도 좌표의 x, y와 로봇의 방향각 θ다. 센서가 표식을 관측하면 예측 위치에서 계산한 거리·방향과 실제 관측을 비교한다. 그 차이에 따라 위치와 방향을 함께 보정한다. 방향만 측정했다고 해서 방향 상태만 바뀌는 것은 아니다. 위치와 방향 사이의 공분산도 보정에 참여한다.

예제에서는 모든 표식이 매 시각 보인다고 가정하고, 가림과 식별자 오류를 제외한다. 바퀴에서 얻은 이동량도 이미 로봇 중심의 전진 거리와 회전각으로 변환되어 있다. 따라서 기본서의 차동 구동 변환을 다시 구현하지 않고, 두 입력이 상태 추정에 어떻게 쓰이는지에 집중한다.

비선형 이동과 야코비안

상태 벡터를 μ = [x, y, θ]ᵀ, 한 구간의 입력을 u = [d, α]ᵀ로 둔다. d는 전진 거리이며 α는 회전각이다. 이 예제의 이산 이동 모델은 구간 시작 방향으로 전진한 뒤 방향을 바꾼다. 실제 연속 원호 운동의 정확한 적분식은 아니므로, 실행 구간을 짧게 잡아 사용하는 모델이다. 시뮬레이터의 실제 상태에도 같은 이동 규칙을 적용한다.

f(μ, u) =
    [x + d cos θ,
     y + d sin θ,
     wrap(θ + α)]ᵀ

wrap은 각도를 −π 이상 π 미만으로 옮기는 함수다. θ가 바뀌면 x와 y의 변화량에 삼각함수가 작용한다. 따라서 앞 장처럼 고정된 상태 전이 행렬 하나로 모든 자세의 움직임을 표현할 수 없다.

야코비안은 여러 출력의 편미분을 행렬로 모은 것이다. 행은 출력 성분, 열은 미분할 입력 성분에 대응한다. 현재 상태 μ 주변에서 작은 변화 δ가 생기면 f(μ + δ, u)를 f(μ, u) + Fδ로 근사한다. 여기서 F는 상태에 대한 야코비안이다. 비선형 함수를 없애는 것이 아니라, 작은 불확실성이 어떻게 퍼지는지 계산할 때 국소적인 선형 근사를 사용한다.

F = ∂f/∂μ =
    [1  0  −d sin θ]
    [0  1   d cos θ]
    [0  0       1 ]

G = ∂f/∂u =
    [cos θ  0]
    [sin θ  0]
    [    0  1]

F의 마지막 열을 보면 방향 오차가 위치 오차로 바뀌는 경로가 드러난다. θ가 0이면 작은 방향 오차가 이번 이동의 y 위치에 먼저 영향을 준다. θ가 π/2이면 같은 오차가 x 위치의 음의 방향에 영향을 준다. 로봇 방향이 달라질 때마다 F와 G를 다시 계산해야 하는 이유다.

입력 잡음은 전진 거리와 회전각의 좌표에서 정의한다. Q의 대각 원소는 각각 거리 오차의 분산과 회전각 오차의 분산이다. 이를 상태 좌표의 불확실성으로 바꾸는 항이 GQGᵀ다. 이 예제에서는 입력 잡음을 구간마다 독립인 영평균 잡음으로 가정하고, 거리 오차와 회전각 오차도 독립으로 둔다.

μ⁻ = f(μ, u)
P⁻ = F P Fᵀ + G Q Gᵀ

위 첨자 −는 관측을 반영하기 전의 예측값을 뜻한다. 평균은 원래 비선형 함수로 계산한다. F에 현재 평균을 곱해서 평균을 예측하는 방식으로 바꾸면 다른 식이 된다. F는 작은 오차의 전달을 설명하는 행렬이라는 점을 구분해야 한다.

각 행렬은 서로 다른 공간의 불확실성을 다룬다

기호	크기	의미	예제의 기준
P	3 × 3	상태 공분산	x, y, θ
Q	2 × 2	입력 잡음 공분산	d, α
R	2 × 2	관측 잡음 공분산	거리, 방향각
F, G	3 × 3, 3 × 2	이동 함수의 미분	상태와 입력에 대해 계산
H	2 × 3	관측 함수의 미분	상태에 대해 계산

각도 단위는 프로그램 전체에서 라디안으로 통일한다. 공분산에는 단위가 섞여 있다. 예를 들어 P의 x와 θ 사이 원소는 미터와 라디안의 곱에 해당한다. 행렬 크기가 맞더라도 Q에 표준편차를 그대로 넣거나 각도를 도 단위로 넣으면 불확실성의 크기가 달라진다.

랜드마크 관측으로 위치를 보정한다

표식의 지도 좌표를 [lₓ, lᵧ]로 두고, Δx = lₓ − x, Δy = lᵧ − y로 정의한다. 거리 제곱 q와 거리 r을 먼저 계산하면 관측 함수가 간단해진다. 방향각은 지도 기준 방위에서 로봇 방향을 빼서 얻는다.

q = Δx² + Δy²
r = √q

h(μ) =
    [r,
     wrap(atan2(Δy, Δx) − θ)]ᵀ

H = ∂h/∂μ =
    [−Δx/r  −Δy/r   0]
    [ Δy/q  −Δx/q  −1]

거리 행의 부호는 로봇이 표식 쪽으로 이동할수록 거리가 줄어드는 것으로 확인할 수 있다. 방향각 행의 θ 미분은 −1이다. 같은 위치에서 로봇이 반시계 방향으로 회전하면 로봇 기준 표식 방향각은 그만큼 줄어든다.

로봇과 표식이 같은 위치에 있으면 방향각을 정의할 수 없고 H의 분모도 0이 된다. 코드에서는 q가 너무 작으면 예외를 발생시킨다. 실제 시스템에서는 센서의 최소 측정 거리까지 고려해 해당 관측을 제외하는 정책이 필요하다. 분모에 임의의 작은 수만 더하면 정의되지 않은 방향 관측을 유효한 정보처럼 사용할 수 있다.

관측 z와 예측 관측 h(μ⁻)의 차이를 혁신량(innovation) ν라고 한다. 거리 성분은 그대로 빼지만 방향 성분은 뺀 뒤 다시 wrap을 적용한다. 예측 방향이 179도이고 관측 방향이 −179도라면 작은 차이는 2도다. 단순 뺄셈의 −358도를 필터에 넣으면 관측이 뜻하는 회전 방향과 크기를 잘못 해석한다.

ν = z − h(μ⁻)       단, ν의 방향각 성분은 정규화한다.
S = H P⁻ Hᵀ + R
K = P⁻ Hᵀ S⁻¹

μ⁺ = μ⁻ + Kν        단, 보정한 θ도 정규화한다.

S는 상태 예측의 불확실성과 센서 잡음을 관측 공간에서 합친 공분산이다. K는 관측 차이를 상태 변화로 바꾸는 칼만 이득이다. 구현에서는 S의 역행렬을 직접 만들지 않고 선형 연립방정식을 푼다. 식의 의미는 같지만 중간 계산을 줄이고 수치 계산을 더 안정적으로 구성할 수 있다.

공분산 갱신에는 조셉 형식(Joseph form)을 사용한다. A = I − KH로 두면 아래와 같다. 수학적으로 동등한 간단한 식보다 계산량은 늘지만, 유한 정밀도에서 공분산의 구조를 유지하는 데 유리하다. 마지막에는 작은 비대칭 성분을 없애기 위해 전치행렬과 평균을 낸다.

A = I − K H
P⁺ = A P⁻ Aᵀ + K R Kᵀ
P⁺ = (P⁺ + P⁺ᵀ) / 2

한 시각에 표식 세 개를 관측하면 예측은 한 번만 하고 보정을 세 번 수행한다. 각 보정에서는 직전 보정으로 바뀐 상태를 기준으로 h와 H를 다시 계산한다. 관측 잡음이 표식 사이에서 독립이라는 가정도 필요하다. 비선형 문제에서는 보정 순서에 따라 국소 근사가 달라질 수 있으므로 예제는 표식 순서를 고정한다.

국소 근사의 범위와 검증 방법

확장 칼만 필터는 상태 분포를 평균과 공분산 하나로 나타내며, 현재 추정값 주변의 기울기를 사용한다. 초기 방향이 크게 틀리거나 위치 불확실성이 넓으면 한 지점의 기울기가 전체 분포를 충분히 설명하지 못할 수 있다. 공분산 숫자가 작아졌다는 사실만으로 실제 위치가 정확해졌다고 판단해서는 안 된다.

완성 코드는 세 가지를 확인한다. 첫째, 직접 구한 F, G, H를 중앙 차분으로 계산한 미분과 비교한다. 둘째, 손으로 계산할 수 있는 한 번의 예측과 보정을 확인한다. 셋째, 여러 시각을 반복하는 동안 상태가 유한하고 공분산이 대칭이며 허용 오차 안에서 양의 준정부호인지 검사한다.

중앙 차분은 입력 성분 하나를 작은 ε만큼 더하고 뺀 두 출력을 비교한다. 방향각 출력의 차이는 여기서도 정규화한다. ε를 작게 할수록 항상 더 정확해지는 것은 아니다. 너무 작으면 부동소수점 뺄셈의 오차가 커질 수 있다. 예제는 특이점과 각도 경계에서 떨어진 상태에서 ε = 10⁻⁶을 사용한다.

이 검사들은 추정 정확도를 통계적으로 보증하지 않는다. 야코비안 검사도 선택한 한 지점의 검사다. 실행 결과를 해석할 때는 수식과 코드의 기본 일치 여부를 확인했다는 범위로 읽어야 한다. 실제 적용에서는 경로와 초기 오차를 바꾸고 실제 오차와 보고된 공분산을 함께 평가해야 한다.

완성 코드

다음 내용을 ekf_landmark.py로 저장한다. Python 3와 numpy가 필요하다. 외부 데이터 파일이나 화면 출력 장치는 사용하지 않는다. 상태와 입력은 1차원 배열로 통일하고, 공분산만 2차원 배열로 둔다. 함수는 전달받은 상태와 공분산을 제자리에서 바꾸지 않고 새 결과를 반환한다.

import numpy as np


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


def motion(state, control):
    x, y, theta = state
    distance, turn = control
    return np.array([
        x + distance * np.cos(theta),
        y + distance * np.sin(theta),
        wrap(theta + turn),
    ])


def motion_jacobians(state, control):
    theta = state[2]
    distance = control[0]
    c, s = np.cos(theta), np.sin(theta)
    F = np.array([
        [1.0, 0.0, -distance * s],
        [0.0, 1.0, distance * c],
        [0.0, 0.0, 1.0],
    ])
    G = np.array([
        [c, 0.0],
        [s, 0.0],
        [0.0, 1.0],
    ])
    return F, G


def observation(state, landmark):
    dx, dy = landmark - state[:2]
    q = dx * dx + dy * dy
    if q <= 1e-12:
        raise ValueError("랜드마크와 로봇의 거리가 너무 가깝다")
    return np.array([
        np.sqrt(q),
        wrap(np.arctan2(dy, dx) - state[2]),
    ])


def observation_jacobian(state, landmark):
    dx, dy = landmark - state[:2]
    q = dx * dx + dy * dy
    if q <= 1e-12:
        raise ValueError("랜드마크와 로봇의 거리가 너무 가깝다")
    r = np.sqrt(q)
    return np.array([
        [-dx / r, -dy / r, 0.0],
        [dy / q, -dx / q, -1.0],
    ])


def predict(state, covariance, control, Q):
    F, G = motion_jacobians(state, control)
    predicted_state = motion(state, control)
    predicted_cov = F @ covariance @ F.T + G @ Q @ G.T
    predicted_cov = 0.5 * (predicted_cov + predicted_cov.T)
    return predicted_state, predicted_cov


def correct(state, covariance, measured, landmark, R):
    expected = observation(state, landmark)
    H = observation_jacobian(state, landmark)
    residual = measured - expected
    residual[1] = wrap(residual[1])
    S = H @ covariance @ H.T + R
    PHt = covariance @ H.T
    K = np.linalg.solve(S, PHt.T).T

    corrected_state = state + K @ residual
    corrected_state[2] = wrap(corrected_state[2])
    A = np.eye(3) - K @ H
    corrected_cov = A @ covariance @ A.T + K @ R @ K.T
    corrected_cov = 0.5 * (corrected_cov + corrected_cov.T)
    return corrected_state, corrected_cov


def numerical_jacobian(function, point, angle_rows):
    epsilon = 1e-6
    result = np.empty((function(point).size, point.size))
    for column in range(point.size):
        offset = np.zeros_like(point)
        offset[column] = epsilon
        difference = function(point + offset) - function(point - offset)
        for row in angle_rows:
            difference[row] = wrap(difference[row])
        result[:, column] = difference / (2.0 * epsilon)
    return result


def check_jacobians():
    state = np.array([1.0, 2.0, 0.4])
    control = np.array([0.3, 0.05])
    landmark = np.array([4.0, 5.0])
    F, G = motion_jacobians(state, control)
    H = observation_jacobian(state, landmark)
    numeric_F = numerical_jacobian(
        lambda value: motion(value, control), state, (2,)
    )
    numeric_G = numerical_jacobian(
        lambda value: motion(state, value), control, (2,)
    )
    numeric_H = numerical_jacobian(
        lambda value: observation(value, landmark), state, (1,)
    )
    for analytic, numeric in ((F, numeric_F), (G, numeric_G), (H, numeric_H)):
        np.testing.assert_allclose(
            analytic, numeric, rtol=1e-6, atol=1e-7
        )


def check_estimate(state, covariance):
    if not np.all(np.isfinite(state)):
        raise AssertionError("상태에 유한하지 않은 값이 있다")
    if not np.all(np.isfinite(covariance)):
        raise AssertionError("공분산에 유한하지 않은 값이 있다")
    np.testing.assert_allclose(
        covariance, covariance.T, rtol=0.0, atol=1e-12
    )
    if np.linalg.eigvalsh(covariance).min() < -1e-10:
        raise AssertionError("공분산의 음의 고윳값이 허용치를 넘었다")
    if not (-np.pi <= state[2] < np.pi):
        raise AssertionError("방향각이 정규화 범위를 벗어났다")


def reference_update():
    state = np.array([0.0, 0.0, 0.0])
    covariance = np.diag([0.04, 0.04, 0.01])
    control = np.array([1.0, 0.0])
    Q = np.diag([0.01, 0.0025])
    R = np.diag([0.04, 0.0025])
    landmark = np.array([4.0, 0.0])
    measured = np.array([2.9, 0.0])
    state, covariance = predict(state, covariance, control, Q)
    state, covariance = correct(
        state, covariance, measured, landmark, R
    )
    np.testing.assert_allclose(
        state, np.array([19.0 / 18.0, 0.0, 0.0]),
        rtol=0.0, atol=1e-12
    )
    check_estimate(state, covariance)
    return state[0]


def simulate():
    rng = np.random.default_rng(20260929)
    landmarks = np.array([
        [-2.0, -2.0],
        [10.0, -2.0],
        [10.0, 10.0],
    ])
    truth = np.array([1.0, 1.0, 0.2])
    state = np.array([1.2, 0.8, 0.25])
    covariance = np.diag([0.3 ** 2, 0.3 ** 2, 0.1 ** 2])
    input_std = np.array([0.015, 0.008])
    sensor_std = np.array([0.08, 0.02])
    Q = np.diag(input_std ** 2)
    R = np.diag(sensor_std ** 2)
    true_control = np.array([0.1, 0.02])
    steps = 80
    updates = 0

    for _ in range(steps):
        truth = motion(truth, true_control)
        odometry = true_control + rng.normal(0.0, input_std)
        state, covariance = predict(state, covariance, odometry, Q)
        check_estimate(state, covariance)

        for landmark in landmarks:
            measured = observation(truth, landmark)
            measured = measured + rng.normal(0.0, sensor_std)
            measured[1] = wrap(measured[1])
            state, covariance = correct(
                state, covariance, measured, landmark, R
            )
            check_estimate(state, covariance)
            updates += 1

    return steps, updates


def main():
    check_jacobians()
    print("야코비안 검증: 통과")
    reference_x = reference_update()
    print(f"기준 보정 x: {reference_x:.6f} m")
    steps, updates = simulate()
    print(f"시뮬레이션: {steps}회 예측, {updates}회 보정")
    print("상태·공분산 검사: 통과")


if __name__ == "__main__":
    main()

줄별 해설

wrap과 motion. 나머지 연산으로 방향각을 정해진 구간에 넣는다. motion은 현재 방향으로 x와 y를 이동시키고 마지막에 회전각을 더한다. 세 성분의 반환 순서가 상태 정의와 같아야 한다. 입력의 회전각을 먼저 더한 뒤 위치를 계산하면 이 장에서 정한 모델과 다른 이동이 된다.

motion_jacobians. sin과 cos는 예측 전 방향으로 계산한다. F는 기존 상태 오차를 다음 상태로 전달하고, G는 입력 잡음을 상태 공간에 투영한다. 이 이동 순서에서는 이번 구간의 회전 잡음이 이번 위치에 직접 들어가지 않는다. 대신 방향 공분산으로 남아 다음 구간의 위치 예측에 영향을 준다.

observation과 observation_jacobian. 두 함수는 동일한 Δx, Δy 정의를 사용한다. 상태에서 표식을 빼는 방식으로 한쪽만 바꾸면 거리 자체는 같을 수 있어도 미분 부호가 어긋난다. q 검사를 두 함수에 넣어 어느 함수를 단독 호출해도 정의되지 않은 계산을 진행하지 않게 한다.

predict. 야코비안을 먼저 계산한 다음 평균을 이동시킨다. predicted_cov에는 기존 공분산의 전달 항과 새로운 입력 잡음 항이 모두 들어간다. 평균을 이동시킨 뒤 그 방향으로 F를 계산하는 실수를 피하도록 계산 순서를 코드에 드러냈다.

correct의 residual과 S. residual은 새 배열이므로 방향각 성분을 바꾸어도 원래 관측은 바뀌지 않는다. S의 크기는 2 × 2다. 여기서 P는 예측 공분산일 수도 있고, 같은 시각의 이전 표식을 이미 반영한 공분산일 수도 있다. 함수는 전달받은 상태와 공분산을 한 쌍으로 사용한다.

correct의 solve. PHt의 크기는 3 × 2다. 구하려는 이득은 K = PHt S⁻¹이므로, 대칭인 S에 대해 S Kᵀ = PHtᵀ를 푼 뒤 전치한다. np.linalg.solve의 두 번째 인수는 여기서 2 × 3이다. 전치를 빼면 단순히 연산 순서만 달라지는 것이 아니라 풀려는 식이 달라진다.

correct의 공분산 갱신. K와 H를 계산할 때 사용한 covariance를 조셉 형식에도 그대로 사용한다. 상태를 보정했다는 이유로 H를 이 식 직전에 다시 계산하지 않는다. 한 번의 보정에서 쓰는 선형 근사는 서로 일관되어야 한다. 다음 표식을 처리할 때 새로운 상태에서 H를 구한다.

numerical_jacobian과 check_jacobians. 열마다 하나의 입력만 움직여 출력 차이를 계산한다. angle_rows는 출력 중 각도로 해석할 행을 지정한다. 이동 함수에서는 세 번째 출력, 관측 함수에서는 두 번째 출력이다. 분석 미분과 수치 미분을 비교하므로 부호나 열 위치를 잘못 넣은 오류를 찾는 데 도움이 된다.

check_estimate. 대칭 공분산의 고윳값을 계산해 음수 허용치를 검사한다. 작은 음수까지 곧바로 실패로 처리하면 반올림 오차를 모델 오류로 오해할 수 있다. 반대로 이 검사가 통과했다고 모델의 잡음 가정이나 추정 정확도까지 확인한 것은 아니다. 대칭화 역시 음의 고윳값을 일반적으로 고쳐 주는 연산은 아니다.

reference_update. 예측 위치는 [1, 0, 0]이고 표식의 예측 거리는 3이다. 관측 거리는 2.9이므로 거리 혁신량은 −0.1이다. 예측 x 분산은 0.04 + 0.01 = 0.05이고 거리 혁신 공분산은 0.05 + 0.04 = 0.09다. 거리의 x 미분이 −1이므로 이득은 −5/9, x 보정량은 1/18이다. 따라서 보정 위치는 19/18이 된다.

simulate. truth는 잡음 없는 실제 입력으로 움직이고, 필터에는 그 입력에 측정 잡음을 더한 odometry를 전달한다. 관측은 truth에서 생성한다. 추정 위치에서 관측을 만들면 필터가 자신의 예측을 관측으로 다시 받는 잘못된 실험이 된다. 실제 상태와 추정 상태의 초기값도 일부러 다르게 둔다.

잡음과 반복 횟수. input_std와 sensor_std는 표준편차이며, 제곱해 Q와 R을 만든다. 이 값은 한 구간의 입력 오차와 관측 오차에 대한 설정이다. 시간 간격을 바꿨을 때 그대로 유지해야 하는 보편적인 상수는 아니다. 난수 생성기는 한 번만 만들고 모든 반복에서 이어서 사용한다.

실행 결과

numpy가 설치된 환경에서 아래 명령을 실행한다. 첫 번째 명령은 경고를 오류로 취급하면서 구문을 컴파일하며 정상일 때 출력하지 않는다. 두 번째 명령은 검사와 시뮬레이션을 실행한다.

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

예상 표준 출력은 다음과 같다. 검사에 실패하면 해당 지점에서 예외가 발생하며, 이후의 통과 문장은 출력되지 않는다.

야코비안 검증: 통과
기준 보정 x: 1.055556 m
시뮬레이션: 80회 예측, 240회 보정
상태·공분산 검사: 통과

80회 이동할 때마다 표식 세 개를 처리하므로 보정 횟수는 240이다. 기준 보정의 숫자는 난수를 쓰지 않고 계산된다. 시뮬레이션에는 시드를 고정했으며, 출력은 환경에 따른 미세한 부동소수점 차이를 나열하는 대신 계산 가능한 기준값과 검사 결과를 보여 준다. 서로 다른 numpy 버전과 계산 환경의 내부 배열이 비트 단위로 같다는 의미는 아니다.

실무에서 자주 틀리는 것

각도 차이를 일반 뺄셈으로 처리한다

두 각도가 각각 정상 범위에 있어도 차이는 경계를 넘어갈 수 있다. 잘못된 코드는 관측을 이미 정규화했다는 이유로 혁신량 처리를 생략한다.

residual = measured - expected
state = state + K @ residual

방향각 차이와 보정 후 상태 방향을 각각 정규화한다. 다음 코드는 correct 함수 안에서 사용하는 형태다.

residual = measured - expected
residual[1] = wrap(residual[1])
state = state + K @ residual
state[2] = wrap(state[2])

표준편차와 분산을 혼동한다

센서 사양의 표준편차를 공분산 대각에 그대로 넣으면 잡음의 크기를 다르게 해석한다. 아래에서 sensor_std는 거리와 방향각의 표준편차 배열이다.

R = np.diag(sensor_std)

독립 잡음이라는 가정 아래 각 성분을 제곱해 넣는다. 상관관계가 있는 센서라면 비대각 원소도 따로 모델링해야 한다.

R = np.diag(sensor_std ** 2)

상태를 이동시킨 뒤 이전 구간의 미분을 계산한다

다음 코드는 새 방향으로 F와 G를 계산한다. 전진 후 회전하는 예제 모델에서 위치 변화의 미분과 맞지 않는다.

state = motion(state, control)
F, G = motion_jacobians(state, control)
covariance = F @ covariance @ F.T + G @ Q @ G.T

예측 전 상태에서 미분을 구한다. 이동 모델을 바꾸려면 평균 함수와 미분 함수도 함께 바꿔야 한다.

F, G = motion_jacobians(state, control)
state = motion(state, control)
covariance = F @ covariance @ F.T + G @ Q @ G.T

한 시각의 표식마다 이동을 다시 예측한다

같은 시각에 받은 여러 관측을 처리하면서 이동 입력까지 반복하면 로봇이 실제보다 여러 번 움직인 것으로 계산된다.

for landmark, measured in observations:
    state, covariance = predict(state, covariance, odometry, Q)
    state, covariance = correct(
        state, covariance, measured, landmark, R
    )

시간 진행은 한 번만 반영하고 관측 보정을 이어 간다. 서로 다른 시각에 수집한 관측이라면 먼저 각 관측의 시각에 맞게 입력 구간을 나누어야 한다.

state, covariance = predict(state, covariance, odometry, Q)
for landmark, measured in observations:
    state, covariance = correct(
        state, covariance, measured, landmark, R
    )

한눈에 보기

예측과 보정에서 사용할 함수와 확인할 조건

단계	핵심 계산	확인할 조건
이동 평균	f(μ, u)	전진과 회전의 순서를 명시한다
이동 공분산	FPFᵀ + GQGᵀ	예측 전 상태에서 F와 G를 구한다
예측 관측	거리와 상대 방향각	표식 좌표와 로봇 좌표의 기준을 맞춘다
혁신량	z − h(μ)	방향각 차이를 정규화한다
상태 보정	μ + Kν	위치와 방향의 상관관계도 반영한다
공분산 보정	조셉 형식	대칭성과 고윳값을 검사한다
반복 관측	표식마다 h와 H 재계산	같은 이동 입력을 중복 적용하지 않는다

이 필터에서 비선형 함수는 평균의 이동과 예상 관측을 담당하고, 야코비안은 그 주변 불확실성의 전달을 담당한다. 두 역할을 구분하면 수식과 코드의 대응이 분명해진다. 추정값이 실제 위치에서 크게 벗어날 수 있는 상황에서는 국소 근사의 범위를 먼저 점검해야 한다.

연습 문제

1. 기준 보정에서 거리 관측을 2.9 대신 3.1로 바꾼다. 보정된 x를 손으로 계산하고, reference_update의 기대값을 함께 바꾸어 확인한다.
2. 예측 방향각이 179도이고 측정 방향각이 −179도일 때 정규화한 혁신량을 도 단위로 구한다. 계산은 라디안으로 수행하고 마지막에만 도 단위로 변환한다.
3. 이동 모델을 구간 중간 방향 θ + α/2로 전진하는 방식으로 바꾼다. 평균 함수와 F, G를 유도한다. 회전 입력에 대한 위치 미분이 기존 코드와 어떻게 다른지 설명한다.
4. simulate에서 처음 40회 동안 표식이 보이지 않는 상황을 만든다. 예측은 계속 수행하고 이후 40회에만 보정한다. 전체 보정 횟수와 관측이 없을 때의 위치 불확실성이 달라지는 이유를 설명한다.

정답과 해설

1. 예측 거리는 3이므로 거리 혁신량은 0.1이다. x에 대한 거리 이득은 −5/9로 같고, 보정량은 −1/18이다. 보정된 x는 17/18이며 소수 여섯 자리 출력은 0.944444다. 표식이 예상보다 멀리 있다는 관측이므로 로봇 위치가 표식에서 멀어지는 방향으로 바뀐다. measured와 검사 기대값을 모두 바꿔야 한다.

   measured = np.array([3.1, 0.0])
   # reference_update의 기대 상태
   expected_state = np.array([17.0 / 18.0, 0.0, 0.0])

2. 정규화 전 차이는 −358도이고, 정규화 후 차이는 2도다. wrap은 같은 방향을 나타내는 각도들 중 정해진 범위의 값을 선택한다.

   predicted_bearing = np.deg2rad(179.0)
   measured_bearing = np.deg2rad(-179.0)
   difference = wrap(measured_bearing - predicted_bearing)
   print(f"{np.rad2deg(difference):.1f}")

   출력은 2.0이다. 각도를 도 단위로 만든 채 wrap에 전달하면 함수 내부의 π와 단위가 달라지므로 잘못된 결과가 된다.

3. β = θ + α/2로 두면 평균은 [x + d cos β, y + d sin β, wrap(θ + α)]ᵀ다. 이 모델 역시 연속 원호 이동의 정확한 적분식 자체는 아니다. 중간 방향으로 이동하는 이산 근사 모델이다.

   F =
       [1  0  −d sin β]
       [0  1   d cos β]
       [0  0       1 ]
   
   G =
       [cos β  −(d/2) sin β]
       [sin β   (d/2) cos β]
       [    0             1]

   회전 입력이 구간 중간 방향을 바꾸므로 G의 두 번째 열에 위치 미분이 생긴다. Q가 대각행렬이어도 GQGᵀ에는 위치와 방향 사이의 교차항이 생길 수 있다. motion과 motion_jacobians를 함께 교체한 뒤 기존 수치 미분 검사로 확인한다.

4. 반복 변수 이름을 step으로 바꾸고 예측과 검사 다음에 아래 조건을 넣는다. step은 0부터 시작하므로 처음 40회는 보정을 건너뛴다.

   if step < 40:
       continue

   남은 40회에서 표식 세 개를 처리하므로 보정은 120회다. 관측이 없는 동안 입력 잡음이 누적되고 방향 불확실성도 위치로 전달된다. 다만 모든 공분산 원소가 매번 증가해야 하는 것은 아니다. 로봇 방향과 상관관계에 따라 좌표별 값은 달라질 수 있다. 또한 이 수정은 관측 잡음을 뽑는 횟수까지 바꾸므로 같은 시드라도 이후 오도메트리 잡음의 난수열이 원래 실행과 달라진다. 같은 입력 조건으로 비교하려면 입력과 관측에 별도의 난수 생성기를 사용하거나 잡음 배열을 미리 생성해야 한다.