Kalman filter와 Particle filter에 대해서 설명해주세요.

이 게시글은 장형기님이 작성한 SLAM 기술 면접 100선에 대한 답을 제 나름대로 정리해본 것입니다.

---

Introduction

Kalman filter 와 Particle filter에 대해 알아보기 앞 서 Filter란 무엇인지부터 알아보겠습니다.

제가 전기과를 나왔기 때문에 처음 이것을 접했을 때 filter는 low pass filter, high pass filter 이런 것처럼 그냥 필요한 부분만 남기는건데 왜 이렇게 식이 복잡하지라는 생각을 했습니다. 저랑 비슷한 생각을 하시는 분이 있다면 이 글을 읽으면 좋을 것 같습니다.

결론부터 말하자면 우리의 관측이나 odometry 측정에는 noise가 낄 수 밖에 없기 때문에 그것을 걸러주는게 filter의 역할이라고 생각하시면 됩니다. 그것을 filtering하는 과정에서 수식이 조금 들어가기 때문에 복잡하다고 생각하실 수 있는데 차근차근 살펴보겠습니다.

---

Preliminaries

• Multivariate gaussian and Conditional Probability

  단일 Gaussian distribution 식은 모두들 알 것이라고 가정하겠습니다.
  Multivariate gaussian의 경우 variable이 2개인 gaussian distribution을 나타내는 것입니다.

  $C = \begin{pmatrix} C_{xx} & C_{xy} \\ C_{yx} & C_{yy} \end{pmatrix}$

  분산을 이렇게 정의한다면 x,y의 공분산을 나타내는 matrix입니다. 분포를 나타내는 variable이라고 생각하시면 됩니다.

  $P(x,y) = \frac{1}{\sqrt{(2\pi)^{k+l}detC}}exp(-\frac{1}{2}\begin{pmatrix} x-E(x) \\ y-E(y)\end{pmatrix}^TC^{-1}\begin{pmatrix} x-E(x) \\ y-E(y)\end{pmatrix})$

  gaussian distribution 식에 대입을 하게 되면 multivariate gaussian을 정의할 수 있습니다.

  왜 이런짓을 했나면 Gaussian distribution을 활용하여 Conditional Probability의 분포도 정의해보려고 했습니다. 그렇게 하려면 고등학교 때 배운 조건부 확률을 사용하면 됩니다.

  $P(y|x) = \frac{P(x,y)}{P(x)}$

  P(x,y)와 P(x)가 모두 gaussian distribution을 따른다면 이 조건부 확률도 gaussian을 따를 것입니다.

  $P(y|x) \sim N(E(y|x),C_{y|x})$

  앞의 식을 토대로 계산을 하면 조건부 확률의 평균과 분산을 구할 수가 있습니다.

  계산을 하면 평균과 분산은 다음과 같습니다.

  $E(y|x) = E(y) + C_{yx}C_{xx}^{-1}(x-E(x))$
  $C_{y|x} = C_{yy} - C_{yx}C_{xx}^{-1}C_{yx}$

  이렇게 두 식을 활용하면 조건부 확률의 평균과 분산을 통해 분포를 계산할 수 있습니다.

  ---

Bayesian Filtering

실제 위치값 $z$, 관측 값 $x_t$의 joint probability $p(z,x_t|x_{0:t-1})$를 봅시다.
이 분포의 평균과 분산은 다음과 같습니다.

$\mu$ = $\begin{pmatrix} \mu_z \\ \mu_{x_t} \end{pmatrix}$, $\Sigma = \begin{pmatrix} \Sigma_{z,z} & \Sigma_{z,x_t} \\ \Sigma_{x_t,z} & \Sigma_{x_t,x_t} \end{pmatrix}$

$x_t$를 조건으로 집어 넣어 조건부 확률을 구해보겠습니다.

$p(z|x_t,x_{0:t-1}) = p(z|x_{0:t})$가 되고 이것은 우리가 관측값을 토대로 계산하는 $z$의 분포를 나타냅니다.

우리는 이미 그 전 관측을 통해 위치값의 분포를 계산했다고 생각합시다.
$\mu_z = \mu_{z|t-1}, \Sigma_{z,z} = \Sigma_{z|t-1}$

그렇다면 아까 조건부 확률식을 통해 t까지 관측을 포함하여 위치값을 업데이트할 수 있습니다.

$\mu_{z|x_t} = \mu_z + \Sigma_{z,x_t}\Sigma_{x_t,x_t}^{-1}(x_t - \mu_{x_t})$

$\Sigma_{z|x_t} = \Sigma_{z,x_t}\Sigma_{x_t,x_t}^{-1}\Sigma_{x_t,z}$

이렇게 업데이트를 하게 됩니다.

그럼 Noise는 어디에서 filtering 하는겁니까? 라는 질문을 할 수 있습니다.

noise는 이미 다 $\Sigma$안에서 계산이 되기 때문에 다 반영이 됩니다.

Example

