7. 칼만 필터(Kalman Filter)

칼만 필터 개요

칼만 필터란?

• 칼만 필터(Kalman filter)는 시간에 따라 변하는 시스템의 상태(state)를 추정하는 방법 중 하나로써, 시스템 모델의 예측값과 노이즈가 포함된 관측값을 바탕으로 현재 상태를 재귀적으로 예측하고 업데이트하는 알고리즘이다.
• 일부 동적 시스템에 대한 정보가 확실하지 않은 곳(노이즈로 인해)에서 사용할 수 있으며, 동적 시스템이 선형의 움직임을 따르거나 가우시안 노이즈를 따른다고 가정할 때 매우 효과적으로 동작한다.
  • (GPS와 같이 대부분의 센서 노이즈는 무작위적이고 중심이 있는 값 주변에서 퍼짐 → 통계적으로 가우시안에 가까움)
• 또한 이전 상태 이외의 기록을 유지할 필요가 없고 연산 과정이 빠르기 때문에 실시간 문제 및 임베디드 시스템에 적합하다.

칼만 필터의 구성 요소

• 칼만 필터는 크게 다음과 같은 구성 요소로 이루어져 있다.
• 상태 변수 (State Variable): 시스템의 현재 상태는 나타내는 값(벡터 등). 위치나, 속도가 될 수 있다.
• 상태 전이 모델 (State Transition Model): 시스템의 상태가 시간에 따라 어떻게 변화하는지를 설명하는 모델.
• 관측 모델 (Observation Model): 실제로 관측된 데이터를 바탕으로 시스템의 상태를 추정하는 모델.
• 노이즈 (Noise): 관측된 데이터의 불확실성을 나타내는 요소로, 칼만 필터에서는 보통 가우시안 분포를 따른다고 가정.

---

칼만 필터 알고리즘

1. 예측 단계 (Prediction)

• 이전 상태로부터 다음 상태를 예측함. (“이전 움짐임대로라면 지금쯤 여기 있어야 해”)

1) 상태 예측

이전 위치에서 현재 속도대로라면, 다음 위치는 이쯤이야

$$\begin{array}{r}\\ {\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}\\ \end{array}$$

• ${\hat{x}}_k^{-}$ : $k$ 시점의 예측된 상태 추정값
• $A$ : 상태 전이 행렬 (시스템이 어떻게 움직이는지 모델링)
• $B$ : 제어 입력 행렬
• $u_{k-1}$ : 제어 입력 (예: 가속명령)
• ${\hat{x}}_{k-1}$ : 이전($k-1$)시간의 상태 추정값

---

2) 오차 공분산 예측

내 예측이 얼마나 불확실한가

$$\begin{array}{r}\\ P_k^{-}=A{P}_{k-1}{A}^T+Q\\ \end{array}$$

• $P_k^{-}$ : 예측된 상태 오차 공분산
• $P_{k-1}$ : 이전($k-1$) 상태 오차 공분산
• $Q$ : 프로세스 노이즈 공분산 (모델의 불확실성)

---

2. 업데이트 단계 (Correction)

• 실제 센서(GPS 등)의 측정(관측)값을 받아 예측을 수정함. (“GPS가 이렇게 말하니까 약간 수정하자”)

1) 칼만 이득 계산 (Kalman Gain)

Kalman Gain은 "센서 데이터를 얼마나 신뢰할 것인가" 결정 (GPS가 정확하면 많이 반영, GPS가 부정확하면 덜 반영)

$$\begin{array}{r}\\ K_k=P_k^{-}{H}^T(H{P}_k^{-}{H}^T+R{)}^{-1}\\ \end{array}$$

• $K_k$ : 칼만 이득 (Kalman Gain)
• $H$ : 측정(관측) 모델 행렬 (상태를 측정값으로 변환)
• $R$ : 측정 노이즈 공분산

---

2) 상태 업데이트

GPS는 예측한 상태보다 오른쪽이라고 하네, 그럼 조금 오른쪽으로 조정하자

$$\begin{array}{r}\\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})\\ \end{array}$$

• $z_k$ : 실제 측정값 (GPS 측정값 등)
• $z_k-H{\hat{x}}_k^{-}$ : 잔차(Innovation), 실제 측정값 - 예측 측정값
• ${\hat{x}}_k$ : 보정된 상태 추정값

---

3) 오차 공분산 업데이트

상태 추정을 보정(개선)했으니 불확실성도 감소

$$\begin{array}{r}\\ P_k=(I-K_{k}H)P_k^{-}\\ \end{array}$$

• $P_k$ : 업데이트된 상태 오차 공분산

