3.3 The Extended Kalman Filter

이 글은 Sebastian thrun의 Probabilistic Robotics를 보고 내용을 정리한 글이며, 나름 쉽게 표현하기 위해서 의역을 한 부분이 있습니다.

​

 

 

1. 왜 선형화를 하는가?(Why Linearize?)

관측값들이 상태(state)의 선형 함수라는 것과 다음 상태가 이전 상태의 선형 함수라는 가정은 칼만 필터(Kalman Filter)에서의 보정(correctness)에 있어 매우 중요합니다. 3.2절에서 설명했듯이, 입력값(input)들은 가우시안 분포를 갖는 노이즈를 포함한다고 가정하였습니다.

 

 

 

Figure 3.3. 가우시안 랜덤 변수의 선형 변환(linear transform)(a)와 비선형 변환(nonlinear transform)(b). 우측 아래쪽의 그림이 원래 랜덤변수 X의 확률밀도이고, 이 변수는 오른쪽 위 그래프의 변환을 통해 변환된다(점선의 값은 평균을 나타낸다). 왼쪽 위 그래프는 변환에 의해 만들어진 랜덤변수 Y이다.

 

그림 3.3은 1차원 가우시안 변수의 선형 및 비선형 변환을 나타낸 그림입니다. 오른쪽 아래의 그래프는 분포 $N(x; \mu, \sigma ^2)$를 따르는 랜덤 변수 $X$의 확률 밀도를 보여줍니다. 여기서 $X$가 오른쪽 위 그림에 나타난 선형 함수 $y = ax + B$를 통과한다고 가정하겠습니다. 그 결과의 랜덤 변수 $Y$의 분포는 평균이 $a\mu + b$이고 분산이 $a^2\sigma ^2$인 가우시안 분포를 따르게 됩니다. 이 가우시안 분포는 그림 3.3a의 왼쪽 위 그림에서 회색 영역입니다. 이 예시는 노이즈가 없을 때의 칼만 필터의 다음 상태(state) 업데이트와 매우 비슷합니다. $X = x_{t-1}$이고, $Y = x_t$에 대응된다고 생각하시면 됩니다.

 

하지만, 실제로 상태 과도(state transition)와 측정값은 선형적인 경우가 거의 없습니다. 예를 들어, 로봇이 일정한 값의 직선 속도와 회전 속도를 가진다면 일반적으로 원 궤적을 그리게 되며, 이는 선형 상태 과도(linear state transition)로 나타낼 수 없습니다.

 

확장 칼만 필터(Extended Kalman Filter, EKF)는 이러한 가정(선형적인 가정)들을 완화시킵니다. 여기서의 가정은 상태 과도 확률(state transition probability)과 측정 확률이 비선형 함수에 의해 조절된다는 것입니다. 즉,

 

이 모델은 방정식 (3.2)와 (3.5)에서 상정했듯이 칼만 필터 하의 선형 가우시안 모델을 엄격하게(strictly) 일반화합니다(식 3.2와 3.5는 이전 포스팅 soohwan-justin.tistory.com/3 을 참조하세요). 여기서 함수 $g$는 식 (3.2)의 $A_t$와 $B_t$를 대체하고, $h$는 식 (3.5)의 $C_t$를 대체합니다. 여기서 임의의 비선형 함수 $g$와 $h$로 인해, belief는 더 이상 가우시안 분포를 따르지 않게 됩니다. 사실, 비선형함수 $g$와 $h$에 대한 belief 업데이트를 정확하게 하는 것은 보통 불가능하고, 베이즈 필터(Bayes filter)는 closed-form solution을 갖지 않습니다.

 

그림 3.3b는 가우시안 랜덤 변수의 비선형 변환을 보여줍니다. 오른쪽 아래와 오른쪽 위의 그래프는 각각 랜덤 변수 $X$와 비선형함수 $g$를 보여줍니다. 변환된 랜덤 변수 $Y = g(X)$의 확률 밀도는 그림 3.3b의 왼쪽 위 그래프에서 회색 영역으로 나타냈습니다. 이 확률 밀도는 closed form으로 계산될 수 없기 때문에, $p(x)$를 따르며, 함수 $g$를 통과하고 $g$의 범위에서의 히스토그래밍 하는 500,000개의 샘플을 통해 추정되었습니다. 보이는 것과 같이, $Y$는 더 이상 가우시안 변수가 아닌데, $g$의 비선형성이 $X$의 확률 밀도를 가우시안 형태를 일그러뜨리는 것처럼 확률도 일그러뜨리기 때문입니다.

 

