칼만 필터(Kalman Filter) 개념 정리 (+ KF, EKF, ESKF, IEKF, IESKF) Part 1

 

본 포스트는 필자가 Kalman filter 공부하면서 배운 내용을 정리한 포스트이다.

다음 포스팅 칼만 필터(Kalman filter) 개념 정리 Part 2에서 더 많은 내용을 확인할 수 있다.
Kalman filter를 이해하는데 필요한 기반 지식은 확률 이론(Probability Theory) 개념 정리 포스팅을 참조하면 된다.
Particle filter에 대해 알고 싶으면 파티클 필터(Particle Filter) 개념 정리 포스팅을 참조하면 된다.

arXiv preprint

English version

Notes on Kalman Filter (KF, EKF, ESKF, IEKF, IESKF)

 

 

Preliminaries

칼만 필터(Kalman filter)는 시간에 따라 변하는 시스템의 상태를 추정하는 방법 중 하나로써 시스템 모델의  예측값과 노이즈가 포함된 관측값을 바탕으로 현재 상태를 재귀적으로 예측하고 업데이트하는 알고리즘을 말한다. 

해당 섹션에서는 칼만 필터를 본격적으로 설명하기 앞서 기반이 되는 지식들을 간단히 소개한다.

Estimation theory

추정 이론(estimation theory)은 관측된 데이터를 바탕으로 모델의 파라미터나 상태를 예측하는 다양한 방법을 정리한 이론이다. 데이터 분석, 신호처리, 기계학습, 금융, 로봇공학 등 다양한 분야에서 널리 쓰이고 있으며 주로 불확실성을 다루는 과정에서 정확한 결정을 내리기 위한 필수적인 도구로 사용되고 있다. 보다 자세한 내용은 해당 포스팅을 참조하면 된다.

 

예를 들어 위 그림과 같이 평균 몸무게가 70kg인 사람을 1년 동안 측정한 데이터가 주어졌다고 가정하자. 그래프를 이루고 있는 초록색 데이터들은 $x[n]$이며 데이터들의 중심값은 추정하고자 하는 파라미터 $\theta$가 된다. 그리고 $\theta$로부터 위 아래로 벌어진 정도(=분산)는 $w[n]$ 을 통해 수학적으로 표현할 수 있다.

위와 같이 관측된 데이터 $x[n]$을 바탕으로 파라미터 $\theta$를 추정하는 문제는 다음과 같은 간단한 수학적 모델로 표현할 수 있다. 
$$\begin{array}{}\text{(1)}& \begin{array}{r}x[n]=\theta +w[n]n=0,1,\cdots ,N-1\end{array}\end{array}$$
- $x[n]$: 관측된 데이터
- $\theta$: 추정하고자 하는 파라미터
- $w[n]$: 랜덤 노이즈

Bayesian philosophy

추정 이론에는 추정해야 하는 파라미터 $\theta$를 보는 관점에 따라 크게 빈도주의와 베이지안 관점으로 나뉜다. 

• Frequentist: 추정해야 하는 파라미터 $\theta$가 미지의 결정론적(deterministic) 파라미터로 보는 빈도주의적 관점
• Bayesian: 추정해야 하는 파라미터 $\theta$가 사전 확률분포(prior)를 가지는 확률 변수(random variable, r.v.)로 간주하는 관점

$$\begin{array}{}\text{(2)}& \begin{array}{rl}& \text{Frequentist: }\underset{\text{r.v.}}{\underbrace{x[n]}}=\underset{\text{deterministic}}{\underbrace{\theta }}+w[n]\\ & \text{Bayesian: }\underset{\text{r.v.}}{\underbrace{x[n]}}=\underset{\text{r.v.}}{\underbrace{\theta }}+w[n]\end{array}\end{array}$$

베이지안 철학은 만약 우리가 파라미터 $\theta$에 대한 사전(prior) 정보를 알고 있다면 이는 더 나은 추정에 활용될 수 있다는 모티브에서 출발하였으며 이를 위해서는 $\theta$에 대한 prior pdf가 미리 주어져 있거나 계산할 수 있어야 한다는 특징이 존재한다.

베이지안 철학 관점에서는 파라미터 $\theta$ 또한 확률변수로 모델링할 수 있으므로 다음과 같은 베이지안 룰이 성립한다.
$$\begin{array}{}\text{(3)}& \begin{array}{rl}p(\theta |x)& =\frac{p(x|\theta )p(\theta )}{p(x)}\end{array}\end{array}$$
- $p(\theta |x)$: 관측된 데이터 $x$가 주어졌을 때 파라미터 $\theta$의 사후 조건부 확률 분포(posterior)
- $p(x|\theta )$:  $\theta$가 주어졌을 때 $x$의 조건부 확률 분포, 또는 가능도(likelihood)

- $p(\theta )$: $\theta$ 사전 확률 분포(prior)
- $p(x)$: $x$의 확률 분포. 추정하고자 하는 파라미터 $\theta$와 무관하므로 일반적으로 고려되지 않는다. 분모에 곱해져서 전체 확률의 크기를 1로 정규화해주는 값으로 $\eta$로 치환되기도 한다.

Estimation problem

추정 문제는 일반적으로 다음과 같이 도식화하여 나타낼 수 있다. 

추정 문제의 목적은 주어진 데이터 $(t_i,{\mathbf{x}}_i)$를 사용하여 최적의 모델 $\mathit{\theta }(t)$를 찾는 것이다. 이 때, 어느 시점을 추정하느냐에 따라 다른 추정 문제가 된다.

• Filtering : 필터링은 관측 데이터 $\{x[0],x[1],\cdots ,x[N-1]\}$가 주어졌을 때 $\theta =x[N-1]$을 추정하는 문제를 말한다. 최적의 파라미터를 추정함으로써 우리는 신호에서 노이즈를 필터링하고자 한다. 필터링에서는 파라미터가 현재와 과거 데이터에만 의존하는 것에 유의하자.}
• Smoothing: 스무딩은 관측 데이터 $\{x[0],x[1],\cdots ,x[N-1]\}$이 주어졌을 때 중간에 있는 $\theta =s[n]$을 추정하는 경우를 말한다. 예를 들어 $s[1]$을 추정하기 위해 모든 관측 데이터가 사용된다. 당연하게도 스무딩은 모든 데이터가 관측되기 전에는 수행할 수 없다.
• Prediction: 예측은 관측 데이터 $\{x[0],x[1],\cdots ,x[N-1]\}$가 주어졌을 때 $\theta =x[N-1+l]$을 추정하는 경우를 말한다. 이 때, $l$은 임의의 양수이다.
• Interpolation: 보간은 관측 데이터 $\{x[0],\cdots ,x[n-1],x[n+1,\cdots ,x[N-1]\}$이 주어졌을 떄 $\theta =x[n]$을 추정하는 경우를 말한다.
  
  

Dynamic system

시간에 따라 상태 변수가 변하는 시스템은 다음과 같이 모델링할 수 있다.
$$\begin{array}{}\text{(4)}& \begin{array}{rlr}& \text{Motion Model: }& {\mathbf{x}}_t=\mathbf{f}({\mathbf{x}}_{t-1},{\mathbf{u}}_t)+{\mathbf{w}}_t\\ & \text{Observation Model: }& {\mathbf{z}}_t=\mathbf{h}({\mathbf{x}}_t)+{\mathbf{v}}_t\end{array}\end{array}$$
- ${\mathbf{x}}_t$: $t$ 시점에서 모델의 상태 변수
- ${\mathbf{u}}_t$: $t$ 시점에서 모델의 제어 입력
- ${\mathbf{z}}_t$: $t$ 시점에서 관측값
- $\mathbf{f}({\mathbf{x}}_{t-1},{\mathbf{u}}_t)$: 이전 상태 ${\mathbf{x}}_{t-1}$와 현재 제어 입력 ${\mathbf{u}}_t$으로부터 현재 상태 ${\mathbf{x}}_t$를 예측하는 모션 모델 함수
- $\mathbf{h}({\mathbf{x}}_t)$: 현재 상태 변수 ${\mathbf{x}}_t$를 관측값 ${\mathbf{z}}_t$으로 변환해주는 관측 모델 함수
- ${\mathbf{w}}_t$: $t$ 시점에서 모션 모델의 노이즈
- ${\mathbf{v}}_t$: $t$ 시점에서 관측 모델의 노이즈

위 표기법을 사용하여 앞서 추정 문제를 다시 그려보면 다음과 같다.

위와 같은 동적 시스템을 그래프로 그려보면 다음과 같다.

동적 시스템은 일반적으로 상태 예측(prediction) 단계와 업데이트(update, 또는 correction) 단계로 나뉜다. 예측 단계는 다음과 같이 현재까지 측정한 모든 관측값 ${\mathbf{z}}_{0:t}$과 현재 상태 ${\mathbf{x}}_t$를 통해 다음 상태 ${\mathbf{x}}_{t+1}$를 예측하는 것을 말한다.

업데이트 단계는 다음과 같이 다음 상태 ${\mathbf{x}}_{t+1}$로부터 새로운 관측값 ${\mathbf{z}}_{t+1}$을 얻는 과정을 말한다.

 

Recursive bayes filter

제어입력과 관측값이 주어졌을 때 현재 상태 ${\mathbf{x}}_t$의 믿을만한 정도, 또는 ${\mathbf{x}}_t$에 대한 Belief $\text{bel}({\mathbf{x}}_t)$은 같이 정의한다.
$$\begin{array}{}\text{(5)}& \begin{array}{r}\text{bel}({\mathbf{x}}_t)=p({\mathbf{x}}_t|{\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})\end{array}\end{array}$$
- ${\mathbf{x}}_t$ : t 시간에서 상태 변수
- ${\mathbf{z}}_{1:t}=\{{\mathbf{z}}_1,\cdots ,{\mathbf{z}}_t\}$ : 1~t 시간에서 관측값
- ${\mathbf{u}}_{1:t}=\{{\mathbf{u}}_1,\cdots ,{\mathbf{u}}_t\}$ : 1~t 시간에서 제어입력
- $\text{bel}({\mathbf{x}}_t)$ : Belief of ${\mathbf{x}}_t$라고 불리며 시작 시간부터 $t$초까지 센서를 통한 관측 ${\mathbf{z}}_{1:t}$과 제어입력 ${\mathbf{u}}_{1:t}$으로 인해 현재 로봇이 ${\mathbf{x}}_t$에 위치할 확률(믿을만한 정도)을 의미

 

이전 섹션에서 언급한 그래프로 설명하면 다음 부분이 $\text{bel}({\mathbf{x}}_t)$에 해당한다.

 
$\text{bel}(\cdot )$은 bayesian rule에 따라 표현되고 전개되기 때문에 bayes filter라고도 불린다. 위 식을 markov assumption과 bayesian rule을 사용하여 전개하면 재귀적인 필터를 유도할 수 있고 이를 recursive bayes filter라고 한다.
$$\begin{array}{}\text{(6)}& \begin{array}{rl}& \text{bel}({\mathbf{x}}_t)=\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\overline{\text{bel}}({\mathbf{x}}_t)\\ & \overline{\text{bel}}({\mathbf{x}}_t)=\int p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\text{bel}({\mathbf{x}}_{t-1})d{\mathbf{x}}_{t-1}\end{array}\end{array}$$
- $\eta =1/p({\mathbf{z}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})$ : 확률분포의 넓이를 1로 정규화하여 확률분포의 정의를 유지시켜주는 값
- $p({\mathbf{z}}_t|{\mathbf{x}}_t)$ : 관측 모델(observation model)
- $\int p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)d{\mathbf{x}}_{t-1}$ : 모션 모델(motion model)

Recursive bayes filter는 위와 같이 이전 스텝의 $\text{bel}({\mathbf{x}}_{t-1})$로부터 현재 스텝의 $\text{bel}({\mathbf{x}}_t)$를 구할 수 있으므로 재귀 필터라고 부른다.  
 

Derivation of recursive bayes filter