---

전체 알고리즘 요약

1. 예측

“이전 움직임으로 보면 지금쯤 여기 있을 것 같아”

• 상태 예측: ${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}$
• 공분산 예측: $P_k^{-}=A{P}_{k-1}{A}^T+Q$

2. 업데이트

“측정값을 보니 좀 다르네. 둘을 적절히 섞어서 최종 상태를 정하자”

• Kalman gain 계산 : $K_k=P_k^{-}{H}^T(H{P}_k^{-}{H}^T+R{)}^{-1}$
• 상태 보정: ${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$
• 공분산 보정: $P_k=(I-K_{k}H)P_k^{-}$

---

+) 공분산이란?

• 공분산(Covariance)은 두 변수가 함께 변하는 정도를 나타내는 통계적 척도로서, 하나의 변수가 증가할 때 다른 변수가 증가하는지 (양의 공분산) 또는 감소하는지 (음의 공분산)를 보여준다.
• 공분산의 크기는 변수들의 결합된 변화량에 비례하지만, 변수들의 척도(단위 등)에 의존하기 때문에 직접적인 해석은 어렵다.
• 따라서 공분산을 정규화(normalize) 하여 상관계수(correlation coefficient)화 할 수 있다.

$$\begin{array}{rl}& \\ & Cov(X,Y)=\frac{{\sum }_{i=1}^n(X_i-\bar{X})(Y_i-\bar{Y})}{n-1}\\ & \\ & {\rho }_{X,Y}=\frac{Cov(X,Y)}{{\sigma }_X{\sigma }_Y}\text{(상관계수: -1, 1 사이)}\\ & \end{array}$$

• $\bar{X}$ : $X$ 의 평균
• $\bar{Y}$ : $Y$ 의 평균

공분산 값	해석
> 0	X와 Y가 같은 방향으로 움직임 (양의 상관관계)
< 0	X와 Y가 반대 방향으로 움직임 (음의 상관관계)
= 0	X와 Y가 서로 관련 없음 (무상관)
*전체 데이터(모집단)의 일부(표본)만으로 모집단의 분산/공분산을 추정할 때, 평균을 $n$으로 나누면 평균이 상대적으로 작게 추정되어 편향(bias)이 생긴다. 이를 보정하기 위해 $n-1$로 나눈다.

• 또한 공분산을 2차원을 나타낸 공분산 행렬을 이용하면 두 개 이상 변수(다변량 분석)에서도 바로 확장 가능하기 때문에 매우 유용하다.

$$\begin{array}{rl}& \\ \text{Covariance matrix}& =\left[\begin{array}{cc}\text{Cov}(X,X)& \text{Cov}(X,Y)\\ \text{Cov}(Y,X)& \text{Cov}(Y,Y)\end{array}\right]\\ & \\ {\sigma }_X^2& =\text{Cov}(X,X)\\ {\sigma }_Y^2& =\text{Cov}(Y,Y)\end{array}$$

예시를 통한 공분산 계산

import numpy as np
 
x = np.array([1, 2, 3, 4, 5])
y = np.array([2, 4, 6, 8, 10])
 
# 상관계수 행렬
cov_matrix = np.cov(x, y)
print(cov_matrix)
 
# 출력
[[2.5 5. ]
 [5.  10.]]

• $\text{Cov}(X,X)$ = 2.5 → $X$의 분산
• $\text{Cov}(Y,Y)$ = 10.0 → $Y$의 분산
• $\text{Cov}(X,Y)$ = 5.0 → $X$와 $Y$의 공분산

---

칼만 필터를 이용한 GPS 위치 보정 예시

상황설정

• 차량이 직선 도로를 따라 이동하고 있고, 시스템은 차량의 위치 $x$와 속도 $v$를 추정하려고 함.
• GPS는 일정 간격으로 노이즈를 포함한 위치 관측값을 제공함.
• GPS의 위치 값은 (x, y)가 아닌 직선도로는 가정하여 하나의 x또는 y의 1차원 값을 사용함.

---

시스템 모델 정의 (Define)

1. 상태 백터 (State vector)

• 추정하고자 하는 값에 대한 정의.

$${\mathbf{x}}_k=\left[\begin{array}{c}{x}_k\\ v_k\end{array}\right]$$

• $x_k$ : $k$ 시점에서의 차량의 위치 (m)
• $v_k$ : $k$ 시점에서의 차량의 속도 (m/s)

2. 상태 전이 모델 (State transition model)

