칼만 필터 맛보기

흔들리는 센서값 사이에서 길을 찾기

로봇이 복도를 따라 똑바로 움직이고 있다고 생각해 봅시다. 바퀴 엔코더는 로봇이 앞으로 1초에 1m씩 가고 있다고 말합니다. 그런데 위치 센서는 매 순간 조금씩 흔들립니다.

어느 순간 센서는 이렇게 말할 수 있습니다.

실제 위치:     0.0, 1.0, 2.0, 3.0, 4.0, ...
센서 측정값:   0.3, 0.7, 2.4, 2.8, 4.5, ...

측정값만 보면 로봇이 갑자기 빨라졌다가 느려졌다가, 심지어 뒤로 살짝 밀린 것처럼 보일 수도 있습니다. 하지만 우리는 압니다. 로봇은 그렇게 정신없이 튀고 있는 것이 아니라, 센서값이 흔들리고 있을 뿐입니다.

그렇다고 센서를 무시할 수도 없습니다. 로봇의 운동 모델, 즉 "아마 이 정도 속도로 움직였을 것이다"라는 예측도 완벽하지 않기 때문입니다. 바퀴가 미끄러질 수도 있고, 모터 출력이 살짝 달라질 수도 있습니다.

칼만 필터는 이 두 정보를 함께 사용합니다.

• 로봇의 운동 모델이 말하는 예측

• 센서가 실제로 관측한 측정값

• 둘 중 어느 쪽을 더 믿을지 결정하는 불확실성

핵심은 간단합니다. 예측하고, 측정값으로 고치고, 다시 예측하고, 다시 고칩니다. 이 반복 구조가 칼만 필터의 심장입니다.

상태란 무엇인가

칼만 필터에서 가장 먼저 정해야 할 것은 "무엇을 추정할 것인가"입니다. 이것을 상태, 영어로는 state라고 부릅니다.

1차원 직선 위를 움직이는 로봇을 생각해 봅시다. 가장 단순하게는 위치만 추정할 수 있습니다.

상태 = 위치

하지만 위치만 알고 있으면 다음 위치를 예측하기가 조금 답답합니다. 로봇이 어디에 있는지는 알지만, 얼마나 빠르게 움직이는지는 모르기 때문입니다. 그래서 이번 장에서는 상태에 위치와 속도를 함께 넣겠습니다.

상태 = [위치, 속도]

NumPy 배열로 쓰면 다음과 같습니다.

import numpy as np

x = np.array([
    [0.0],  # position
    [1.0],  # velocity
])

여기서 x는 현재 우리가 믿고 있는 로봇의 상태입니다. 첫 번째 값은 위치, 두 번째 값은 속도입니다.

로봇공학에서는 이렇게 여러 값을 하나로 묶은 벡터를 자주 사용합니다. 앞 장들에서 위치를 [x, y]로 표현했던 것처럼, 이제는 "추정하고 싶은 값들"을 하나의 벡터로 묶는다고 보면 됩니다.

운동 모델: 다음 상태를 예상하기

로봇이 현재 위치 p에 있고 속도 v로 움직인다고 합시다. 시간 간격이 dt라면, 다음 위치는 대략 이렇게 예상할 수 있습니다.

다음 위치 = 현재 위치 + 현재 속도 * dt
다음 속도 = 현재 속도

속도가 갑자기 변하지 않는다고 가정하는 모델입니다. 현실의 로봇은 완전히 이렇게 움직이지 않지만, 짧은 시간 간격에서는 꽤 쓸 만한 가정입니다.

이를 행렬로 쓰면 다음과 같습니다.

[p_next]   [1  dt] [p]
[v_next] = [0   1] [v]

이때 가운데 행렬을 상태 전이 행렬이라고 부릅니다. 현재 상태를 다음 상태로 옮겨 주는 행렬입니다.

Python으로는 이렇게 쓸 수 있습니다.

dt = 0.1

F = np.array([
    [1.0, dt],
    [0.0, 1.0],
])

x_pred = F @ x

F @ x는 "운동 모델에 따라 다음 상태를 예측한다"는 뜻입니다. 여기서 @는 행렬 곱입니다. 3부에서 배운 행렬 곱이 이제 로봇의 상태 예측에 사용되고 있습니다.

