제5부 상태 추정: 칼만필터

5.2 칼만필터 (KF)

제5부 상태 추정: 칼만필터 · 핵심. 5.1.8의 1D 융합을 벡터·행렬로 확장한다. 선형 가우시안 시스템에서 예측(predict)·갱신(update) 두 단계를 재귀적으로 반복하며 상태를 최적 추정한다. 칼만 이득의 의미, 정상상태 칼만이 곧 4.4의 최적 관측기임을, 그리고 실전 구현을.

이 장을 마치면 칼만필터의 다섯 방정식을 쓰고, 칼만 이득이 예측·측정 신뢰를 어떻게 가중하는지 설명하며, 등속 추적 같은 선형 문제에 직접 구현할 수 있어야 한다.

5.2.1 무엇을 — 선형 가우시안 최적 추정

모델(5.1.6):

$$x[k] = A\cdot x[k-1] + B\cdot u[k-1] + w, \qquad w \sim N(0, Q)$$ — 프로세스 잡음

$$z[k] = C\cdot x[k] + v, \qquad v \sim N(0, R)$$ — 측정 잡음

목표: 측정 z[1..k]이 들어올 때마다, 상태 $x[k]$의 최적 추정 $\hat{x}[k]$와 그 불확실성 $P[k]$를 재귀적으로(과거를 다 저장 안 하고) 갱신한다. 칼만필터는 5.1.8의 융합을 매 시각 수행하는 것 — 예측했다가, 측정으로 교정.

5.2.2 두 단계 — 예측과 갱신

예측은 모델로 미래를 예측하며 덜 확실해지고, 갱신은 측정을 융합해 더 확실해진다 — 5.1.7의 베이즈 사이클 그대로.

5.2.3 예측 (시간 갱신)

직전 추정 $\hat{x}, P$를 동역학으로 한 스텝 전파한다(5.1.5.1·5.1.5.2).

$$\hat{x}^-[k] = A\cdot\hat{x}[k-1] + B\cdot u[k-1]$$ — 사전 평균: 상태를 모델로 전파하기

$$P^-[k] = A\cdot P[k-1]\cdot A^T + Q$$ — 사전 공분산: $A\Sigma A^T$ 전파 + 프로세스 잡음 $Q$

$AP A^T$: 불확실성이 동역학을 거쳐 어떻게 전파되나(5.1.5.1).
$+Q$: 모델이 못 담은 외란·오차로 불확실성이 늘어난다(5.1.5.2). 측정 없이 예측만 오래 하면 $P$가 계속 커진다(추측항법의 드리프트).

5.2.4 갱신 (측정 갱신)

측정 $z[k]$를 융합한다.

$$\tilde{y} = z[k] - C\cdot\hat{x}^-[k]$$ — ① 혁신(innovation): 측정 − 예측측정

$$S = C\cdot P^-\cdot C^T + R$$ — ② 혁신 공분산: 예측의 측정 불확실성 + 센서 잡음

$$K = P^-\cdot C^T\cdot S^{-1}$$ — ③ 칼만 이득

$$\hat{x}[k] = \hat{x}^-[k] + K\cdot\tilde{y}$$ — ④ 사후 평균: 예측을 혁신만큼 보정

$$P[k] = (I - K\cdot C)\cdot P^-$$ — ⑤ 사후 공분산: 측정으로 줄어듦

5.1.8과 같다: 스칼라($C=1$)면 $K=P^-/(P^-+R)$, $\hat{x}=\hat{x}^-+K(z-\hat{x}^-)$, $P=(1-K)P^-$ — 5.1.8의 그 세 줄. 칼만필터는 이를 행렬로 확장해 상태 간 상관까지 고려한 것. $P^-C^T$ 부분이 "측정한 양의 불확실성을 측정 안 한 상태로 전파해" 위치만 재고도 속도를 교정하게 한다.

수치 안정: ⑤는 반올림으로 $P$가 비대칭·음정부호가 될 수 있다. 조셉(Joseph) 형식 $P=(I-KC)P^-(I-KC)^T+KRK^T$이 대칭·준양정을 더 잘 보존한다(5.2.8).

