칼만 필터 - 1차원에서 2차원까지

이 장에서 배우는 것

창고 로봇이 선반 사이를 이동할 때 위치 센서는 조금씩 흔들리는 값을 내놓는다. 모터에 전달한 속도로 이동 거리를 계산해도 실제 위치와 차이가 생긴다. 센서 값만 따라가면 추정 위치가 튀고, 이동 계산만 누적하면 오차가 쌓인다. 칼만 필터(Kalman filter)는 두 정보의 불확실성을 함께 계산하여 현재 상태를 추정한다.

앞 장에서 확률변수의 평균과 공분산으로 불확실성을 표현했다. 여기서는 그 표현을 시간에 따라 움직인다. 먼저 직선 통로에서 위치 하나를 추정하고, 이어서 평면에서 위치와 속도를 함께 추정한다. 두 예제 모두 선형 모델을 사용하며, 로봇의 방향이나 회전 운동은 이번 장의 상태에 넣지 않는다.

• 예측과 갱신이 각각 상태와 불확실성에 어떤 변화를 주는지 설명한다.
• 칼만 이득을 계산하고 센서 관측이 추정값에 반영되는 정도를 해석한다.
• 평면 위치와 속도를 상태 벡터로 묶고 행렬의 역할과 크기를 구분한다.
• 과정 잡음과 관측 잡음의 설정을 바꾸어 필터의 반응을 비교한다.

문제 상황

로봇이 물건을 싣고 직선 통로를 초당 1m의 일정한 속도로 이동한다고 하자. 바닥의 위치 센서는 1초마다 로봇의 위치를 알려 준다. 어느 순간 이동 계산은 1m를 가리키지만 센서는 2m를 보고한다. 센서를 그대로 믿을지, 이동 계산을 유지할지 정하려면 두 값이 얼마나 불확실한지 알아야 한다.

예를 들어 이동 계산의 오차 분산과 센서 오차 분산이 모두 2m²라면 두 위치의 중간인 1.5m를 선택할 근거가 생긴다. 그러나 센서 분산이 훨씬 크다면 중간값도 센서를 너무 많이 반영한 결과일 수 있다. 평균을 취한다는 동작보다 중요한 것은 각 정보에 부여하는 신뢰의 크기다.

실습은 계산을 따라갈 수 있는 고정 관측으로 시작한다. 그다음 평면 이동 예제에서는 난수로 센서 잡음을 만든다. 센서 위치만 관측하면서 속도까지 추정하려면 시간에 따른 위치 변화가 속도와 어떻게 연결되는지 모델에 넣어야 한다. 이 연결을 행렬로 표현하는 것이 1차원 예제에서 확장되는 부분이다.

예측과 갱신을 한 번씩 계산한다

직선 통로에서 추정할 상태는 위치 x 하나다. 현재 추정 위치를 x, 그 오차 분산을 P라고 쓰자. 이동 간격 dt와 알려진 속도 u를 사용하면 다음 위치의 예측은 x_pred = x + u × dt다. 속도 u는 이번 모델의 입력이며 추정 대상이 아니다. 예측 오차가 이전 오차와 독립인 과정 잡음 때문에 늘어난다고 가정하면 P_pred = P + Q가 된다.

과정 잡음(process noise)의 분산 Q는 이동 모델이 설명하지 못하는 한 단계의 위치 오차를 나타낸다. 바퀴의 미끄러짐이나 속도 입력의 오차를 이 항에 포함할 수 있다. 반면 관측 잡음(measurement noise)의 분산 R은 센서가 현재 위치를 보고할 때 생기는 오차를 나타낸다. 두 값은 모두 이 예제에서 m² 단위지만 발생 원인은 다르다.

새 관측 z가 들어오면 예측과 관측의 차이인 잔차 r = z − x_pred를 계산한다. 예측 오차와 관측 잡음이 서로 독립이면 잔차의 분산은 S = P_pred + R이다. 칼만 이득(Kalman gain)은 K = P_pred / S이며, 위치 갱신은 x_new = x_pred + K × r가 된다. 즉, 예상하지 못한 관측 차이 중 K만큼을 추정 위치에 반영한다.