불확실성도 함께 움직인다

칼만 필터가 단순한 위치 계산과 다른 점은, 값뿐 아니라 그 값을 얼마나 믿는지도 함께 다룬다는 것입니다.

앞 장에서 평균과 분산을 배웠습니다. 평균이 대표값이라면, 분산은 값이 얼마나 퍼져 있는지 나타냅니다. 칼만 필터에서도 같은 감각을 사용합니다.

현재 상태 추정값이 이렇게 있다고 합시다.

위치는 0m쯤일 것이다.
속도는 1m/s쯤일 것이다.

그런데 이 믿음이 아주 확실할 수도 있고, 꽤 불안할 수도 있습니다.

위치: 0m일 것 같은데, 오차가 클 수도 있다.
속도: 1m/s일 것 같은데, 정확하지는 않다.

이 불확실성을 행렬로 표현한 것이 P입니다. 보통 오차 공분산 행렬이라고 부릅니다. 이름은 어렵지만, 지금은 "상태 추정이 얼마나 흔들릴 수 있는지 담은 행렬"이라고 이해하면 충분합니다.

P = np.array([
    [1.0, 0.0],
    [0.0, 1.0],
])

대각선의 첫 번째 값은 위치의 불확실성, 두 번째 값은 속도의 불확실성을 나타냅니다. 값이 클수록 "잘 모르겠다"는 뜻입니다.

예측 단계에서는 상태만 다음으로 이동하는 것이 아닙니다. 불확실성도 함께 변합니다.

Q = np.array([
    [0.01, 0.0],
    [0.0, 0.1],
])

P_pred = F @ P @ F.T + Q

여기서 Q는 과정 잡음, 또는 process noise를 나타냅니다. 모델이 얼마나 틀릴 수 있는지를 뜻합니다. 예를 들어 바퀴가 미끄러지거나, 모터가 예상보다 조금 덜 움직이거나, 바닥 상태가 달라지는 일이 여기에 해당합니다.

Q가 크면 필터는 "내 운동 모델이 별로 믿을 만하지 않다"고 생각합니다. 반대로 Q가 작으면 "로봇은 거의 모델대로 움직인다"고 보는 것입니다.

예측 단계

이제 칼만 필터의 첫 번째 단계인 예측을 정리해 봅시다.

예측 단계에서는 이전 상태와 운동 모델을 사용해 다음 상태를 예상합니다.

상태 예측:
x_pred = F x

불확실성 예측:
P_pred = F P F.T + Q

Python 함수로 만들면 다음과 같습니다.

def predict(x, P, F, Q):
    x_pred = F @ x
    P_pred = F @ P @ F.T + Q
    return x_pred, P_pred

이 함수의 결과는 아직 센서를 보지 않은 값입니다. 로봇이 "내가 아는 운동 법칙대로라면 아마 여기쯤 있을 거야"라고 말하는 단계입니다.

하지만 로봇은 실제 세상에 있습니다. 바퀴가 조금 미끄러졌을 수도 있고, 바닥에 작은 턱이 있었을 수도 있습니다. 그래서 다음 단계에서 센서 측정값을 반영합니다.

측정 모델: 센서는 상태의 일부만 본다

이번 예제에서 센서는 로봇의 위치만 측정한다고 가정하겠습니다. 속도는 직접 측정하지 않습니다.

상태는 [위치, 속도]인데, 센서가 보는 것은 위치 하나뿐입니다.

상태: [위치, 속도]
측정: 위치

이를 행렬로 표현하면 다음과 같습니다.

z = [1  0] [p]
          [v]

즉, 상태 벡터에서 첫 번째 값인 위치만 꺼내겠다는 뜻입니다.

Python으로는 다음과 같습니다.

H = np.array([
    [1.0, 0.0],
])

H @ x_pred는 현재 예측 상태에서 센서가 보았을 법한 측정값을 계산합니다.

z_pred = H @ x_pred

실제 센서 측정값 z와 예측 측정값 z_pred의 차이를 잔차, 또는 residual이라고 부릅니다.

y = z - z_pred

