베이즈 필터, 칼만 필터, 확장 칼만 필터

 

📌bayes filter 

베이즈 필터는 로봇이 자신의 위치와 방향을 추론할 수 있도록 확률을 계산하는 데 사용되는 알고리즘으로 recursive bayesian estimation 이라고도 한다. 기본적으로 로봇이 최근에 획득한 데이터를 기반으로 좌표계에서 가장 가능성이 높은 위치를 업데이트한다.  

실제로 사용되는 칼만필터나 확장 칼만필터의 뼈대가 되는 프레임워크다.
기본적으로 시간 t에서의 belief 에서 최근의 관측값(측정값)과 제어명령을 고려해 t+1에서의 belief 로 발전시킨다. 

- 개념

• state: 로봇의 위치, 방향 등 추정하고자하는 특징을 말한다.
• belief: 현재 자신의 상태에 대한 추정치(확률 분포)이다. 예를 들어 초기 시작 위치가 0,0 이라면 해당 위치에 확률 분포가 집중되어 있을테고, 움직이기 시작하면서 불확실성이 높아질 것이다.
• prediction: 모델을 이용해 미래 상태를 예측한다.
• innovation: 예측한 상태와 측정한 센서 데이터를 비교하고 차이를 반영해 belief를 갱신한다. 차이가 클수록 더 많이 조정되고 이 단계에서 bayes rule 을 사용한다.
• recursive: 이 과정이 반복되며 belief가 갱신된다. 
• 정규 분포와 선형성: 칼만필터는 베이즈 필터의 특수한 경우인데, 움직임과 센서 모델이 선형이고 불확실성이 정규분포를 따르면 칼만필터를 사용할 수 있다.

- 상태 추정의 수학적 표현

우선 시간 t에서 관측값 z와 로봇 제어 입력 u 로 상태 x를 추정한다고 생각해 다음과 같은 식으로 시작할 수 있다. 

$$\large{p(x_{t} | z_{1:t}, u_{1:t})}$$

과거의 모든 측정값($z_{1:t}$ 1 부터 t 까지의 측정값)과 제어 입력($u_{1:t}$ 1부터 t까지 제어 입력)이 주어졌을 때 상태 $x_{t}$ 의 확률을 의미한다.

목표는 이 확률을 재귀적인 형태로 표현해 t-1 에서의 belief를 기반으로 업데이트 할 수 있도록 만드는 것이다.

- 과정

먼저 과정은 다음과 같다.

• 이전 belief 업데이트: 이전 belief 를 기반으로 현재 상태를 예측
• 관측 모델 활용: 관측값(측정값)을 기반으로 현재 상태가 실제로 어떨지 평가
• 운동 모델 활용: 시스템 제어 명령을 고려해 상태 변화를 예측
• 베이즈 정리, Markov process, 전체확률의 법칙: 앞선 정보를 결합해 최적의 상태 추정 결과 획득 

베이즈 정리
$p(x|y)=\large{\frac{p(y|x)p(x)}{p(y)}}$ 

markov process(markov chain)
확률은 그 직전 상태만 같다면 다음 상태의 확률은 같다고 가정, 이전에 어떤 상태를 거쳤는 지에는 무관. 
즉 다음 상태는 현재 상태에만 의존

 

단계 1 베이즈 룰 적용

앞에서 본 것처럼 시간 t에서의 belief, bel($x_{t}$)를 다음과 같이 정의하고 베이즈 정리를 적용한다. 

$$p(x_{t} | z_{1:t}, u_{1:t}) $$

$p( x_{t} | z_{1:t}, u_{1:t} )=\Large{\frac{p( z_{1:t}, u_{1:t} | x_{t} )p( x_{t} )}{p( z_{1:t}, u_{1:t} )}}$ 
가 아닌 이유.. 모델링 하기 나름이라서? 

보기 쉽게 밑을 잠시 제거해보면 $p(x|z,u)=\large{\frac{p(z|x,u)p(x|u)}{p(z|u)}}= \frac{p(z_{1:t}|x_{t},u_{1:t})p(x_{t}|u_{1:t})}{p(z_{1:t}|u_{1:t})} $