위치 하나를 직접 관측하는 이 경우에는 분산이 음수가 아니고 R이 양수이면 K가 0 이상 1 미만이다. K가 0에 가까우면 예측을 많이 유지하고, 1에 가까우면 관측 쪽으로 크게 이동한다. 뒤에서 사용할 행렬 이득의 모든 원소가 같은 범위를 갖는다는 뜻은 아니다.

x_pred = x + u * dt
P_pred = P + Q
residual = z - x_pred
S = P_pred + R
K = P_pred / S
x_new = x_pred + K * residual
P_new = (1.0 - K) * P_pred

마지막 분산 식은 정확한 산술에서 성립하는 간단한 형태다. 완성 코드에서는 같은 결과를 주는 P_new = (1 − K)²P_pred + K²R를 사용한다. 이 형태는 갱신된 오차에 남는 예측 오차와 센서 잡음의 기여를 각각 보여 준다. 행렬 계산으로 확장했을 때에도 수치 오차로 공분산의 성질이 흐트러지는 문제를 줄이는 데 도움이 된다.

초기값 x = 0, P = 1에 u = 1, dt = 1, Q = 1, R = 2를 넣어 보자. 예측 위치는 1이고 예측 분산은 2다. 첫 관측이 2이면 잔차는 1, 잔차 분산은 4, 이득은 0.5가 된다. 갱신 위치는 1.5이고 갱신 분산은 1이다. 관측으로 불확실성을 줄였지만 분산이 0이 되지는 않는다.

이 설정에서는 다음 단계에서도 예측 분산이 2, 갱신 분산이 1로 반복된다. 따라서 매 단계 이득도 0.5다. 이것은 선택한 초기 분산과 잡음 값이 만드는 결과이며 모든 칼만 필터에서 이득이 일정하다는 뜻은 아니다. 관측값은 추정 위치를 바꾸지만, 여기서 공분산의 변화는 모델과 잡음 설정으로 결정된다.

평면 위치와 속도를 상태로 묶는다

평면에서는 상태 벡터(state vector)를 [px, py, vx, vy] 순서로 정의한다. 위치의 단위는 m이고 속도의 단위는 m/s다. 일정한 속도를 가정하는 상태 전이 행렬 F는 위치에 속도 × dt를 더하고 속도는 그대로 유지한다. 센서는 위치만 보고하므로 관측 행렬 H는 상태에서 앞의 두 성분을 꺼낸다.

state = [px, py, vx, vy]

F = [[1, 0, dt, 0 ],
     [0, 1, 0,  dt],
     [0, 0, 1,  0 ],
     [0, 0, 0,  1 ]]

H = [[1, 0, 0, 0],
     [0, 1, 0, 0]]

예측은 state_pred = F @ state와 P_pred = F @ P @ F.T + Q다. 여기서 @는 행렬 곱을 나타낸다. 분산 하나였던 P는 이제 4×4 공분산 행렬이다. 대각 원소에는 각 상태의 오차 분산이 들어가고, 비대각 원소에는 서로 다른 상태 오차가 함께 변하는 정도가 들어간다. 위치와 속도의 교차 공분산은 m²/s처럼 두 상태의 단위를 곱한 단위를 갖는다.

평면 위치·속도 모델에서 각 배열이 나타내는 정보와 크기

기호	크기	의미
state, P	(4,), (4, 4)	상태 평균과 상태 오차 공분산
F, Q	(4, 4), (4, 4)	상태 전이와 한 단계 과정 잡음 공분산
z, H	(2,), (2, 4)	위치 관측과 상태를 관측으로 바꾸는 행렬
R, S	(2, 2), (2, 2)	관측 잡음 공분산과 잔차 공분산
K	(4, 2)	두 관측 잔차를 네 상태의 보정량으로 변환

관측 공간에서 잔차를 만들어야 하므로 residual = z − H @ state_pred다. 이어서 S = H @ P_pred @ H.T + R, K = P_pred @ H.T @ S⁻¹을 계산한다. 갱신은 state_new = state_pred + K @ residual이다. 역행렬 기호는 수식의 뜻을 표현한 것이며, 코드에서는 역행렬을 직접 만들지 않고 연립방정식을 푼다.

위치만 관측하는데 속도가 바뀌는 이유는 교차 공분산에 있다. 예측 위치에 속도가 들어가므로 속도 오차는 다음 위치 오차에도 영향을 준다. 관측 위치가 예상보다 앞에 있으면 필터는 초기 위치가 달랐을 가능성과 속도가 달랐을 가능성을 함께 반영한다. K의 아래 두 행이 속도에 적용할 보정량을 결정한다.