잔차는 "예측과 관측이 얼마나 어긋났는가"를 말해 줍니다. 만약 예측 위치가 5.0m인데 센서가 5.2m라고 했다면 잔차는 0.2m입니다. 센서가 예측보다 조금 앞쪽을 가리키고 있는 셈입니다.

보정 단계

보정 단계에서는 예측값을 센서 측정값 쪽으로 살짝 당깁니다. 문제는 얼마나 당길 것인가입니다.

센서가 매우 정확하다면 예측값을 센서 쪽으로 많이 당겨야 합니다. 반대로 센서가 매우 시끄럽다면 측정값이 튀어도 너무 따라가면 안 됩니다.

이때 등장하는 값이 칼만 이득, Kalman gain입니다. 칼만 이득은 예측과 측정 사이에서 어느 쪽을 더 믿을지 결정하는 가중치입니다.

칼만 이득이 크다  -> 측정값을 많이 믿는다.
칼만 이득이 작다  -> 예측값을 많이 믿는다.

보정 단계의 식은 다음과 같습니다.

잔차:
y = z - H x_pred

잔차 불확실성:
S = H P_pred H.T + R

칼만 이득:
K = P_pred H.T inv(S)

상태 보정:
x = x_pred + K y

불확실성 보정:
P = (I - K H) P_pred

여기서 R은 센서 측정 잡음의 분산입니다. 센서가 많이 흔들리면 R을 크게 잡고, 센서가 정확하면 R을 작게 잡습니다.

Python 함수로 만들면 다음과 같습니다.

def update(x_pred, P_pred, z, H, R):
    y = z - H @ x_pred
    S = H @ P_pred @ H.T + R
    K = P_pred @ H.T @ np.linalg.inv(S)

    x = x_pred + K @ y
    I = np.eye(x.shape[0])
    P = (I - K @ H) @ P_pred

    return x, P, K

이제 칼만 필터의 기본 구조가 완성되었습니다.

1. 예측한다.
2. 센서값을 본다.
3. 예측과 센서값의 차이를 계산한다.
4. 칼만 이득으로 적절히 보정한다.
5. 다음 시간으로 넘어간다.

이 구조는 베이즈 추정과 닮아 있습니다. 이전 장에서 사전 믿음, 관측, 사후 믿음을 배웠습니다. 칼만 필터에서는 예측 결과가 사전 믿음이 되고, 센서 측정값이 관측이 되며, 보정된 상태가 사후 믿음이 됩니다.

1차원 이동 로봇 시뮬레이션

이제 직접 시뮬레이션을 만들어 봅시다. 로봇은 1차원 직선 위를 일정한 속도로 움직입니다. 실제 위치는 부드럽게 증가하지만, 센서 측정값에는 노이즈가 들어갑니다. 칼만 필터는 이 흔들리는 측정값을 사용해 더 부드러운 위치를 추정합니다.

먼저 필요한 라이브러리를 불러옵니다.

import numpy as np
import matplotlib.pyplot as plt

전체 코드는 다음과 같습니다.

import numpy as np
import matplotlib.pyplot as plt

np.random.seed(7)

# 시간 설정
dt = 0.1
total_time = 10.0
times = np.arange(0.0, total_time + dt, dt)

# 실제 로봇 운동
true_position = 0.0
true_velocity = 1.0

# 센서 노이즈 설정
measurement_std = 0.8
measurement_var = measurement_std ** 2

# 칼만 필터 초기값
x = np.array([
    [0.0],  # estimated position
    [0.0],  # estimated velocity
])

P = np.array([
    [10.0, 0.0],
    [0.0, 10.0],
])

F = np.array([
    [1.0, dt],
    [0.0, 1.0],
])

H = np.array([
    [1.0, 0.0],
])

Q = np.array([
    [0.01, 0.0],
    [0.0, 0.05],
])

R = np.array([
    [measurement_var],
])

def predict(x, P, F, Q):
    x_pred = F @ x
    P_pred = F @ P @ F.T + Q
    return x_pred, P_pred

def update(x_pred, P_pred, z, H, R):
    y = z - H @ x_pred
    S = H @ P_pred @ H.T + R
    K = P_pred @ H.T @ np.linalg.inv(S)

    x = x_pred + K @ y
    I = np.eye(x.shape[0])
    P = (I - K @ H) @ P_pred

    return x, P, K