• $k-1$ 시점의 상태로부터 $k$ 시점의 상태를 예측해야함.
• 차량이 등속 운동한다고 가정함. (모델단순화를 위해. 등속 운동 가정이 실제 움직임과 다르더라도, 칼만 필터는 GPS 측정값을 통해 이러한 오차를 보정하고 더 정확한 위치를 추정한다)
• 따라서, 이전 시점으로부터의 현재 위치와 속도는 다음과 같이 정의할 수 있다.

$$\begin{array}{r}\\ x_k=x_{k-1}+v_{k-1}\cdot \Delta t\\ \end{array}$$ $$v_k=v_{k-1}$$

• 위 식을 아래와 같이 행렬로 나타낼 수 있고, $\mathbf{F}$가 상태 전이 행렬이 된다.

$$\begin{array}{rl}& \\ {\mathbf{x}}_k& =\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{k-1}\\ v_{k_{k-1}}\end{array}\right]\\ & \\ {\mathbf{x}}_k& =\mathbf{F}{\mathbf{x}}_{k-1},\mathbf{F}=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\\ & \end{array}$$

• $x_{k-1}$: $k-1$ 시점의 위치

• $\Delta t$: 시간 간격

• $v_{k-1}=v_k$: 속도 (등속 운동 가정)

• $\mathbf{F}$: 상태 전이 행렬

• 단, 현실에서 차량이 완전히 등속으로 움직이는 경우는 적다. 따라서 예측 모델이 완벽하지 않다는 것을 방지하기 위해 불확실성을 의미하는 항 (${\mathbf{w}}_{k-1}$)을 추가한다.

$$\begin{array}{r}\\ {\mathbf{x}}_k=\mathbf{F}{\mathbf{x}}_{k-1}+{\mathbf{w}}_{k-1}\\ \end{array}$$

• ${\mathbf{w}}_{\mathbf{k}-1}$: 프로세스 노이즈 벡터.
• ${\mathbf{w}}_{k-1}\sim \mathcal{N}(0,\mathbf{Q})$: 평균이 0이고, 공분산이 $\mathbf{Q}$인 정규분포(Gaussian distribution)를 따른다.
• $\mathbf{Q}\in {\mathbb{R}}^{n\times n}$ : 위치와 속도의 예측 노이즈 공분산 행렬은 일반적으로 아래와 같이 정의한다. (위치와 속도의 예측 오차는 서로 직접적인 영향을 주지 않는다고 가정함)

$$\mathbf{Q}=\left[\begin{array}{cc}{\sigma }_x^2& 0\\ 0& {\sigma }_v^2\end{array}\right]$$

3. 관측 모델 (Measurement model)

• GPS 값으로부터 현재 시점의 측정값을 계산하는 과정으로, GPS는 차량의 ‘위치’ 정보만 측정한다.
• 관측값 ${\mathbf{z}}_k$는 상태 벡터 ${\mathbf{x}}_k$를 이용해 아래와 같이 나타낼 수 있다.
• 관측값에는 노이즈가 포함되어 있으므로 노이즈에 대한 항 ${\mathbf{m}}_k$ 를 추가한다.

$$\begin{array}{rl}& \\ & z_k=x_k+m_k=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}{x}_k\\ v_k\end{array}\right]+m_k\\ & \\ & {\mathbf{z}}_k=\mathbf{H}{\mathbf{x}}_k+{\mathbf{m}}_k,\mathbf{H}=\left[\begin{array}{cc}1& 0\end{array}\right]\\ & \end{array}$$

• ${\mathbf{z}}_k$: 관측 벡터
• $\mathbf{H}$: 관측 모델 행렬
• ${\mathbf{m}}_k$: 관측 노이즈 벡터
• ${\mathbf{m}}_{k-1}\sim \mathcal{N}(0,\mathbf{R})$: 평균이 0이고, 공분산이 $\mathbf{R}$인 정규분포(Gaussian distribution)를 따른다.
• $\mathbf{R}$: 공분산 $\mathbf{R}$은 관측 값이 1차원이므로 스칼라 값을 가질 것. 즉, $\mathbf{R}=[{\sigma }^2]$

---

칼만 필터 적용

[초기값]

• ${\hat{\mathbf{x}}}_0=\left[\begin{array}{c}0\\ 1\end{array}\right]$ (상태 추정)

• ${\mathbf{P}}_0=\left[\begin{array}{cc}10& 0\\ 0& 1\end{array}\right]$ (오차 공분산)

• $\Delta t=$ 1초 (시간 간격)

• $\mathbf{Q}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$ (예측 노이즈 공분산)

• $\mathbf{R}=4$ (측정 노이즈 공분산)

• $z_1$ = 1.2m (GPS 측정값)