$bel(x_{t}) = \Large{\frac{p(z_{t} | x_{t}, z_{1:t-1},u_{1:t}) p(x_{t} | z_{1:t-1}, u_{1:t})}{p(z_{t} | z_{1:t-1}, u_{1:t})}}$ 이전 측정이 다음 측정에 영향을 미칠 수 없어 분모를 상수로 치환한다? 

$bel(x_{t}) = p(x_{t} | z_{1:t}, u_{1:t}) $
$=\eta \ p(z_{t} | x_{t},z_{1:t-1},u_{1:t}) p(x_{t} | z_{1:t-1}, u_{1:t}) $

 

단계 2 마르코프 가정 적용

다음으로 $x_{t}$ 를 알고있을 때 어떤 것을 관측할 확률 을 수정해야한다. 이때 마르코프 가정을 사용하는데 현재 상태를 알 때 과거는 미래와 독립적이라고 가정한다. 따라서 과거의 제어 명령이나 측정값은 무시할 수 있다.

이 가정을 통해 $p(z_{t} | x_{t},z_{1:t-1}, u_{1:t}) = p(z_{t}|x_{t})$ 와 같다고 볼 수 있고 다음과 같이 단순화된다.

$$bel(x_{t}) = \eta p(z_{t} | x_{t}) p(x_{t} | z_{1:t-1}, u_{1:t})$$

 

단계3 전체 확률의 법칙 적용

$x_{t-1}$ 을 확률 분포에 도입하기 위해 전체 확률의 법칙을 사용한다. 
도입하는 이유는 재귀 함수로 모델링하기 위해서이다.

핵심은 과거의 모든 관측(측정값)과 제어 입력이 주어졌을 때 현재 상태를 찾는 문제가 
이전 상태를 알 때 현재 상태를 찾는 문제로 바뀌는 것이다.

$p(x_{t} | z_{1:t-1},u_{1:t})$ 를 전체 확률의 법칙을 이용해 변환한다. 
변수 $x_{t-1}$을 추가하고 모든 가능한 이전 상태에 대해 적분한다.

$$bel(x_{t}) = \eta \ p(z_{t} | x_{t}) \int p(x_{t} | x_{t-1},z_{1:t-1}, u_{1:t}) p(x_{t-1}|z_{1:t-1},u_{1:t})dx_{t-1}$$

조건부 확률  $p(A)$ 는 중간 변수 $B$ 를 도입해 $p(A)=\int p(A|B)p(B) dB $ 로 나타낼 수 있기 때문이다.

단계4 추가적인 마르코프 가정 적용

이전 상태를 알고 있다면 과거는 더 이상 중요하지 않기 때문에
$p(x_{t} | x_{t-1}, z_{1:t-1}, u_{1:t})$를 $p(x_{t} | x_{t-1}, u_{t})$로 단순화한다.

과거의 관측값을 제거하고, 유일하게 중요한 정보는 현재 상태에서 다음 상태로 어떻게 이동하는지 알려주는 $u_{t}$ 이다. 

$$bel(x_{t}) = \eta p(z_{t} | x_{t}) \int p(x_{t} | x_{t-1}, u_{t}) p(x_{t-1}|z_{1:t-1},u_{1:t})dx_{t-1}$$

 

단계 5 독립 가정 적용

남은 것은 $p(x_{t-1}|z_{1:t-1},u_{1:t})$ 인데 이는 $u_{t}$를 제외하고는 초기의 belief와 동일하다.

즉 $p(x_{t-1}|z_{1:t-1},u_{1:t-1})=bel(x_{t-1})$ 

여기서 미래의 제어 명령 $u_{t}$ 도 현재 상태를 추정하는데 도움이 되지 않는다는 가정으로
$u_{t}$ 를 제거할 수 있고 아래와 같이 단순화된다.

$$bel(x_{t}) = \eta \ p(z_{t} | x_{t}) \int p(x_{t} | x_{t-1}, u_{t}) p(x_{t-1}|z_{1:t-1},u_{1:t-1})dx_{t-1}$$

$$=  \eta \ p(z_{t} | x_{t}) \int p(x_{t} | x_{t-1}, u_{t}) bel(x_{t-1}) dx_{t-1} $$

결론