EKF는 true belief의 가우시안 근사를 계산합니다. 그림 3.4b의 왼쪽 위의 그래프에서 점선의 그래프는 랜덤 변수 $Y$의 확률 밀도를 가우시안 근사 한 것을 보여줍니다. EKF는 belief $bel(x_t)$를 시간 $t$에서의 평균 $\mu _t$와 공분산 $\Sigma _t$로 나타냅니다. 그러므로, EKF는 칼만 필터에서 belief를 나타내는 방식을 사용하지만, 다른 점은 근사화를 한다는 것입니다. EKF의 목표는 정확한 사후 확률을 계산하는 것에서 이것의 평균과 공분산을 효과적으로 추정하는 것으로 바뀌었다고 보면 됩니다.

 

 

 

그림 3.4. 확장 칼만 필터에 적용된 선형화. 가우시안 변수를 비선형 함수 g에 입력으로 넣는 대신, 함수 g를 선형 근사화한 식에 입력으로 넣는다. 그 선형 함수는 원래 가우시안 변수의 평균값에 해당하는 지점에서의 접선에 해당한다. 이 선형화는 근사화로 인한 오차를 발생시키며, 이는 왼쪽 위 그래프에서 점선 그래프와 굵은 선 그래프의 차이에 해당한다.

 

 

2. 테일러급수 전개를 통한 선형화(Linearization Via Taylor Expansion)

EKF 근사에서 중요한 아이디어는 선형화 입니다. 그림 3.4는 그 기본적인 컨셉을 보여줍니다. 여기서 선형화는 비선형 함수$g$를 $g$의 평균에서의 접선인 선형 함수로 근사하는 것입니다(그림 3.4의 오른쪽 위 그래프에서 점선으로 나타냈습니다). 가우시안 분포를 따르는 변수를 이 선형 근사의 결과를 통해 사영(projecting)하는 것은 그림 3.4의 왼쪽 위 그래프의 점선 그래프처럼 가우시안 분포를 따르는 확률 밀도의 결과가 나오게 됩니다. 왼쪽 위 그래프의 굵은 그래프는 몬테카를로(Monte-Carlo) 근사의 평균과 공분산을 나타냅니다. 이 굵은 선 가우시안 그래프와 점선 가우시안 그래프의 차이는 $g$의 선형 근사에 의해 생기는 오차를 나타냅니다.

 

하지만, 이 선형화의 중요한 장점은 효율성(efficiency)입니다. 가우시안 분포를 따르는 몬테카를로 추정은 500,000개 샘플을 비선형 함수 $g$에 통과시키고 그 평균과 분산을 계산하여 얻어진 결과입니다. 반면, EKF에 의해 적용된 선형화는 단지 선형 근사의 결정(determination of the linear approximation) 후, 그 결과 가우시안의 closed-form 계산만을 필요로 합니다. 사실, $g$가 한번 선형화 되면, EKF의 belief 전파(propagation) 기법은 칼만 필터의 경우와 같다고 보면 됩니다.

 

이 기법은 측정값 함수 $h$가 포함되었을 때 가우시안의 곱에도 동일하게 적용됩니다. EKF는 $h$를 $h$의 접선으로 근사하고, 따라서 사후확률 blief의 가우시안 속성을 유지하게 됩니다.

 

비선형 함수를 선형화 하는데는 여러 방법이 있는데, EKF는 (1차) 테일러급수 전개를 이용합니다. 테일러급수 전개는 함수 $g$의 선형 근사를 $g'$의 값과 그 기울기를 이용합니다. 그 기울기는 다음과 같이 편미분으로 주어집니다.

 

분명히, $g$와 그 기울기 값은 $g$의 매개변수에 의존합니다. 그 매개변수를 선택하는 가장 논리적인 선택은 그 선형화를 하는 시간에 가장 적절한 것 같은 상태를 고르는 것입니다. 예를 들어 가우시안에서는, 사후 확률 $\mu _{t-1}$의 평균입니다. 다시 말해, $g$의 근사는 그 함수의 매개변수에 $\mu _{t-1}$(그리고 $u_t$까지)를 대입하여 얻어지며, 선형 추정은 $\mu _{t-1}$과 $u_t$에서의 $g$의 기울기에 비례하는 항에 의해 결정됩니다.

 

 

상태 과도 확률은 아래와 같이 근사됩니다.

 

여기서 $G_t$는 크기가 $n \times n$인 크기의 행렬이며, $n$은 상태의 차원이라는 것에 주목하세요. 이 행렬은 종종 "자코비안"이라고 불립니다. 이 자코비안의 값은 $u_t$와 $\mu _{t-1}$에 의존하며, 따라서 시간에 따라 값이 달라지게 됩니다.

 

EKF는 측정 함수 $h$에 대해 똑같은 선형화를 적용합니다. 여기서 테일러 급수 전개는 $\overline{\mu} _t$ 근처에서 만들어지며, 그 시간에서 가장 적절할 것으로 보이는 $h$의 선형화는 다음과 같습니다.

 

 