true_positions = []
measurements = []
estimated_positions = []
estimated_velocities = []
kalman_gains = []

for t in times:
    # 실제 로봇 위치 업데이트
    true_position = true_position + true_velocity * dt

    # 노이즈가 있는 위치 측정
    z_value = true_position + np.random.normal(0.0, measurement_std)
    z = np.array([[z_value]])

    # 칼만 필터: 예측 후 보정
    x_pred, P_pred = predict(x, P, F, Q)
    x, P, K = update(x_pred, P_pred, z, H, R)

    true_positions.append(true_position)
    measurements.append(z_value)
    estimated_positions.append(x[0, 0])
    estimated_velocities.append(x[1, 0])
    kalman_gains.append(K[0, 0])

plt.figure(figsize=(10, 5))
plt.plot(times, true_positions, label='true position', linewidth=3)
plt.scatter(times, measurements, label='noisy measurements', s=15, alpha=0.5)
plt.plot(times, estimated_positions, label='kalman estimate', linewidth=3)
plt.xlabel('time [s]')
plt.ylabel('position [m]')
plt.grid(True)
plt.legend()
plt.show()

그래프를 보면 측정값은 위아래로 흔들리지만, 칼만 필터의 추정 위치는 훨씬 부드럽게 실제 위치를 따라갑니다. 여기서 중요한 점은 칼만 필터가 단순히 이동 평균처럼 값을 부드럽게 만드는 것만은 아니라는 사실입니다.

칼만 필터는 로봇의 운동 모델을 사용합니다. "이 로봇은 위치와 속도를 가진 물체이며, 짧은 시간 동안 대략 이전 속도로 움직인다"는 구조를 알고 있습니다. 그래서 단순히 측정값 몇 개를 평균내는 것보다 더 로봇다운 추정을 할 수 있습니다.

속도도 함께 추정된다

이번 예제에서 센서는 위치만 측정했습니다. 그런데 상태 벡터에는 속도도 들어 있습니다.

처음에는 속도 추정값을 0.0으로 시작했습니다. 실제 속도는 1.0m/s인데도 말입니다. 그런데 시간이 지나면 필터는 위치 측정값의 변화를 보면서 속도도 점점 추정해 냅니다.

아래 코드를 추가해서 속도 추정 결과를 그려 봅시다.

plt.figure(figsize=(10, 4))
plt.plot(times, estimated_velocities, label='estimated velocity', linewidth=3)
plt.axhline(true_velocity, color='black', linestyle='--', label='true velocity')
plt.xlabel('time [s]')
plt.ylabel('velocity [m/s]')
plt.grid(True)
plt.legend()
plt.show()

이 그래프는 칼만 필터의 재미있는 면을 보여 줍니다. 직접 측정하지 않은 속도도, 위치가 시간에 따라 어떻게 바뀌는지를 통해 어느 정도 알아낼 수 있습니다.

물론 마법은 아닙니다. 필터가 속도를 추정할 수 있는 이유는 상태 전이 행렬 F 안에 위치와 속도의 관계가 들어 있기 때문입니다.

다음 위치 = 현재 위치 + 속도 * dt

이 관계가 없다면 위치 측정만으로 속도를 상태 안에서 자연스럽게 추정하기 어렵습니다. 결국 좋은 추정은 좋은 모델에서 시작합니다.

칼만 이득을 관찰해 보기

칼만 이득은 필터가 측정값을 얼마나 받아들이는지 알려 줍니다. 시뮬레이션에서 저장한 kalman_gains를 그려 봅시다.

plt.figure(figsize=(10, 4))
plt.plot(times, kalman_gains, linewidth=3)
plt.xlabel('time [s]')
plt.ylabel('kalman gain for position')
plt.grid(True)
plt.show()

초반에는 칼만 이득이 비교적 큽니다. 왜냐하면 초기 상태를 잘 모르기 때문입니다. 우리는 P를 크게 시작했습니다.

P = np.array([
    [10.0, 0.0],
    [0.0, 10.0],
])

이 말은 "처음 위치와 속도에 대해 자신이 없다"는 뜻입니다. 자신이 없으니 센서값을 적극적으로 받아들입니다.