Recursive bayes filter의 수식은 다음과 같이 유도된다.
$$\begin{array}{}\text{(7)}& \begin{array}{rl}\text{bel}({\mathbf{x}}_t)& =p({\mathbf{x}}_t|{\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t,{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t-1})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\text{bel}({\mathbf{x}}_{t-1})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot \overline{\text{bel}}({\mathbf{x}}_t)\end{array}\end{array}$$
 
Step 1: 
$$\begin{array}{}\text{(8)}& \begin{array}{rl}\text{bel}({\mathbf{x}}_t)& =p({\mathbf{x}}_t|{\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t,{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\end{array}\end{array}$$
Bayesian rule을 적용한다.  $p(x|y)=\frac{p(y|x)p(x)}{p(y)}$
 
Step 2:
$$\begin{array}{}\text{(9)}& \begin{array}{rl}\text{bel}({\mathbf{x}}_t)& =p({\mathbf{x}}_t|{\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t,{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\end{array}\end{array}$$ 

현재 상태는 바로 이전 상태에만 의존성을 지니는 Markov Assumption을 적용한다.
 
Step 3:
$$\begin{array}{}\text{(10)}& \begin{array}{rl}\text{bel}({\mathbf{x}}_t)& =p({\mathbf{x}}_t|{\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t,{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})d{\mathbf{x}}_{t-1}\end{array}\end{array}$$
 
총합의 법칙(Law of total probability) 또는 Marginalization를 적용한다. $p(x)={\int }_{y}p(x|y)\cdot p(y)dy$
 
Step 4:
$$\begin{array}{}\text{(11)}& \begin{array}{rl}\text{bel}({\mathbf{x}}_t)& =p({\mathbf{x}}_t|{\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t,{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})d{\mathbf{x}}_{t-1}\end{array}\end{array}$$
현재 상태는 바로 이전 상태에만 의존성을 지니는 Markov Assumption을 적용한다.
 
Step 5:
$$\begin{array}{}\text{(12)}& \begin{array}{rl}\text{bel}({\mathbf{x}}_t)& =p({\mathbf{x}}_t|{\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t,{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t-1})d{\mathbf{x}}_{t-1}\end{array}\end{array}$$
$t$ 시점에서 제어 입력 ${\mathbf{u}}_t$는 $t-1$ 시점에서 상태 변수 ${\mathbf{x}}_{t-1}$에 영향을 주지 않으므로 생략한다.

 
Step 6:
$$\begin{array}{}\text{(13)}& \begin{array}{rl}\text{bel}({\mathbf{x}}_t)& =p({\mathbf{x}}_t|{\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t,{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t-1})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\cdot \text{bel}({\mathbf{x}}_{t-1})d{\mathbf{x}}_{t-1}\end{array}\end{array}$$

 
$p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t-1})$는 $\text{bel}({\mathbf{x}}_{t-1})$의 정의와 동일하므로 치환한다.
 
Step 7:
$$\begin{array}{}\text{(14)}& \begin{array}{rl}\text{bel}({\mathbf{x}}_t)& =p({\mathbf{x}}_t|{\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t,{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot p({\mathbf{x}}_t|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\cdot p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t-1})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot {\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\cdot \text{bel}({\mathbf{x}}_{t-1})d{\mathbf{x}}_{t-1}\\ & =\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\cdot \overline{\text{bel}}({\mathbf{x}}_t)\end{array}\end{array}$$
 
${\int }_{{\mathbf{x}}_{t-1}}p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\cdot \text{bel}({\mathbf{x}}_{t-1})d{\mathbf{x}}_{t-1}$를 $\overline{\text{bel}}({\mathbf{x}}_t)$로 치환한다.
 

Gaussian belief case 

 $\text{bel}({\mathbf{x}}_t)$가 가우시안 분포를 따르는 경우 이를 특별히 칼만 필터(kalman filter)라고 한다.
$$\begin{array}{}\text{(15)}& \begin{array}{rl}& \overline{\text{bel}}({\mathbf{x}}_t)\sim \mathcal{N}({\hat{\mathbf{x}}}_{t|t-1},{\mathbf{P}}_{t|t-1})\text{(Kalman Filter Prediction)}\\ & \text{bel}({\mathbf{x}}_t)\sim \mathcal{N}({\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t})\text{(Kalman Filter Correction)}\end{array}\end{array}$$

평균과 분산은 $(\hat{\mathbf{x}}$, $\mathbf{P})$ 또는 $(\hat{\mathit{\mu }},\mathbf{\Sigma })$로 표현하기도 한다. 이는 표기법만 다를 뿐 동일한 의미를 지닌다. 

Kalman filter (KF)

NOMENCLATURE of kalman filter

•  스칼라는 일반 소문자로 표기한다 e.g., a
• 벡터는 굵은 소문자로 표기한다 e.g., $\mathbf{a}$
• 행렬은 굵은(bold) 대문자로 표기한다 e.g., $\mathbf{R}$
• prediction: $\overline{\text{bel}}({\mathbf{x}}_t)\sim \mathcal{N}({\hat{\mathbf{x}}}_{t|t-1},{\mathbf{P}}_{t|t-1})$
  
  • ${\hat{\mathbf{x}}}_{t|t-1}$ : $t-1$ 스텝의 correction 값이 주어졌을 때 $t$ 스텝의 평균. 일부 문헌은 ${\mathbf{x}}_t^{-}$로도 표기함.
  • ${\hat{\mathbf{P}}}_{t|t-1}$ : $t-1$ 스텝의 correction 값이 주어졌을 때 $t$ 스텝의 공분산. 일부 문헌은 ${\mathbf{P}}_t^{-}$로도 표기함.
• correction: $\text{bel}({\mathbf{x}}_t)\sim \mathcal{N}({\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t})$
  
  • ${\hat{\mathbf{x}}}_{t|t}$ : $t$ 스텝의 prediction 값이 주어졌을 때 $t$ 스텝의 평균. 일부 문헌은 ${\mathbf{x}}_t^{+}$로도 표기함.
  • ${\hat{\mathbf{P}}}_{t|t}$ : $t$ 스텝의 prediction 값이 주어졌을 때 $t$ 스텝의 공분산. 일부 문헌은 ${\mathbf{P}}_t^{+}$로도 표기함.

 

시간 $t$에 로봇의 위치를 ${\mathbf{x}}_t$, 로봇의 센서로 부터 관측한 값을 ${\mathbf{z}}_t$, 로봇의 제어입력을 ${\mathbf{u}}_t$라고 하면 이를 통해 모션 모델(motion model)과 관측 모델(observation model)을 정의할 수 있다. 이 때, 모션 모델과 관측 모델은 선형이어야(linear model) 한다는 제약조건이 있다. 모션 모델과 관측 모델은 다음과 같다.
$$\begin{array}{}\text{(16)}& \begin{array}{rlr}& \text{Motion Model: }& {\mathbf{x}}_t={\mathbf{F}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+{\mathbf{w}}_t\\ & \text{Observation Model: }& {\mathbf{z}}_t={\mathbf{H}}_t{\mathbf{x}}_t+{\mathbf{v}}_t\end{array}\end{array}$$
- ${\mathbf{x}}_t$: 모델의 상태 변수(state variable)
- ${\mathbf{u}}_t$: 모델의 입력(input)
- ${\mathbf{z}}_t$: 모델의 관측값(measurement)
- ${\mathbf{F}}_t$: 모델의 상태천이(state transition) 행렬
- ${\mathbf{B}}_t$: 모델에 입력 ${\mathbf{u}}_t$가 주어졌을 때 ${\mathbf{u}}_t$를 상태 변수로 변환해주는 행렬
- ${\mathbf{H}}_t$: 모델의 관측(observation) 행렬
- ${\mathbf{w}}_t\sim \mathcal{N}(0,{\mathbf{Q}}_t)$: 모션 모델의 노이즈. ${\mathbf{Q}}_t$는 ${\mathbf{w}}_t$의 공분산 행렬을 의미한다.
- ${\mathbf{v}}_t\sim \mathcal{N}(0,{\mathbf{R}}_t)$: 관측 모델의 노이즈. ${\mathbf{R}}_t$는 ${\mathbf{v}}_t$의 공분산 행렬을 의미한다.


확률변수가 모두 가우시안 분포를 따른다고 가정하면 $p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t),p({\mathbf{z}}_t|{\mathbf{x}}_t)$는 다음과 같이 나타낼 수 있다. 
$$\begin{array}{}\text{(17)}& \begin{array}{rlrl}& p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)& & \sim \mathcal{N}({\mathbf{F}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t,{\mathbf{Q}}_t)\\ & & & =\frac{1}{\sqrt{\text{det}(2\pi {\mathbf{Q}}_t)}}\exp (-\frac{1}{2}({\mathbf{x}}_t-{\mathbf{F}}_t{\mathbf{x}}_{t-1}-{\mathbf{B}}_t{\mathbf{u}}_t{)}^{⊺}{\mathbf{Q}}_t^{-1}({\mathbf{x}}_t-{\mathbf{F}}_t{\mathbf{x}}_{t-1}-{\mathbf{B}}_t{\mathbf{u}}_t))\end{array}\end{array}$$
$$\begin{array}{}\text{(18)}& \begin{array}{rlrl}& p({\mathbf{z}}_t|{\mathbf{x}}_t)& & \sim \mathcal{N}({\mathbf{H}}_t{\mathbf{x}}_t,{\mathbf{R}}_t)\\ & & & =\frac{1}{\sqrt{\text{det}(2\pi {\mathbf{R}}_t)}}\exp (-\frac{1}{2}({\mathbf{z}}_t-{\mathbf{H}}_t{\mathbf{x}}_t{)}^{⊺}{\mathbf{R}}_t^{-1}({\mathbf{z}}_t-{\mathbf{H}}_t{\mathbf{x}}_t))\end{array}\end{array}$$

다음으로 칼만 필터를 통해 구해야 하는 $\overline{\text{bel}}({\mathbf{x}}_t),\text{bel}({\mathbf{x}}_t)$은 아래와 같이 나타낼 수 있다.
$$\begin{array}{}\text{(19)}& \begin{array}{rl}& \overline{\text{bel}}({\mathbf{x}}_t)=\int p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\text{bel}({\mathbf{x}}_{t-1})d{\mathbf{x}}_{t-1}\sim \mathcal{N}({\hat{\mathbf{x}}}_{t|t-1},{\mathbf{P}}_{t|t-1})\\ & \text{bel}({\mathbf{x}}_t)=\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\overline{\text{bel}}({\mathbf{x}}_t)\sim \mathcal{N}({\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t})\end{array}\end{array}$$

 

칼만 필터는 prediction 스텝에서 이전 스텝의 posterior 값과 모션 모델을 사용하여 예측값 $\overline{\text{bel}}({\mathbf{x}}_t)$을 먼저 구한 후 correction 스텝에서 예측값과 관측 모델을 사용하여 보정된 값 $\text{bel}({\mathbf{x}}_t)$을 구하는 방식으로 동작한다. ($\text{17}$), ($\text{18}$)를 ($\text{19}$)에 대입하여 정리하면 prediction, correction 스텝의 평균과 공분산 $({\hat{\mathbf{x}}}_{t|t-1},{\mathbf{P}}_{t|t-1}),({\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t})$을 각각 구할 수 있다. 자세한 유도 과정은 "Derivation of Kalman filter" 섹션을 참조하면 된다

 

초기값 $\text{bel}({\mathbf{x}}_0)$은 다음과 같이 주어진다.
$$\begin{array}{}\text{(20)}& \begin{array}{r}\text{bel}({\mathbf{x}}_0)\sim \mathcal{N}({\hat{\mathbf{x}}}_0,{\mathbf{P}}_0)\end{array}\end{array}$$
- ${\hat{\mathbf{x}}}_0$: 일반적으로 0으로 설정한다
- ${\mathbf{P}}_0$ : 일반적으로 작은 값(<1e-2)으로 설정한다.

Prediction step

Prediction은 $\overline{\text{bel}}({\mathbf{x}}_t)$를 구하는 과정을 말한다. $\overline{\text{bel}}({\mathbf{x}}_t)$는 가우시안 분포를 따르므로 평균 ${\hat{\mathbf{x}}}_{t|t-1}$과 분산 ${\mathbf{P}}_{t|t-1}$을 각각 구해보면 아래와 같이 구할 수 있다.
$$\begin{array}{}\text{(21)}& \boxed{{\displaystyle \begin{array}{rl}& {\hat{\mathbf{x}}}_{t|t-1}={\mathbf{F}}_t{\hat{\mathbf{x}}}_{t-1|t-1}+{\mathbf{B}}_t{\mathbf{u}}_t\\ & {\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^{⊺}+{\mathbf{Q}}_t\end{array}}}\end{array}$$

Correction step

Correction은 $\text{bel}({\mathbf{x}}_t)$를 구하는 과정을 말한다. $\text{bel}({\mathbf{x}}_t)$ 또한 가우시안 분포를 따르므로 평균 ${\hat{\mathbf{x}}}_{t|t}$과 분산 ${\mathbf{P}}_{t|t}$을 각각 구해보면 다음과 같다. 이 때, ${\mathbf{K}}_t$는 칼만 게인(kalman gain)을 의미한다.
$$\begin{array}{}\text{(22)}& \boxed{{\displaystyle \begin{array}{rl}& {\mathbf{K}}_t={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}+{\mathbf{R}}_t{)}^{-1}\\ & {\hat{\mathbf{x}}}_{t|t}={\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{K}}_t({\mathbf{z}}_t-{\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1})\\ & {\mathbf{P}}_{t|t}=(\mathbf{I}-{\mathbf{K}}_t{\mathbf{H}}_t){\mathbf{P}}_{t|t-1}\end{array}}}\end{array}$$

1D Kalman filter

지금까지 설명한 칼만 필터는 상태 변수가 벡터인(=${\mathbf{x}}_t$) 경우에 대한 내용이었다. 다음으로 상태 변수가 스칼라인(=$x_t$) 1D 칼만 필터를 살펴보자. 1D 칼만 필터는 기존 nD 칼만 필터와 모든 내용이 동일하지만 행렬이 아닌 스칼라 값을 통해 식이 구성된다는 점이 다르다. 1D 칼만 필터 버전으로 나타낸 Belief는 다음과 같다.
$$\begin{array}{}\text{(23)}& \begin{array}{rl}& \overline{\text{bel}}(x_t)\sim \mathcal{N}({\overline{\mu }}_t,{\overline{\sigma }}_t^2)\\ & \text{bel}(x_t)\sim \mathcal{N}({\mu }_t,{\sigma }_t^2)\end{array}\end{array}$$
- ${\overline{\mu }}_t$:  prediction 스텝의 평균 
- ${\overline{\sigma }}_t^2$:  prediction 스텝의 분산 
- ${\mu }_t$:  correction 스텝의 평균 
- ${\sigma }_t^2$:  correction 스텝의 분산 

모션 모델과 관측 모델은 다음과 같다.
$$\begin{array}{}\text{(24)}& \begin{array}{rlr}& \text{Motion Model: }& x_t=x_{t-1}+u_t+{\sigma }_{\text{motion},t}^2\\ & \text{Observation Model: }& z_t=x_t+{\sigma }_{\text{obs},t}^2\end{array}\end{array}$$
- $u_t$:  모션 모델의 제어 입력 
- ${\sigma }_{\text{motion},t}^2$: 모션 모델의 노이즈
- ${\sigma }_{\text{obs},t}^2$: 관측 모델의 노이즈

1D 칼만 필터의 precition 스텝은 다음과 같다.
$$\begin{array}{}\text{(25)}& \boxed{{\displaystyle \begin{array}{rl}& {\overline{\mu }}_t={\mu }_{t-1}+u_t\\ & {\overline{\sigma }}_t^2={\sigma }_{t-1}^2+{\sigma }_{\text{motion},t}^2\end{array}}}\end{array}$$

다음으로 1D 칼만 필터의 correction 스텝은 다음과 같다.
$$\begin{array}{}\text{(26)}& \boxed{{\displaystyle \begin{array}{rl}& K_t=\frac{{\overline{\sigma }}_t^2}{{\overline{\sigma }}_t^2+{\sigma }_{\text{obs},t}^2}\\ & {\mu }_t={\overline{\mu }}_t+K_t({\mu }_{\text{obs,t}}-{\overline{\mu }}_t)=\frac{{\mu }_{\text{obs,t}}{\overline{\sigma }}_t^2+{\overline{\mu }}_t{\sigma }_{\text{obs,t}}^2}{{\overline{\sigma }}_t^2+{\sigma }_{\text{obs},t}^2}\\ & {\sigma }_t^2=(1-K_t){\overline{\sigma }}_t^2=\frac{{\overline{\sigma }}_t^2{\sigma }_{\text{obs},t}^2}{{\overline{\sigma }}_t^2+{\sigma }_{\text{obs},t}^2}\end{array}}}\end{array}$$
- ${\mu }_{\text{obs,t}}$: 관측 모델의 평균 
- ${\sigma }_{\text{obs},t}^2$: 관측 모델의 노이즈 

식을 자세히 살펴보면 앞서 언급했던 벡터 버전 칼만 필터와 구조가 동일한 것을 확인할 수 있다. ${\mathbf{F}}_t,{\mathbf{B}}_t,{\mathbf{H}}_t$는 $1$이 되어 생략되고 ${\mathbf{Q}}_t\leftrightarrow {\sigma }_{\text{motion},t}$와 대응하며 ${\mathbf{R}}_t\leftrightarrow {\sigma }_{\text{obs},t}$와 대응하는 것을 알 수 있다. 보다 쉽게 한눈에 비교하기 위해 그림을 첨부하였다.

 

Discussion

Discussion about KF and posterior pdf

다음과 같이 간단한 1차원에서 로봇의 위치 $x_t$를 KF로 추정하는 문제가 주어졌다고 하자. 세로축은 확률밀도함수(probability density function, pdf), 가로축은 1차원 위치 $x$라고 하고 $t$ 시점의 로봇의 위치를 상태 변수 $x_t$라고 하자. 

 

위 그림을 단계 별로 자세히 살펴보면 다음과 같다.

1. 로봇은 이전 스텝의 위치 $x_{t-1}$로부터 모션 모델에 의해 prediction을 수행하여 prior $\overline{\text{bel}}(x_t)$를 예측한다.
2. 다음으로 로봇의 센서로부터 현재의 위치를 측정하여 likelihood $p(z_t|x_t)$를 얻는다.
3. prior $\overline{\text{bel}}(x_t)$와 likelihood $p(z_t|x_t)$로부터 correction 스텝을 수행하여 posterior $\text{bel}(x_t)$를 얻는다.
4. 1$\sim$3 과정을 다음 스텝에 대해 반복하여 $x_{t+1}$에 대한 posterior $\text{bel}(x_{t+1})$를 얻는다. 

 

따라서 KF의 한 스텝은 이전 상태 변수로부터 prior를 예측(=prediction)하고 관측값이 주어지면 이를 통해 likelihood를 구한 후 bayesian rule에 따라 posterior pdf를 구하는(=correction) 전형적인 Bayesian 필터의 특성을 지님을 알 수 있다. Bayesian 필터링을 가우시안 분포를 따르는 Belief에 대하여 재귀적으로 수행하는 것이 칼만 필터이다. 

 

Discussion about Kalman gain

Correction 스텝을 자세히 살펴 보면  평균 ${\hat{\mathbf{x}}}_{t|t}$을 구할 때 칼만 게인 ${\mathbf{K}}_t$에 따라 가중치가 다르게 더해진다.
$$\begin{array}{}\text{(27)}& \begin{array}{r}{\hat{\mathbf{x}}}_{t|t}=\underset{\text{prediction}}{\underbrace{{\hat{\mathbf{x}}}_{t|t-1}}}+{\mathbf{K}}_t\underset{\text{innovation}}{\underbrace{({\mathbf{z}}_t-{\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1})}}\end{array}\end{array}$$
- innovation: 관측값(${\mathbf{z}}_t$)와 예측값(${\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1}$)의 차이, 즉 에러(error)값 

${\mathbf{K}}_t\to \mathbf{0}$이면 센서의 관측값 ${\mathbf{z}}_t$를 전혀 신뢰하지 않고 오직 시스템의 예측값만을 반영하겠다는 것을 의미한다(관측값 ${\mathbf{z}}_t$ 반영 안함). 반면에 ${\mathbf{K}}_t\to \mathbf{1}$이면 센서의 관측값 ${\mathbf{z}}_t$를 100% 신뢰하여 센서 관측값만을 반영하겠다는 것을 의미한다(예측값 ${\hat{\mathbf{x}}}_{t|t-1}$ 반영 안함).

또한, 센서 노이즈 $\mathbf{R}$이 큰 경우 ${\mathbf{K}}_t$는 감소하게 되고 이는 센서의 관측값보다 시스템 모델의 예측값을 더 반영하겠다는 것을 의미한다. 반면에 시스템 노이즈 $\mathbf{Q}$가 큰 경우 ${\mathbf{P}}_{t|t-1}$이 증가하여 ${\mathbf{K}}_t$ 또한 증가하게 되고 이는 곧 시스템 모델의 예측값보다 관측값을 더 반영하겠다는 것을 의미한다.
$$\begin{array}{}\text{(28)}& \begin{array}{rl}& {\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^{⊺}+{\mathbf{Q}}_t\\ & {\mathbf{K}}_t={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}+{\mathbf{R}}_t{)}^{-1}\end{array}\end{array}$$
- ${\mathbf{R}}_t(\uparrow )\Rightarrow (\ast +{\mathbf{R}}_t{)}^{-1}(\downarrow )\Rightarrow {\mathbf{K}}_t(\downarrow )$  
- ${\mathbf{Q}}_t(\uparrow )\Rightarrow {\mathbf{P}}_{t|t-1}(\uparrow )\Rightarrow {\mathbf{K}}_t(\uparrow )$  

 

 

Summary

칼만 필터를 함수로 표현하면 다음과 같다. 
$$\begin{array}{}\text{(29)}& \boxed{{\displaystyle \begin{array}{rl}& \text{KalmanFilter}({\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{P}}_{t-1|t-1},{\mathbf{u}}_t,{\mathbf{z}}_t)\{\\ & \text{(Prediction Step)}\\ & {\hat{\mathbf{x}}}_{t|t-1}={\mathbf{F}}_t{\hat{\mathbf{x}}}_{t-1|t-1}+{\mathbf{B}}_t{\mathbf{u}}_t\\ & {\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^{⊺}+{\mathbf{Q}}_t\\ & \\ & \text{(Correction Step)}\\ & {\mathbf{K}}_t={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}+{\mathbf{R}}_t{)}^{-1}\\ & {\hat{\mathbf{x}}}_{t|t}={\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{K}}_t({\mathbf{z}}_t-{\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1})\\ & {\mathbf{P}}_{t|t}=(\mathbf{I}-{\mathbf{K}}_t{\mathbf{H}}_t){\mathbf{P}}_{t|t-1}\\ & \text{return}{\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t}\\ & \}\end{array}}}\end{array}$$
 

Extended kalman filter (EKF)

칼만 필터는 모션 모델과 관측 모델이 선형이라는 가정 하에 상태를 추정한다. 하지만 현실세계의 대부분의 현상들은 비선형으로 모델링되므로 앞서 정의한 칼만 필터를 그대로 적용하면 정상적으로 동작하지 않는다. 비선형의 모션 모델과 관측 모델에서도 칼만 필터를 사용하기 위해 확장칼만필터(extended kalman filter, EKF)가 제안되었다. EKF는 테일러 1차 근사(talyor 1st approximation)을 사용하여 비선형 모델을 선형 모델로 근사한 후 칼만 필터를 적용하는 방법을 사용한다. EKF에서 모션 모델과 관측 모델은 다음과 같다.
$$\begin{array}{}\text{(30)}& \begin{array}{rlr}& \text{Motion Model: }& {\mathbf{x}}_t=\mathbf{f}({\mathbf{x}}_{t-1},{\mathbf{u}}_t,{\mathbf{w}}_t)\\ & \text{Observation Model: }& {\mathbf{z}}_t=\mathbf{h}({\mathbf{x}}_t,{\mathbf{v}}_t)\end{array}\end{array}$$
- ${\mathbf{x}}_t$: 모델의 상태 변수(state variable)
- ${\mathbf{u}}_t$: 모델의 입력(input)
- ${\mathbf{z}}_t$: 모델의 관측값(measurement)
- $\mathbf{f}(\cdot )$: 비선형 모션(motion) 모델 함수 
- $\mathbf{h}(\cdot )$: 비선형 관측(observation) 모델 함수
- ${\mathbf{w}}_t\sim \mathcal{N}(0,{\mathbf{Q}}_t)$: 모션 모델의 노이즈. ${\mathbf{Q}}_t$는 ${\mathbf{w}}_t$의 공분산 행렬을 의미
- ${\mathbf{v}}_t\sim \mathcal{N}(0,{\mathbf{R}}_t)$: 관측 모델의 노이즈. ${\mathbf{R}}_t$는 ${\mathbf{v}}_t$의 공분산 행렬을 의미
 
위 식에서 $\mathbf{f}(\cdot )$은 비선형 모션 모델을 의미하고 $\mathbf{h}(\cdot )$은 비선형 관측 모델을 의미한다. $\mathbf{f}(\cdot ),\mathbf{h}(\cdot )$에 각각 1차 테일러 근사를 수행하면 다음과 같다.
$$\begin{array}{}\text{(31)}& \begin{array}{rl}& {\mathbf{x}}_t\approx \mathbf{f}({\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{u}}_t,0)+{\mathbf{F}}_t({\mathbf{x}}_{t-1}-{\hat{\mathbf{x}}}_{t-1|t-1})+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_t\\ & {\mathbf{z}}_t\approx \mathbf{h}({\hat{\mathbf{x}}}_{t|t-1},0)+{\mathbf{H}}_t({\mathbf{x}}_t-{\hat{\mathbf{x}}}_{t|t-1})+{\mathbf{H}}_{\mathbf{v}}{\mathbf{v}}_t\end{array}\end{array}$$

이 때, ${\mathbf{F}}_t$는 ${\hat{\mathbf{x}}}_{t-1|t-1}$에서 계산한 모션 모델의 자코비안 행렬을 의미하며 ${\mathbf{H}}_t$는 ${\hat{\mathbf{x}}}_{t|t-1}$에서 계산한 관측 모델의 자코비안 행렬을 의미한다. 그리고 ${\mathbf{F}}_{\mathbf{w}},{\mathbf{H}}_{\mathbf{v}}$는 각각 ${\mathbf{w}}_t=0,{\mathbf{v}}_t=0$에서 노이즈에 대한 자코비안 행렬을 의미한다. 자코비안에 대한 자세한 내용은 해당 포스트 를 참조하면된다.
$$\begin{array}{}\text{(32)}& \begin{array}{rlr}& {\mathbf{F}}_t=\frac{\partial \mathbf{f}}{\partial {\mathbf{x}}_{t-1}}{|}_{{\mathbf{x}}_{t-1}={\hat{\mathbf{x}}}_{t-1|t-1}}& {\mathbf{F}}_{\mathbf{w}}=\frac{\partial \mathbf{f}}{\partial {\mathbf{w}}_t}{|}_{\begin{array}{cc}& {\mathbf{x}}_{t-1}={\hat{\mathbf{x}}}_{t-1|t-1}\\ & {\mathbf{w}}_t=0\end{array}}\end{array}\end{array}$$
$$\begin{array}{}\text{(33)}& \begin{array}{rlr}& {\mathbf{H}}_t=\frac{\partial \mathbf{h}}{\partial {\mathbf{x}}_t}{|}_{{\mathbf{x}}_t={\hat{\mathbf{x}}}_{t|t-1}}& {\mathbf{H}}_{\mathbf{v}}=\frac{\partial \mathbf{h}}{\partial {\mathbf{v}}_t}{|}_{\begin{array}{cc}& {\mathbf{x}}_t={\hat{\mathbf{x}}}_{t|t-1}\\ & {\mathbf{v}}_t=0\end{array}}\end{array}\end{array}$$
 
($\text{31})$ 식을 전개하면 다음과 같다.
$$\begin{array}{}\text{(34)}& \boxed{{\displaystyle \begin{array}{rl}{\mathbf{x}}_t& ={\mathbf{F}}_t{\mathbf{x}}_{t-1}+\mathbf{f}({\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{u}}_t,0)-{\mathbf{F}}_t{\hat{\mathbf{x}}}_{t-1|t-1}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_t\\ & ={\mathbf{F}}_t{\mathbf{x}}_{t-1}+{\tilde{\mathbf{u}}}_t+{\tilde{\mathbf{w}}}_t\end{array}}}\end{array}$$
- ${\tilde{\mathbf{u}}}_t=\mathbf{f}({\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{u}}_t,0)-{\mathbf{F}}_t{\hat{\mathbf{x}}}_{t-1|t-1}$
- ${\tilde{\mathbf{w}}}_t={\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_t\sim \mathcal{N}(0,{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺})$

$$\begin{array}{}\text{(35)}& \boxed{{\displaystyle \begin{array}{rl}{\mathbf{z}}_t& ={\mathbf{H}}_t{\mathbf{x}}_t+\mathbf{h}({\hat{\mathbf{x}}}_{t|t-1},0)-{\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{H}}_{\mathbf{v}}{\mathbf{v}}_t\\ & ={\mathbf{H}}_t{\mathbf{x}}_t+{\tilde{\mathbf{z}}}_t+{\tilde{\mathbf{v}}}_t\end{array}}}\end{array}$$
- ${\tilde{\mathbf{z}}}_t=\mathbf{h}({\hat{\mathbf{x}}}_{t|t-1},0)-{\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1}$
- ${\tilde{\mathbf{v}}}_t={\mathbf{H}}_{\mathbf{v}}{\mathbf{v}}_t\sim \mathcal{N}(0,{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺})$
 

확률변수가 모두 가우시안 분포를 따른다고 가정하면 $p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t),p({\mathbf{z}}_t|{\mathbf{x}}_t)$는 다음과 같이 나타낼 수 있다. 
$$\begin{array}{}\text{(36)}& \begin{array}{rlrl}& p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)& & \sim \mathcal{N}({\mathbf{F}}_t{\mathbf{x}}_{t-1}+{\tilde{\mathbf{u}}}_t,{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺})\\ & & & =\frac{1}{\sqrt{\text{det}(2\pi {\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺})}}\exp (-\frac{1}{2}({\mathbf{x}}_t-{\mathbf{F}}_t{\mathbf{x}}_{t-1}-{\tilde{\mathbf{u}}}_t{)}^{⊺}({\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺}{)}^{-1}({\mathbf{x}}_t-{\mathbf{F}}_t{\mathbf{x}}_{t-1}-{\tilde{\mathbf{u}}}_t))\end{array}\end{array}$$
$$\begin{array}{}\text{(37)}& \begin{array}{rlrl}& p({\mathbf{z}}_t|{\mathbf{x}}_t)& & \sim \mathcal{N}({\mathbf{H}}_t{\mathbf{x}}_t+{\tilde{\mathbf{z}}}_t,{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺})\\ & & & =\frac{1}{\sqrt{\text{det}(2\pi {\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺})}}\exp (-\frac{1}{2}({\mathbf{z}}_t-{\mathbf{H}}_t{\mathbf{x}}_t-{\tilde{\mathbf{z}}}_t{)}^{⊺}({\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺}{)}^{-1}({\mathbf{z}}_t-{\mathbf{H}}_t{\mathbf{x}}_t-{\tilde{\mathbf{z}}}_t))\end{array}\end{array}$$

${\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺}$는 선형화된 모션 모델의 노이즈를 의미하며 ${\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺}$은 선형화된 관측 모델의 노이즈를 의미한다. 다음으로 칼만 필터를 통해 구해야 하는 $\overline{\text{bel}}({\mathbf{x}}_t),\text{bel}({\mathbf{x}}_t)$은 아래와 같이 나타낼 수 있다.
$$\begin{array}{}\text{(38)}& \begin{array}{rl}& \overline{\text{bel}}({\mathbf{x}}_t)=\int p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)\text{bel}({\mathbf{x}}_{t-1})d{\mathbf{x}}_{t-1}\sim \mathcal{N}({\hat{\mathbf{x}}}_{t|t-1},{\mathbf{P}}_{t|t-1})\\ & \text{bel}({\mathbf{x}}_t)=\eta \cdot p({\mathbf{z}}_t|{\mathbf{x}}_t)\overline{\text{bel}}({\mathbf{x}}_t)\sim \mathcal{N}({\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t})\end{array}\end{array}$$

EKF 또한 KF와 동일하게 prediction에서 이전 스텝의 값과 모션 모델을 사용하여 예측값 $\overline{\text{bel}}({\mathbf{x}}_t)$을 먼저 구한 후 correction에서 관측값과 관측 모델을 사용하여 보정된 값 $\text{bel}({\mathbf{x}}_t)$을 구하는 방식으로 동작한다. $p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)$, $p({\mathbf{z}}_t|{\mathbf{x}}_t)$를 위 식에 대입하여 정리하면 prediction, correction 스텝의 평균과 공분산 $({\hat{\mathbf{x}}}_{t|t-1},{\mathbf{P}}_{t|t-1}),({\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t})$을 각각 구할 수 있다. 자세한 유도 과정은"Derivation of Kalman filter"섹션을 참조하면 된다

 

초기값 $\text{bel}({\mathbf{x}}_0)$은 다음과 같이 주어진다.
$$\begin{array}{}\text{(39)}& \begin{array}{r}\text{bel}({\mathbf{x}}_0)\sim \mathcal{N}({\hat{\mathbf{x}}}_0,{\mathbf{P}}_0)\end{array}\end{array}$$
- ${\hat{\mathbf{x}}}_0$: 일반적으로 0으로 설정한다
- ${\mathbf{P}}_0$ : 일반적으로 작은 값(<1e-2)으로 설정한다.
 

Prediction step

Prediction은 $\overline{\text{bel}}({\mathbf{x}}_t)$를 구하는 과정을 말한다. 공분산 행렬을 구할 때 선형화된 자코비안 행렬 ${\mathbf{F}}_t$가 사용된다. 
$$\begin{array}{}\text{(40)}& \boxed{{\displaystyle \begin{array}{rl}& {\hat{\mathbf{x}}}_{t|t-1}=\mathbf{f}({\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{u}}_t,0)\\ & {\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^{⊺}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺}\end{array}}}\end{array}$$

Correction step

Correction은 $\text{bel}({\mathbf{x}}_t)$를 구하는 과정을 말한다. 칼만 게인과 공분산 행렬을 구할 때 선형화된 자코비안 행렬 ${\mathbf{H}}_t$가 사용된다.
$$\begin{array}{}\text{(41)}& \boxed{{\displaystyle \begin{array}{rl}& {\mathbf{K}}_t={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}+{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺}{)}^{-1}\\ & {\hat{\mathbf{x}}}_{t|t}={\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{K}}_t({\mathbf{z}}_t-\mathbf{h}({\hat{\mathbf{x}}}_{t|t-1},0))\\ & {\mathbf{P}}_{t|t}=(\mathbf{I}-{\mathbf{K}}_t{\mathbf{H}}_t){\mathbf{P}}_{t|t-1}\end{array}}}\end{array}$$
 

Summary

확장칼만필터를 함수로 표현하면 다음과 같다. 
$$\begin{array}{}\text{(42)}& \boxed{{\displaystyle \begin{array}{rl}& \text{ExtendedKalmanFilter}({\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{P}}_{t-1|t-1},{\mathbf{u}}_t,{\mathbf{z}}_t)\{\\ & \text{(Prediction Step)}\\ & {\hat{\mathbf{x}}}_{t|t-1}={\mathbf{f}}_t({\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{u}}_t,0)\\ & {\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^{⊺}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺}\\ & \\ & \text{(Correction Step)}\\ & {\mathbf{K}}_t={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}+{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺}{)}^{-1}\\ & {\hat{\mathbf{x}}}_{t|t}={\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{K}}_t({\mathbf{z}}_t-{\mathbf{h}}_t({\hat{\mathbf{x}}}_{t|t-1},0))\\ & {\mathbf{P}}_{t|t}=(\mathbf{I}-{\mathbf{K}}_t{\mathbf{H}}_t){\mathbf{P}}_{t|t-1}\\ & \text{return}{\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t}\\ & \}\end{array}}}\end{array}$$
 

Error-state kalman filter (ESKF)

NOMENCLATURE of error-state kalman filter

• prediction: $\overline{\text{bel}}(\delta {\mathbf{x}}_t)\sim \mathcal{N}(\delta {\hat{\mathbf{x}}}_{t|t-1},{\mathbf{P}}_{t|t-1})$     
  • $\delta {\hat{\mathbf{x}}}_{t|t-1}$ : $t-1$ 스텝의 correction 값이 주어졌을 때 $t$ 스텝의 평균. 일부 문헌은 $\delta {\mathbf{x}}_t^{-}$로도 표기함.
  • ${\hat{\mathbf{P}}}_{t|t-1}$ : $t-1$ 스텝의 correction 값이 주어졌을 때 $t$ 스텝의 공분산. 일부 문헌은 ${\mathbf{P}}_t^{-}$로도 표기함.
• correction: $\text{bel}(\delta {\mathbf{x}}_t)\sim \mathcal{N}(\delta {\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t})$  
  • $\delta {\hat{\mathbf{x}}}_{t|t}$ : $t$ 스텝의 prediction 값이 주어졌을 때 $t$ 스텝의 평균. 일부 문헌은 $\delta {\mathbf{x}}_t^{+}$로도 표기함.
  • ${\hat{\mathbf{P}}}_{t|t}$ : $t$ 스텝의 prediction 값이 주어졌을 때 $t$ 스텝의 공분산. 일부 문헌은 ${\mathbf{P}}_t^{+}$로도 표기함.

에러상태 칼만필터(error-state kalman filter, ESKF)는 기존의 상태 변수 ${\mathbf{x}}_t$의 평균과 분산을 추정하는 EKF와 달리 에러 상태 변수 $\delta {\mathbf{x}}_t$의 평균과 분산을 추정하는 칼만 필터 알고리즘을 말한다. 상태변수를 바로 추정하지 않고 에러상태를 통해 추정하기 때문에 indirect kalman filter라고도 불린다. 또다른 이름으로는 error-state extended kalman filter(ES-EKF)로도 불린다. ESKF는 기존 상태변수룰 true 상태 변수라고 부르며 이를 다음과 같이 명목(nominal) 상태와 에러(error) 상태의 합으로 표현한다.
$$\begin{array}{}\text{(43)}& \begin{array}{r}{\mathbf{x}}_{\text{true},t}={\mathbf{x}}_t+\delta {\mathbf{x}}_t\end{array}\end{array}$$
- ${\mathbf{x}}_{\text{true},t}$: 기존 KF, EKF에서 업데이트된 $t$ 스텝의 true 상태변수
- ${\mathbf{x}}_t$: $t$ 스텝의 명목(nominal) 상태 변수. 에러가 없는 상태를 의미한다
- $\delta {\mathbf{x}}_t$: $t$ 스텝의 에러(error) 상태 변수


위 식을 해석하면 실제(true) 추정하고자 하는 상태 변수 ${\mathbf{x}}_{\text{true},t}$는 에러가 없는 명목(nominal) 상태 ${\mathbf{x}}_t$과 모델 및 센서 노이즈로 부터 발생하는 에러 상태 $\delta {\mathbf{x}}_t$의 합으로 나타낼 수 있다는 의미이다. 이 때, nominal 상태는 (상대적으로) 큰 값을 가지며 비선형성을 가진다. 반면에 에러 상태는 0 근처의 작은 값을 가지고 선형성을 가진다.
 
기존의 EKF는 비선형성이 큰 true (nominal + error) 상태 변수를 선형화하여 필터링하기 때문에 속도가 느리고 시간이 지날수록 에러 값이 누적되는 반면에, ESKF는 에러 상태만을 선형화하여 필터링하기 때문에 속도 및 정확성이 더욱 빠른 장점이 있다. 기존 EKF와 비교했을 때 ESKF이 가지는 장점들을 정리하면 다음과 같다(Madyastha el tal., 2011):

• 방향(orientation)에 대한 에러 상태 표현법이 최소한의 파라미터를 가진다. 즉, 자유도만큼의 최소 파라미터를 가지기 때문에 over-parameterized로 인해 발생하는 특이점(signularity) 같은 현상이 발생하지 않는다.
• 에러 상태 시스템은 항상 원점(origin) 근처에서만 동작하기 때문에 선형화하기 용이하다. 따라서 짐벌락 같은 파라미터 특이점 현상이 발생하지 않으며 항상 선형화를 수행할 수 있다.
• 에러 상태는 일반적으로 값이 작기 때문에 2차항 이상의 값들은 무시할 수 있다. 이는 자코비안 연산을 쉽고 빠르게 수행할 수 있도록 도와준다. 몇몇 자코비안은 상수화하여 사용하기도 한다.

다만 ESKF는 prediction 속도는 빠르지만 nominal 상태에서 일반적으로 비선형을 가진 큰 값들이 처리되므로 nominal 상태가 처리되는 correction 스텝의 속도가 느린 편이다[2]. 
 

ESKF의 모션 모델과 관측 모델은 다음과 같다.
$$\begin{array}{}\text{(44)}& \begin{array}{rlr}& \text{Error-state Motion Model: }& {\mathbf{x}}_t+\delta {\mathbf{x}}_t=\mathbf{f}({\mathbf{x}}_{t-1},\delta {\mathbf{x}}_{t-1},{\mathbf{u}}_t,{\mathbf{w}}_t)\\ & \text{Error-state Observation Model: }& {\mathbf{z}}_t=\mathbf{h}({\mathbf{x}}_t,\delta {\mathbf{x}}_t,{\mathbf{v}}_t)\end{array}\end{array}$$
- ${\mathbf{x}}_t$: 모델의 nominal 상태 변수
- $\delta {\mathbf{x}}_t$: 모델의 에러 상태 변수
- ${\mathbf{u}}_t$: 모델의 입력(input)
- ${\mathbf{z}}_t$: 모델의 관측값(measurement)
- $\mathbf{f}(\cdot )$: 비선형 모션(motion) 모델 함수
- $\mathbf{h}(\cdot )$: 비선형 관측(observation) 모델 함수
- ${\mathbf{w}}_t\sim \mathcal{N}(0,{\mathbf{Q}}_t)$: 에러 상태 모델의 노이즈. ${\mathbf{Q}}_t$는 ${\mathbf{w}}_t$의 공분산 행렬을 의미
- ${\mathbf{v}}_t\sim \mathcal{N}(0,{\mathbf{R}}_t)$: 관측 모델의 노이즈. ${\mathbf{R}}_t$는 ${\mathbf{v}}_t$의 공분산 행렬을 의미
 
$\mathbf{f}(\cdot ),\mathbf{h}(\cdot )$에 각각 1차 테일러 근사를 수행하면 다음과 같다. 해당 전개는 [5]를 참고하여 작성하였다. 
$$\begin{array}{}\text{(45)}& \begin{array}{rl}& {\mathbf{x}}_t+\delta {\mathbf{x}}_t\approx \mathbf{f}({\mathbf{x}}_{t-1|t-1},\delta {\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{u}}_t,0)+{\mathbf{F}}_t(\delta {\mathbf{x}}_{t-1}-\delta {\hat{\mathbf{x}}}_{t-1|t-1})+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_t\\ & {\mathbf{z}}_t\approx \mathbf{h}({\mathbf{x}}_{t|t-1},\delta {\hat{\mathbf{x}}}_{t|t-1},0)+{\mathbf{H}}_t(\delta {\mathbf{x}}_t-\delta {\hat{\mathbf{x}}}_{t|t-1})+{\mathbf{H}}_{\mathbf{v}}{\mathbf{v}}_t\end{array}\end{array}$$

이 때, 두 자코비안 ${\mathbf{F}}_t,{\mathbf{H}}_t$ 모두 true 상태 ${\mathbf{x}}_{\text{true},t}$가 아닌 에러 상태 $\delta {\mathbf{x}}_t$에 대한 자코비안임에 유의한다. 해당 자코비안 부분이 EKF와 가장 다른 부분이다
$$\begin{array}{}\text{(46)}& \boxed{{\displaystyle \begin{array}{rlr}& {\mathbf{F}}_t=\frac{\partial \mathbf{f}}{\partial \delta {\mathbf{x}}_{t-1}}{|}_{\begin{array}{c}\delta {\mathbf{x}}_{t-1}=\delta {\hat{\mathbf{x}}}_{t-1|t-1}\end{array}}& {\mathbf{F}}_{\mathbf{w}}=\frac{\partial \mathbf{f}}{\partial {\mathbf{w}}_t}{|}_{\begin{array}{cc}& \delta {\mathbf{x}}_{t-1}=\delta {\hat{\mathbf{x}}}_{t-1|t-1}\\ & {\mathbf{w}}_t=0\end{array}}\end{array}}}\end{array}$$
$$\begin{array}{}\text{(47)}& \boxed{{\displaystyle \begin{array}{rlr}& {\mathbf{H}}_t=\frac{\partial \mathbf{h}}{\partial \delta {\mathbf{x}}_t}{|}_{\delta {\mathbf{x}}_t=\delta {\hat{\mathbf{x}}}_{t|t-1}}& {\mathbf{H}}_{\mathbf{v}}=\frac{\partial \mathbf{h}}{\partial {\mathbf{v}}_t}{|}_{\begin{array}{cc}& \delta {\mathbf{x}}_t=\delta {\hat{\mathbf{x}}}_{t|t-1}\\ & {\mathbf{v}}_t=0\end{array}}\end{array}}}\end{array}$$

${\mathbf{H}}_t$는 다음과 같이 연쇄법칙을 통해 표현할 수 있다.
$$\begin{array}{}\text{(48)}& \begin{array}{r}{\mathbf{H}}_t=\frac{\partial \mathbf{h}}{\partial \delta {\mathbf{x}}_t}=\frac{\partial \mathbf{h}}{\partial {\mathbf{x}}_{\text{true},t}}\frac{\partial {\mathbf{x}}_{\text{true},t}}{\partial \delta {\mathbf{x}}_t}\end{array}\end{array}$$

이 중 앞 부분 $\frac{\partial \mathbf{h}}{\partial {\mathbf{x}}_{\text{true},t}}$은 EKF에서 구한 자코비안과 동일하지만 에러 상태 변수에 대한 자코비안 $\frac{\partial {\mathbf{x}}_{\text{true},t}}{\partial \delta {\mathbf{x}}_t}$이 추가되었다. 이에 대한 자세한 내용은 Quaternion kinematics for the error-state Kalman filter 내용 정리 포스트를 참고하면 된다.
 
($\text{45})$ 식을 전개하면 다음과 같다.
$$\begin{array}{}\text{(49)}& \begin{array}{rl}{\mathbf{x}}_t+\delta {\mathbf{x}}_t& ={\mathbf{F}}_t\delta {\mathbf{x}}_{t-1}+\mathbf{f}({\mathbf{x}}_{t-1|t-1},\delta {\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{u}}_t,0)-{\mathbf{F}}_t\delta {\hat{\mathbf{x}}}_{t-1|t-1}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_t\end{array}\end{array}$$
 
이전 correction 스텝에서 항상 $\delta {\hat{\mathbf{x}}}_{t-1|t-1}=0$으로 초기화되므로 관련된 항에 0을 대입한다.
$$\begin{array}{}\text{(50)}& \begin{array}{rl}{\mathbf{x}}_t+\delta {\mathbf{x}}_t& ={\mathbf{F}}_t\delta {\mathbf{x}}_{t-1}+\mathbf{f}({\mathbf{x}}_{t-1|t-1},0,{\mathbf{u}}_t,0)+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_t\end{array}\end{array}$$
 
nominal 상태 변수 ${\mathbf{x}}_t$는 정의 상 노이즈가 없는 $\mathbf{f}({\mathbf{x}}_{t-1},0,{\mathbf{u}}_t,0)$과 동일하므로 서로 소거된다.
$$\begin{array}{}\text{(51)}& \boxed{{\displaystyle \begin{array}{rl}\delta {\mathbf{x}}_t& ={\mathbf{F}}_t\delta {\mathbf{x}}_{t-1}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_t\\ & ={\mathbf{F}}_t\delta {\mathbf{x}}_{t-1}+{\tilde{\mathbf{w}}}_t\\ & =0+{\tilde{\mathbf{w}}}_t\end{array}}}\end{array}$$
- ${\tilde{\mathbf{w}}}_t={\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_t\sim \mathcal{N}(0,{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺})$

관측 모델 함수도 동일하게 에러 상태 변수의 값을 0으로 치환하면 아래와 같이 전개된다.
$$\begin{array}{}\text{(52)}& \boxed{{\displaystyle \begin{array}{rl}{\mathbf{z}}_t& ={\mathbf{H}}_t\delta {\mathbf{x}}_t+\mathbf{h}({\mathbf{x}}_{t|t-1},\delta {\hat{\mathbf{x}}}_{t|t-1},0)-{\mathbf{H}}_t\delta {\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{H}}_{\mathbf{v}}{\mathbf{v}}_t\\ & =\mathbf{h}({\mathbf{x}}_{t|t-1},0,0)+{\mathbf{H}}_{\mathbf{v}}{\mathbf{v}}_t\\ & ={\tilde{\mathbf{z}}}_t+{\tilde{\mathbf{v}}}_t\end{array}}}\end{array}$$
- ${\tilde{\mathbf{z}}}_t=\mathbf{h}({\mathbf{x}}_{t|t-1},0,0)$
- ${\tilde{\mathbf{v}}}_t={\mathbf{H}}_{\mathbf{v}}{\mathbf{v}}_t\sim \mathcal{N}(0,{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺})$


확률 변수가 모두 가우시안 분포를 따른다고 가정하면 $p(\delta {\mathbf{x}}_t|\delta {\mathbf{x}}_{t-1},{\mathbf{u}}_t),p({\mathbf{z}}_t|\delta {\mathbf{x}}_t)$는 다음과 같이 나타낼 수 있다. 
$$\begin{array}{}\text{(53)}& \begin{array}{rlrl}& p(\delta {\mathbf{x}}_t|\delta {\mathbf{x}}_{t-1},{\mathbf{u}}_t)& & \sim \mathcal{N}(0,{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺})\\ & & & =\frac{1}{\sqrt{\text{det}(2\pi {\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺})}}\exp (-\frac{1}{2}(\delta {\mathbf{x}}_t{)}^{⊺}({\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺}{)}^{-1}(\delta {\mathbf{x}}_t))\end{array}\end{array}$$
$$\begin{array}{}\text{(54)}& \begin{array}{rlrl}& p({\mathbf{z}}_t|\delta {\mathbf{x}}_t)& & \sim \mathcal{N}({\tilde{\mathbf{z}}}_t,{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺})\\ & & & =\frac{1}{\sqrt{\text{det}(2\pi {\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺})}}\exp (-\frac{1}{2}({\mathbf{z}}_t-{\tilde{\mathbf{z}}}_t{)}^{⊺}({\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺}{)}^{-1}({\mathbf{z}}_t-{\tilde{\mathbf{z}}}_t))\end{array}\end{array}$$

${\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺}$는 선형화된 에러 상태 모션 모델의 노이즈를 의미하며 ${\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺}$은 선형화된 관측 모델의 노이즈를 의미한다. 다음으로 칼만 필터를 통해 구해야 하는 $\overline{\text{bel}}(\delta {\mathbf{x}}_t),\text{bel}(\delta {\mathbf{x}}_t)$은 아래와 같이 나타낼 수 있다.
$$\begin{array}{}\text{(55)}& \begin{array}{rl}& \overline{\text{bel}}(\delta {\mathbf{x}}_t)=\int p(\delta {\mathbf{x}}_t|\delta {\mathbf{x}}_{t-1},{\mathbf{u}}_t)\text{bel}(\delta {\mathbf{x}}_{t-1})d{\mathbf{x}}_{t-1}\sim \mathcal{N}(\delta {\hat{\mathbf{x}}}_{t|t-1},{\mathbf{P}}_{t|t-1})\\ & \text{bel}(\delta {\mathbf{x}}_t)=\eta \cdot p({\mathbf{z}}_t|\delta {\mathbf{x}}_t)\overline{\text{bel}}(\delta {\mathbf{x}}_t)\sim \mathcal{N}(\delta {\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t})\end{array}\end{array}$$

ESKF 또한 EKF와 동일하게 prediction에서 이전 스텝의 값과 모션 모델을 사용하여 예측값 $\overline{\text{bel}}(\delta {\mathbf{x}}_t)$을 먼저 구한 후 correction에서 관측값과 관측 모델을 사용하여 보정된 값 $\text{bel}(\delta {\mathbf{x}}_t)$을 구하는 방식으로 동작한다. $p(\delta {\mathbf{x}}_t|\delta {\mathbf{x}}_{t-1},{\mathbf{u}}_t)$, $p({\mathbf{z}}_t|\delta {\mathbf{x}}_t)$를 위 식에 대입하여 정리하면 prediction, correction 스텝의 평균과 공분산 $(\delta {\hat{\mathbf{x}}}_{t|t-1},{\mathbf{P}}_{t|t-1}),(\delta {\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t})$을 각각 구할 수 있다. 자세한 유도 과정은"Derivation of Kalman filter"섹션을 참조하면 된다.

 

초기값 $\text{bel}(\delta {\mathbf{x}}_0)$은 다음과 같이 주어진다.
$$\begin{array}{}\text{(56)}& \begin{array}{r}\text{bel}(\delta {\mathbf{x}}_0)\sim \mathcal{N}(0,{\mathbf{P}}_0)\end{array}\end{array}$$
- $\delta {\hat{\mathbf{x}}}_0=0$ : 항상 0의 값을 가진다
- ${\mathbf{P}}_0$ : 일반적으로 작은 값(<1e-2)으로 설정한다.
 

Prediction step

Prediction은 $\overline{\text{bel}}(\delta {\mathbf{x}}_t)$를 구하는 과정을 말한다. 공분산 행렬을 구할 때 선형화된 자코비안 행렬 ${\mathbf{F}}_t$가 사용된다. 
$$\begin{array}{}\text{(57)}& \boxed{{\displaystyle \begin{array}{rl}& \delta {\hat{\mathbf{x}}}_{t|t-1}={\mathbf{F}}_t\delta {\hat{\mathbf{x}}}_{t-1|t-1}=0\leftarrow \text{Always 0}\\ & {\hat{\mathbf{x}}}_{t|t-1}=\mathbf{f}({\hat{\mathbf{x}}}_{t-1|t-1},0,{\mathbf{u}}_t,0)\\ & {\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^{⊺}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺}\end{array}}}\end{array}$$

위 식에서 ${\mathbf{F}}_t$는 에러 상태에 대한 자코비안임에 유의한다. 에러 상태 변수 $\delta \hat{\mathbf{x}}$는 매 correction 스텝이 끝나면 0으로 초기화된다. 여기에 선형 자코비안 ${\mathbf{F}}_t$를 곱해도 값은 0이 되기 때문에 prediction 스텝에서 $\delta \hat{\mathbf{x}}$ 값은 항상 0이 된다. 따라서 에러 상태 $\delta {\hat{\mathbf{x}}}_{t|t-1}$는 prediction 스텝에서는 변하지 않고 nominal 상태 ${\hat{\mathbf{x}}}_{t|t-1}$와 에러 상태의 공분산 ${\mathbf{P}}_{t|t-1}$만 업데이트된다.

Correction step

Correction은 $\text{bel}(\delta {\mathbf{x}}_t)$를 구하는 과정을 말한다. 칼만 게인과 공분산 행렬을 구할 때 선형화된 자코비안 행렬 ${\mathbf{H}}_t$가 사용된다.
$$\begin{array}{}\text{(58)}& \boxed{{\displaystyle \begin{array}{rl}& {\mathbf{K}}_t={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}+{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}{)}^{-1}\\ & \delta {\hat{\mathbf{x}}}_{t|t}={\mathbf{K}}_t({\mathbf{z}}_t-\mathbf{h}({\hat{\mathbf{x}}}_{t|t-1},0,0))\\ & {\hat{\mathbf{x}}}_{t|t}={\hat{\mathbf{x}}}_{t|t-1}+\delta {\hat{\mathbf{x}}}_{t|t}\\ & {\mathbf{P}}_{t|t}=(\mathbf{I}-{\mathbf{K}}_t{\mathbf{H}}_t){\mathbf{P}}_{t|t-1}\\ & \text{reset }\delta {\hat{\mathbf{x}}}_{t|t}\\ & \delta {\hat{\mathbf{x}}}_{t|t}\leftarrow 0\\ & {\mathbf{P}}_{t|t}\leftarrow \mathbf{G}{\mathbf{P}}_{t|t}{\mathbf{G}}^{⊺}\end{array}}}\end{array}$$

위 식에서 ${\mathbf{H}}_t$는 관측 모델에 대한 true 상태 ${\mathbf{x}}_{\text{true},t}$의 자코비안이 아닌 에러 상태 $\delta {\mathbf{x}}_t$의 자코비안임에 유의한다.  ${\mathbf{P}}_{t|t-1},{\mathbf{K}}_t$ 또한 EKF와 기호만 같을 뿐 실제 값은 다름에 유의한다. 즉, 전체적인 공식은 EKF와 동일하지만 $\mathbf{F},\mathbf{H},\mathbf{P},\mathbf{K}$ 행렬이 에러 상태 $\delta {\mathbf{x}}_t$에 대한 값을 의미하는 점이 다르다. 

Reset

nominal 상태가 정상적으로 업데이트되면 다음으로 에러 상태의 값을 0으로 리셋해야 한다. 리셋을 하는 이유는 새로운 nominal 상태에 대한 새로운 에러(new error)를 표현해야 하기 때문이다. 리셋으로 인해 에러 상태의 공분산 ${\mathbf{P}}_{t|t}$이 업데이트된다.

리셋 함수를 $\mathbf{g}(\cdot )$라고 하면 이는 다음과 같이 나타낼 수 있다. 이에 대한 자세한 내용은 Quaternion kinematics for the error-state Kalman filter 내용 정리 포스트의 챕터 6 내용을 참고하면 된다. 
 $$\begin{array}{}\text{(59)}& \begin{array}{rl}& \delta \mathbf{x}\leftarrow \mathbf{g}(\delta \mathbf{x})=\delta \mathbf{x}-\delta {\hat{\mathbf{x}}}_{t|t-1}\end{array}\end{array}$$

ESKF의 리셋 과정은 다음과 같다.
$$\begin{array}{}\text{(60)}& \begin{array}{rl}& \delta {\hat{\mathbf{x}}}_{t|t}\leftarrow 0\\ & {\mathbf{P}}_{t|t}\leftarrow \mathbf{G}{\mathbf{P}}_{t|t}{\mathbf{G}}^{⊺}\end{array}\end{array}$$

$\mathbf{G}$는 다음과 같이 정의된 리셋에 대한 자코비안을 의미한다.
 $$\begin{array}{}\text{(61)}& \begin{array}{r}\mathbf{G}={\frac{\partial \mathbf{g}}{\partial \delta \mathbf{x}}|}_{\delta {\mathbf{x}}_t=\delta {\hat{\mathbf{x}}}_{t|t}}\end{array}\end{array}$$
 

Summary

ESKF를 함수로 표현하면 다음과 같다.
$$\begin{array}{}\text{(62)}& \boxed{{\displaystyle \begin{array}{rl}& \text{ErrorStateKalmanFilter}({\hat{\mathbf{x}}}_{t-1|t-1},\delta {\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{P}}_{t-1|t-1},{\mathbf{u}}_t,{\mathbf{z}}_t)\{\\ & \text{(Prediction Step)}\\ & \delta {\hat{\mathbf{x}}}_{t|t-1}={\mathbf{F}}_t{\hat{\mathbf{x}}}_{t-1|t-1}=0\leftarrow \text{Always 0}\\ & {\hat{\mathbf{x}}}_{t|t-1}=\mathbf{f}({\hat{\mathbf{x}}}_{t-1|t-1},0,{\mathbf{u}}_t,0)\\ & {\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^{⊺}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺}\\ & \\ & \text{(Correction Step)}\\ & {\mathbf{K}}_t={\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}({\mathbf{H}}_t{\mathbf{P}}_{t|t-1}{\mathbf{H}}_t^{⊺}+{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺}{)}^{-1}\\ & \delta {\hat{\mathbf{x}}}_{t|t}={\mathbf{K}}_t({\mathbf{z}}_t-{\mathbf{h}}_t({\hat{\mathbf{x}}}_{t|t-1},0,0))\\ & {\hat{\mathbf{x}}}_{t|t}={\hat{\mathbf{x}}}_{t|t-1}+\delta {\hat{\mathbf{x}}}_{t|t}\\ & {\mathbf{P}}_{t|t}=(\mathbf{I}-{\mathbf{K}}_t{\mathbf{H}}_t){\mathbf{P}}_{t|t-1}\\ & \text{reset }\delta {\hat{\mathbf{x}}}_{t|t}\\ & \delta {\hat{\mathbf{x}}}_{t|t}\leftarrow 0\\ & {\mathbf{P}}_{t|t}\leftarrow \mathbf{G}{\mathbf{P}}_{t|t}{\mathbf{P}}^{⊺}\\ & \text{return}\delta {\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t}\\ & \}\end{array}}}\end{array}$$

Iterated extended kalman filter (IEKF)

Iterated extended kalman filter (IEKF)는 EKF에서 correction 스텝 부분을 반복적으로 수행하는 알고리즘이다. EKF는 비선형 함수를 선형화하여 상태 변수를 추정하기 때문에 선형화 과정에서 필연적으로 오차가 발생할 수 밖에 없다. IEKF는 이러한 선형화 오차를 줄이기 위해서 correction 스텝이 종료된 후 업데이트 변화량 $\delta {\hat{\mathbf{x}}}_{t|t,j}$이 충분히 크다면 다시 선형화를 수행하여 반복적으로 correction 스텝을 진행한다. 

 

이 때, $\delta \hat{\mathbf{x}}$는 문맥 상 에러 상태 변수가 아닌 업데이트 변화량으로 해석된다. 즉 ${\hat{\mathbf{x}}}_{t|t,j+1}\leftarrow {\hat{\mathbf{x}}}_{t|t,j}+\delta {\hat{\mathbf{x}}}_{t|t,j}$와 같이 j번째 posterior 값에 더해져서 j+1 번째 posterior 값을 업데이트하는 용도로만 사용된다. 다시 말하면, 상태 추정의 대상이 아니다.

 

Compare to EKF

Commonality 1

IEKF에서 모션 모델과 관측 모델은 다음과 같다. 이는 EKF와 완전히 동일하다.

$$\begin{array}{}\text{(63)}& \begin{array}{rlr}& \text{Motion Model: }& {\mathbf{x}}_t=\mathbf{f}({\mathbf{x}}_{t-1},{\mathbf{u}}_t,{\mathbf{w}}_t)\\ & \text{Observation Model: }& {\mathbf{z}}_t=\mathbf{h}({\mathbf{x}}_t,{\mathbf{v}}_t)\end{array}\end{array}$$
- ${\mathbf{x}}_t$: 모델의 상태 변수(state variable)
- ${\mathbf{u}}_t$: 모델의 입력(input)
- ${\mathbf{z}}_t$: 모델의 관측값(measurement)
- $\mathbf{f}(\cdot )$: 비선형 모션(motion) 모델 함수 
- $\mathbf{h}(\cdot )$: 비선형 관측(observation) 모델 함수
- ${\mathbf{w}}_t\sim \mathcal{N}(0,{\mathbf{Q}}_t)$: 모션 모델의 노이즈. ${\mathbf{Q}}_t$는 ${\mathbf{w}}_t$의 공분산 행렬을 의미
- ${\mathbf{v}}_t\sim \mathcal{N}(0,{\mathbf{R}}_t)$: 관측 모델의 노이즈. ${\mathbf{R}}_t$는 ${\mathbf{v}}_t$의 공분산 행렬을 의미

 

Commonality 2

다음으로 선형화 과정도 EKF의 ($\text{34}$), ($\text{35}$)와 완전히 동일하다. 

$$\begin{array}{}\text{(64)}& \boxed{{\displaystyle \begin{array}{rl}{\mathbf{x}}_t& ={\mathbf{F}}_t{\mathbf{x}}_{t-1}+\mathbf{f}({\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{u}}_t,0)-{\mathbf{F}}_t{\hat{\mathbf{x}}}_{t-1|t-1}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_t\\ & ={\mathbf{F}}_t{\mathbf{x}}_{t-1}+{\tilde{\mathbf{u}}}_t+{\tilde{\mathbf{w}}}_t\end{array}}}\end{array}$$
- ${\tilde{\mathbf{u}}}_t=\mathbf{f}({\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{u}}_t,0)-{\mathbf{F}}_t{\hat{\mathbf{x}}}_{t-1|t-1}$
- ${\tilde{\mathbf{w}}}_t={\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_t\sim \mathcal{N}(0,{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺})$

$$\begin{array}{}\text{(65)}& \boxed{{\displaystyle \begin{array}{rl}{\mathbf{z}}_t& ={\mathbf{H}}_t{\mathbf{x}}_t+\mathbf{h}({\hat{\mathbf{x}}}_{t|t-1},0)-{\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{H}}_{\mathbf{v}}{\mathbf{v}}_t\\ & ={\mathbf{H}}_t{\mathbf{x}}_t+{\tilde{\mathbf{z}}}_t+{\tilde{\mathbf{v}}}_t\end{array}}}\end{array}$$
- ${\tilde{\mathbf{z}}}_t=\mathbf{h}({\hat{\mathbf{x}}}_{t|t-1},0)-{\mathbf{H}}_t{\hat{\mathbf{x}}}_{t|t-1}$
- ${\tilde{\mathbf{v}}}_t={\mathbf{H}}_{\mathbf{v}}{\mathbf{v}}_t\sim \mathcal{N}(0,{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺})$

 

Commonality 3

IEKF는 EKF와 동일하게 $\mathbf{F},\mathbf{H},\mathbf{K}$ 행렬이 true 상태 변수 ${\mathbf{x}}_{\text{true}}$에 대한 행렬을 사용한다.

 

Difference 1 (Iterative nature)

EKF:  prediction 값으로부터 한 번에 correction 값이 나온다.

IEKF:  correction 값이 다시 prediction 값이 되어 correction 스텝을 반복적으로 진행한다.

$$\begin{array}{}\text{(66)}& \boxed{{\displaystyle \begin{array}{rl}& \text{EKF Correction : }{\hat{\mathbf{x}}}_{t|t-1}\to {\hat{\mathbf{x}}}_{t|t}\\ & \text{IEKF Correction : }{\hat{\mathbf{x}}}_{t|t,j}\leftrightarrows {\hat{\mathbf{x}}}_{t|t,j+1}\end{array}}}\end{array}$$

- $j$: $j$-th iteration

 

Difference 2 (Different innovation term)

($\text{65}$) 식을 전개하여 innovation term ${\mathbf{r}}_t$을 만들어보면 다음과 같다.

$$\begin{array}{}\text{(67)}& \begin{array}{r}{\mathbf{r}}_t={\mathbf{z}}_t-\mathbf{h}({\hat{\mathbf{x}}}_{t|t-1},0)-{\mathbf{H}}_t({\mathbf{x}}_t-{\hat{\mathbf{x}}}_{t|t-1})\end{array}\end{array}$$

 

 

EKF의 경우 correction 스텝 ($\text{41}$)의 두 번째 줄을 보면 칼만 게인 ${\mathbf{K}}_t$에 ${\mathbf{r}}_t$가 곱해져서 posterior를 계산하는 것을 볼 수 있다.

$$\begin{array}{}\text{(68)}& \begin{array}{rlrl}\text{EKF correction : }& {\hat{\mathbf{x}}}_{t|t}& & ={\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{K}}_t({\mathbf{r}}_t)\\ & & & ={\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{K}}_t({\mathbf{z}}_t-\mathbf{h}({\hat{\mathbf{x}}}_{t|t-1},0))\end{array}\end{array}$$

 

EKF: ${\mathbf{H}}_t({\mathbf{x}}_t-{\hat{\mathbf{x}}}_{t|t-1})$ 부분은 ${\mathbf{x}}_t={\hat{\mathbf{x}}}_{t|t-1}$이 대입되어 소거되고 나머지 부분만 사용된다. 

IEKF: 매 순간 correction 스텝을 반복하면서 새로운 상태(new operating point)에 대한 선형화를 수행하기 때문에 해당 부분이 소거되지 않는다. 

$$\begin{array}{}\text{(69)}& \boxed{{\displaystyle \begin{array}{rl}& \text{EKF innovation term : }{\mathbf{r}}_t={\mathbf{z}}_t-\mathbf{h}({\hat{\mathbf{x}}}_{t|t-1},0)\\ & \text{IEKF innovation term : }{\mathbf{r}}_{t,j}={\mathbf{z}}_t-\mathbf{h}({\hat{\mathbf{x}}}_{t|t,j},0)-{\mathbf{H}}_{t,j}({\hat{\mathbf{x}}}_{t|t-1}-{\hat{\mathbf{x}}}_{t|t,j})\end{array}}}\end{array}$$

 

만약 첫 번째 iteration $j=0$의 경우 ${\hat{\mathbf{x}}}_{t|t,0}={\hat{\mathbf{x}}}_{t|t-1}$이므로 소거되어 EKF와 동일한 식이 되지만 $j=1$ 이상부터는 다른 값이 되므로 소거되지 않는다.

 

Prediction step

Prediction은 $\overline{\text{bel}}({\mathbf{x}}_t)$를 구하는 과정을 말한다. 공분산 행렬을 구할 때 선형화된 자코비안 행렬 ${\mathbf{F}}_t$가 사용된다. 이는 EKF의 prediction 스텝과 완전히 동일하다.
$$\begin{array}{}\text{(70)}& \boxed{{\displaystyle \begin{array}{rl}& {\hat{\mathbf{x}}}_{t|t-1}=\mathbf{f}({\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{u}}_t,0)\\ & {\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^{⊺}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺}\end{array}}}\end{array}$$

 

Correction step

Correction은 $\text{bel}({\mathbf{x}}_t)$를 구하는 과정을 말한다. 칼만 게인과 공분산 행렬을 구할 때 선형화된 자코비안 행렬 ${\mathbf{H}}_t$가 사용된다. IEKF의 correction 스텝은 업데이트 변화량 $\delta {\hat{\mathbf{x}}}_{t|t,j}$가 충분히 작아질 때까지 반복적(iterative)으로 수행한다.

$$\begin{array}{}\text{(71)}& \boxed{{\displaystyle \begin{array}{rl}& \text{set }ϵ\\ & \text{start j-th loop }\\ & {\mathbf{K}}_{t,j}={\mathbf{P}}_{t|t-1}{\mathbf{H}}_{t,j}^{⊺}({\mathbf{H}}_{t,j}{\mathbf{P}}_{t|t-1}{\mathbf{H}}_{t,j}^{⊺}+{\mathbf{H}}_{\mathbf{v},j}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v},j}^{⊺}{)}^{-1}\\ & \delta {\hat{\mathbf{x}}}_{t|t,j}={\mathbf{K}}_{t,j}({\mathbf{z}}_t-\mathbf{h}({\hat{\mathbf{x}}}_{t|t,j},0)-{\mathbf{H}}_t({\hat{\mathbf{x}}}_{t|t-1}-{\hat{\mathbf{x}}}_{t|t,j}))\\ & {\hat{\mathbf{x}}}_{t|t,j+1}={\hat{\mathbf{x}}}_{t|t,j}+\delta {\hat{\mathbf{x}}}_{t|t,j}\\ & \text{iterate until }\delta {\hat{\mathbf{x}}}_{t|t,j}<ϵ.\\ & \text{end loop}\\ & {\mathbf{P}}_{t|t,n}=(\mathbf{I}-{\mathbf{K}}_{t,n}{\mathbf{H}}_{t,n}){\mathbf{P}}_{t|t-1}\end{array}}}\end{array}$$

 

Summary

IEKF를 함수로 표현하면 다음과 같다.

$$\begin{array}{}\text{(72)}& \boxed{{\displaystyle \begin{array}{rl}& \text{IteratedExtendedKalmanFilter}({\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{P}}_{t-1|t-1},{\mathbf{u}}_t,{\mathbf{z}}_t)\{\\ & \text{(Prediction Step)}\\ & {\hat{\mathbf{x}}}_{t|t-1}={\mathbf{f}}_t({\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{u}}_t,0)\\ & {\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^{⊺}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺}\\ & \\ & \text{(Correction Step)}\\ & \text{set }ϵ\\ & \text{start j-th loop }\\ & {\mathbf{K}}_{t,j}={\mathbf{P}}_{t|t-1}{\mathbf{H}}_{t,j}^{⊺}({\mathbf{H}}_{t,j}{\mathbf{P}}_{t|t-1}{\mathbf{H}}_{t,j}^{⊺}+{\mathbf{H}}_{\mathbf{v},j}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v},j}^{⊺}{)}^{-1}\\ & \delta {\hat{\mathbf{x}}}_{t|t,j}={\mathbf{K}}_{t,j}({\mathbf{z}}_t-\mathbf{h}({\hat{\mathbf{x}}}_{t|t,j},0)-{\mathbf{H}}_t({\hat{\mathbf{x}}}_{t|t-1}-{\hat{\mathbf{x}}}_{t|t,j}))\\ & {\hat{\mathbf{x}}}_{t|t,j+1}={\hat{\mathbf{x}}}_{t|t,j}+\delta {\hat{\mathbf{x}}}_{t|t,j}\\ & \text{iterate until }\delta {\hat{\mathbf{x}}}_{t|t,j}<ϵ.\\ & \text{end loop}\\ & {\mathbf{P}}_{t|t,n}=(\mathbf{I}-{\mathbf{K}}_{t,n}{\mathbf{H}}_{t,n}){\mathbf{P}}_{t|t-1}\\ & \text{return}{\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t}\\ & \}\end{array}}}\end{array}$$

 

Iterated error-state kalman filter (IESKF)

IEKF가 correction 스텝에서 true 상태 변수 ${\mathbf{x}}_{\text{true}}$를 반복적으로 추정한다면 IESKF는 에러 상태 변수 $\delta {\mathbf{x}}_t$를 반복적으로 추정하는 알고리즘이다. 두 상태 변수 사이의 관계는 다음과 같다.

$$\begin{array}{}\text{(73)}& \begin{array}{r}{\mathbf{x}}_{\text{true},t}={\mathbf{x}}_t+\delta {\mathbf{x}}_t\end{array}\end{array}$$
- ${\mathbf{x}}_{\text{true},t}$: 기존 KF, EKF에서 업데이트된 $t$ 스텝의 true 상태변수
- ${\mathbf{x}}_t$: $t$ 스텝의 명목(nominal) 상태 변수. 에러가 없는 상태를 의미한다
- $\delta {\mathbf{x}}_t$: $t$ 스텝의 에러(error) 상태 변수

 

반복적(iterative)으로 값이 업데이트 되는 IESKF 과정에서 ${\mathbf{x}}_{\text{true},t}$는 업데이트 된 이후 상태, nominal  ${\mathbf{x}}_t$는 업데이트 되기 전 상태, 에러 상태 변수 $\delta {\mathbf{x}}_t$는 업데이트 변화량으로 해석하면 된다.

 

Compare to ESKF

Commonality 1

IESKF의 모션 모델과 관측 모델은 다음과 같다. 이는 ESKF와 완전히 동일하다.
$$\begin{array}{}\text{(74)}& \begin{array}{rlr}& \text{Error-state Motion Model: }& {\mathbf{x}}_t+\delta {\mathbf{x}}_t=\mathbf{f}({\mathbf{x}}_{t-1},\delta {\mathbf{x}}_{t-1},{\mathbf{u}}_t,{\mathbf{w}}_t)\\ & \text{Error-state Observation Model: }& {\mathbf{z}}_t=\mathbf{h}({\mathbf{x}}_t,\delta {\mathbf{x}}_t,{\mathbf{v}}_t)\end{array}\end{array}$$
- ${\mathbf{x}}_t$: 모델의 nominal 상태 변수
- $\delta {\mathbf{x}}_t$: 모델의 에러 상태 변수
- ${\mathbf{u}}_t$: 모델의 입력(input)
- ${\mathbf{z}}_t$: 모델의 관측값(measurement)
- $\mathbf{f}(\cdot )$: 비선형 모션(motion) 모델 함수
- $\mathbf{h}(\cdot )$: 비선형 관측(observation) 모델 함수
- ${\mathbf{w}}_t\sim \mathcal{N}(0,{\mathbf{Q}}_t)$: 에러 상태 모델의 노이즈. ${\mathbf{Q}}_t$는 ${\mathbf{w}}_t$의 공분산 행렬을 의미
- ${\mathbf{v}}_t\sim \mathcal{N}(0,{\mathbf{R}}_t)$: 관측 모델의 노이즈. ${\mathbf{R}}_t$는 ${\mathbf{v}}_t$의 공분산 행렬을 의미

 

Commonality 2

IESKF의 두 자코비안 ${\mathbf{F}}_t,{\mathbf{H}}_t$ 모두 true 상태 ${\mathbf{x}}_{\text{true},t}$가 아닌 ESKF와 동일하게 에러 상태 $\delta {\mathbf{x}}_t$에 대한 자코비안을 의미한다.
$$\begin{array}{}\text{(75)}& \boxed{{\displaystyle \begin{array}{rlr}& {\mathbf{F}}_t=\frac{\partial \mathbf{f}}{\partial \delta {\mathbf{x}}_{t-1}}{|}_{\begin{array}{c}\delta {\mathbf{x}}_{t-1}=\delta {\hat{\mathbf{x}}}_{t-1|t-1}\end{array}}& {\mathbf{F}}_{\mathbf{w}}=\frac{\partial \mathbf{f}}{\partial {\mathbf{w}}_t}{|}_{\begin{array}{cc}& \delta {\mathbf{x}}_{t-1}=\delta {\hat{\mathbf{x}}}_{t-1|t-1}\\ & {\mathbf{w}}_t=0\end{array}}\end{array}}}\end{array}$$
$$\begin{array}{}\text{(76)}& \boxed{{\displaystyle \begin{array}{rlr}& {\mathbf{H}}_t=\frac{\partial \mathbf{h}}{\partial \delta {\mathbf{x}}_t}{|}_{\delta {\mathbf{x}}_t=\delta {\hat{\mathbf{x}}}_{t|t-1}}& {\mathbf{H}}_{\mathbf{v}}=\frac{\partial \mathbf{h}}{\partial {\mathbf{v}}_t}{|}_{\begin{array}{cc}& \delta {\mathbf{x}}_t=\delta {\hat{\mathbf{x}}}_{t|t-1}\\ & {\mathbf{v}}_t=0\end{array}}\end{array}}}\end{array}$$

${\mathbf{H}}_t$는 다음과 같이 연쇄법칙을 통해 표현할 수 있다.
$$\begin{array}{}\text{(77)}& \begin{array}{r}{\mathbf{H}}_t=\frac{\partial \mathbf{h}}{\partial \delta {\mathbf{x}}_t}=\frac{\partial \mathbf{h}}{\partial {\mathbf{x}}_{\text{true},t}}\frac{\partial {\mathbf{x}}_{\text{true},t}}{\partial \delta {\mathbf{x}}_t}\end{array}\end{array}$$

 

Commonality 3

선형화된 에러 상태 변수 ${\hat{\mathbf{x}}}_t$ 또한 ESKF와 동일하게 구할 수 있다.

$$\begin{array}{}\text{(78)}& \boxed{{\displaystyle \begin{array}{rl}\delta {\mathbf{x}}_t& ={\mathbf{F}}_t\delta {\mathbf{x}}_{t-1}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_t\\ & ={\mathbf{F}}_t\delta {\mathbf{x}}_{t-1}+{\tilde{\mathbf{w}}}_t\\ & =0+{\tilde{\mathbf{w}}}_t\end{array}}}\end{array}$$
- ${\tilde{\mathbf{w}}}_t={\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_t\sim \mathcal{N}(0,{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺})$

 

Difference (MAP-based derivation)

ESKF derivation: 

ESKF의 correction 스텝은 아래 수식을 전개하여 평균 $\delta {\hat{\mathbf{x}}}_{t|t}$와 공분산 ${\mathbf{P}}_{t|t}$를 유도한다.

$$\begin{array}{}\text{(79)}& \begin{array}{r}\text{bel}(\delta {\mathbf{x}}_t)=\eta \cdot p({\mathbf{z}}_t|\delta {\mathbf{x}}_t)\overline{\text{bel}}(\delta {\mathbf{x}}_t)\sim \mathcal{N}(\delta {\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t})\end{array}\end{array}$$

- $p({\mathbf{z}}_t|\delta {\mathbf{x}}_t)\sim \mathcal{N}({\tilde{\mathbf{z}}}_t,{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v}}^{⊺})$ : ($\text{54}$) 참조

- $\overline{\text{bel}}(\delta {\mathbf{x}}_t)\sim \mathcal{N}(\delta {\hat{\mathbf{x}}}_{t|t-1},{\mathbf{P}}_{t|t-1})$

 

$$\begin{array}{}\text{(80)}& \boxed{{\displaystyle \begin{array}{rl}& \delta {\hat{\mathbf{x}}}_{t|t}={\mathbf{K}}_t({\mathbf{z}}_t-\mathbf{h}({\hat{\mathbf{x}}}_{t|t-1},0,0))\\ & {\mathbf{P}}_{t|t}=(\mathbf{I}-{\mathbf{K}}_t{\mathbf{H}}_t){\mathbf{P}}_{t|t-1}\end{array}}}\end{array}$$

 

IESKF derivation:

IESKF는 maximum a posteriori(MAP)을 사용하여 correction 스텝을 유도한다. 이 때, MAP 추정을 위해 Gauss-Newton 최적화 방식이 사용된다. 보다 자세한 내용은 해당 섹션을 참조하면 된다. 해당 유도 과정은 [6],[7],[8],[9],[10],[11] 을 참고하여 작성하였다. 이 중 [6]이 가장 자세하게 IESKF 유도 과정에 대해 설명하고 있다.

 

EKF를 MAP 방식으로 유도하면 최종적으로 아래와 같은 식을 최적화해야 한다.

$$\begin{array}{}\text{(81)}& \begin{array}{r}\arg \underset{{\mathbf{x}}_t}{min}{\Vert {\mathbf{z}}_t-\mathbf{h}({\mathbf{x}}_{\text{true},t},0)\Vert }_{{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}^{-1}{\mathbf{H}}_{\mathbf{v}}^{⊺}}+{\Vert {\mathbf{x}}_{\text{true},t}-{\hat{\mathbf{x}}}_{t|t-1}\Vert }_{{\mathbf{P}}_{t|t-1}^{-1}}\end{array}\end{array}$$

 

위 식을 nominal 상태 ${\mathbf{x}}_t$와 에러 상태 $\delta {\mathbf{x}}_t$에 대한 식으로 표현하면 다음과 같다. 

$$\begin{array}{}\text{(82)}& \begin{array}{r}\arg \underset{\delta {\hat{\mathbf{x}}}_{t|t}}{min}{\Vert {\mathbf{z}}_t-\mathbf{h}({\mathbf{x}}_t,\delta {\mathbf{x}}_t,0)\Vert }_{{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}^{-1}{\mathbf{H}}_{\mathbf{v}}^{⊺}}+{\Vert \delta {\hat{\mathbf{x}}}_{t|t-1}\Vert }_{{\mathbf{P}}_{t|t-1}^{-1}}\end{array}\end{array}$$

 

Prediction 에러 상태 변수 $\delta {\hat{\mathbf{x}}}_{t|t-1}$은 다음과 같이 정의한다. 
$$\begin{array}{}\text{(83)}& \begin{array}{rl}\delta {\hat{\mathbf{x}}}_{t|t-1}& ={\mathbf{x}}_{\text{true},t}-{\hat{\mathbf{x}}}_{t|t-1}\\ & \sim \mathcal{N}(0,{\mathbf{P}}_{t|t-1})\end{array}\end{array}$$
Posterior(=correction) 에러 상태 변수 $\delta {\hat{\mathbf{x}}}_{t|t}$는 다음과 같이 정의한다. 
$$\begin{array}{}\text{(84)}& \begin{array}{rl}& \delta {\hat{\mathbf{x}}}_{t|t}={\mathbf{x}}_{\text{true},t|t}-{\hat{\mathbf{x}}}_{t|t}\end{array}\end{array}$$
위 식을 이항하면 아래 공식이 성립한다.
$$\begin{array}{}\text{(85)}& \begin{array}{rl}& {\mathbf{x}}_{\text{true},t|t}={\hat{\mathbf{x}}}_{t|t}+\delta {\hat{\mathbf{x}}}_{t|t}\end{array}\end{array}$$

 

($\text{82}$)의 앞 부분은 ($\text{52}$) 선형화와 동일하게 진행하면 된다. 

$$\begin{array}{}\text{(86)}& \begin{array}{r}{\mathbf{z}}_t-\mathbf{h}({\mathbf{x}}_{t|t-1},\delta {\hat{\mathbf{x}}}_{t|t-1},0)\approx {\mathbf{z}}_t-\mathbf{h}({\mathbf{x}}_{t|t-1},0,0)-{\mathbf{H}}_t\delta {\mathbf{x}}_t\end{array}\end{array}$$

 

위 식에서 $\delta {\mathbf{x}}_t$는 ESKF에서는 항상 0이므로 제거하였으나 IESKF에서는 0이 아닌 값을 가지므로 제거하지 않는다. 다음으로 ($\text{82}$)의 뒷 부분에 ($\text{85}$)를 대입하면 아래와 같이 전개된다.

$$\begin{array}{}\text{(87)}& \begin{array}{rl}\delta {\hat{\mathbf{x}}}_{t|t-1}& ={\mathbf{x}}_{\text{true},t}-{\hat{\mathbf{x}}}_{t|t-1}\\ & =({\hat{\mathbf{x}}}_{t|t}+\delta {\hat{\mathbf{x}}}_{t|t})-{\hat{\mathbf{x}}}_{t|t-1}\\ & \approx {\hat{\mathbf{x}}}_{t|t}-{\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{J}}_t\delta {\hat{\mathbf{x}}}_{t|t}\\ & \sim \mathcal{N}(0,{\mathbf{P}}_{t|t-1})\end{array}\end{array}$$

 

이 때, ${\mathbf{J}}_t$는 다음과 같이 정의된다.

$$\begin{array}{}\text{(88)}& \begin{array}{r}{\mathbf{J}}_t=\frac{\partial }{\partial \delta {\hat{\mathbf{x}}}_{t|t}}(({\hat{\mathbf{x}}}_{t|t}+\delta {\hat{\mathbf{x}}}_{t|t})-{\hat{\mathbf{x}}}_{t|t-1}){|}_{\delta {\hat{\mathbf{x}}}_{t|t}=0}\end{array}\end{array}$$

($\text{87}$)의 세 번째 줄과 네 번째 줄을 이항한 후 정리하면 아래와 같은 공식을 얻는다. 

$$\begin{array}{}\text{(89)}& \begin{array}{r}\delta {\hat{\mathbf{x}}}_{t|t}\sim \mathcal{N}(-{\mathbf{J}}_t^{-1}({\hat{\mathbf{x}}}_{t|t}-{\hat{\mathbf{x}}}_{t|t-1}),{\mathbf{J}}_t^{-1}{\mathbf{P}}_{t|t-1}{\mathbf{J}}_t^{-⊺})\end{array}\end{array}$$

 

지금까지 계산한 ($\text{86}$), ($\text{87}$)를 ($\text{82}$)에 대입하면 아래와 같은 MAP 추정 문제가 된다.

$$\begin{array}{}\text{(90)}& \begin{array}{rl}\arg \underset{\delta {\hat{\mathbf{x}}}_{t|t}}{min}& {\Vert {\mathbf{z}}_t-\mathbf{h}({\hat{\mathbf{x}}}_{t|t-1},0,0)-{\mathbf{H}}_t\delta {\hat{\mathbf{x}}}_{t|t}\Vert }_{{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}^{-1}{\mathbf{H}}_{\mathbf{v}}^{⊺}}\\ & +{\Vert {\hat{\mathbf{x}}}_{t|t}-{\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{J}}_t\delta {\hat{\mathbf{x}}}_{t|t}\Vert }_{{\mathbf{P}}_{t|t-1}^{-1}}\end{array}\end{array}$$

 

IESKF는 이를 반복적으로(iterative)하게 추정함으로써 에러 상태 변수 $\delta {\hat{\mathbf{x}}}_{t|t}$가 특정 값 $ϵ$ 이하로 수렴할 때까지 반복한다. $j$번째 iteration에 대한 표현은 다음과 같다.

$$\begin{array}{}\text{(91)}& \boxed{{\displaystyle \begin{array}{r}\delta {\hat{\mathbf{x}}}_{t|t,j}\sim \mathcal{N}(-{\mathbf{J}}_{t,j}^{-1}({\hat{\mathbf{x}}}_{t|t,j}-{\hat{\mathbf{x}}}_{t|t-1}),{\mathbf{J}}_{t,j}^{-1}{\mathbf{P}}_{t|t-1}{\mathbf{J}}_{t,j}^{-⊺})\end{array}}}\end{array}$$

$$\begin{array}{}\text{(92)}& \boxed{{\displaystyle \begin{array}{rl}\arg \underset{\delta {\hat{\mathbf{x}}}_{t|t,j}}{min}& {\Vert {\mathbf{z}}_t-\mathbf{h}({\hat{\mathbf{x}}}_{t|t-1},0,0)-{\mathbf{H}}_t\delta {\hat{\mathbf{x}}}_{t|t,j}\Vert }_{{\mathbf{H}}_{\mathbf{v}}{\mathbf{R}}^{-1}{\mathbf{H}}_{\mathbf{v}}^{⊺}}\\ & +{\Vert {\hat{\mathbf{x}}}_{t|t,j}-{\hat{\mathbf{x}}}_{t|t-1}+{\mathbf{J}}_{t,j}\delta {\hat{\mathbf{x}}}_{t|t,j}\Vert }_{{\mathbf{P}}_{t|t-1}^{-1}}\end{array}}}\end{array}$$

 

위 수식으로부터 업데이트 수식을 유도하는 과정은 Derivation of IESKF update step 섹션을 참조하면 된다.

 

Prediction step

Prediction은 $\overline{\text{bel}}(\delta {\mathbf{x}}_t)$를 구하는 과정을 말한다. 공분산 행렬을 구할 때 선형화된 자코비안 행렬 ${\mathbf{F}}_t$가 사용된다. 이는 ESKF와 완전히 동일하다.
$$\begin{array}{}\text{(93)}& \boxed{{\displaystyle \begin{array}{rl}& \delta {\hat{\mathbf{x}}}_{t|t-1}={\mathbf{F}}_t\delta {\hat{\mathbf{x}}}_{t-1|t-1}=0\leftarrow \text{Always 0}\\ & {\hat{\mathbf{x}}}_{t|t-1}=\mathbf{f}({\hat{\mathbf{x}}}_{t-1|t-1},0,{\mathbf{u}}_t,0)\\ & {\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^{⊺}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺}\end{array}}}\end{array}$$

 

Correction step

Correction은 $\text{bel}({\mathbf{x}}_t)$를 구하는 과정을 말한다. 칼만 게인과 공분산 행렬을 구할 때 선형화된 자코비안 행렬 ${\mathbf{H}}_t$가 사용된다. IESKF의 correction 스텝은 업데이트 변화량 $\delta {\hat{\mathbf{x}}}_{t|t,j}$가 충분히 작아질 때까지 반복적(iterative)으로 수행한다. 앞서 유도한 ($\text{92}$)을 미분한 후 0으로 설정하여 풀면 아래와 같은 공식을 얻는다. 보다 자세한 유도 과정은 Derivation of IESKF update step 섹션을 참조하면 된다.

$$\begin{array}{}\text{(94)}& \boxed{{\displaystyle \begin{array}{rl}& \text{set }ϵ\\ & \text{start j-th loop }\\ & {\mathbf{S}}_{t,j}={\mathbf{H}}_{t,j}{\mathbf{J}}_{t,j}^{-1}{\mathbf{P}}_{t|t-1}{\mathbf{J}}_{t,j}^{-⊺}{\mathbf{H}}_{t,j}^{⊺}+{\mathbf{H}}_{\mathbf{v},j}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v},j}^{⊺}\\ & {\mathbf{K}}_{t,j}={\mathbf{J}}_{t,j}^{-1}{\mathbf{P}}_{t|t-1}{\mathbf{J}}_{t,j}^{-⊺}{\mathbf{H}}_{t,j}^{⊺}{\mathbf{S}}_{t,j}^{-1}\\ & \delta {\hat{\mathbf{x}}}_{t|t,j}={\mathbf{K}}_{t,j}({\mathbf{z}}_t-\mathbf{h}({\hat{\mathbf{x}}}_{t|t,j},0,0)+{\mathbf{H}}_{t,j}{\mathbf{J}}_{t,j}^{-1}({\hat{\mathbf{x}}}_{t|t,j}-{\hat{\mathbf{x}}}_{t|t-1}))-{\mathbf{J}}_{t,j}^{-1}({\hat{\mathbf{x}}}_{t|t,j}-{\hat{\mathbf{x}}}_{t|t-1})\\ & {\hat{\mathbf{x}}}_{t|t,j+1}={\hat{\mathbf{x}}}_{t|t,j}⊞\delta {\hat{\mathbf{x}}}_{t|t,j}\\ & \text{iterate until }\delta {\hat{\mathbf{x}}}_{t|t,j}<ϵ.\\ & \text{end loop}\\ & {\mathbf{P}}_{t|t,n}=(\mathbf{I}-{\mathbf{K}}_{t,n}{\mathbf{H}}_{t,n}){\mathbf{J}}_{t,n}^{-1}{\mathbf{P}}_{t|t-1}{\mathbf{J}}_{t,n}^{-⊺}\end{array}}}\end{array}$$

 

Summary

IESKF를 함수로 표현하면 다음과 같다.

$$\begin{array}{}\text{(95)}& \boxed{{\displaystyle \begin{array}{rl}& \text{IteratedErrorStateKalmanFilter}({\hat{\mathbf{x}}}_{t-1|t-1},\delta {\hat{\mathbf{x}}}_{t-1|t-1},{\mathbf{P}}_{t-1|t-1},{\mathbf{u}}_t,{\mathbf{z}}_t)\{\\ & \text{(Prediction Step)}\\ & \delta {\hat{\mathbf{x}}}_{t|t-1}={\mathbf{F}}_t{\hat{\mathbf{x}}}_{t-1|t-1}=0\leftarrow \text{Always 0}\\ & {\hat{\mathbf{x}}}_{t|t-1}=\mathbf{f}({\hat{\mathbf{x}}}_{t-1|t-1},0,{\mathbf{u}}_t,0)\\ & {\mathbf{P}}_{t|t-1}={\mathbf{F}}_t{\mathbf{P}}_{t-1|t-1}{\mathbf{F}}_t^{⊺}+{\mathbf{F}}_{\mathbf{w}}{\mathbf{Q}}_t{\mathbf{F}}_{\mathbf{w}}^{⊺}\\ & \\ & \text{(Correction Step)}\\ & \text{set }ϵ\\ & \text{start j-th loop }\\ & {\mathbf{S}}_{t,j}={\mathbf{H}}_{t,j}{\mathbf{J}}_{t,j}^{-1}{\mathbf{P}}_{t|t-1}{\mathbf{J}}_{t,j}^{-⊺}{\mathbf{H}}_{t,j}^{⊺}+{\mathbf{H}}_{\mathbf{v},j}{\mathbf{R}}_t{\mathbf{H}}_{\mathbf{v},j}^{⊺}\\ & {\mathbf{K}}_{t,j}={\mathbf{J}}_{t,j}^{-1}{\mathbf{P}}_{t|t-1}{\mathbf{J}}_{t,j}^{-⊺}{\mathbf{H}}_{t,j}^{⊺}{\mathbf{S}}_{t,j}^{-1}\\ & \delta {\hat{\mathbf{x}}}_{t|t,j}={\mathbf{K}}_{t,j}({\mathbf{z}}_t-\mathbf{h}({\hat{\mathbf{x}}}_{t|t,j},0,0)+{\mathbf{H}}_{t,j}{\mathbf{J}}_{t,j}^{-1}({\hat{\mathbf{x}}}_{t|t,j}-{\hat{\mathbf{x}}}_{t|t-1}))-{\mathbf{J}}_{t,j}^{-1}({\hat{\mathbf{x}}}_{t|t,j}-{\hat{\mathbf{x}}}_{t|t-1})\\ & {\hat{\mathbf{x}}}_{t|t,j+1}={\hat{\mathbf{x}}}_{t|t,j}⊞\delta {\hat{\mathbf{x}}}_{t|t,j}\\ & \text{iterate until }\delta {\hat{\mathbf{x}}}_{t|t,j}<ϵ.\\ & \text{end loop}\\ & {\mathbf{P}}_{t|t,n}=(\mathbf{I}-{\mathbf{K}}_{t,n}{\mathbf{H}}_{t,n}){\mathbf{J}}_{t,n}^{-1}{\mathbf{P}}_{t|t-1}{\mathbf{J}}_{t,n}^{-⊺}\\ & \text{return}{\hat{\mathbf{x}}}_{t|t},{\mathbf{P}}_{t|t}\\ & \}\end{array}}}\end{array}$$

 

Wrap-up

지금까지 설명한 KF, EKF, ESKF, IEKF, IESKF를 한 장의 슬라이드로 표현하면 다음과 같다. 클릭하면 큰 그림으로 볼 수 있다.
 
Kalman Filter (KF)

 
Extended Kalman Filter (EKF)

Error-state Kalman Filter (ESKF)

Iterated Extended Kalman Filter (IEKF)

Iterated Error-state Kalman Filter (IESKF)

References

[1] Kalman Filter - Wikipedia
[2] (Paper) Sola, Joan. "Quaternion kinematics for the error-state Kalman filter." arXiv preprint arXiv:1711.02508 (2017).
[3] (Youtube) Robot Mapping Coure - Freiburg Univ  
[4] (Blog) [SLAM] Kalman filter and EKF(Extended Kalman Filter) - jinyongjeong
[5] (Blog) Error-State Kalman Filter understanding and formula derivation - CSDN
[6] (Paper) He, Dongjiao, Wei Xu, and Fu Zhang. "Kalman filters on differentiable manifolds." arXiv preprint arXiv:2102.03804 (2021).
[7] (Book) SLAM in Autonomous Driving book (SAD book)
[8] (Paper) Xu, Wei, and Fu Zhang. "Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter." IEEE Robotics and Automation Letters 6.2 (2021): 3317-3324.
[9] (Paper) Huai, Jianzhu, and Xiang Gao. "A Quick Guide for the Iterated Extended Kalman Filter on Manifolds." arXiv preprint arXiv:2307.09237 (2023).
[10] (Paper) Bloesch, Michael, et al. "Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback." The International Journal of Robotics Research 36.10 (2017): 1053-1072.

[11] (Paper) Skoglund, Martin A., Gustaf Hendeby, and Daniel Axehill. "Extended Kalman filter modifications based on an optimization view point." 2015 18th International Conference on Information Fusion (Fusion). IEEE, 2015.
 [12] (Blog) From MAP, MLE, OLS, GN to IEKF, EKF

[13] (Book) Thrun, Sebastian. "Probabilistic robotics." Communications of the ACM 45.3 (2002): 52-57.