예시를 쓸까말까 100만번 생각해보고 쓰지만 써야 이해가 더 잘될 것 같아서 쓰겠습니다.

이전까지 계산된 이전 시점 위치값을 토대로 새로운 관측값이 들어오면 업데이트를 하게 됩니다.

업데이트하기 전에 미리 현재 위칙을 간략하게 예측을 하게 되는데 이것은 정의된 model에 따라 예측을 진행합니다.

• Dynamic model:

  $z_t = Az_{t-1} + Bu_{t-1} + Gv$
  $v \sim N(0,Q)$

• Measurement model:
  $x_t = Cz_t +w$
  $w \sim N(0,R)$

  이러한 model을 따라 prediction을 하고 아까 말한 조건부 확률로 업데이트를 하면 우리는 noise를 고려하여 위치값을 계산할 수 있습니다.

  Prediction(Dynamic Update)
  $\mu_{z_t|x_{t-1}} = E(z_t|x_{0:t-1},u_{0:t-1}) \\ =E(Az_{t-1}+Bu_{t-1}+Gv|x_{0:t-1},u_{0:t-1})\\=A\mu_{z_{t-1}|x_{t-1}}+Bu_{t-1}$

  $\Sigma_{z_t|x_{t-1}} = E[(z_t-\mu_{z_t|x_{t-1}})(z_t-\mu_{z_t|x_{t-1}})^T|x_{0:t-1},u_{0:t-1}]\\=E[(Az_{t-1}+Gv-A\mu_{z_{t-1}|x_{t-1}})(Az_{t-1}+Gv-A\mu_{z_{t-1}|x_{t-1}})^T|x_{0:t-1},u_{0:t-1}]\\=A\Sigma_{z_{t-1}|x_{t-1}}A^T+GQG^T$

  이렇게 $P(z_t|x_{t-1})$를 구할 수가 있습니다.

  Measurement Update

  계산 과정이 길어서 제가 쓰지는 못하지만 똑같이 계산을 해서 결과값을 얻자면

  $\mu_{z_t|x_t} = \mu_{z_t|x_{t-1}} + \Sigma_{z_t|x_{t-1}}C^T(C\Sigma_{z_t|x_{t-1}}C^T+R)^{-1}(x_t-C\mu_{z_t|x_{t-1}})$

  $\Sigma_{z_t|x_t} = \Sigma_{z_t|x_{t-1}} - \Sigma_{z_t|x_{t-1}}C^T(C\Sigma_{z_t|x_{t-1}}C^T+R)^{-1}C\Sigma_{z_t|x_{t-1}}$
  여기서 공통적으로 사용되는 부분은 K라고 변수로 만들 수 있습니다.

  $K = \Sigma_{z_t|x_{t-1}}C^T(C\Sigma_{z_t|x_{t-1}}C^T+R)^{-1}$

  이것이 Kalman filter에서 쓰이는 Kalman gain이 되는 것입니다.

  ---

Kalman Filter

사실 위에서 계산한 linear model에 대한 prediction과 update 과정을 Kalman filter라고 합니다.
noise를 포함하여 위치의 평균과 분산을 구해 위치값을 확률적으로 계산하는 것입니다.

그렇다면 Extended Kalman filter는 무엇인가요?
Prediction을 보면 그냥 linear model을 활용해서 하는 것을 볼 수 있습니다.
그렇다면 비선형 모델은?? 사실 바로 못합니다 ㅠ
그렇기 때문에 비선형 모델에도 적용할 수 있는 kalman filter를 만든 것이 extended kalman filter입니다.
비선형 함수를 선형으로 만드는 방법인 taylor series를 활용하여 linear model로 만들고 kalman filter돌리는 것이 extended kalman filter입니다.

---

Particle Filter

이거 하나하나 어떻게 다 계산하고 있습니까?! 분포를 정확히 알면 좋지만 그걸 다 계산하는 것은 비효율적으로 보입니다. 그러면 그냥 sampling을 통해 분포를 근사하고 그거에서 계산하자!하는 것이 particle Filter입니다. Paritcle을 sampling한다고 해서 particle filter인 것으로 알고 있습니다.

Importance Sampling

E[$f$] =$\int f(z)p(z) dz \approx \frac{1}{N}\Sigma f(z_n)p(z_n) \approx \sum^\infty_{n=1}\frac{1}{N}f(z^n), z^n \sim p(z)$ 라고 볼 수 있습니다.
우리는 분포 p(z)에서 뽑은 z로 f의 평균을 계산할 수 있습니다. 이건 그냥 무한대로 뽑아서 평균 계산하면 된다는 것입니다.

하지만 문제는 우리가 p(z)를 알지 못한다는 것입니다. 그래서 사용하는 것이 importance sampling입니다.