$\bar{bel}(x_{t})= \int p(x_{t} | x_{t-1}, u_{t}) bel(x_{t-1}) dx_{t-1} $ 라고 하면

$$bel(x_{t}) = \eta \ p(z_{t}|x_{t}) \cdot \int p(x_{t}|x_{t-1},u_{t}) \cdot bel(x_{t-1}) dx_{t-1} $$

$$ bel(x_{t}) = \eta \ p(z_{t}|x_{t}) \bar{bel}(x_{t}) $$

• η: 정규화 상수 (normalization constant) 
• P(zt | Xt): 관측 모델 (Observation Model) 
• P(Xt | Xt-1, ut): 운동 모델 (Motion Model) 
• P(Xt-1 | z1:t-1, u1:t-1): 시간 t-1에서의 믿음 (belief)

 

- 베이즈 필터의 two step

일반적으로 베이즈 필터는 예측과 보정 단계로 나뉘며 두 단계를 반복한다.
적분기호가 있는 부분을 예측, 두 번째 부분을 보정단계라 한다.

predition step: 

이전의 belief 값과 현재의 제어 입력을 이용해 현재의 상태를 예측한다.
예측 단계에서는 운동 모델(motion model) 을 사용하며 $x_{t-1}$ 에서 $t$ 로 어떻게 이동하는지 나타낸다.

$$\bar{bel}(x_{t})=\int p(x_{t}|u_{t},x_{t-1}) bel(x_{t-1}) dx_{t-1}$$

운동 모델: $p(x_{t}|u_{t},x_{t-1})$

 

correction step:

예측 step에서 구한 값과 실제로 관찰한 값을 바탕으로 다음 상태값을 보정한다.
보정 단계에서는 관측 모델을 사용하며 새로운 관측값을 사용해 예측에서의 오류를 수정하는 역할을 한다.

$$bel(x_{t})=\eta \ p(z_{t}|x_{t}) \bar{bel} (x_{t})$$

관측 모델: $p(z_{t}|x_{t})$

 

- 베이즈 필터 기반 필터

베이즈 필터를 구현하려면 운동 모델과 관측 모델을 구체적으로 정의해야한다. 확률 분포를 어떻게 표현할 것인지, 선형 모델을 사용할 것인지 등 다양한 디자인이 필요하다. 

베이즈 필터는 재귀적 추정을 위한 프레임워크이고 여러 가정을 통한 다양한 필터 (kalman filter, extended kalman filter, unscented kalman filter, particle filter, histogram filters 등) 가 있다. 

• kalman filter : 간단하지만 선형 시스템과 가우시안 노이즈에 대한 가정이 필요하다. 
• extended kalman filter : 비선형 시스템에 적용하기 위해 칼만 필터를 확장했지만 여전히 선형화 과정에서 오차가 발생할 수 있다. 
• unscented kalman filter : 확장 칼만 필터보다 정확도가 높지만 계산 비용이 더 많이 든다. 
• particle filter : 비선형 시스템과 비가우시안 노이즈에 강하지만 계산 비용이 매우 높다. 
• histogram filters : 이산적인 상태 공간에서 작동하며 간단하지만 정확도가 낮을 수 있다.

---

📌Kalman Filter 

칼만 필터는 시스템에서 상태를 추정하기 위한 알고리즘으로 노이즈가 있는 측정값과 입력을 바탕으로 상태를 재귀적으로 추정한다. 때문에 베이즈 필터의 특수한 형태이기도 하며 시스템 모델과 측정값이 선형이고, 노이즈가 가우시안 분포를 따른다고 가정한다. 

가우시안: 주변화와 조건화  

가우시안 분포의 특성 활용:
칼만 필터 유도에는 가우시안 분포의 주변화(marginalization) 및 조건화(conditioning) 연산이 주로 사용된다.
두 변수에 대한 확률분포가 있고 활률 분포가 가우시안 분포라면 주변 분포도 가우시안 분포이다. Xa를 주변화하면 p(Xb)도 가우시안 분포가 되고 조건부 분포도 마찬가지이다. p(Xa | Xb) - Xb 를 안다고 할 떄 Xa에 대한 확률 분포는 다시 정규 분포가되고 반대도 마찬가지이다.

