칼만 필터(Kalman filter)

선행 자료

![베이즈 룰(bayes rule)]

배경지식

추정이론

• Filtering : n개의 관측데이터 $x[0],...,x[n-1]$ 이 주어졌을때, $\theta =x[n-1]$ 로 산정하고 마지막 관측데이터를 추정하는 것
  • 현재와 과거 데이터에 의존하여, 현재 신호에서 노이즈를 필터링하고자 하는 것
• Smoothing : 관측된 중간 시점의 데이터를 추정하는 경우, 모든 데이터가 관측되어야 수행 가능
• Prediction : 임의의 양수 $l$ 에 대해 $\theta =x[n-1+l]$ 를 추정하는 경우
  • 즉 미래의 데이터를 추정하는 문제
• Interpolation : 관측되지 않은 중간 시점의 데이터를 추정하는 문제

Dynamic System

• 시간에 따라 상태가 변하는 시스템

• 아래와 같이 모델링 가능

  • $MotionModel:x_t=f(x_{t-1},u_t)+w_t$
    • 이전의 상태($x_{t-1}$)를 기반으로 현재의 상태($x_t$)가 무엇이 될지 물리 법칙이나 시스템의 논리에 따라 계산 (내부적 추측)
  • $ObservationModel:z_t=h(x_t)+v_t$
    • 결정된 상태($x_t$)에 따라 실제 세계에서 우리 눈(센서)에는 어떻게 보일지($z_t$)를 계산하는 모델
  • 예측과 보정(or Filtering) 의 순환관계에 있다고 생각 가능

• 노이즈

  구분	노이즈 기호	의미	예시 (예: 무릎 각도 측정)
  Motion Model	${\mathbf{w}}_t$	시스템 모델의 불확실성	근육의 떨림이나 모델이 계산하지 못한 급격한 움직임
  Observation Model	${\mathbf{v}}_t$	센서 측정의 불확실성	센서 자체의 전기적 노이즈나 부착 위치의 흔들림

  • 시스템의 노이즈가 크면 센서 값에 더 의존, 만대로 센서의 노이즈가 크면 자신의 예측을 더 신뢰

• 관측 시스템의 그래프
  

  • Transition

    • 예측 단계
      

      • 관측값 없이 우리의 모델링에 따라서 다음 상태를 예측

    • 업데이트(Filtering) 단계
      

      • 관측 모델에 따라 $z_{t+1}$ 을 얻어내고, 그 값을 기반으로 ${\hat{x}}_{t+1}$ 을 업데이트 (Filtering)

Recursive Bayes Filter

• Belief가 시시각각 어떻게 변해가는지 추정해가는 과정
  • 예측 -> 업데이트 -> 예측 -> 업데이트 ...
• 확정되지 않은 믿음($\overline{bel}$) 에 실제 관측값($z_t$)을 끼얹어, 더 정확한 $bel({\mathbf{x}}_t)$ 를 만들어 내는 과정

Belief

• 필터링의 수학적 상태
  • 지금 이 순간의 상태($x_t$)가 무엇인가? 를 알아내는 것
  • 현실의 노이즈때문에 하나의 값으로 확정할 수 없음
• 하나의 점이 아닌 확률 분포로 표현
  • $bel({\mathbf{x}}_t)=p({\mathbf{x}}_t\mid {\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})$
    • 의미: 지금까지의 모든 데이터($\mathbf{z},\mathbf{u}$)를 고려했을 때, ${\mathbf{x}}_t$가 여기에 있을 확률이 이만큼임

Recursive bayes filter

• 위에서 정의한 Belief를 어떻게 계산할 것인가? 에 대한 알고리즘
• 예측
  • $\overline{bel}({\mathbf{x}}_t)=\int p({\mathbf{x}}_t\mid {\mathbf{x}}_{t-1},{\mathbf{u}}_t)bel({\mathbf{x}}_{t-1})d{x}_{t-1}$
    • $p({\mathbf{x}}_t\mid {\mathbf{x}}_{t-1},{\mathbf{u}}_t)$ : 모션 모델
    • $bel({\mathbf{x}}_{t-1})$ (과거의 믿음) : 바로 직전 시점에 내가 어디에 있었을지에 대한 확률 분포
    • 적분 : t-1에서 A일 확률 $\times$ A에 대한 확률 분포 ... 모든 가능성을 합쳐서 새로운 예측값을 만듦
      • 전확률정리에 기반함