• 초기값에 의해 $\mathbf{F}$와 $\mathbf{H}$는 다음과 같이 설정할 수 있다.

$$\mathbf{F}=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right],\mathbf{H}=\left[\begin{array}{cc}1& 0\end{array}\right]$$

(Prediction) 1. 상태 예측

$$\begin{array}{rl}& \\ {\hat{\mathbf{x}}}_1^{-}& =\mathbf{F}{\hat{\mathbf{x}}}_0\\ & \\ {\hat{\mathbf{x}}}_1^{-}& =\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]\left[\begin{array}{c}0\\ 1\end{array}\right]=\left[\begin{array}{c}1\\ 1\end{array}\right]\\ & \\ & \end{array}$$

(Prediction) 2. 오차 공분산 예측

$$\begin{array}{rl}& \\ {\mathbf{P}}_1^{-}& =\mathbf{F}{\mathbf{P}}_0{\mathbf{F}}^T+\mathbf{Q}\\ & \\ {\mathbf{P}}_1^{-}& =\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]\left[\begin{array}{cc}10& 0\\ 0& 1\end{array}\right]\left[\begin{array}{cc}1& 0\\ 1& 1\end{array}\right]+\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}12& 1\\ 1& 2\end{array}\right]\\ & \\ & \end{array}$$

(Update) 3. 칼만 이득 계산 (Kalman Gain)

$$\begin{array}{rl}& \\ {\mathbf{K}}_1& ={\mathbf{P}}_1^{-}{\mathbf{H}}^T(\mathbf{H}{\mathbf{P}}_1^{-}{\mathbf{H}}^T+\mathbf{R}{)}^{-1}\\ & \\ {\mathbf{K}}_1& =\left[\begin{array}{cc}12& 1\\ 1& 2\end{array}\right]\left[\begin{array}{c}1\\ 0\end{array}\right](\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{cc}12& 1\\ 1& 2\end{array}\right]\left[\begin{array}{c}1\\ 0\end{array}\right]+4{)}^{-1}\\ & \\ & =\left[\begin{array}{c}12\\ 1\end{array}\right]16^{-1}=\left[\begin{array}{c}0.75\\ 0.06\end{array}\right]\\ & \\ & \end{array}$$

(Update) 4. 상태 업데이트

$$\begin{array}{rl}& \\ {\hat{\mathbf{x}}}_1& ={\hat{\mathbf{x}}}_1^{-}+{\mathbf{K}}_1(z_1-\mathbf{H}{\hat{\mathbf{x}}}_1^{-})\\ & \\ {\hat{\mathbf{x}}}_1& =\left[\begin{array}{c}1\\ 1\end{array}\right]+\left[\begin{array}{c}0.75\\ 0.06\end{array}\right](1.2-\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}1\\ 1\end{array}\right])\\ & \\ & =\left[\begin{array}{c}1\\ 1\end{array}\right]+\left[\begin{array}{c}0.15\\ 0.012\end{array}\right]=\left[\begin{array}{c}1.15\\ 1.012\end{array}\right]\\ & \\ & \end{array}$$

(Update) 5. 오차 공분산 업데이트

$$\begin{array}{rl}& \\ {\mathbf{P}}_1& =(\mathbf{I}-{\mathbf{K}}_1\mathbf{H}){\mathbf{P}}_1^{-}\\ & \\ {\mathbf{P}}_1& =(\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]-\left[\begin{array}{c}0.75\\ 0.06\end{array}\right]\left[\begin{array}{cc}1& 0\end{array}\right])\left[\begin{array}{cc}12& 1\\ 1& 2\end{array}\right]\\ & \\ & =\left[\begin{array}{cc}0.25& 0\\ -0.06& 0\end{array}\right]\left[\begin{array}{cc}12& 1\\ 1& 2\end{array}\right]=\left[\begin{array}{cc}3& 0.25\\ -0.72& -0.06\end{array}\right]\\ & \\ & \end{array}$$

---

Python을 이용한 칼만 필터 예제

1차원 위치 추적 문제

• 위의 1차원 GPS 위치값 예측 예시를 참고하여 작성된 코드.

import numpy as np
import matplotlib.pyplot as plt
 
# 초기 상태
x = np.array([[0],   # 위치
              [0]])  # 속도
 
# 상태 전이 행렬
F = np.array([[1, 1],  # 위치 업데이트
              [0, 1]])  # 속도 업데이트
 
# 측정(관측) 모델 행렬
H = np.array([[1, 0]])  # 위치 관측
 
# 오차 공분산 행렬
P = np.eye(2)
 