3. EKF 알고리즘(The EKF Algorithm)

Table 3.3은 EKF 알고리즘을 나타냅니다. 여기서 많은 부분이 Table 3.1의 칼만 필터 알고리즘과 비슷합니다.

 

Table 3.3. 확장 칼만 필터 알고리즘

 

Table 3.1. 칼만 필터 알고리즘

 

두 알고리즘의 가장 중요한 차이점은 아래 표에 요약되어 있습니다.

 

 

여기서 칼만 필터에서의 예측 부분은 EKF에서는 비선형 함수로 대체되었습니다. 그리고 EKF는 선형 시스템 행렬 $A_t$, $B_t$, $C_t$에 대응되는 행렬 대신 자코비안 $G_t$와 $H_t$를 사용합니다. 여기서 $G_t$는 $A_t$와 $B_t$에 대응되며, $H_t$는 $C_t$에 대응됩니다.

 

 

4. 실질적인 고려사항(Practical Considerations)

확장 칼만 필터의 한계점은, 테일러 급수 전개를 이용해서 상태 과도와 측정값을 근사한다는 것입니다. 대부분의 로보틱스에 관한 문제에서 상태 과도와 측정값은 비선형입니다. 확장 칼만 필터에 적용된 선형 근사의 장점은 두 가지의 중요 요소에 의존합니다. 이는 불확실성의 정도(degree of uncertainty)와 근사화된 함수의 지역적 비선형성의 정도(degree of local nonlinearity of the functions that are being approximated)입니다.

 

그림 3.5. 불확실성에 대한 근사화 퀄리티(approximation quality)의 의존성. 오른쪽 아래의 두 가우시안 그래프들은 같은 평균을 가지고 있고, 같은 비선형 함수(오른쪽 위)를 통과한다. 왼쪽 위의 가우시안 그래프의 불확실성이 더 높을수록 모양이 더 많이 일그러진 램덤 변수의 확률 밀도 그래프가 나온다. 왼쪽 위 그림에서 굵은 선 그래프는 이렇게 주어진 확률밀도에서 추출된 가우시안 분포이다. 점선 그래프는 확장 칼만 필터를 이용한 선형화에 의해 만들어진 가우시안 그래프이다.

 

그림 3.5는 불확실성에의 의존을 보여줍니다. 위 그래프에서는 두 개의 가우시안 랜덤변수가 같은 비선형 함수를 통과합니다. 두 개의 가우시안은 같은 평균을 갖고 있지만, (a)의 불확실성이 (b)의 불확실성보다 더 큽니다. 왜냐하면 테일러급수 전개는 단지 평균에만 의존하기 때문에, 두 가우시안 변수는 똑같은 선형과 과정을 거치게 된 것입니다.

왼쪽 위 그림의 회색 영역은 몬테카를로 추정(Monte Carlo estimation)에 의해 계산된 랜덤 변수들의 결과인 확률 밀도를 나타냅니다. 더 넓은 분포의 가우시안 분포를 따르는 결과는 좁은 분포의 가우시안의 결과에 비해 더 일그러진 모양의 그래프로부터 얻게 되었습니다. 이 확률 밀도들의 가우시안 근사는 각 그림에서 굵은 선 그래프로 나타냈고, 선형화 된 함수로부터 추정된 결과는 점선으로 나타냈습니다.

여기서 점선 그래프와 굵은 선 그래프의 비교는 불확실성이 높으면 일반적으로 평균과 공분산이 부정확하게 추정된다는 것을 보여줍니다.

​

선형 가우시안 근사의 퀄리티(quality of the linear Gaussian approximation)의 두 번째 요소는 함수 $g$의 지역적 비선형성(the local nonlinearity of the function $g$) 이며, 이는 그림 3.6에 나타내었습니다.

 

 

 

그림 3.6. 함수 g의 지역적 비선형성에 대한 근사화의 퀄리티의 의존성. 두 개의 가우시안 변수는 같은 공분산을 가지고 있으며 같은 함수를 통과한다. 확장 칼만 필터에 적용된 선형 근사는 왼쪽 그림에서 점선으로 나타내었고, 굵은 선은 몬테카를로 추정에 의해 구한 정확한 그래프이다.

위 그림을 보면, 공분산이 같은 두 개의 가우시안 랜덤 변수가 같은 함수를 통과합니다. (a)에서는 평균이 함수 $g$의 비선형적인 부분에 있고, (b)에서는 평균이 함수 $g$의 선형적인 부분에 있습니다. 왼쪽의 결과를 보면, 선형적인 부분에서의 몬테카를로 추정과의 차이가 비선형적인 부분에서의 차이보다 더 작습니다.