선형 모델 (Linear Model) 가정:

선형 모델이란 두 모델(운동, 관측모델) 모두 f(x) = Ax+b 를 통해 표현됨을 의미한다. 핵심은 이러한 선형 함수를 통해 가우시안 분포를 변환해도 여전히 가우시안 분포라는 것이다. 칼만 필터에 이렇게 운동 모델과 관측 모델이 선형적이라는 가정을 한다.

여기에 불확실성을 더할 수 있다. 예를들어 가우시안 belief로 시작하면 불확실성이 증가할 수 있는데 이를 가우시안 노이즈를 통해 가우시안을 유지한다.  

🔷칼만 필터 모델 (Kalman Filter Model) 

- 운동 모델 (Motion Model)

$x_{t} = A_{t}x_{t-1} + B_{t}u_{t}+ \epsilon_{t}$

$A_{t}x_{t-1}$ 은 아무것도 하지 않을 때 상태가 어떻게 변하는지를 뜻하고 $B_{t}u_{t}$ 는 제어 명령에 따라 상태에 더해지는 항이다. 끝으로 가우시안 오차가 있다. 

• $\large{A_{t}}$ : 상태 전이 행렬 (State Transition Matrix).
  • $n \times n$ 행렬이며 $n$ 은 상태 벡터의 차원이다. 제어 명령이 없을 때 상태가 t-1 에서 t로 어떻게 변하는지 나타낸다.
• $\large{B_{t}}$: 제어 입력 행렬 (Control Input Matrix).
  • $n \times l$ 행렬이며 $l$ 은 제어 명령($u_{t}$)의 차원이다. 기본적으로 제어 명령이 상태에 미치는 영향을 나타낸다.
• $\large{u_{t}}$: 제어 명령 (Control Command).
• $\large{\epsilon_{t}}$: 운동 모델 노이즈 (Motion Model Noise).
  • 평균이 0인 가우시안 분포를 따른다.

- 관측 모델 (Observation Model):

$z_{t} = C_{t} x_{t}+\delta _{t}$

행렬 C는 상태공간에서 관찰 공간으로 매핑되는 행렬이다. 이를 상태 x와 곱하면 예측된 관측값이 나온다.

• $C_{t}$: 관측 행렬 (Observation Matrix).
  • k x n 행렬이며 k는 관측값 $z_{t}$의 차원이다. 상태를 관측값으로 변환하는 행렬이다. 즉 상태 x에서 무엇을 관측할 것인지, 무엇을 관측할 것으로 예상되는지 알려준다.
• $\delta_{t}$: 관측 모델 노이즈 (Observation Model Noise).
  • 평균이 0인 가우시안 분포를 따른다. 

두 모델에 전부 가우시안 노이즈를 나타내는 변수가 있는데 이는 평균이 0이고 독립인 가우시안 노이즈이다.
보통 두 개의 공분산(covariance) 행렬 $\epsilon_{t}$ 와 $\delta_{t}$ 로 특징지어진다. 

🔷선형 모델의 의미 

- 운동 모델 (Motion model)

$p(x_{t}|u_{t}, x_{t-1})$

• 이전 상태($x_{t-1}$)와 제어 명령($u_{t}$)이 주어졌을 때 현재 상태($x_{t}$)에 대한 확률 분포이다.
• 칼만 필터는 이 분포가 가우시안 분포를 따른다고 가정한다.

표준 가우시안 확률 밀도 함수를 적을 수 있다.