5.2.5 칼만 이득의 의미

$K$는 "예측을 신뢰할까, 측정을 신뢰할까"의 최적 가중이다(5.1.8의 확장).

센서가 정밀하면($R$ 작음) → $K$ 큼 → 측정을 따른다(혁신을 크게 반영).
센서가 부정확하거나 예측이 확실하면($R$ 큼 또는 $P^-$ 작음) → $K$ 작음 → 예측을 유지.
$K$는 측정공간의 혁신 ỹ를 상태공간으로 되돌려 모든 상태를 교정한다 — 상관($P^-$의 비대각) 덕에 안 잰 상태까지.

매 스텝 $P$에 따라 $K$가 자동으로 조정된다 — 처음에는 불확실($P$ 큼)해 측정을 많이 신뢰하다가, 확신이 쌓이면($P$ 작아짐) 측정에 덜 좌우된다.

5.2.6 최적성과 정상상태 = 최적 관측기

최적성: 선형 가우시안에서 칼만필터는 최소평균제곱오차(MMSE) 추정량으로 최적이다. 잡음이 가우시안이 아니어도, 선형 추정량 중에서는 최적(BLUE).
정상상태: 시불변 시스템이면 $P[k]$가 정상값 $P\infty$로 수렴하고, 따라서 $K$도 상수가 된다. 그러면 갱신식이 $\dot{\hat{x}} = A\hat{x}+Bu+K(z-C\hat{x})$ — 4.4의 Luenberger 관측기와 똑같다! 즉 칼만필터 = 잡음을 최적으로 처리한 관측기, 정상상태 칼만 이득 = 4.4.6에서 미룬 최적 $L$.
쌍대성: $P\infty$는 추정용 리카티 방정식을 만족하며, 이는 LQR의 제어용 리카티(4.5)와 쌍대다. 추정과 제어가 대칭임이 여기서 완결된다(LQG = LQR + KF, 4.5.7).

5.2.7 튜닝 — Q, R

칼만 실무의 핵심은 잡음 공분산 $Q, R$ 고르기다(5.1.10).

조정	효과
$Q$ 크게 (모델 불신)	$P^-$·$K$ 커짐 → 측정을 더 신뢰 → 빠른 추종, 잡음 더 탐
$R$ 크게 (측정 불신)	$K$ 작아짐 → 예측을 더 신뢰 → 부드럽지만 지연·둔감

실전: $R$은 센서 사양·실측(정지 상태 분산)으로 비교적 객관적으로 정해진다. $Q$는 모델 오차를 대표해 정하기 어렵다 — 보통 $Q$를 조정 변수로 두고 튜닝한다. $Q$를 너무 작게 설정하면 필터가 측정을 무시하다 발산한다(5.2.8).

5.2.8 구현 주의 — 초기화·수치·발산

초기화: $\hat{x}_0$(최선의 초기 추정), $P_0$(그 불확실성 — 모르면 크게 설정해 측정이 빨리 반영되게).
수치 안정: 공분산 갱신에 조셉 형식, 더 까다로우면 제곱근 필터(square-root/UD)로 $P$의 양정부호를 보장.
필터 발산: $Q$가 너무 작거나 모델이 틀리면 $P$가 과도하게 작아져 필터가 측정을 사실상 무시 → 추정이 진실에서 벗어나도 못 고친다. 대책: $Q$를 충분히, 혁신 ỹ을 모니터링(이론상 ỹ은 $S$ 크기의 백색잡음이어야 — 아니면 모델·튜닝 문제, 1.4.8의 잔차 분석과 같은 정신).

5.2.9 도구로 — KF 구현 (등속 추적)

명료한 예측·갱신 구현과 고전 예제(위치만 재서 위치·속도 추정):

# kf.py — 칼만필터와 등속(constant-velocity) 추적
import numpy as np