# 프로세스 노이즈 공분산
Q = np.array([[1, 0],
              [0, 1]])
 
# 관측 노이즈 공분산
R = np.array([[10]])
 
# 측정값 (임의의 잡음 포함)
measurements = [i + np.random.normal(0, 3) for i in range(50)]
 
# 칼만 필터 적용
predicted_positions = []
for z in measurements:
    # 예측 단계
    x = F @ x
    P = F @ P @ F.T + Q
 
    # 업데이트 단계
    y = z - (H @ x)[0, 0]  # 잔차
    y = np.array([[y]])  # 잔차를 2차원 배열로 변환
    S = H @ P @ H.T + R   # 잔차 공분산
    K = P @ H.T @ np.linalg.inv(S)  # 칼만 이득
 
    x = x + K @ y  # 상태 업데이트
    P = (np.eye(2) - K @ H) @ P  # 오차 공분산 업데이트
 
    predicted_positions.append(x[0, 0])
 
# 결과 시각화
plt.plot(measurements, label='Measurements', color='red')
plt.plot(predicted_positions, label='Kalman Filter Prediction', color='blue')
plt.xlabel('Time')
plt.ylabel('Position')
plt.title('Kalman Filter Example')
plt.legend()
plt.show()

---

2차원 위치 추적 문제

• GPS 따위로부터 x위치, y위치, x속도, y속도에 대한 상태를 예측하도록 모델링.

import numpy as np
import matplotlib.pyplot as plt
 
# 초기 상태 [x, y, vx, vy]
x = np.array([[0],    # x 위치
              [0],    # y 위치
              [0],    # x 속도
              [0]])   # y 속도
 
# 상태 전이 행렬 (Δt = 1)
F = np.array([[1, 0, 1, 0],
              [0, 1, 0, 1],
              [0, 0, 1, 0],
              [0, 0, 0, 1]])
 
# 측정 행렬: 위치만 측정
H = np.array([[1, 0, 0, 0],
              [0, 1, 0, 0]])
 
# 초기 공분산
P = np.eye(4) * 500  # 초기 불확실성 크게
 
# 프로세스 노이즈 공분산 (가정치)
Q = np.eye(4)
 
# 측정 노이즈 공분산 (GPS 오차)
R = np.array([[10, 0],
              [0, 10]])
 
# 임의의 이동 경로 + 잡음이 있는 GPS 측정값 생성
true_positions = []
measurements = []
for t in range(50):
    true_x = t * 0.5
    true_y = np.sin(t * 0.1) * 5
    noise_x = np.random.normal(0, 3)
    noise_y = np.random.normal(0, 3)
    z = np.array([[true_x + noise_x],
                  [true_y + noise_y]])
 
    true_positions.append((true_x, true_y))
    measurements.append(z)
 
# 필터 수행
filtered_positions = []
 
for z in measurements:
    # 예측 단계
    x = F @ x
    P = F @ P @ F.T + Q
 
    # 업데이트 단계
    y = z - (H @ x)                  # 잔차
    S = H @ P @ H.T + R             # 잔차 공분산
    K = P @ H.T @ np.linalg.inv(S)  # 칼만 이득
 
    x = x + K @ y                   # 상태 업데이트
    P = (np.eye(4) - K @ H) @ P     # 공분산 업데이트
 
    filtered_positions.append((x[0, 0], x[1, 0]))
 
# 시각화
true_xs, true_ys = zip(*true_positions)
meas_xs, meas_ys = zip(*[(z[0,0], z[1,0]) for z in measurements])
filt_xs, filt_ys = zip(*filtered_positions)
 
plt.figure(figsize=(10, 6))
plt.plot(meas_xs, meas_ys, 'r.', label='GPS Measurements')
plt.plot(filt_xs, filt_ys, 'b-', label='Kalman Filter')
plt.plot(true_xs, true_ys, 'g--', label='True Path')
plt.xlabel('X')
plt.ylabel('Y')
plt.title('2D Kalman Filter for GPS Position Tracking')
plt.legend()
plt.axis('equal')
plt.grid()
plt.show()

---

참고

• 선형 칼만 필터의 원리 이해 - gaussian37
• How a Kalman filter works, in pictures | Bzarg
• [제어시스템공학-4] Kalman Filter(칼만필터) 개념정리(1)
• [수학] 칼만 필터(Kalman Filter)란 무엇인가? (로봇, 자율주행, SLAM 알고리즘) : 네이버 블로그
• 칼만 필터 - 공돌이의 수학정리노트 (Angelo’s Math Notes)
• Kalman filter 소개
• 칼만 필터(Kalman Filter) 개념 정리 | Site Unreachable