• 업데이트
  • $bel({\mathbf{x}}_t)=\eta \cdot p({\mathbf{z}}_t\mid {\mathbf{x}}_t)\overline{bel}({\mathbf{x}}_t)$
    • $p({\mathbf{z}}_t\mid {\mathbf{x}}_t)$ : 관측 모델 (즉 우도, 가설이 맞을때 센서에 관측값에 대한 확률)
    • $\overline{bel}({\mathbf{x}}_t)$ (사전 확률, Prior): 방금 위에서 구한, 센서를 보기 전의 '예측된 믿음'
    • 즉 베이즈 정리 그 자체를 계산하는 것
• 재귀 필터 : 이전 bel로부터 현재 bel을 구하는 과정

Recursive bayes filter의 유도

• 총합의 법칙, Marginalization

  • $p(x)={\int }_{y}p(x\mid y)\cdot p(y)dy$
  • x확률 = 어떤조건이 발생할 확률 $\times$ 그 조건일때 결과가 나올 x확률의 총합

• $bel(x_t)=p(x_t\mid z_{1:t},u_{1:t})$ : 모든 관측데이터에 대해 $x_t$의 확률

  • $=p(x_t\mid z_t,z_{1:t-1},u_{1:t})$
    • $z_t$에 대해서만 특수하게 생각
  • $=\eta \cdot p(z_t\mid x_t,z_{1:t-1},u_{1:t})\cdot p(x_t|z_{1:t-1},u_{1:t})$
    • $x_t$ 와 $z_t$ 에 대한 베이지안 룰 (전자는 우도, 후자는 사전확률)
  • $=\eta \cdot p(z_t\mid x_t)\cdot p(x_t|z_{1:t-1},u_{1:t})$
    • 마르코브 가정으로 $z_t$ 예측 모델 간단화, $ObservationModel$ 참조 $z_t$ 는 $x_t$ 만 영향을 줌
  • $=\eta \cdot p(z_t\mid x_t)\cdot {\int }_{x_{t-1}}p(x_t|x_{t-1},z_{1:t-1},u_{1:t})\cdot p(x_{t-1}|z_{1:t-1},u_{1:t})d{x}_{t-1}$
    • Marginalization 적용, $x_t$ 는 $x_{t-1}$ 이라는 조건이 일어날 확률과 그 조건이 일어났을때 $x_t$가 일어날 확률의 총합과 같음
    • $x_t=\int p(x_t|x_{t-1})\cdot p(x_{t-1})d{x}_{t-1}$
  • $=\eta \cdot p(z_t\mid x_t)\cdot {\int }_{x_{t-1}}p(x_t|x_{t-1},u_t)\cdot p(x_{t-1}|z_{1:t-1},u_{1:t})d{x}_{t-1}$
    • 마르코브 가정으로 $x_t$에 대해 $x_{t-1}$ , $u_t$만 남김, $MotionModel$ 참조, $x_t$는 $x_{t-1}$ , $u_t$만 영향을 줌
  • $=\eta \cdot p(z_t\mid x_t)\cdot {\int }_{x_{t-1}}p(x_t|x_{t-1},u_t)\cdot p(x_{t-1}|z_{1:t-1},u_{1:t-1})d{x}_{t-1}$
    • $x_{t-1}$ 에 대해 $u_t$ 는 의미가 없으므로 제거
  • $=\eta \cdot p(z_t\mid x_t)\cdot {\int }_{x_{t-1}}p(x_t|x_{t-1},u_t)\cdot bel(x_{t-1})d{x}_{t-1}$
    • bel 의 정의에 의해 대체
    • 정리: 마르코브 가정, Marginalization, 베이지안룰에 의해 bel을 이전 bel과 모션 모델의 곱으로 표현
  • $=\eta \cdot p(z_t\mid x_t)\cdot \overline{bel}(x_t)$
    • 이전에 보정한 믿음 (확률분포) $\times$ $MotionModel$ 의 곱의 모든 경우의수의 합은 정의에 의해 $\overline{bel}$ (즉 관측없이 예측한 확률 분포)

Recursive bayes filter 정리

• Dynamic System 이라는 물리적인 모델이 있음

  • 이는 추측과 관측 두개의 모델링을 진행함