class KalmanFilter:
    def __init__(self, A, B, C, Q, R, x0, P0):
        self.A,self.B,self.C,self.Q,self.R,self.x,self.P = A,B,C,Q,R,x0,P0
    def predict(self, u=0.0):
        self.x = self.A @ self.x + self.B @ np.atleast_1d(u)  # (B가 열벡터라 B*u는 브로드캐스트 버그 — @ 사용)
        self.P = self.A @ self.P @ self.A.T + self.Q
    def update(self, z):
        S = self.C @ self.P @ self.C.T + self.R
        K = self.P @ self.C.T @ np.linalg.inv(S)        # 칼만 이득
        self.x = self.x + K @ (z - self.C @ self.x)     # 사후 평균
        I = np.eye(self.P.shape[0])
        self.P = (I - K @ self.C) @ self.P              # (조셉 형식 권장)
        return self.x

dt, sa, sz = 0.1, 0.5, 0.3                              # 표본주기, 가속잡음, 측정잡음
A = np.array([[1, dt],[0, 1]]); B = np.zeros((2,1)); C = np.array([[1.0, 0.0]])
Q = sa**2 * np.array([[dt**4/4, dt**3/2],[dt**3/2, dt**2]])   # 가속 잡음 → 상태 잡음
R = np.array([[sz**2]])
kf = KalmanFilter(A, B, C, Q, R, x0=np.array([0.,0.]), P0=np.eye(2)*10)

# ---- 시뮬레이션: 참 등속(위치 0, 속도 2.0 m/s)을 만들고 위치만 잡음 섞어 측정 ----
rng = np.random.default_rng(0)
true = np.array([0.0, 2.0])
for k in range(40):
    true = A @ true                                      # 참 궤적(등속) 전파
    z = np.array([true[0] + rng.normal(0, sz)])           # 위치만 잡음 섞어 측정
    kf.predict()
    kf.update(z)
    # kf.x = [위치, 속도] 추정 — 매 스텝 이렇게 predict→update를 반복한다

참고: $B$가 열벡터((2,1))인데 self.B * u로 스칼라 $u$와 곱하면 (2,1) 배열이 되고, 이를 (2,) 상태벡터에 더하면 넘파이가 브로드캐스트해 (2,2) 행렬로 오염시키는 버그가 있었다(입력이 없는 이 예제에서는 $u=0$이라 값은 안 바뀌지만 모양이 어긋난다) — @ np.atleast_1d(u)로 고쳐야 실제로 동작한다. 아래는 이 수정된 코드를 실제로 실행한 결과다.

검증(docker ce-verify, control 0.10.2 / numpy 2.2.6 / Python 3.10, 시드 고정 rng=default_rng(0), 40스텝):

참 속도 = 2.000 m/s (등속이므로 전 구간 동일)
최종 스텝: 참 상태 = [8. 2.], KF 추정 = [8.09519409 2.11785079]
최종 P =
[[0.01502631 0.01370685]
 [0.01370685 0.02625081]]

속도 추정 오차 RMS (전체 40스텝)      = 0.4351 m/s
속도 추정 오차 RMS (마지막 10스텝)      = 0.0475 m/s
속도 추정 오차 (첫 5스텝 평균)           = 0.8361 m/s
측정 잡음 표준편차(sz)                  = 0.3000 m

k= 0  참=( 0.200, 2.000)  추정=( 0.236, 0.023)  z= 0.238
k=10  참=( 2.200, 2.000)  추정=( 2.078, 1.740)  z= 2.013
k=20  참=( 4.200, 2.000)  추정=( 4.093, 1.980)  z= 4.161
k=30  참=( 6.200, 2.000)  추정=( 6.124, 1.971)  z= 5.897

위치만 재는데($C=[1,0]$, 속도는 한 번도 직접 측정 안 함) 속도 추정 오차가 첫 5스텝 평균 $0.836 \,\mathrm{m/s}$에서 마지막 10스텝 $0.0475 \,\mathrm{m/s}$로 급격히 줄어든다 — $P$의 비대각 상관($P^-C^T$)이 위치 혁신을 속도로 전파한 결과다(가관측, 4.2). $sa$(가속잡음, $Q$를 키우는 조정 변수)를 키우면 $K$가 커져 더 민첩하지만 더 변동이 큰 추정이, 줄이면 더 부드럽지만 더 느린 추정이 나온다(5.2.7의 튜닝 표 그대로) — 직접 재현해 확인하라. ✅ 검증됨.