행렬 공분산은 A = I − K @ H를 두고 P_new = A @ P_pred @ A.T + K @ R @ K.T로 갱신한다. 이를 조셉 형태(Joseph form)라고 한다. 코드에서는 계산 뒤 P_new와 그 전치의 평균을 취해 부동소수점 계산에서 생기는 작은 비대칭도 정리한다. 이 조치는 잘못된 Q나 R을 올바르게 만드는 기능은 아니다.

이 모델은 고정된 창고 좌표계에서 직선 이동을 근사한다. 입력 속도를 아는 1차원 예제와 달리 평면 예제에서는 vx와 vy도 추정한다. 따라서 두 예제는 단순히 배열의 길이만 다른 것이 아니라 무엇을 이미 알고 무엇을 알아낼지에 대한 가정도 다르다.

잡음 설정은 예측과 관측의 비중을 바꾼다

같은 예측 오차 분산에서 R을 키우면 센서를 덜 믿으므로 이득이 작아진다. 같은 이전 공분산에서 Q를 키우면 예측이 더 불확실해져 이득이 커진다. 이 방향은 위치 하나를 직접 관측하는 예제에서 식으로 확인할 수 있다. 행렬 모델에서는 축 사이의 상관관계까지 작용하므로 모든 이득 원소를 하나의 숫자처럼 해석하지 않는다.

비교할 때는 한 번에 하나의 설정만 바꾸는 편이 원인을 파악하기 쉽다. 완성 코드의 잡음 비교는 매번 동일한 초기 분산 P = 1에서 시작한다. 여러 단계의 필터를 연속 실행하면서 설정을 바꾸면 이전 설정이 만든 P까지 결과에 섞인다. 아래 실험은 첫 갱신만 비교하여 그 영향을 분리한다.

평면 실습의 Q는 [0.1, 0.1, 0.01, 0.01]을 대각에 둔다. 위치와 속도의 한 단계 모델 오차를 서로 독립이라고 보는 단순한 설정이다. 특정 가속도 모델에서 유도한 공분산은 아니다. 또한 dt = 1초를 전제로 하므로 실행 간격을 바꿀 때 F만 바꾸고 Q를 그대로 재사용해서는 같은 불확실성 모델이 유지되지 않는다.

센서 표준편차가 1m라면 R의 해당 대각 원소는 1m²다. 표준편차가 2m이면 분산은 4m²가 된다. 실제 설정에서는 정지 상태에서 센서 값을 반복 수집하고 평균에서 벗어난 정도를 살펴볼 수 있다. 일정한 편향은 잡음 분산을 키운다고 제거되지 않는다. 모델과 센서의 체계적인 오차는 별도로 확인해야 한다.

선형 모델에 가우스 잡음을 가정하면 칼만 필터는 평균과 공분산으로 조건부 분포를 갱신한다. 여기서는 잡음의 평균이 0이고, 시간 사이 및 과정 잡음과 관측 잡음 사이에 상관이 없다고 둔다. 센서 오차가 연속해서 같은 방향으로 나타나거나 큰 이상값이 섞이면 이 가정에 맞지 않는다. Q와 R은 단순히 화면을 부드럽게 만드는 조절값이 아니라 오차에 관한 가정이다.

완성 코드

다음 내용을 kalman_demo.py로 저장한다. Python 3와 numpy만 사용한다. 첫 부분은 손으로 확인할 수 있는 직선 이동이며, 두 번째 부분은 평면에서 위치와 속도를 한 번 갱신한다. 마지막 부분은 고정된 난수 시드로 40회의 관측을 만들고 반복 계산에서 공분산의 기본 성질이 유지되는지 확인한다.

import numpy as np


def scalar_step(x, p, z, u, dt, q, r):
    x_pred = x + u * dt
    p_pred = p + q
    residual = z - x_pred
    s = p_pred + r
    k = p_pred / s
    x_new = x_pred + k * residual
    p_new = (1.0 - k) ** 2 * p_pred + k ** 2 * r
    return x_new, p_new, k