시간이 지나면서 필터는 로봇의 위치와 속도를 점점 파악합니다. 그러면 불확실성이 줄어들고, 칼만 이득도 안정된 값으로 내려갑니다. 이제 센서값이 한 번 크게 튀어도 필터는 그대로 휘청거리지 않습니다.

센서를 더 믿을 때와 덜 믿을 때

칼만 필터를 실험할 때 가장 먼저 만져 볼 값은 R과 Q입니다.

R은 센서 측정 잡음입니다. R이 크면 필터는 센서를 덜 믿습니다. 측정값이 튀어도 추정값이 많이 움직이지 않습니다. 대신 실제 움직임이 갑자기 바뀌었을 때 반응이 느릴 수 있습니다.

Q는 운동 모델의 잡음입니다. Q가 크면 필터는 모델을 덜 믿습니다. 그래서 센서 측정값에 더 민감하게 반응합니다. 반대로 Q가 작으면 "로봇은 거의 내가 세운 운동 모델대로 움직인다"고 생각합니다.

다음 코드를 바꿔 가며 그래프를 비교해 보세요.

# 센서가 매우 시끄럽다고 가정
R = np.array([[2.0 ** 2]])

# 센서가 꽤 정확하다고 가정
R = np.array([[0.2 ** 2]])

또는 과정 잡음을 바꿔 봅니다.

# 운동 모델을 강하게 믿음
Q = np.array([
    [0.001, 0.0],
    [0.0, 0.001],
])

# 운동 모델이 자주 틀린다고 가정
Q = np.array([
    [0.1, 0.0],
    [0.0, 0.5],
])

로봇이 거의 일정한 속도로 움직인다면 작은 Q가 잘 맞을 수 있습니다. 하지만 로봇이 자주 가속하거나 멈추거나 바퀴가 미끄러진다면 너무 작은 Q는 오히려 문제를 일으킵니다. 필터가 현실의 변화에 둔감해지기 때문입니다.

노이즈가 있는 측정값과 추정값 비교하기

그래프만 봐도 차이가 느껴지지만, 숫자로도 비교해 봅시다. 대표적인 방법은 평균 제곱근 오차, RMSE를 계산하는 것입니다. 오차를 제곱해서 평균낸 뒤, 다시 제곱근을 취한 값입니다. 값이 작을수록 실제 위치에 더 가깝습니다.

true_positions_arr = np.array(true_positions)
measurements_arr = np.array(measurements)
estimated_positions_arr = np.array(estimated_positions)

measurement_rmse = np.sqrt(np.mean((measurements_arr - true_positions_arr) ** 2))
estimate_rmse = np.sqrt(np.mean((estimated_positions_arr - true_positions_arr) ** 2))

print('measurement rmse:', measurement_rmse)
print('kalman estimate rmse:', estimate_rmse)

대부분의 경우 칼만 필터 추정값의 RMSE가 원래 측정값보다 작게 나옵니다. 물론 항상 그런 것은 아닙니다. 모델이 완전히 틀렸거나 Q, R을 지나치게 이상하게 설정하면 결과가 나빠질 수 있습니다.

이 점이 중요합니다. 칼만 필터는 모든 문제를 자동으로 해결하는 버튼이 아닙니다. 칼만 필터는 모델과 센서의 불확실성을 수학적으로 균형 있게 섞는 방법입니다. 좋은 결과를 얻으려면 로봇의 움직임과 센서 특성을 어느 정도 이해해야 합니다.

위치만 추정하는 더 단순한 필터

지금은 상태를 [위치, 속도]로 두었습니다. 하지만 칼만 필터의 감각을 더 단순하게 보고 싶다면 위치 하나만 추정할 수도 있습니다.

이 경우 상태는 스칼라 하나입니다.

x = 위치

로봇이 거의 움직이지 않는 물체라고 가정하면 예측은 단순합니다.

다음 위치 = 현재 위치

이 구조는 정지한 물체의 위치를 여러 번 측정할 때 유용합니다. 하지만 움직이는 로봇에는 부족합니다. 로봇이 계속 이동하는데 "다음 위치는 현재 위치와 같다"고 가정하면, 예측 모델이 현실을 따라가지 못합니다.