5.2.10 정직 — 가정·튜닝·선형 한정

가정 의존(5.1.10): 백색·가우시안·독립 잡음, 정확한 모델 $A,C$. 위배되면 최적이 아니거나 발산.
$Q,R$ 튜닝이 실무 병목: 통계를 모르면 추정도 모른다. 혁신 모니터링으로 검증(5.2.8).
선형 한정: KF는 $A,C$가 선형이어야 한다. 현실의 회전·거리·각도 측정은 비선형 → 다음 장 EKF·UKF(5.3).
이상치·편향에 약함: 가우시안은 큰 이상치에 취약 — 견고 변형·이상치 제거가 별도 필요.

5.2.11 정리 — 그리고 다음 장

칼만필터 = 예측($\hat{x}^-=A\hat{x}+Bu$, $P^-=APA^T+Q$) + 갱신($K=P^-C^T(CP^-C^T+R)^{-1}$, $\hat{x}=\hat{x}^-+K\tilde{y}$, $P=(I-KC)P^-$)의 재귀(5.2.2~5.2.4). 5.1.8 스칼라 융합의 행렬판.
칼만 이득 $K$가 예측·측정 신뢰를 $P$에 따라 자동으로 가중하고, 상관을 통해 안 잰 상태까지 교정한다(5.2.5).
선형 가우시안 최적(MMSE) 추정이며, 정상상태 칼만 = 최적 Luenberger 관측기(4.4.6), 추정 리카티는 LQR과 쌍대(5.2.6).
실무는 $Q,R$ 튜닝·수치 안정·발산 방지가 관건(5.2.7·5.2.8), 비선형이면 EKF·UKF로(5.2.10).

다음 5.3에서는 비선형 시스템을 위한 확장칼만(EKF)·무향칼만(UKF)을 다룬다 — 자코비안 선형화 vs 시그마점 전파. 실제 로봇(IMU·GPS)이 비선형이라 거의 항상 이쪽을 쓴다.

5.2.12 연습·생각해 볼 것

사이클 이해. 측정이 한동안 안 들어오면(예측만 반복) $P$는 어떻게 되나? 측정이 다시 들어오면? 이것이 GPS 끊김 구간의 추측항법과 어떻게 연결되나? (5.2.3·5.2.4)
이득 극한. 갱신식에서 R→0(완벽한 센서)이면 $K$와 $\hat{x}$은? $R\to\infty$(무용한 센서)이면? 각 경우 필터가 무엇을 따르나? (5.2.5)
스칼라 확인. $C=1$, 스칼라일 때 5.2.4의 다섯 식이 5.1.8의 세 식과 일치함을 보여라. (5.2.4)
정상상태=관측기. 정상상태에서 칼만 이득이 상수가 되면 갱신식이 Luenberger 관측기(4.4)와 같아짐을 설명하라. 그러면 칼만은 관측기와 무엇이 다른가? (5.2.6)
튜닝 진단. 추정이 측정 잡음을 그대로 따라 변동한다면 $Q,R$을 어떻게 조정하나? 반대로 측정보다 크게 지연된다면? (5.2.7)
발산. $Q$를 0으로 두면(모델 완벽 가정) $P$가 0에 수렴해 필터가 측정을 무시하게 된다. 모델이 사실 틀렸다면 무슨 일이? 어떻게 막나? (5.2.8)
(손으로) kf.py로 등속 추적을 시뮬레이션하라(참 궤적 생성→잡음 측정→KF). 위치만 재는데 속도 추정이 수렴하는지, $Q$를 키우면 추정이 더 민첩하지만 더 크게 변동하는지 확인하라. (검증 필요)

막히면 "예측은 불확실 ↑, 갱신은 불확실 ↓, K는 측정 신뢰도, 정상상태 칼만 = 최적 관측기"로 돌아가라.