def vector_step(state, p, z, f, h, q, r):
    state_pred = f @ state
    p_pred = f @ p @ f.T + q
    residual = z - h @ state_pred
    s = h @ p_pred @ h.T + r
    cross = p_pred @ h.T
    k = np.linalg.solve(s.T, cross.T).T
    state_new = state_pred + k @ residual
    a = np.eye(state.size) - k @ h
    p_new = a @ p_pred @ a.T + k @ r @ k.T
    p_new = 0.5 * (p_new + p_new.T)
    return state_new, p_new, k


def covariance_ok(p):
    symmetric = np.allclose(p, p.T, rtol=0.0, atol=1e-10)
    nonnegative = np.linalg.eigvalsh(p).min() >= -1e-10
    return bool(symmetric and nonnegative)


def main():
    print("1D: step z estimate variance gain")
    x, p = 0.0, 1.0
    for step, z in enumerate([2.0, 2.0, 4.0, 4.0], start=1):
        x, p, k = scalar_step(x, p, z, 1.0, 1.0, 1.0, 2.0)
        print(f"{step} {z:.1f} {x:.4f} {p:.4f} {k:.4f}")

    print("Noise: case Q R first_gain")
    cases = [
        ("base", 1.0, 2.0),
        ("small_Q", 0.1, 2.0),
        ("large_Q", 4.0, 2.0),
        ("small_R", 1.0, 0.5),
        ("large_R", 1.0, 8.0),
    ]
    for name, q_value, r_value in cases:
        _, _, k = scalar_step(
            0.0, 1.0, 2.0, 1.0, 1.0, q_value, r_value
        )
        print(f"{name} {q_value:.1f} {r_value:.1f} {k:.4f}")

    dt = 1.0
    f = np.array([
        [1.0, 0.0, dt, 0.0],
        [0.0, 1.0, 0.0, dt],
        [0.0, 0.0, 1.0, 0.0],
        [0.0, 0.0, 0.0, 1.0],
    ])
    h = np.array([
        [1.0, 0.0, 0.0, 0.0],
        [0.0, 1.0, 0.0, 0.0],
    ])
    q = np.diag([0.1, 0.1, 0.01, 0.01])
    r = np.diag([1.0, 1.0])
    initial = np.array([0.0, 0.0, 1.0, 0.5])
    state, p, k = vector_step(
        initial, np.eye(4), np.array([1.2, 0.4]), f, h, q, r
    )
    print("2D: first state [px py vx vy]")
    print(" ".join(f"{value:.4f}" for value in state))
    print(f"2D: position_gain={k[0, 0]:.4f} "
          f"velocity_gain={k[2, 0]:.4f}")

    rng = np.random.Generator(np.random.PCG64(2026))
    truth = initial.copy()
    state = initial.copy()
    p = np.eye(4)
    process_std = np.sqrt(np.diag(q))
    sensor_std = np.sqrt(np.diag(r))
    finite = True
    cov_ok = True
    steps = 40
    for _ in range(steps):
        truth = f @ truth + rng.normal(size=4) * process_std
        z = h @ truth + rng.normal(size=2) * sensor_std
        state, p, _ = vector_step(state, p, z, f, h, q, r)
        finite = finite and bool(
            np.isfinite(state).all() and np.isfinite(p).all()
        )
        cov_ok = cov_ok and covariance_ok(p)

    if not (finite and cov_ok):
        raise RuntimeError("Filter validation failed")
    print(f"2D simulation: steps={steps}, "
          f"finite={finite}, covariance_ok={cov_ok}")


if __name__ == "__main__":
    main()

줄별 해설

scalar_step의 첫 두 줄은 이동 입력을 평균에 더하고 과정 잡음 분산을 공분산에 더한다. 분산에는 속도 입력을 그대로 더하지 않는다. 알려진 입력이 평균을 이동시키는 효과와, 그 입력을 믿기 어려운 정도가 분산을 늘리는 효과를 구분한 것이다.

residual과 s는 관측과 예측이 얼마나 다른지, 그리고 그 차이가 어느 정도 흔들릴 수 있는지를 나타낸다. k = p_pred / s는 예측 분산이 전체 잔차 분산에서 차지하는 비율을 구한다. 이어지는 두 줄에서 평균과 분산을 각각 갱신하며, 반환값에 이득을 포함해 계산 결과를 관찰할 수 있게 한다.

