IESKF(Iterated error state Kalman Filter)이란 무엇일까?

Iterated error state Kalman Filter에 대한 설명을 간략하게 포스팅해보자.

최근 LiDAR-inertial Odometry 및 SLAM을 공부하던 도중, FAST-LIO 시리즈가 주목을 받고 있는 것을 알 수 있었다. 소스코드도 다 공개가 되어 있고, 최근에 진행된 SLAM 대회인 HILTI SLAM Challenge 2022의 solution을 살펴보면 대부분 baseline을 FAST-LIO 시리즈로 잡고 가는 것도 확인했다. (링크를 타고 들어가서 팀이름을 누르면 간단한 레포트를 확인할 수 있다!)

FAST-LIO 시리즈는 최적화 방식을 Filter-based로 풀어가는데 여기서 사용하는 Filter의 방식이 바로 IESKF(Iterated error state Kalman Filter)이다.

따라서 이번 포스팅에서는 IESKF(Iterated error state Kalman Filter)가 어떤 것이고 기존에 있던 Filter 방식보다 어떤 장점이 있는지 간단하게 포스팅하고자 한다.

우선 이번 포스팅은 Kalman Filter, Extended Kalman Filter등 여러 Filter를 알고 있다는 전제하에 작성되었다.

EKF에 대한 지식이 부족하다면 아래의 포스팅을 참고해보자.

• Slam 7강 (Kalman Filter & EKF) 요약
• [SLAM] Kalman filter and EKF(Extended Kalman Filter)

Error state Kalman Filter란?

IESKF(Iterated error state Kalman Filter)를 알아보기 전에 우선 ESKF(Error state Kalman Filter)가 무엇인지 한번 알아보자.

ESKF(Error state Kalman Filter)는 기존의 EKF(Extended Kalman Filter)와 Input과 Output이 살짝 다르고, 전체적인 구조는 비슷하다고 이해했다.

ES-EKF라고도 표현하기도 하는데 이 포스팅에서는 ESKF로 통일하겠다.

두 Filter 모두 state를 더 잘 구하는 것이 목표인데, EKF의 경우 state 자체를 구하는 것이 목적이고, ESKF의 경우 이전 state와 predict state의 Error term state를 0으로 만드는 것이 목적이다!

말로만 설명을 하는 것보다 수식을 보면서 이해를 하는 것이 훨씬 빠르다고 생각한다.

EKF(Extended Kalman Filter)에 대한 수식은 아래와 같다.

Input으로는 이전 state를 가우시안 분포로 모델링한 $\mu_{t-1}$, control input $u$, measurement $z$, Covaraince Matrix $P_{t-1}$이 들어간다. 그리고 EKF를 통해 나오는 값은 현재 state를 가우시안 분포로 모델링한 $\mu_{t}$, Covaraince Matrix $P_{t}$이다.

ESKF(Error state Kalman Filter)에 대한 수식을 보면 아래와 같다.

언뜻 보면, 차이점이 별로 없어보인다. 동일하게 Prediction step과 Update step을 수행하는데 state를 가우시안 분포로 모델링(Linearization)하는 대신, state vector $s_t$를 구하는데 사용하는 function $f$를 non-linear function을 그대로 사용하고 $e_t = \mu_t - \mu_{t-1}$ 이라는 error term을 Linearization을 수행하게 된다.

여기서 사용하는 Covaraince Matrix $P_{t}$ 또한 error term에 대한 Covaraince matrix이다.

또 다른 그림을 통해 Error state Kalman Filter를 이해해보자.

우리가 추정하고 싶은 state가 true state $x$라고 할 때, EKF의 경우 이 state를 한번에 선형화(Linearization)을 진행하는 반면, ESKF의 경우 True state와 Nominal state의 차이를 줄이는 error term을 선형화(Linearization)을 진행한다.

Nominal state란 Motion model을 사용하여 예측한 state라고 생각하면 된다.

정리를 해보자면 다음과 같다.

• ESKF는 state vector 자체를 선형화하고 예측하는 대신, error term을 0으로 만드는 것이 목적이다.
• ESKF는 EKF와 동일하게 Prediction step과 Update step을 거친다.
• ESKF에서의 Covariance matrix는 error term에 대한 Covariance Matrix이다.

Error state Kalman Filter의 장점

EKF보다 ESKF가 가지고 있는 장점이 무엇일까?

결론적으로 두 가지를 이야기하고 있는데

1. State vector 자체를 predict & update하는 것보다 Error term만을 활용해서 predict & update하는 것이 선형화(linearize)가 더 잘된다.
2. 최적화를 할 때 Rotation(=orientation)을 최적화 하는 것이 제약조건(Constraints)이 많은데 Error term 자체가 값이 작으므로 제약조건에서 비교적 자유로울 수 있다.

Reference를 참고해보면, 항공쪽에서 state estimation을 할 때와, IMU와 같이 measurement가 빠르게 들어올 때, Error state Kalman Filter를 사용하는 것 같았다.

자세한 내용은 아래의 링크를 참고해보자.

• Quaternion kinematics for the error-state Kalman filter 의 Chapter 5
• Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation

Iterated error state Kalman Filter란?

이름에서도 알 수 있는데, Error state Kalman Filter를 iterative하게 사용하는 방법이다.

사실 Error state Kalman Filter를 이해했다면 Iterated error state Kalman Filter를 이해하는 것은 비교적 쉽다! Error state Kalman Filter의 목적이 error term을 0으로 만드는 것이므로, (컴퓨팅 파워가 허락한다면) 반복적으로 Update step을 진행하여 error term을 0으로 만드는 것이다.

아래는 Error state Kalman Filter와 Iterated error state Kalman Filter의 차이점을 나타낸 것이다.

Iterated error state Kalman Filter의 경우 update step을 여러번 진행하기 때문에 기존의 Error state Kalman Filter보다 계산량이 많지만, 원하는 결과에 좀 더 정확하게 도달할 수 있다는 장점이 있다.

---

이렇게 해서 Iterated error state Kalman Filter에 대한 포스팅을 마무리 하겠다. 아래에는 포스팅을 작성하면서 참고한 자료들이다. 😆😆

Refernce

• A “quick” review of Error State - Extended Kalman Filter
• Coursera Lesson 4: An Improved EKF - The Error State Extended Kalman Filter
• Quaternion kinematics for the error-state Kalman filter
• Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation