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

5.3 비선형 추정 (EKF·UKF)

제5부 상태 추정: 칼만필터 · 비선형. 실제 로봇은 비선형이다 — 회전·거리·각도 측정, 운동학이 다 비선형이라 선형 KF(5.2)를 그대로 못 쓴다. 확장칼만(EKF)은 자코비안으로 선형화하고(1.1.9), 무향칼만(UKF)은 시그마점을 실제 비선형 함수에 통과시킨다. 둘이 실전 추정의 주력이다.

이 장을 마치면 EKF·UKF의 차이를 설명하고, 비선형 모델에 EKF를 적용하며(자코비안 유도), 실전 함정(각도 wrapping·자코비안 버그·일관성)을 피할 수 있어야 한다.

5.3.1 문제 — 현실은 비선형

KF(5.2)는 $x[k]=Ax+Bu$, $z=Cx$처럼 선형을 요구했다. 그런데 현실은:

$$x[k] = f(x[k-1], u[k-1]) + w, \qquad w \sim N(0, Q)$$ — $f$: 비선형 운동 모델

$$z[k] = h(x[k]) + v, \qquad v \sim N(0, R)$$ — $h$: 비선형 측정 모델

예: 이동로봇의 운동학($f$에 $\cos\theta, \sin\theta$), 랜드마크까지 거리 측정 $h=\sqrt{(x-x_l)^2+(y-y_l)^2}$, 방위각 h=atan2(...). 다 비선형이라 $A, C$가 없다. 그래도 칼만의 예측·갱신 틀은 살리고 싶다 — 비선형을 어떻게든 다루자.

5.3.1.1 개념 지도 — EKF vs UKF 갈림길

같은 비선형 $f(x)$를 앞에 두고, EKF와 UKF는 서로 다른 길을 택한다 — EKF는 함수를 선형으로 근사하고, UKF는 분포를 점들로 근사한다. 아래 두 갈래를 먼저 눈에 담고 5.3.2~5.3.5를 읽으면 세부가 어디로 가는지 잃지 않는다.

5.3.2 EKF — 자코비안으로 선형화

가장 흔한 답: 매 스텝, 현재 추정 주변에서 $f, h$를 1차 선형화한다(1.1.9의 테일러·자코비안 그대로). 선형화된 행렬을 KF의 $A, C$ 자리에 넣는다.

$$F = \partial f/\partial x \,\big|_{\hat{x}, u}$$ — 운동 모델 자코비안, $A$ 역할

$$H = \partial h/\partial x \,\big|_{\hat{x}^-}$$ — 측정 모델 자코비안, $C$ 역할

EKF 방정식(KF와 거의 같되, 평균은 실제 비선형 함수로, 공분산은 자코비안으로):

예측:
- $\hat{x}^- = f(\hat{x}, u)$ ← 평균은 비선형 $f$ 그대로 통과
- $P^- = F\cdot P\cdot F^T + Q$ ← 공분산은 선형화 $F$로 전파
갱신:
- $\tilde{y} = z - h(\hat{x}^-)$ ← 혁신도 비선형 $h$ 그대로
- $S = H\cdot P^-\cdot H^T + R$
- $K = P^-\cdot H^T\cdot S^{-1}$
- $\hat{x} = \hat{x}^- + K\cdot\tilde{y}$
- $P = (I - K\cdot H)\cdot P^-$

핵심: 평균($\hat{x}$)은 실제 비선형 함수로 전파하고, 공분산($P$)만 선형 근사로 전파한다. 그래서 약한 비선형에는 잘 맞지만, 공분산 전파가 1차 근사라 강한 비선형에서는 오차가 쌓여 발산할 수 있다(5.3.3).

5.3.3 EKF의 장단


장점	KF의 자연스러운 확장, 표준·보편(로봇 `robot_localization` 기본), 계산 저렴, 약한 비선형에 잘 동작
단점	자코비안을 손으로 유도해야(복잡·버그 잦음), 1차 근사라 강한 비선형에는 부정확·발산, 함수가 미분 불가면 못 씀

자코비안 유도·구현 실수는 EKF 실패의 대표적 원인이다(5.3.7). 그래서 미분 없이 가는 길이 등장한다.

5.3.4 UKF — 시그마점으로 전파

EKF가 "함수를 선형으로 근사"한다면, UKF는 분포를 점들로 근사한다. 발상: "비선형 함수를 근사하느니, 잘 고른 표본점들을 실제 함수에 통과시켜 결과의 평균·공분산을 다시 재자."

① 현재 $(\hat{x}, P)$를 대표하는 시그마점(sigma points) $2n+1$개를 고른다
- $\chi_i = \hat{x} \pm (\sqrt{(n+\lambda)P})_i$ (평균과 공분산을 정확히 포착하게)
② 각 점을 실제 비선형 함수에 통과: $\mathcal{Y}_i = f(\chi_i)$ ← 선형화 없음!
③ 통과된 점들의 가중 평균·공분산을 다시 계산:
- $\hat{x}^- = \sum \mathrm{Wm}_i \mathcal{Y}_i$, $P^- = \sum \mathrm{Wc}_i (\mathcal{Y}_i - \hat{x}^-)(\mathcal{Y}_i - \hat{x}^-)^T + Q$

이것이 무향 변환(unscented transform)이다. 측정 갱신도 시그마점을 $h$에 통과시켜 같은 식으로.

자코비안 불필요(미분 없이 — derivative-free): $f, h$를 함수로 호출만 하면 된다.
고차 정확도: 평균·공분산을 2~3차까지 포착(EKF는 1차) → 강한 비선형에 더 정확.

5.3.5 무향 변환 — 조금 더

시그마점은 $2n+1$개(상태 차원 $n$):

$\chi_0 = \hat{x}$ (중심)
$\chi_i = \hat{x} + (\sqrt{(n+\lambda)P})_i \quad i=1..n$ (각 주축 +방향)
$\chi_i = \hat{x} - (\sqrt{(n+\lambda)P})_{i-n} \quad i=n+1..2n$ (각 주축 −방향)

$\sqrt P$는 행렬 제곱근(촐레스키 분해), $\lambda=\alpha^2(n+\kappa)-n$은 점들의 퍼짐을 정하는 스케일링 파라미터($\alpha,\beta,\kappa$로 조절). 각 시그마점의 가중치(평균용 $Wm$, 공분산용 $W_c$)는 Van der Merwe/Julier 스케일 무향변환의 정해진 공식으로 나온다:

$$\lambda = \alpha^2(n+\kappa) - n$$

$\mathrm{Wm}_0 = \lambda/(n+\lambda)$ (중심점 $\chi_0$의 평균 가중치)
$\mathrm{Wc}_0 = \lambda/(n+\lambda) + (1 - \alpha^2 + \beta)$ (중심점의 공분산 가중치 — 평균용과 다름!)
$\mathrm{Wm}_i = \mathrm{Wc}_i = 1/(2(n+\lambda)) \quad i=1..2n$ (나머지 $2n$개 점은 평균·공분산 가중치가 같음)

가중 평균·공분산 복원:

$$\begin{aligned} \hat{x}^- &= \sum_i \mathrm{Wm}_i\cdot\mathcal{Y}_i \\ P^- &= \sum_i \mathrm{Wc}_i\cdot(\mathcal{Y}_i-\hat{x}^-)(\mathcal{Y}_i-\hat{x}^-)^T + Q \end{aligned}$$

전형적 기본값: $\alpha$는 아주 작게(예: $10^{-3}$) — 시그마점을 평균 가까이 조밀하게 모아 국소적 퍼짐만 보게 한다. $\kappa$는 0 또는 3−n(스칼라 무향변환의 원래 관례). $\beta=2$는 상태가 가우시안일 때 4차 모멘트까지 정확히 맞추는 최적값(가우시안이 아니면 다른 값이 나을 수 있음).

검산: $Wm$은 항상 $\Sigma Wm_i = \lambda/(n+\lambda) + 2n\cdot1/(2(n+\lambda)) = (\lambda+n)/(n+\lambda)$... 정리하면 1이 된다(어떤 $\alpha,\beta,\kappa$를 넣어도) — 평균 복원이 항상 참값을 향하게 하는 정규화 조건이다. $W_c$는 $W_{c0}$에 $(1-\alpha^2+\beta)$가 더 붙어 $Wm_0$과 달라지므로 ΣWc≠ΣWm일 수 있다(공분산 쪽에 추가 보정이 필요해서다).

5.3.5.1 α·κ가 나쁘면 — 극단에서 무슨 일이

파라미터를 극단값으로 설정하면 수치가 불안정해진다:

$\alpha$가 너무 크면: 시그마점이 평균에서 멀리 퍼져 나가 국소 선형화 취지에서 벗어나고, 강한 비선형 영역까지 표본을 배치해 근사가 오히려 악화된다. $W_{c0}=\lambda/(n+\lambda)+(1-\alpha^2+\beta)$의 $-\alpha^2$항이 커지며 $W_{c0}$가 크게 음수로 떨어질 수 있고, 중심점의 (음의) 가중치가 나머지 점들의 기여를 압도하면 $P^-$가 준양정(PSD)이 아니게 될 수 있다 — 공분산 고유값이 음수로 나오는, 이론상 있을 수 없는 상태.
$\alpha$가 너무 작거나 $\kappa$를 잘못 고르면: $n+\lambda$가 0에 가까워져(또는 음수가 되어) 가중치 $1/(2(n+\lambda))$가 폭발적으로 커지거나(극단적 상쇄 오차), 시그마점 생성에 쓰는 $(n+\lambda)P$가 음수가 되어 촐레스키 분해 자체가 실패한다(시그마점이 정의되지 않음).

검증(docker ce-verify, control 0.10.2 / numpy 2.2.6 / Python 3.10) — 위 두 실패 모드를 실제로 재현했다:

① α=1e-3, κ=3−n(n=2, 관례값) → n+λ=3·10⁻⁶ (거의 0)
   Wm0=-666665.67, Wc0=-666662.67, Wi(i≥1)=+166666.67   ← 부호 반대의 거대한 값끼리 상쇄
   (선형계에서는 상쇄가 정확해 문제없지만, 반올림오차·비선형 잔차가 이 거대한 값에 곱해져 증폭된다)

② 강한 비선형 f(x)=[e^1.5x0, x1²−x0], x̂=[0,0], P=I₂, α=4.0, κ=−1.5 (n=2)
   Wc0=-12.250  →  P_pred 최소 고유값 = -55.98   ← 비양정(!), 이론상 불가능한 음의 분산
   α=8.0, κ=−1.9 → Wc0=-60.31 → 최소 고유값 = -617.31  (더 심함, x̂,P는 위와 동일)

③ 대조: 선형 f(x)=Ax에서는 α를 어떤 값으로 넣어도(1e-3~1.0) UKF의 x̂⁻,P⁻가 KF의 A x̂, APAᵀ+Q와
   소수점 10자리까지 일치(무향변환이 아핀 함수에서 엄밀함을 보여줌) — 문제는 비선형+극단 α·κ 조합.

실무 처방: $\alpha\in[10^{-4},10^{-1}]$ 정도의 무난한 범위에서 시작하고, $\kappa=0$이 $\kappa=3-n$보다 대체로 안전하다($n+\lambda$가 0에 가까워질 위험이 작음). $P^-$를 계산할 때마다 고유값 부호를 점검하거나 대칭·양정을 강제하는 제곱근 UKF(square-root UKF) 구현을 쓰면 이 실패를 조기에 검출한다.

5.3.6 EKF vs UKF — 무엇을 언제

	EKF	UKF
비선형 처리	자코비안 1차 선형화	시그마점(고차)
자코비안	필요(유도·구현)	불필요
정확도	약한 비선형	강한 비선형에 유리
비용	저렴	약간 높음(점 2n+1개)
보급	표준·보편	늘어나는 추세

고르기(견해): 자코비안이 쉽고 비선형이 약하면 EKF(보편·검증됨). 자코비안이 까다롭거나 비선형이 세면 UKF(미분 불필요·더 정확). 둘 다 여전히 가우시안 가정이다 — 분포가 다봉(multimodal)·심한 비가우시안이면 파티클 필터(표본 기반, 비쌈)로 가야 한다(5.3.9).

5.3.7 실전 함정 — 자코비안·각도·일관성

로봇 추정에서 실제로 오류를 일으키는 것들:

자코비안 버그(EKF): $F, H$ 유도·코딩 실수가 드러나지 않게 성능을 저하시킨다. 수치 자코비안으로 교차검증하라.
각도 wrapping: 방위각·관절각은 $[-\pi, \pi]$를 주기로 순환한다(wrap). 혁신 $\tilde{y}=z-h(\hat{x})$을 단순히 빼면 $\pi$와 $-\pi$가 $2\pi$ 차이로 잘못 계산된다 → 반드시 atan2나 wrap 함수로 $[-\pi,\pi]$에 정규화. 비선형 추정 실패의 흔한 숨은 원인.
일관성 점검: 필터가 자기 불확실성을 정직하게 아는가? 혁신이 $S$에 비해 너무 크거나(과신, $P$ 너무 작음) 너무 작으면(과소신) 튜닝·모델 문제. NEES/NIS(정규화 오차/혁신 제곱) 통계로 검증(1.4.8·5.2.8의 잔차 정신).
초기화·발산: 초기 $\hat{x}_0$이 너무 틀리면 선형화점이 어긋나 EKF가 발산. $P_0$를 넉넉히, $Q$를 충분히.

5.3.8 도구로 — EKF 예제 (거리 측정)

2D 위치 $[x,y]$를 추정하는데, 알려진 랜드마크 (xₗ,yₗ)까지의 거리만 측정($h=\sqrt{(x-x_l)^2+(y-y_l)^2}$, 비선형):

# ekf_range.py — 거리 측정 EKF 갱신부 (핵심만)
import numpy as np
def h(x, lm):                       # 측정 모델: 랜드마크까지 거리
    return np.array([np.hypot(x[0]-lm[0], x[1]-lm[1])])
def H_jac(x, lm):                   # 측정 자코비안 ∂h/∂x
    d = np.hypot(x[0]-lm[0], x[1]-lm[1])
    return np.array([[(x[0]-lm[0])/d, (x[1]-lm[1])/d]])

def ekf_update(x, P, z, lm, R):
    H = H_jac(x, lm)
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    x = x + K @ (z - h(x, lm))      # 혁신: 비선형 h 사용
    P = (np.eye(len(x)) - K @ H) @ P
    return x, P
# 예측부는 운동 모델 f와 그 자코비안 F로 (등속·유니사이클 등)

자코비안 $H$가 어떻게 거리 측정의 방향 민감도를 담는지 보라(랜드마크 방향 단위벡터). (여러분 환경에서 재현·검증 — 검증 필요. 각도 측정이 섞이면 wrapping 처리 필수.)

5.3.8.1 도구로 — UKF 최소 스케치 (시그마점 + 예측 한 스텝)

EKF 코드(5.3.8)와 나란히 비교하도록, UKF의 예측부만 최소로 — 자코비안이 전혀 없다는 점에 주목:

# ukf_predict.py — 시그마점 생성 + UKF 예측 한 스텝 (5.3.5의 식 그대로)
import numpy as np

def sigma_points(x, P, alpha, kappa, n):
    lam = alpha**2 * (n + kappa) - n
    S = np.linalg.cholesky((n + lam) * P)      # 행렬 제곱근
    X = np.zeros((2*n+1, n)); X[0] = x
    for i in range(n):
        X[i+1]   = x + S[:, i]
        X[i+1+n] = x - S[:, i]
    return X, lam

def ukf_weights(n, alpha, beta, kappa):
    lam = alpha**2 * (n + kappa) - n
    Wm = np.full(2*n+1, 1.0/(2*(n+lam))); Wc = Wm.copy()
    Wm[0] = lam/(n+lam)
    Wc[0] = lam/(n+lam) + (1 - alpha**2 + beta)
    return Wm, Wc

def ukf_predict(x, P, f, Q, alpha=1e-3, beta=2.0, kappa=None):
    n = len(x)
    kappa = 3 - n if kappa is None else kappa
    X, lam = sigma_points(x, P, alpha, kappa, n)
    Wm, Wc = ukf_weights(n, alpha, beta, kappa)
    Y = np.array([f(xi) for xi in X])           # ← 자코비안 없이, f를 단순히 호출
    x_pred = np.sum(Wm[:, None] * Y, axis=0)
    P_pred = Q.copy()
    for i in range(2*n+1):
        d = (Y[i] - x_pred).reshape(-1, 1)
        P_pred += Wc[i] * (d @ d.T)
    return x_pred, P_pred
# 갱신부는 같은 무향변환을 h(x)에 적용해 예측측정 ẑ⁻,S를 만들고,
# 교차공분산 Pxz=ΣWcᵢ(𝒴ᵢ−x̂⁻)(𝒵ᵢ−ẑ⁻)ᵀ로 K=Pxz·S⁻¹을 구해 EKF와 같은 형태로 보정한다.

검증(docker ce-verify, control 0.10.2 / numpy 2.2.6 / Python 3.10): 선형 $f(x)=A\cdot x$에 위 ukf_predict를 실행해 KF의 $\hat{x}^-=A\cdot\hat{x}$, $P^-=APA^T+Q$와 비교하면(무향변환은 아핀 함수에서 엄밀해야 함, 5.3.5.1의 ③) —

x_pred (UKF) = [1.2 2.0]  |  x_pred (KF) = [1.2 2.0]  |  최대오차 = 1.16e-10
P_pred (UKF) 최대오차(대 KF) = 6.38e-14

부동소수점 오차 수준($1\mathrm{e}-10$대)까지 KF와 정확히 일치 — 시그마점·가중치 구현이 5.3.5의 공식을 올바르게 재현했다는 뜻이다. 이 ukf_predict에 5.3.8의 $h(x,lm)$을 무향변환으로 통과시키면(갱신부) 거리측정 UKF가 완성된다.

5.3.9 정직 — 한계

EKF는 발산할 수 있다: 강한 비선형·부정확한 초기값에서 선형화가 어긋나 추정이 실패한다. UKF가 더 견디지만 모든 경우를 해결하지는 못한다.
둘 다 가우시안 한정: 단봉 가우시안 가정. 다봉(여러 가설)·심한 비가우시안이면 파티클 필터(몬테카를로, 표본 다수, 비쌈)나 다가설 추정으로.
$Q,R$ 튜닝·일관성: KF와 같은 병목(5.2.7). 비선형이라 검증이 더 까다롭다(NEES/NIS).
각도·제약: wrapping(5.3.7), 상태 제약(예: 공분산이 음수 위치를 허용) 등 기하를 코드가 처리해야 한다.

5.3.9.1 숫자로 — 초기오차가 큰 EKF의 발산 (진자)

"초기값이 나쁘면 선형화가 어긋나 발산한다"를 1.1.8의 진자 모델로 실제로 보이자. $J \ddot{\theta} + b \dot{\theta} + m g l \sin\theta= \tau$($m=l=1, g=9.8, b=0.3$, $\tau=0$ 자유진동, 강한 비선형 — 참 초기각 $\theta_0=2.5 \,\mathrm{rad}\approx143^\circ$)를 오일러 이산화한 $f$로 EKF를 구동하되, 측정을 진자 추의 수평 위치 $h(\theta)=l\cdot\sin\theta$로 둔다(각도를 직접 재는 대신, 옆에서 카메라로 보는 비선형 측정 — 자코비안 $H=\partial h/\partial\theta=l\cdot\cos\theta$도 선형화점에 따라 완전히 달라진다). 초기 오차만 다른 두 경우를 비교:

# ekf_pendulum_divergence.py — 초기오차 대·소 비교 (1.1.8 진자 + 비선형 측정 h=l·sinθ)
import numpy as np
m,l,g,b,J = 1.0,1.0,9.8,0.3,1.0; dt=0.02
Q = np.diag([1e-6, 1e-5]); R = np.array([[0.01**2]])

def f(x):
    th, om = x
    return np.array([th+dt*om, om+dt*(-b/J*om-(m*g*l/J)*np.sin(th))])
def F_jac(x):
    th, om = x
    return np.array([[1.0, dt],[-dt*(m*g*l/J)*np.cos(th), 1.0-dt*b/J]])
def h(x):  return np.array([l*np.sin(x[0])])
def H_jac(x): return np.array([[l*np.cos(x[0]), 0.0]])

def run_ekf(true0, x0_hat, P0, N, seed):
    rng = np.random.default_rng(seed); true = np.array(true0,float)
    x = np.array(x0_hat,float); P = np.array(P0,float); err=[]
    for k in range(N):
        true = f(true)
        z = h(true) + rng.normal(0, np.sqrt(R[0,0]), size=1)
        F = F_jac(x); x = f(x); P = F@P@F.T + Q       # 예측: 평균=비선형 f, 공분산=선형화 F
        H = H_jac(x); S = H@P@H.T+R; K = P@H.T@np.linalg.inv(S)
        x = x + K@(z-h(x)); P = (np.eye(2)-K@H)@P      # 갱신: 혁신도 비선형 h
        err.append(abs(x[0]-true[0]))
    return np.array(err)

true0 = [2.5, 0.0]
errA = run_ekf(true0, x0_hat=[2.3, 0.0], P0=np.diag([0.05,0.05]), N=250, seed=1)  # 작은 초기오차
errB = run_ekf(true0, x0_hat=[0.1, 0.0], P0=np.diag([0.05,0.05]), N=250, seed=1)  # 큰 초기오차

검증(docker ce-verify, control 0.10.2 / numpy 2.2.6 / Python 3.10, dt=0.02, 250스텝=5초):

Case A (작은 초기오차: x̂0=[2.3,0.0], 참=[2.5,0.0], 초기|오차|=0.200 rad)
  RMS 오차(전체)=0.0035 rad, RMS(마지막 50스텝)=0.0037 rad, 최대오차=0.0148 rad

Case B (큰 초기오차: x̂0=[0.1,0.0], 참=[2.5,0.0], 초기|오차|=2.400 rad)
  RMS 오차(전체)=0.5689 rad, RMS(마지막 50스텝)=0.0218 rad, 최대오차=1.8964 rad

  t(s)   CaseA    CaseB
  0.00   0.0148   1.8964
  0.50   0.0117   0.6722
  1.00   0.0015   0.2672
  1.50   0.0018   0.7626
  2.00   0.0056   0.3251
  2.50   0.0005   0.1189

초기 오차가 0.2 rad인 A는 즉시 정착해 5초 내내 오차가 0.01 rad 안팎이다. 초기 오차가 2.4 rad인 B는 전체 구간 RMS가 A의 약 160배(0.569 vs 0.0035, 정확히는 약 163.4배)이고, $t=1.5s$에 다시 0.76 rad까지 튀는 등 2초 넘게 오차가 큰 폭으로 진동하다가 뒤늦게 정착한다 — 초기 선형화점($\hat{x}0=0.1$)에서 계산한 $H=l \cos(0.1)\approx0.995$가 참값 근방($\theta\approx2.5$, $\cos(2.5)\approx-0.80$)의 실제 민감도와 부호까지 다른 방향을 가리켜, 혁신을 상당 기간 잘못된 방향·크기로 상태에 되돌렸기 때문이다. 이 예제에서는 진자가 계속 여러 각도를 거치며 결국 좋은 선형화점을 만나 회복하지만(마지막 50스텝은 A와 비슷한 수준으로 정착), 측정이 국소적이거나 초기오차가 더 크면 이 회복 자체가 안 일어날 수 있다 — 그것이 실제 발산이다. UKF는 $P_0$가 담는 범위 안에서는 시그마점이 곡률을 실제로 통과시켜 이런 국소 방향 오류를 덜 겪지만, 초기오차가 $P_0$의 스케일 자체를 크게 벗어나면(이 예제처럼 참값이 초기 표준편차 $\sqrt0.05\approx0.22 \,\mathrm{rad}$의 십여 배 밖에 있으면) UKF도 시그마점이 참값 근방을 전혀 표본하지 못해 같은 함정에 빠질 수 있다 — "초기값이 대략은 맞아야 한다"는 전제는 EKF·UKF 둘 다에 있다(검증 필요, 이 장에서 UKF로 같은 실험을 재현하지는 않았다).

5.3.10 정리 — 그리고 다음 장

비선형 모델 $x=f(x,u)+w$, $z=h(x)+v$엔 KF를 못 쓴다. EKF는 매 스텝 자코비안 $F,H$로 선형화해 공분산을 전파하고(평균은 비선형 그대로), UKF는 시그마점을 실제 함수에 통과시킨다(미분 없이, 고차)(5.3.2·5.3.4).
EKF: 보편·저렴·약한 비선형, 자코비안 부담. UKF: 미분 불필요·강한 비선형에 유리. 둘 다 가우시안 — 그 밖은 파티클(5.3.6·5.3.9).
실전 성패는 자코비안 정확성·각도 wrapping·일관성(NEES/NIS)·초기화가 결정한다(5.3.7).

다음 5.4에서는 이를 실제로 묶는 센서 융합 — IMU+GPS+Compass로 자세·위치를 추정한다(EKF·ESKF). 제5부를 마무리하고, 제7부 ROS2 robot_localization(7.5)으로 가는 다리를 놓는다.

5.3.11 연습·생각해 볼 것

자코비안 유도. 측정 h(x,y)=atan2(y−yₗ, x−xₗ)(랜드마크 방위각)의 자코비안 $\partial h/\partial[x,y]$를 구하라. 거리 측정(5.3.8)과 어떻게 다른가?
EKF vs UKF. 다음 각각에 EKF·UKF 중 무엇이 적절한지 이유와 함께: ⓐ 약한 비선형·자코비안 간단, ⓑ 자코비안 유도가 매우 번거로운 복잡한 $f$, ⓒ 강한 비선형으로 EKF가 발산. (5.3.6)
각도 함정. 방위각 추정에서 측정 $z=179^\circ$, 예측 $h(\hat{x})=-179^\circ$일 때, 혁신을 단순히 z−h로 빼면 얼마인가? 올바른 혁신은? 왜 wrapping이 필요한가? (5.3.7)
평균 vs 공분산. EKF에서 평균은 비선형 $f$로, 공분산은 선형 $F$로 전파한다고 했다. 왜 이 비대칭이 강한 비선형에서 문제를 일으키나? UKF는 이를 어떻게 피하나? (5.3.2·5.3.4)
일관성. EKF의 혁신이 예측 공분산 $S$가 말하는 것보다 지속적으로 크다. 필터가 무엇을 잘못하고 있나(과신? 과소신?)? 어떻게 진단·교정하나? (5.3.7)
가우시안 한계. 로봇이 대칭 복도에서 "여기 아니면 저기"라는 두 가설 사이에 있다. EKF·UKF로 이 상황을 표현할 수 있나? 무엇이 필요한가? (5.3.9)
(손으로) ekf_range.py의 갱신부에 등속/유니사이클 예측부를 붙여, 거리 측정만으로 2D 위치를 추정해 보라. 자코비안을 일부러 틀리게 넣으면 추정이 어떻게 저하되나(자코비안 버그 체험)? (검증 필요)

막히면 "EKF=자코비안 선형화(평균은 비선형), UKF=시그마점 통과(미분 없음), 둘 다 가우시안, 각도는 wrap"으로 돌아가라.