• Recursive bayes filter 를 이용해서 잘 추측했는지 검증함

  1. 관측없이 추측만 진행
  2. 관측을 기반으로 추측을 업데이트

• 추측의 방법은 $\overline{bel}({\mathbf{x}}_t)=\int p({\mathbf{x}}_t\mid {\mathbf{x}}_{t-1},{\mathbf{u}}_t)bel({\mathbf{x}}_{t-1})d{x}_{t-1}$ 를 계산하는것

  • 이전 조건의 확률 분포에 대해 Motion Model의 값을 곱해 더하는것
  • 이전의 Motion Model의 결과값을 그대로 믿지 않고 bel의 확률분포를 적용해 모든 경우의 수를 적용해봄
    • Motion Model은 하나의 값, 선 하나를 내뱉음, 그것에 확률 분포를 적용 (노이즈 적용) -> 분산이 커짐

• 업데이트 방법은 $bel({\mathbf{x}}_t)=p({\mathbf{x}}_t\mid {\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})$ $=\eta \cdot p(z_t\mid x_t)\cdot \overline{bel}(x_t)$ 를 계산하는것

  • 관측값과 입력값에 대한 사후 확률(분포)
  • 사후 확률이기 때문에 우도와 사전확률의 곱으로 표현 가능
    • 우도는 마르코브 체인으로 간단화 가능
    • 사전 확률의 경우 아래와 같은 수학적 trick 으로 이전 사후 확률을 기반으로 표현 가능
      • 마르코프 가정
      • Marginalization
  • 최종적으로 나온 수식, 우도와 예측 확률분포를 곱해 추측한값을 "보정"하는 과정
    • 추측에서 구한 확률 분포에 센서의 관측치를 곱연산함 (벤다이어그램으로 치면 원이 하나 생기고 곱셈, 즉 교집합을 찾은것) -> 분산이 작아짐 (뾰족해짐)

Kalman Filter

접근법

• Recursive bayes filter 를 두가지 가정 아래에서 간소화하는 접근법
  1. Model들이 선형이다.
  2. 노이즈가 정규분포이다.
     • 이 가정에 의해 Belief 가 정규분포가 됨
  • 가우시안에 선형변환을 진행해도 가우시안인 성질을 이용하기 위함 (합과 곱)
  • 참조
    • 모델은 선형인데 노이즈가 가우시안이 아닐 수 있음
      • 칼만 필터가 최적이 아닌 문제
    • 비선형인데 노이즈는 가우시안일 수 있음
      • 확장 칼만 필터를 사용 가능

모델링

• $MotionModel:x_t=F_t{x}_{t-1}+B_t{u}_t+w_t$
• $ObservationModel:z_t=H_t{x}_t+v_t$
• $w_t\sim \mathcal{N}(0,Q_t)$ : 모션 모델의 노이즈, 또 그에 대한 공분산 행렬
• $v_t\sim \mathcal{N}(0,R_t)$ : 관측 모델의 노이즈, 또 그에 대한 공분산 행렬
• $\mathbf{F},\mathbf{B},\mathbf{H}$ : 물리법칙에 따라 직접 작성하고 고정
• $\mathbf{Q},\mathbf{R}$: 시스템과 센서의 상태에 따라 최적의 값을 찾아줌 (튜닝값)

칼만필터 알고리즘

• 접근
  • 예측 단계에서 우리는 $\int \mathbf{p}\mathbf{(}{\mathbf{x}}_{\mathbf{t}}\mid {\mathbf{x}}_{\mathbf{t}\mathbf{-}\mathbf{1}}\mathbf{,}{\mathbf{u}}_{\mathbf{t}}\mathbf{)}\dots$를 계산
  • 업데이트 단계에서 우리는 $\eta \cdot \mathbf{p}\mathbf{(}{\mathbf{z}}_{\mathbf{t}}\mid {\mathbf{x}}_{\mathbf{t}}\mathbf{)}\dots$를 계산