$$p(x_{t}|u_{t}, x_{t-1}) = \det(2\pi R_{t})^{-1/2} \exp(-\frac{1}{2} (x_{t} - (A_{t}x_{t-1} + B_{t}u_{t}))^{T}  R_{t}^{-1} (x_{t} - (A_{t}x_{t-1} + B_{t}u_{t}))$$

$R_{t}$ 는 motion noise 이다. 

Ax는 평균이고 지수 내부 표현식은 거리이다? 공분산 행렬의 역행렬로 스케일된다?

- 관측 모델 (Observation Model)

$p(z_{t} | x_{t})$: 현재 상태(Xt)가 주어졌을 때 관측값(zt)에 대한 확률 분포이다. 칼만 필터는 이 분포 역시 가우시안 분포를 따른다고 가정한다.
이전과 유사하게 가우시안 확률 밀도 함수를 가진다.

$$p(z_{t}|x_{t}) = det(2 \pi Q_{t})^{-1/2} \exp(- \frac{1}{2} (z_{t}-C_{t}x_{t})^{T}Q_{t}^{-1}(z_{t}-C_{t}x_{t}))$$

$Q_{t}$는 measurment noise 이다. 

 

🔷칼만 필터 알고리즘 (Kalman Filter Algorithm) 

현재의 belief $bel(x_{t})$ 가 예측된 belief $\bar{bel(x_{t})}$ 에 관측을 곱한 것에 비례함을 알 수 잇다.

칼만 필터는 예측 단계와 보정 단계를 반복적으로 수행하여 상태를 추정한다.

입력: 이전 상태의 평균(μt-1)과 공분산 행렬(Σt-1), 제어 명령(Ut), 현재 관측값(Zt).

출력: 현재 상태의 평균(μt)과 공분산 행렬(Σt).

 

- 예측 단계:

2. 예측된 평균 ($\bar{\mu}_{t}): \bar{\mu}_{t} = A_{t} \mu_{t-1} + B_{t} u_{t}$

• 이전 평균에 선형 모션 모델을 적용해 예측된 평균을 계산한다.

3. 예측된 공분산(covariance) 행렬 $(\bar{\Sigma}_{t}): \bar{\Sigma}_{t}  = A_{t}\Sigma_{t-1} A_{t}^{T} + R_{t}$

• 이전 공분산을 $A_{t} \Sigma_{t-1} A_{t}^{T}$ 를 통해 변환하고 $R_{t}$ 를 더하여 예측된 공분산을 계산한다.\

$x_{t}=A_{t}x_{t-1} $ 에 대해 $x$ 의 공분산이 $\Sigma$ 이고 선형변환 $y=Ax$ 를 적용하면 공분산은 다음과 같다.
$Cov(Ax)=A Cov(x) A^{T}$

때문에 $A_{t} \Sigma_{t-1} A_{t}^{T}$ 가 되고 추가적인 노이 $R_{t}$ 를 더한 것

- 보정 단계: 

4. 칼만 이득 (Kalman Gain): $K_{t} = \bar{\Sigma}_{t} C_{t}^{T} (C_{t}\bar{\Sigma}_{t} C_{t}^{T} +Q_{t})^{-1}$

• 예측된 상태 불확실성($\bar{\Sigma}_{t}$) 과 관측 불확실성($Q_{t}$)을 기반으로, 관측모델 $C_{t}$ 를 매개로 하여 얼마나 가중치를 둘지 결정한다.
• 즉 에측과 관측 중 어느쪽을 얼마나 신뢰할지 결정해주는 가중치이다.
• 칼만 이득 식이 어떻게 유도된걸까..?
  • $\bar{\Sigma}_{t}$ 는 예측 오차 공분산, $C_{t}\bar{\Sigma}_{t} C_{t}^{T} +Q_{t}$ 는 관측 오차 공분산 
  • 공분산을 최소화하도록 수학적으로 최적화된 공식?

5. 업데이트된 평균 (μt): $\mu_{t} = \bar{\mu}_{t}+ K_{t} (z_{t} - C_{t} \bar{\mu}_{t}) $

• 예측된 평균 $\bar{\mu_{t}}$에서 시작하여 보정을 한다.
• 보정은 $K_{t}(z_{t}-C_{t} \bar{\mu_{t}})$ 이며 실제 관측값과 예측된 상태기반 관측값의 차이이다.
• 칼만 이득은 이 차이를 통해 다시 상태공간으로 매핑한다.

6. 업데이트된 공분산 행렬 (Σt): $\Sigma_{t} = (I - K_{t}C_{t}) \bar{\Sigma}_{t}$

• $K_{t}$ 와 $C_{t}$ 를 사용해 예측된 공분산을 줄인다.

7. return $\mu_{t}, \Sigma_{t}$
업데이트된 $\mu_{t}, \Sigma_{t}$ 를 반환한다. 이는 시간 t에서 gaussian belief 이며 다음 시간 단계에서의 입력 역할을 한다.

 

- 1차원 칼만필터 예시

예측 단계: 붉은색 가우시안 분포 (prediction).
관측 단계: 초록색 가우시안 분포 (measurement).
보정 단계: 푸른색 가우시안 분포 (correction).

• 예측과 관측값을 결합하여 불확실성을 줄인다.
• 칼만 필터는 예측과 관측값의 불확실성을 고려하여 가중 평균을 계산한다. 

---

📌Extended Kalman Filter 

칼만필터는 시스템 모델이 선형이고 노이즈가 가우시안 분포를 따라야 사용할 수 있다. 하지만 실제 세계는 그렇지 않은 경우가 대다수이다. 따라서 비선형 시스템에서 사용하기 위해 칼만 필터를 확장한 EKF가 등장했다. 

non-linear 시스템이기에 가우시안 확률 분포를 가지는 input을 넣어도 output은 가우시안 확률 분포를 따르지 않는다. 때문에 동작점에서 Local Linearization 을 적용한다. 

동작점에서 비선형 함수를 테일러 급수로 전개하여 선형 근사를 한다. 이때 1차 항의 계수는 Jacobian matrix 로 나타난다.

🔷EKF 선형화

- 선형화 (EKF Linearization):

비선형 함수를 선형 함수로 근사하여 칼만 필터를 적용하는 것.

방법: 테일러 전개(Taylor Expansion)를 사용하여 비선형 함수를 특정 지점에서 선형화한다.
선형화 지점(linearization point)을 선택하고, 해당 지점에서 함수의 1차 도함수(jacobian matrix)를 계산한다. 원래의 비선형 함수 대신 선형화된 함수를 사용한다.

문제점: 선형화는 근사적인 방법이므로 오차가 발생할 수 있다. 

- Linearization Error에 미치는 영향

선형화 오차는 두 가지 요인에 따라 달라진다.

• 선형화 함수의 정확도: 선형화된 함수가 원래의 비선형 함수를 얼마나 잘 근사하는가. 
• 불확실성 (Uncertainty): 입력 가우시안 분포의 불확실성.
  • 입력 분포가 넓게 퍼져 있을수록(불확실성이 클수록) 선형화 오차가 커진다.
• 불확실성과 오차 관계:
  • 불확실성이 클수록 (분산이 클수록), 선형화 지점에서 멀리 떨어진 영역의 확률 밀도가 높아지므로, 선형화 오차가 커진다.
  • 불확실성이 작을수록 (분산이 작을수록), 선형화 지점 근처에 확률 밀도가 집중되므로, 선형화 오차가 작아진다.
• 함수의 형태와 오차 관계: 비선형성이 강한 함수는 선형 함수로 근사하기 어려우므로, 선형화 오차가 커진다. 

🔷선형화된 모델

선형화된 운동 모델 및 관측 모델 EKF에서는 원래의 비선형 운동 모델 및 관측 모델 대신 선형화된 모델을 사용한다. 비선형 함수를 1차 테일러 전개를 통해 국소적으로 선형화한다. 

선형화된 운동 모델

$x_{t} = g(u_{t}, x_{t-1}) ≈ g(u_{t}, \mu_{t-1}) + G_{t} (x_{t-1} - \mu_{t-1}) $

$p(x_{t}|u_{t}, x_{t-1}) \approx \det(2 \pi R_{t})^{-1/2} \exp(-\frac{1}{2} (x_{t} - g(u_{t}, \mu_{t-1}) -G_{t}(x_{t-1}-\mu_{t-1}))^{T}$
$R_{t}^{-1} (x_{t} - g(u_{t}, \mu_{t-1})- G_{t}(x_{t-1} -\mu_{t-1})) )$

마찬가지로 $R_{t}$ 는 motion noise 

선형화된 관측 모델

$z_{t} = h(x_{t}) \approx h(\bar{\mu}_{t}) + H_{t} (x_{t} - \bar{\mu}_{t}) $

$$p(z_{t}|x_{t}) = \det(2 \pi Q_{t})^{-1/2} \exp(- \frac{1}{2} (z_{t}- h(\bar{\mu}_{t}) - H_{t}(x_{t}-\bar{\mu}_{t}))^{T} Q_{t}^{-1}(z_{t}-h(\bar{\mu}_{t})-H_{t}(x_{t}-\bar{\mu}_{t})))$$

G와 H는 각각 운동 모델과 관측 모델의 jacobian 행렬. 

🔷Extended Kalman Filter Algorithm

칼만 필터 알고리즘과 매우 유사하지만, 선형화된 모델을 사용한다.

• 주요 변경 사항: 예측 단계에서 평균을 계산할 때 선형화된 운동 모델 $G(u_{t}, \mu_{t-1})$ 을 사용한다.
• 관측 단계에서 예상 관측값을 계산할 때 선형화된 관측 모델 $h(\bar{\mu}_{t})$을 사용한다.
• 칼만 이득을 계산할 때 $A_{t}$와 $C_{t}$ 대신 jacobian 행렬 $G$ 와 $H$ 를 사용한다. 

1. 확장 칼만 필터
2. 상태 예측: $\bar{\mu}_{t} = g(u_{t},\mu_{t-1})$
3. 공분산 예측: $\bar{\Sigma}_{t} = G_{t}\Sigma_{t-1} G_{t}^{T} + R_{t}$
4. 칼만 이득 계산: $K_{t} = \bar{\Sigma}_{t} H_{t}^{T} (H_{t}\bar{\Sigma}_{t} H_{t}^{T} +Q_{t})^{-1}$
5. 상태업데이트: $\mu_{t} = \bar{\mu}_{t}+ K_{t} (z_{t} - h(\bar{\mu}t))$
6. 공분산 업데이트: $\Sigma_{t} = (I - K_{t}H_{t}) \bar{\Sigma}_{t}$
7. return $\mu_{t}, \Sigma_{t}$

 

---

📌particle filter 

파티클 필터는 시스템의 상태를 추정하기위한 알고리즘으로 비선형이거나 비가우시안인 경우에 유용한 필터이다.
상태를 직접 분포로 표현하지 않고 샘플(파티클)로 확률분포를 근사한다.

예를들어 사진과 같이 빨간 점들이 시스템이 있을 수 있는 위치를 설명한다.

따라서 매개변수 형태가 아닌 비매개변수적 샘플을 사용하며, 이것이 파티클 필터의 핵심 아이디어이다. 

 

칼만필터는 상태를 추정하기 위해 가우시안 분포를 사용했다.

 

예를 들어 다음과 같은 확률분포가 있다. 이를 표현하기 위한 아이디어로 샘플로 근사한다. 

어떤 영역에서는 샘플이 밀집되어 있고 다른 영역에서는 널널하다. 
단위면적당 파티클의 수는 시스템이 그곳에 있을 확률이 얼마나 높은지를 설명한다. 
따라서 파티클은 확률밀도함수의 근사치라 할 수 있다. 

이때 샘플에 가중치를 부여해 필요한 샘플 수를 줄일 수 있다. 모든 샘플에 가중치를 부여해 가중치가 클수록 해당 영역에 더 많은 확률 질량이 위치한다고 해석하여 아래와 같은 샘플을 볼 수 있다.

아이디어는 확률 분포를 표현하기 위해 가중 샘플 집합을 사용하는 것이다.

예를 들어 로봇의 1차원 위치에 대한 확률 분포를 표현하고 싶다면 이런 샘플 집합으로 표현할 수 있다. 하지만 좋은 근사치는 아닌데 샘플이 없는 곳에서의 값은 0이기 때문이고, 샘플은 함수의 근사치임을 명심해야한다. 이제 이러한 가중 샘플로 로봇의 위치와 방향에 대한 belief 를 추정할 것이다.

샘플 집합(파티클 필터)의 모든 샘플에는 두 변수가 있다.

• 첫 번째로 state hypothesis, 샘플이 어디에 위치한다고 생각하는지 이다.
• 두 번째 변수는 가중치로 실수이며 모든 가중치의 합은 1이다.

이러한 샘플이 J개 있는 것이며 높은 가중치일수록 거기에 있을 확률이 높은 것이다.

이 샘플집합은 모든 샘플의 합으로 주어진 belief 혹은 사후 확률 posterior를 나타낸다. 즉 샘플의 가중치*위치에 중심을 둔 디랙 함수이다. 모든 샘플 위치에는 디랙 임펄스가 있고 이함수에 대해 적분하면 1일 것이다. 다만 이는 근사치이기에 좋은 확률함수를 표현하기 위해서는 샘플이 많이 필요할 수 있다. 

샘플은 어떻게 얻을 수 있을까?

사후 확률 분포를 직접 샘플링하기 어려워 importance sampling을 진행한다??
이는 실제로 샘플을 생성하고자 하는 함수와 비교해 샘플을 생성할 때 다른 분포를 사용할 수 있다는 것이다. 파란 곡선에서 샘플을 얻고 싶을 때 이를 target distribution이라한다. 이를 위해 proposal function $\pi$ 인 빨간 곡선에서 샘플을 생성한다. 
뽑은 샘플에서 목표와 proposal function을 평가해야한다. 그리고 가중치는 목표를 proposal 으로 나눈 값이다.

중요한 것은 파란 곡선이 더 커야한다는 것이다. 만약 목표가 0보다 크지만 $\pi$ 가 0이라면 해당 샘플을 생성할 수 없기 때문이다. 

 

🔷알고리즘

베이즈 필터에서는 이전 상태로부터 현재 상태를 예측하는 prediction과
현재 관측값을 반영하여 갱신하는 correction 단게로 구성됐다.
파티클 필터는 재귀적 베이즈필터의 근사적 구현방식이다.

• 모션 모델을 이용해 샘플링하고(prediction)
• 관측 모델을 사용해 각 파티클의 likelihood(관측에 대한 확률)를 계산하고 가중치를 부여한다.(correction)

1. 첫 번째 예측 단계: 샘플을 뽑아 샘플 집합을 형성한다.
   • 이전의 belief (파티클 집합)과 제어 입력을 이용해 다음에 어디있을지 추정하는 샘플을 생성한다.
   • $x_{t}^{[j]}$ : j 번째 파티클
   • proposal distribution에서 샘플링 하는 과정이다.
2. 두 번째 보정 단계: 목표 분포와 proposal 분포 사이 불일치를 보정한다.
   • 샘플이 얼마나 관측값과 잘 맞는지 평가하는 단계이다.
   • 중요도 샘플링을 사용 (목표 / 제안 = 가중치)로 계산하고 가중치를 부여한다.
   • $w_{t}^{[j]}$: j 번째 파티클의 가중치
3. 세 번째 단계: resampling 단계이다. 
   • 가중치가 큰 파티클을 더 많이 선택하여 복원추출한다.
   • 이 과정으로 새로운 샘플 집합을 만들어 모두 균등한 가중치를 가진다(가중치 정보가 빈도 정보로 변환된다??)

이를 관측값이나 제어 명령이 새로 들어올 때마다 반복한다.  

🔷표준화된 알고리즘

 

1. 입력
   • 이전 시점 t−1의 샘플 집합 (belief)
   • 현재 제어 명령 (control)
   • 현재 관측값 (observation)
2. 초기화
   • 예측 샘플을 담을 버킷 (예: 빈 리스트) 준비
   • 보정 샘플을 담을 임시 버킷 준비 ($\bar{x}$)
3. 샘플링 (예측 단계)
   • 제안 분포 $\pi$에서 샘플링
     • 제안 분포는 사용자 정의 (보통 이전 belief + 동작 모델 기반)
4. 가중치 계산 (보정 단계)
   • 각 샘플에 대해 목표 분포 / 제안 분포 계산
     • 보통 관측 모델을 이용해 가중치 계산
     • 즉, 가중치 ∝ likelihood(observation | state)
5. 임시 샘플 집합 업데이트
   • 샘플과 해당 가중치를 x̄에 추가
6. 리샘플링 단계
   • 계산된 가중치에 비례하여 샘플 J개를 선택
   • 선택된 샘플들을 균등 가중치로 새로운 샘플 집합 $x_{t}$ ​에 추가
7. 반복
   • 새로운 관측값과 제어 명령이 들어오면 1~6단계를 반복