vector_step에서는 예측에 F를 곱한다. f @ p @ f.T처럼 공분산의 양쪽에 행렬이 붙는 이유는 상태 오차의 두 성분 사이 곱의 평균을 변환하기 때문이다. 평균처럼 F를 한 번만 곱하면 필요한 공분산 변환이 되지 않는다.

h @ state_pred는 예측 상태를 센서가 보고할 위치로 바꾼다. 따라서 잔차는 길이 2이고 S는 2×2다. cross는 4×2 행렬이다. 코드의 배열 크기를 확인하면 관측 잔차가 어떻게 네 상태의 보정량으로 바뀌는지 추적할 수 있다.

np.linalg.solve(s.T, cross.T).T는 K @ S = cross를 전치하여 푼다. 즉 S.T @ K.T = cross.T에서 K.T를 구한 뒤 다시 전치한다. 이 방식은 역행렬 전체를 만들지 않고 필요한 곱을 구한다. R이 양의 정부호이고 예측 공분산이 유효하면 S도 양의 정부호가 되어 이 계산을 수행할 수 있다.

a는 관측 반영 뒤 남는 예측 오차의 변환을 나타낸다. 다음 줄은 남은 예측 오차와 관측 잡음의 기여를 더한다. 그 뒤 대칭화를 수행하지만 큰 음의 고유값을 숨기거나 분산을 강제로 양수로 바꾸지는 않는다.

covariance_ok는 대칭성과 양의 준정부호 여부를 수치적으로 검사한다. 대칭 행렬용 함수 eigvalsh로 고유값을 구하고, 최소값이 작은 허용 오차보다 아래로 내려가지 않는지 확인한다. 허용값은 이번 상태 단위와 규모에 맞춘 값이며 다른 규모의 문제에 그대로 적용할 기준은 아니다.

첫 반복문의 관측은 고정된 네 값이다. enumerate의 단계 번호는 1부터 시작하며, 각 관측은 이동 예측을 한 번 수행한 뒤의 시각에 해당한다. 잡음 비교 반복문은 매번 x = 0, P = 1을 다시 전달하므로 앞 사례의 결과가 다음 사례에 영향을 주지 않는다.

평면 모델의 np.diag는 대각 공분산 행렬을 만든다. 초기 상태는 원점에서 x 방향으로 1m/s, y 방향으로 0.5m/s 이동하는 평균이다. 초기 공분산 np.eye(4)는 각 성분의 분산을 수치상 1로 두지만 위치와 속도 분산의 물리 단위는 서로 다르다.

고정 관측 [1.2, 0.4]를 넣는 호출은 행렬 계산을 확인하기 위한 독립적인 한 단계 예제다. 그 뒤 state와 p를 다시 초기화하므로 해당 관측은 40회 시뮬레이션에 섞이지 않는다. truth는 시뮬레이터만 아는 실제 상태이며 필터에는 z만 전달한다.

난수 생성기는 시드 2026으로 고정한다. Q와 R은 대각 행렬이므로 각 대각 원소의 제곱근을 표준정규 난수에 곱하면 원하는 잡음 분산을 만들 수 있다. 비대각 공분산을 사용하는 모델에서는 이 방식만으로 상태 사이의 상관관계를 생성할 수 없다.

마지막 반복문은 실제 상태를 한 단계 전진시킨 뒤 같은 시각의 관측을 만든다. 이후 필터가 예측과 갱신을 수행한다. 검사는 마지막 상태만 보는 대신 모든 단계의 결과를 누적한다. finite와 cov_ok 중 하나라도 거짓이면 예외가 발생하므로 성공 문구가 출력되지 않는다.

실행 결과

numpy가 준비된 환경에서 다음 명령을 실행한다. 첫 명령은 경고를 오류로 취급하여 문법 컴파일을 수행하며 정상일 때 출력이 없다. 두 번째 명령도 경고를 오류로 취급한다. 아래는 코드의 예상 표준 출력이다.

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

1D: step z estimate variance gain
1 2.0 1.5000 1.0000 0.5000
2 2.0 2.2500 1.0000 0.5000
3 4.0 3.6250 1.0000 0.5000
4 4.0 4.3125 1.0000 0.5000
Noise: case Q R first_gain
base 1.0 2.0 0.5000
small_Q 0.1 2.0 0.3548
large_Q 4.0 2.0 0.7143
small_R 1.0 0.5 0.8000
large_R 1.0 8.0 0.2000
2D: first state [px py vx vy]
1.1355 0.4323 1.0645 0.4677
2D: position_gain=0.6774 velocity_gain=0.3226
2D simulation: steps=40, finite=True, covariance_ok=True