• 가우시안 분포의 원리
  • 상수 + 가우시안 변수 = 새로운 가우시안 변수
    • 평균을 이동한 것으로 생각 가능
  • $MotionModel:x_t=F_t{x}_{t-1}+B_t{u}_t+w_t$ 에서 $w_t$ 는 가우시안 분포이므로, $\mathcal{N}(0,Q_t)$ 에서 $w_t\sim \mathcal{N}(F_t{x}_{t-1}+B_t{u}_t,Q_t)$ 로 이동한 가우시안 분포로 생각가능
  • $ObservationModel:z_t=H_t{x}_t+v_t$ 또한 $w_t\sim \mathcal{N}(H_t{x}_t,Q_t)$ 를 따르는 정규분포
  • 위를 $\overline{bel}(x_t)$ 와 $bel(x_t)$ 에 대입하고 또 다른 정규분포를 유도하면 아래와 같은 결론이 나옴
  • 예측
    • $\overline{bel}(x_t)=\int p(x_t|x_{t-1},u_t)bel(x_{t-1})d{x}_{t-1}\sim \mathcal{N}({\hat{x}}_{t|t-1},P_{t|t-1})$
      
  • 업데이트
    • $bel(x_t)=\eta ·p(z_t|x_t)\overline{bel}(x|t)\sim \mathcal{N}({\hat{x}}_{t|t},P_{t|t})$
      
    • $K_t$ 는 칼만 게인을 뜻함
      • 추측과 센서중 어떤것을 더 믿을지 결정하는 가중치

시각화 및 알고리즘

• Phase 1. 예측 단계 (Time Update)
  • 센서 값을 보기 전, 오직 물리 법칙($\mathbf{F},\mathbf{B}$)만 믿고 "아마 이쯤 갔겠지?" 하고 짐작하는 단계입니다.
  1. 상태 예측 (State Prediction)

- 의미: 어제의 위치에 오늘의 이동 명령을 더해 새로운 위치를 점찍습니다. 그림의 빨간색 분포의 중심점이 이동하는 과정입니다.
2. 오차 공분산 예측 (Error Covariance Prediction)

- 의미: 이동하면서 발생하는 불확실성(${\mathbf{Q}}_t$)을 더합니다. 그림에서 종 모양이 옆으로 펑퍼짐하게 퍼지는(안개가 짙어지는) 이유가 바로 이 수식 때문입니다.

• Phase 2. 보정 단계 (Measurement Update)
  • 이제 눈을 뜨고 센서 값(${\mathbf{z}}_t$)을 확인하여, 방금 한 짐작을 수정합니다.
  3. 칼만 이득 계산 (Kalman Gain)

- 의미: "내 추측($\mathbf{P}$)"과 "센서의 말($\mathbf{R}$)" 중 누구를 더 믿을지 결정하는 가중치(${\mathbf{K}}_t$)를 구합니다. GPS가 튀면 $\mathbf{R}$이 커져서 $\mathbf{K}$가 작아지므로, 센서 말을 무시하게 됩니다.

4. 상태 업데이트 (State Update)

- 의미: **(나의 예측) + (가중치 $\times$ 예측 오차)**의 형태입니다. 그림에서 빨간 선과 파란 선 사이의 최적의 지점인 검은색 선으로 중심을 옮기는 과정입니다.
5. 오차 공분산 업데이트 (Error Covariance Update)

- 의미: 정보를 하나 더 얻었으니 불확실성을 줄입니다. 안개가 걷히고 종 모양이 다시 뾰족(Sharp)해집니다.

칼만필터 알고리즘 유도

• 평균을 이동한 것으로 생각 가능
• $MotionModel:x_t=F_t{x}_{t-1}+B_t{u}_t+w_t$ 에서 $w_t$ 는 가우시안 분포이므로, $\mathcal{N}(0,Q_t)$ 에서 $w_t\sim \mathcal{N}(F_t{x}_{t-1}+B_t{u}_t,Q_t)$ 로 이동한 가우시안 분포로 생각가능
• $ObservationModel:z_t=H_t{x}_t+v_t$ 또한 $w_t\sim \mathcal{N}(H_t{x}_t,Q_t)$ 를 따르는 정규분포
• 위를 $\overline{bel}(x_t)$ 와 $bel(x_t)$ 에 대입하고 또 다른 정규분포를 유도하면 아래와 같은 결론이 나옴

• 확률 분포 변수를 $\overline{bel}(x_t)$ 에 대입

• 칼만 필터의 정의
  

• $\overline{bel}$ 에 $MotionModel$ 대입
  

• 가우시안 수식에 대입
  

• exp의 곱이므로 지수의 합으로 간단하게 정리 및 L로 치환
  

• TODO