그래서 이동 로봇에서는 보통 위치뿐 아니라 속도까지 상태에 넣는 편이 자연스럽습니다. 더 복잡한 로봇에서는 상태가 훨씬 길어집니다.

2D 이동 로봇 상태 예:
[x, y, theta, v, omega]

드론 상태 예:
[x, y, z, roll, pitch, yaw, vx, vy, vz]

상태가 길어진다고 해서 원리가 완전히 달라지는 것은 아닙니다. 여전히 예측하고, 측정값으로 보정합니다. 다만 행렬의 크기가 커지고, 모델이 더 복잡해질 뿐입니다.

칼만 필터를 로봇 관점에서 다시 보기

지금까지의 과정을 로봇의 하루 일과처럼 표현해 봅시다.

로봇은 먼저 자기 머릿속으로 생각합니다.

나는 이전에 여기 있었고, 이 정도 속도로 움직이고 있었어.
그러니 지금쯤은 여기쯤 있을 거야.

이것이 예측입니다.

그다음 센서를 봅니다.

센서는 내가 저쪽에 있다고 말하네.
내 예상과 조금 다르군.

이것이 측정입니다.

마지막으로 둘을 비교합니다.

내 예측도 완벽하지 않고, 센서도 완벽하지 않아.
둘 중 어느 쪽을 더 믿을지 불확실성을 보고 결정하자.

이것이 보정입니다.

이 과정을 빠르게 반복하면 로봇은 흔들리는 센서값 속에서도 비교적 안정적인 자기 위치를 유지할 수 있습니다. SLAM, 자율주행, 드론 자세 추정, 모바일 로봇 위치 추정 같은 분야에서 칼만 필터 계열 방법이 오래도록 사용되는 이유가 바로 여기에 있습니다.

자주 하는 실수

칼만 필터를 처음 구현할 때는 코드가 실행되더라도 결과가 이상하게 나올 수 있습니다. 다음 실수들을 특히 조심해야 합니다.

• dt를 실제 데이터의 시간 간격과 다르게 설정한다.

• 표준편차를 넣어야 할 곳과 분산을 넣어야 할 곳을 혼동한다.

• 상태 벡터의 순서와 F, H 행렬의 의미가 서로 맞지 않는다.

• 초기 불확실성 P를 지나치게 작게 잡아 필터가 처음부터 자기 추정을 과신한다.

• 센서가 매우 시끄러운데 R을 너무 작게 잡아 측정값을 과하게 따라간다.

특히 R에는 보통 표준편차가 아니라 분산을 넣습니다. 센서 표준편차가 0.8m라면 R에는 0.8 ** 2를 넣어야 합니다.

measurement_std = 0.8
R = np.array([[measurement_std ** 2]])

이 작은 차이 때문에 필터의 반응이 크게 달라질 수 있습니다.

이번 장에서 얻은 감각

이번 장에서는 칼만 필터의 전체 이론을 깊게 파고들지는 않았습니다. 대신 로봇 위치 추정에 필요한 핵심 구조를 직접 구현했습니다.

칼만 필터의 핵심은 예측과 보정입니다. 예측 단계에서는 이전 상태와 운동 모델로 다음 상태를 계산하고, 불확실성도 함께 증가시킵니다. 보정 단계에서는 센서 측정값을 반영해 상태를 갱신하고, 불확실성을 줄입니다.

이제 우리는 다음 감각을 갖게 되었습니다.

• 상태는 로봇이 추정하고 싶은 값들의 묶음이다.

• 운동 모델은 이전 상태에서 다음 상태를 예측하는 규칙이다.

• 센서 모델은 상태가 센서 측정값으로 어떻게 보이는지 설명한다.

• Q는 운동 모델의 불확실성, R은 센서의 불확실성이다.

• 칼만 이득은 예측과 측정값 사이의 균형을 정한다.

다음 장에서는 이 생각을 여러 센서로 확장합니다. 엔코더는 바퀴가 얼마나 돌았는지 알려 주고, IMU는 가속도와 회전을 알려 주며, 카메라와 LiDAR는 주변 세상을 관측합니다. 센서 하나는 늘 부족하지만, 여러 센서를 함께 쓰면 로봇은 훨씬 더 믿을 만한 판단을 할 수 있습니다.