직선 이동의 마지막 추정이 관측 4보다 큰 것은 오류가 아니다. 직전 추정 3.625에서 한 단계 이동을 예측하면 4.625이고, 관측 4를 절반 반영하면 4.3125가 된다. 필터는 현재 관측뿐 아니라 이동 모델도 사용한다.

평면 예제에서는 예측 위치가 [1, 0.5]이고 잔차가 [0.2, −0.1]이다. x축 예측 위치 분산은 2.1, 잔차 분산은 3.1이므로 위치 이득은 2.1/3.1이다. 위치와 속도의 교차 공분산은 1이어서 속도 이득은 1/3.1이다. 같은 잔차가 서로 다른 비율로 위치와 속도를 수정하는 것을 출력에서 확인할 수 있다.

마지막 두 참값은 수치 계산이 유한하고 공분산이 검사 기준을 만족한다는 뜻이다. 위치 추정 정확도가 충분하다는 증거는 아니다. 정확도를 평가하려면 추정 위치와 실제 위치의 차이를 별도로 기록해야 한다. 난수 시드는 실험을 재현하기 위한 것이며, 여기서는 환경별 마지막 자리 차이의 영향을 받기 쉬운 난수 기반 통계를 출력하지 않는다.

실무에서 자주 틀리는 것

표준편차를 분산 자리에 넣는다

센서 표준편차가 0.5m인데 R에 0.5를 넣으면 의도한 0.25m²보다 큰 관측 분산을 사용한다. 그 결과 센서를 계획보다 적게 반영한다. 변수 이름에 표준편차인지 분산인지를 드러내면 혼동을 줄일 수 있다.

틀린 코드다.

sensor_std = 0.5
r = np.eye(2) * sensor_std

고친 코드다.

sensor_std = 0.5
r = np.eye(2) * sensor_std ** 2

공분산 변환에 원소별 곱을 사용한다

numpy 배열의 *는 원소별 곱이다. 같은 크기의 행렬끼리는 오류 없이 실행될 수 있어 더 주의해야 한다. 상태 사이의 오차 전파를 계산하려면 행렬 곱을 사용한다.

틀린 코드다.

p_pred = f * p * f.T + q

고친 코드다.

p_pred = f @ p @ f.T + q

서로 다른 시각의 예측과 관측을 비교한다

아래에서 truth가 아직 이전 시각에 있는데 관측을 먼저 만들면, 필터는 다음 시각으로 예측한 위치를 이전 시각의 센서 값에 맞춘다. 일정하게 이동하는 로봇에서는 뒤처지는 오차를 만들 수 있다. 실무에서는 수신 시각과 측정 시각도 구분해야 한다.

틀린 코드다.

z = h @ truth + rng.normal(size=2) * sensor_std
truth = f @ truth + rng.normal(size=4) * process_std
state, p, _ = vector_step(state, p, z, f, h, q, r)

고친 코드다.

truth = f @ truth + rng.normal(size=4) * process_std
z = h @ truth + rng.normal(size=2) * sensor_std
state, p, _ = vector_step(state, p, z, f, h, q, r)

초기 공분산을 실제 상태 값으로 만든다

초기 위치가 원점이라는 이유로 위치 분산을 0으로 두거나, 음의 속도를 분산으로 사용하면 불확실성의 의미가 깨진다. 공분산은 상태 값의 크기가 아니라 초기 추정 오차에 대한 지식을 표현한다. 아래 수정은 초기 위치 표준편차를 0.5m, 속도 표준편차를 0.2m/s로 가정한 예다.

틀린 코드다.

state = np.array([0.0, 0.0, 1.0, -0.5])
p = np.diag(state)

고친 코드다.

state = np.array([0.0, 0.0, 1.0, -0.5])
initial_std = np.array([0.5, 0.5, 0.2, 0.2])
p = np.diag(initial_std ** 2)

한눈에 보기

필터의 계산과 설정을 확인할 때 사용할 기준