$\int f(z) \frac{p(z)}{q(z)}q(z)dz = E_{z\sim q}[f(z) \frac{p(z)}{q(z)}] \approx \frac{1}{N}\sum^N_{n=1}\frac{p(z^{(n)})}{q(z^{(n)})}f(z^{(n)})$
우리가 알고 있는 분포 q(z)를 이용하여 앞에 weight를 곱해 f(z)를 sampling하는 것이라고 생각할 수 있습니다. 이것을 활용하여 Sampling을 진행하여 Kalman filter와 같이 prediction과 update를 진행하면 됩니다.

Dynamic Update

dynamic update는 이전까지의 관측값을 가지고 현재의 위치를 추정하는 것입니다.

$P(z_t|x_{0:t-1},u_{0:t-1}) = \int p(z_t|z_{t-1},u_{t-1})p(z_{t-1}|x_{0:t-1},u_{0:t-2})dz_{t-1}$
이것을 bayes rule로 조금 변형하면
$\int p(z_t|z_{t-1},u_{t-1})\frac{p_(x_{t-1}|z_{t-1})p(z_{t-1}|x_{0:t-2},u_{0:t-2})}{\int p_(x_{t-1}|z_{t-1})p(z_{t-1}|x_{0:t-2},u_{0:t-2})dz_{t-1} }dz_{t-1}$ 가 됩니다.
이전 t-1위 위치는 이미 계산을 했기 때문에 우리는 이것을 아는 분포 q(z)라고 판단할 수 있습니다.
이를 통해 importance sampling을 진행하면 우리는 이 분포를 계산할 수 있습니다.

따라서 최종 식은

$E_{z \sim q} [w_{t-1} \dot p(z_t|z_{t-1},u_{t-1})] = \frac{1}{N}\Sigma_nw^{(n)}_{t-1}p(z_t|z^{(n)}_{t-1},u_{t-1})$

where
$z^{(n)}_{t-1} \sim p(z_{t-1}|x_{0:t-2},u_{0:t-1})$, $w^{(n)}_{t-1} = \frac{p(x_{t-1}|z^{(n)}_{t-1})}{\Sigma_np(x_{t-1}|z^{(n)}_{t-1})}$

이렇게 됩니다. 뭔가 weight가 되어 sampling을 통해 분포를 계산한다는 것만 알아주면 됩니다.

Measurement update

$E[h(z_t)] = \mu_{z_t|x_t} \approx \frac{1}{N}\Sigma_nw^{(n)}_{t}h(z_t^{(n)})$

where

$z_t^{(n)} \sim p(z_t|x_{0:t-1},u_{0:t-1}), w_t^{(n)}=\frac{p(x_t|z_t^{(n)})}{\frac{1}{N}\Sigma_np(x_t|z_t^{(n)})}$

measurement식은 이렇게 됩니다. meausrement model을 $h(z_t)$로 놓고 앞이랑 똑같이 한거라 다를게 없습니다.

이렇게 Sampling을 통해 분포를 추정할 수 있다는 것을 볼 수 있습니다.

왜 평균만 구했는데 분포를 구했다고 하나요? 왜냐면 optimization을 mean square error로 하면 어차피 평균이 나오기 때문에 평균값을 구하는 것입니다.

---

Conclusion

지금까지 글을 보면 Kalman filter의 경우 bayes rule로 그냥 관측을 조건으로 조건부 확률 분포를 계산했다고 생각하시면 되고 그것이 linear model 기반이라면 그냥 Kalman filter 비선형 model 기반이라면 entended kalman filter라고 생각하시면 됩니다. 생각보다 어렵지 않죠..?

Particle filter는 importance sampling을 통해 분포를 추정하여 계산하는 filter입니다.

Kalman filter와 Particle filter의 장단점을 알아보고 마치겠습니다.

• Kalman filter

  • 장점

  1. 효율성: 상태와 공분산 업데이트에 닫힌 수식이 있어 실시간 처리에 적합합니다.

  2. 간결성: 이론적으로 명확하고 구현이 간단합니다.

  • 단점

  1. Gaussian 노이즈 가정: 측정 오차나 프로세스 노이즈가 Gaussian 분포를 따르지 않을 경우 Kalman filter의 추정 정확도가 낮아집니다.

  2. 정확도가 Bounded되어있다: 이론적 계산이고 다양한 가정을 포함하기 때문에 정확도의 최대값이 bounded되어 있습니다.

• Particle filter

  • 장점

  1. 비선형성 지원: particle filter는 비선형 시스템과 gaussian noise가 아닌 noise도 처리할 수 있습니다.

  2. 표본만 많아진다면 model을 정확하게 근사할 수 있게 됩니다.

  • 단점

  1. 입자수에 의존적: 표본이 많아진다면 정확도는 매우 높아지겠지만 적으면 또 한없이 낮아집니다

  2. 설계 복잡성: 설계와 튜닝이 Kalman filter보다 어렵습니다.

지금까지 가장 많이 쓰이는 두 filter를 알아보았습니다.
결국 noise를 효과적으로 처리하여 데이터를 추정할 수 있게 해주는 것들이라고 생각해주면 되겠습니다.

감사합니다.