항목	역할	확인할 점
예측	모델로 다음 상태와 공분산 계산	dt와 관측 시각이 일치하는가
잔차	관측과 예측 관측의 차이	같은 좌표계와 단위에서 뺐는가
칼만 이득	잔차를 상태 보정량으로 변환	예측·관측 불확실성을 함께 반영하는가
Q	한 단계 모델 오차의 공분산	상태 단위와 시간 간격에 맞는가
R	센서 오차의 공분산	표준편차를 제곱했는가
P	추정 오차의 공분산	대칭성과 양의 준정부호 성질을 유지하는가

필터의 한 주기는 모델로 앞으로 이동하고, 새 관측과의 차이를 계산하고, 그 차이를 불확실성에 따라 나누어 반영하는 과정이다. 평면으로 확장해도 이 순서는 유지된다. 이번 장의 F와 H는 선형 관계를 표현한다. 다음 장에서는 로봇의 방향처럼 비선형 관계가 들어왔을 때 예측과 갱신을 어떻게 구성하는지 살펴본다.

연습 문제

1. 직선 이동의 초기 조건을 그대로 두고 첫 관측만 z = 3으로 바꾼다. 예측 위치, 잔차, 이득, 갱신 위치와 갱신 분산을 계산한다.
2. P = 1, Q = 1에서 첫 갱신의 R을 2에서 8로 바꾼다. z = 2일 때 이득과 갱신 위치를 구하고, 변화의 이유를 설명한다.
3. 평면 예제의 첫 관측을 [1.0, 0.5]로 바꾼다. 상태와 공분산 중 무엇이 관측 갱신으로 달라지는지 설명한다. 실제 수치를 계산할 때 예측과 갱신을 구분한다.
4. 40회 시뮬레이션에 위치 오차의 제곱합을 누적하고 평균 제곱 오차를 출력한다. 두 위치 성분을 합친 오차의 정의와 단위를 함께 적는다.

정답과 해설

1. 예측 위치는 1이고 예측 분산은 2다. 잔차는 3 − 1 = 2이며 이득은 2 / (2 + 2) = 0.5다. 갱신 위치는 1 + 0.5 × 2 = 2이고 갱신 분산은 1이다. 관측값이 바뀌어도 같은 모델과 잡음 설정이라면 이 단계의 이득과 공분산은 바뀌지 않는다.

2. 잔차 분산은 2 + 8 = 10이므로 이득은 0.2다. 예측 위치 1에서 관측 2까지의 차이 중 0.2만 반영하여 갱신 위치는 1.2가 된다. R이 커지면 관측 오차가 더 크다고 가정하므로 예측 위치를 더 많이 유지한다. 갱신 분산은 0.8² × 2 + 0.2² × 8 = 1.6으로, R = 2일 때보다 크다.

3. 예측 상태는 [1.0, 0.5, 1.0, 0.5]이고 관측 잔차는 [0, 0]이다. 따라서 갱신으로 평균 상태가 추가로 바뀌지는 않는다. 그러나 센서가 예측과 일치한다는 정보를 얻었으므로 공분산은 줄어든다. 위치 분산은 예측 시 2.1에서 갱신 뒤 약 0.6774로 줄고, 속도 분산은 1.01에서 약 0.6874로 줄어든다. 잔차가 0이라고 공분산 갱신을 생략해서는 안 된다.

4. 시뮬레이션 반복문 앞에 누적 변수를 두고, 매번 필터 갱신 뒤 아래 계산을 추가한다. 단계별 오차는 x축과 y축 위치 오차 제곱의 합이다. 이를 단계 수로 나누면 두 성분을 합친 위치 평균 제곱 오차가 되며 단위는 m²다. 아래 코드는 기존 변수와 반복문에 추가하는 부분이다.

   # Before the simulation loop:
   position_error_sum = 0.0
   
   # Inside the loop, after vector_step:
   error = state[:2] - truth[:2]
   position_error_sum += float(error @ error)
   
   # After the loop:
   position_mse = position_error_sum / steps
   print(f"position_mse={position_mse:.6f}")

   이 값은 지정한 시드의 한 실험에 대한 결과다. 설정의 일반적인 성능을 비교하려면 여러 시드에서 같은 지표를 모아야 한다. 필터의 Q와 R만 바꾸는 비교에서는 실제 상태와 관측을 만드는 잡음 설정을 고정해야 동일한 환경에서 비교할 수 있다.