[Review] A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation

VO 분야에서는 거의 교과서로 불리는 논문이다. 매우 유명한 MSCKF를 제안하고 다양한 실험적 검증을 수행했다. IMU-Camera 센서 퓨전을 연구할 계획이라면 원문을 꼭 읽어보길 추천한다.

한줄 요약 :

3차원 피처 위치를 사용하지 않으면서도 최적(Optimal)의 선형 오차(Linearization errors)를 얻을 수 있는 칼만 필터의 IMU-Camera 측정치 모델을 제시한다.

길게 풀어서 설명하면, 매 시각 카메라 시점을 스테이트로 추정하고, 일정 조건을 만족하면 각 시점에서 공통으로 관찰하고 있는 피처에 대한 기하적 조건을 이용해 측정치 업데이트를 수행하는 것이 MSCKF의 원리이다.

서론(Introduction)

본 논문에서는 영상기반 관성항법(vision-aided inertial navigation)의 고질적인 문제인 계산량 문제를 해결할 수 있는 방법을 제시한다. 사용할 핵심 아이디어는 정적인 대상(Static feature)을 여러 자세(several camera poses)에서 관찰할 경우, 기하 조건(Geometric constraints)를 얻을 수 있다는 것이다.

선행연구의 문제점

SLAM(Simultaneous Localization and Mapping)
SLAM은 여러 각도에서 관찰한 3D 피처 사이의 관계(Correlation)을 이용한다.
다만 매우 많은 수의 3D 피처(thousands of features)를 사용하기 때문에 알고리즘 복잡도가 매우 높으며, 복잡도에 비해 성능이 좋지 않은 편이다.
카메라 시점만 추정하는 방법
연속 이미지와 IMU를 이용한 카메라 시점 변화를 추정해 실시간성을 얻을 수 있다.
Epipolar geometry를 사용하기도 한다.
Multiple camera geometry를 이용하는 방법들
augmented-state Kalman filter를 사용하는 방법
모든 카메라 시점을 동시에 추정하는 방법
이러한 방법들은 어떤 피처가 여러 장면에서 관찰되는 경우, 각 장면 사이의 관계를 고려하지 않아 정확성이 떨어진다.
랜드마크를 사용하는 방법
랜드마크를 이용해 연속 장면의 카메라 시점을 추정하는 방법이 제안되었지만, IMU를 사용하지 않는다.
또한 궤적(Trajectory)와 랜드마크 사이의 관계가 제대로 고려되지 않아서 공분산 행렬(Covariance matrix)를 계산할 수 없다.
Variable State Dimension Filter (VSDF)
동적 모델이 사용되는 경우 복잡도가 피처 개수에 대해 Quadratic으로 증가한다.

Multi-state constraint filter(MSCKF)의 장점

Delayed linearization을 이용해 강건하다.
복잡도가 피처 개수에 대해 선형이다.
여러 카메라 시점이 직접적으로 표현되기 때문에 연산량이 적고, pairwise displacement 추정에서 정보 손실이 일어나지 않는다.
SLAM과는 다르게 3차원 피처가 state에 포함되지 않지만, optimal pose estimation이 가능하다.

Estimator description

Frame definition

IMU frame : ${I}$

Global ECEF frame : ${G}$

Pseudocode 1 : Multi-State Constraint Filter

Propagation: IMU 측정치가 들어올 때마다 필터 스테이트와 공분산 업데이트
Image registration: 새로운 이미지가 들어올 때마다
스테이트와 공분산이 현재 카메라 시점 추정치와 augment됨
이미지 프로세싱 모듈 시작
Update: 해당 이미지의 피처 측정치가 사용 가능하면 EKF 업데이트

스테이트 Augmentation은 피처 측정치를 처리하는 데 반드시 필요하다. EKF의 측정치 업데이트에서 트래킹되고 있는 각 피처가 카메라 시점의 제한조건으로 사용되기 때문이다. 따라서 EKF의 스테이트 벡터는 IMU 스테이트와 과거 $N_{MAX}$개의 카메라 시점으로 구성되어 있다.
$$
[ X_{IMU}, X_{CAM} ]
$$

EKF 스테이트 벡터 구조

IMU 스테이트는 다음과 같다.

$$ X_{IMU}= \begin{bmatrix} ^I_G\bar{q}^T & b^{T}_g & ^G{v_I}^T & {b_a}^T & ^G{p_I}^T \end{bmatrix}^T $$

$^I_G\bar{q}^T$는 unit quaternion으로 $G$프레임에서 $I$프레임으로의 회전을 의미한다. $b^{T}_g$와 ${b_a}^T$는 자이로와 가속도계 바이어스이다. $^G{v_I}^T$와 $^G{p_I}^T$는 G프레임에서의 IMU 속도와 위치이다. 바이어스는 Gaussian noise $n_{wg}$와 $n_{wa}$에 해당하는 Random walk로 모델링 되었다. 위 스테이트에 대한 오차 모델은

$$ \tilde{X}_{IMU}= \begin{bmatrix} {\delta\theta_I}^T & \tilde{b}^{T}_g & ^G\tilde{v_I}^T & {\tilde{b}_a}^T & ^G{\tilde{p}_I}^T \end{bmatrix}^T $$

와 같다. 여기서 쿼터니언을 제외한 나머지 스테이트의 오차 모델은 additive로, 예를 들면 $\tilde{x}=x-\hat{x}$와 같이 정의된다. 쿼터니언은 $\bar{q}=\delta\bar{q}\bigotimes\hat{\bar{q}}$와 같이 정의되는데, 이때 에러 쿼터니언

$$ \delta\bar{q}$는 $\delta\bar{q}\approx\begin{bmatrix}\frac{1}{2}\delta\theta^T & 1 \end{bmatrix}^T $$

이다. 에러 쿼터니언은 추정된 쿼터니언을 그만큼 회전시키면 실제 쿼터니안이 구해지게 된다고 생각하면 된다. 이때 에러 쿼터니언은 $\delta\theta^T$로 표현이 가능하기 때문에 쿼터니언의 오차를 $\delta\theta^T$로 나타내는 것이다.

이제 어떤 시간 $k$에 카메라 시점 $N$개가 스테이트 벡터에 포함되어 있는 상황을 생각해보자.

$$ \hat{\bf{X}}_k=\begin{bmatrix} \hat{X}_{IMU_K}^T & ^{C_1}_G\hat{\bar{q}}^T & ^{G}\hat{p}_{C_1}^T & \cdots & ^{C_N}_G\hat{\bar{q}}^T & ^{G}\hat{p}_{C_N}^T \end{bmatrix}^T $$

여기서 $^{C_i}_G\hat{\bar{q}}^T$, $^{G}\hat{p}_{C_i}^T$는 i번째 카메라 각도와 위치 추정치이다. 따라서 에러 스테이트 벡터는 다음과 같다.

$$ \tilde{\bf{X}}_k=\begin{bmatrix} \tilde{X}_{IMU_K}^T & \delta\theta_{C_1}^T & ^{G}\tilde{p}_{C_1}^T & \cdots & \delta\theta_{C_N}^T & ^{G}\tilde{p}_{C_N}^T \end{bmatrix}^T $$

Propagation

필터 propagation은 연속시간 IMU 시스템 모델을 기반으로 descretization해서 구한다.

1. 연속 시간 시스템 모델링

시간에 따른 IMU 상태변수 변화는 다음과 같다.

$$ ^I_G{\dot{\bar{q}}}(t)=\frac{1}{2}\Omega(w(t))^I_G{\bar{q}}(t) \\ \dot{b}_g(t) = n_{wg}(t)\\ ^G\dot{v}_I(t)= {^Ga(t)}\\ \dot{b}_a(t)=n_{wa}(t)\\ ^G\dot{P}_I(t)={^G{v}_I(t)} $$

${^Ga(t)}$는 G프레임에서의 동체 가속도이고,

$$ w=\begin{bmatrix}w_x & w_y & w_z\end{bmatrix}^T $$

는 I프레임에서의 회전각속도이다. 또한

$$ \Omega(w)=\begin{bmatrix}-[w\times] & w \\ -w^T & 0 \end{bmatrix},\\\\\\ [w\times]=\begin{bmatrix} 0 & -w_z & w_y \\ w_z & 0 & -w_x \\ -w_y & w_x & 0 \end{bmatrix} $$

이다. 자이로와 가속도계 측정치는

$$ w_m=w+C(^I_G\bar{q})w_G+b_g+n_g $$

$$ a_m=C(^I_G\bar{q})(^Ga-^Gg+2[w_G\times] {^GV_I}+[w_g\times]^2 \ {^Gp_I})+b_a+n_a $$

와 같다. 노테이션이 낯설어서 그렇지 일반적인 스트랩다운 IMU 측정치 모델과 동일하다. 여기서 $C(\cdot)$은 회전 행렬, $n_g$, $n_a$는 자이로와 가속도계의 Gaussian noise이다. 여기서 지구 회전 $w_G$가 식에 포함되어 있는 점을 기억하자. 사실 MEMS급 IMU에서는 지구회전각속도를 측정하기 어렵기 때문에 식에서 제외해도 사실상 성능에 큰 영향을 미치지 않지만, 컴퓨팅 파워가 부족한 경우가 아니라면 여전히 고려하는 경우가 optimal case이다.

이제 스테이트 propagation 식의 기댓값을 구하면,

$$ ^I_G{\dot{\hat{\bar{q}}}}=\frac{1}{2}\Omega(\hat{w})^I_G\hat{\bar{q}} \\ \dot{\hat{b}}_g = {\bf 0}_{3\times 1} \\ ^G\dot{\hat{v}}_I=C^T_{\hat{q}}\hat{a}-2[w_G\times] \ {^G\hat{v}_I}-[w_G\times]^2{^G\hat{p}_I}+ {^Gg} \\ \dot{\hat{b}}_a={\bf 0}_{3\times 1}\\ ^G\dot{\hat{P}}_I=^G\hat{v}_I $$

이때 $C_{\hat{q}} = C(^I_G\hat{\bar{q}})$, $\hat{a}=a_m-\hat{b}_a$, $\hat{w}=w_m-\hat{b}_g-C_{\hat{q}}w_G$이다. 연속시간에서의 IMU 에러 스테이트 모델을 선형화하면

$$ \dot{\tilde{X}}_{IMU}=F\tilde{X}_{IMU}+Gn_{IMU} $$

이고, 이때

$$ n_{IMU}=\begin{bmatrix} n^T_g &n^T_{wg} &n^T_a & n^T_{wa} \end{bmatrix}^T $$

은 시스템 노이즈 벡터이다. 노이즈 벡터의 공분산인 $Q_{IMU}$는 센서의 캘리브레이션 단계에서 미리 계산된다.

여기서 F, G는 각각

$$ F=\begin{bmatrix} -[\hat{w}\times] & -I_3 & {\bf 0}_{3\times3} & {\bf 0}_{3\times3}& {\bf 0}_{3\times3}\\ {\bf 0}_{3\times3}& {\bf 0}_{3\times3}& {\bf 0}_{3\times3}& {\bf 0}_{3\times3}& {\bf 0}_{3\times3}\\ -C^T_{\hat{q}}[\hat{a}\times]& {\bf 0}_{3\times3} &-2[w_G\times] & -C^T_{\hat{q}}&-[w_G\times]^2\\ {\bf 0}_{3\times3}& {\bf 0}_{3\times3}& {\bf 0}_{3\times3}& {\bf 0}_{3\times3}& {\bf 0}_{3\times3}\\ {\bf 0}_{3\times3}& {\bf 0}_{3\times3} & I_3 & {\bf 0}_{3\times3}& {\bf 0}_{3\times3} \end{bmatrix} $$

$$ G=\begin{bmatrix} -I_3 & {\bf 0}_{3\times3} & {\bf 0}_{3\times3}& {\bf 0}_{3\times3}\\ {\bf 0}_{3\times3}& I_3 & {\bf 0}_{3\times3}& {\bf 0}_{3\times3}\\ {\bf 0}_{3\times3}& {\bf 0}_{3\times3} & -C^T_{\hat{q}}& {\bf 0}_{3\times3}\\ {\bf 0}_{3\times3}& {\bf 0}_{3\times3}& {\bf 0}_{3\times3}& I_3\\ {\bf 0}_{3\times3}& {\bf 0}_{3\times3} & {\bf 0}_{3\times3}& {\bf 0}_{3\times3} \end{bmatrix} $$

와 같다.

2. 불연속 시간 적용

IMU 측정치가 업데이트될 때마다 EKF 추정치는 5차 Runge-Kutta 수치적분을 이용해 propagation 된다. 앞서 설명했듯이 MSCKF는 IMU 스테이트와 카메라 시점 스테이트가 augment되어있기 때문에 covariance 계산 시 이를 임의로 partitioning해주게 된다.

$$ P_{k|k}=\begin{bmatrix} P_{II_{k|k}} & P_{IC_{k|k}} \\ {P_{IC_{k|k}}}^T & P_{CC_{k|k}} \end{bmatrix} $$

$P_{II_{k|k}}$는 15$\times$15 IMU 공분산 행렬이고, $P_{CC_{k|k}}$는 $6N\times6N$ 카메라 시점 추정치의 공분산이고, $P_{IC_{k|k}}$는 IMU-카메라의 공분산이다. 이를 이용해 공분산은 propagate하면

$$ P_{k+1|k}=\begin{bmatrix} P_{II_{k+1|k}} & \Phi(t_k+T,t_k) P_{IC_{k|k}} \\ {P_{IC_{k|k}}}^T\Phi(t_k+T,t_k)^T & P_{CC_{k|k}} \end{bmatrix} $$

이고, $P_{II_{k+1|k}}$는 Lyapunov eqution인 $$ \dot{P}_{II}=FP_{II}+P_{II}F^T+GQ_{IMU}G^T $$ 를 수치적분한 결과이다. State transition matrix인 $\Phi$ 역시 $$ \dot{\Phi}(t_k+\tau,t_k)=F\Phi(t_k+\tau,t_k), \ \tau \in [0,T] $$ 의 수치적분을 통해 얻어진다.

3. 스테이트 Augmentation

새로운 이미지가 입력될 때마다 카메라 시점 추정치는 IMU 자세 추정치로부터 업데이트된다.

$$ ^C_G\hat{\bar{q}}={^C_I\bar{q}}\otimes {^I_G\hat{\bar{q}}}\\\\\\ {^G\hat{p}}_C={^G\hat{p}}_I+C^T_{\hat{q}}{^I\hat{p}}_C $$

이 추정치는 스테이트 벡터에 augment되고, 공분산 행렬도 마찬가지로 augment된다.

$$ P_{k|k} \leftarrow \begin{bmatrix}I_{6N+15}\\J\end{bmatrix} P_{k|k} \begin{bmatrix}I_{6N+15}\\J\end{bmatrix}^T $$

여기서 피처 자세와 위치의 스테이크가 각각 3개씩이므로 6N개가 augment되고, 자코비안은

$$ J=\begin{bmatrix} C(^C_I\bar{q}) & 0_{3\times9}& 0_{3\times3}& 0_{3\times6N}\\ [C^T_{\hat{q}}{^Ip_C}\times] & 0_{3\times9}& I_{3}& 0_{3\times6N} \end{bmatrix} $$

4. 측정치 모델

Residual $r$은 다음과 같이 정의된다.
$$
r=H\tilde{X}+\text{noise}
$$
$H$는 측정치 자코비안이고, 노이즈는 스테이트 에러와 uncorrelated인 Gaussian noise이다.

이 논문의 핵심 아이디어인 정적 피처를 여러 카메라 관점에서 관찰하는 경우, 각 관점에 제한조건이 생긴다는 것을 이용하자. 그런데 연속적인 관찰로 정적인 피처를 찾아냈다고 하자. 이 피처 정보를 사용하기 위해서는 스테이트를 정의해야 한다. 일반적인 접근과는 다르게 각 카메라 시점이 아닌 피처 단위로 정보를 사용한다. 동일한 피처에 대한 조건식을 이용해 피처 위치를 스테이트로 넣지 않으면서도 피처 정보를 사용할 수 있다.

어떤 피처 $f_j$가, 카메라 시점 $(^{C_i}G\bar{q},{^Gp{C_i}})$ 집합인 $M_j$에 의해 관찰됐다고 하자. 각 카메라 시점 $M_j$에 의한 관찰된 피처는 모델에 의해서

$$ Z^{(j)}_i=\frac{1}{^{C_i}Z_j}\begin{bmatrix}^{C_i}X_j \\ ^{C_i}Y_j\end{bmatrix}+n^{(j)}_i $$

과 같다. 이때 $n^{(j)}_i$는 이미지 노이즈 벡터로, 공분산 $R^{(j)}_i=\sigma^2_{im}I_2$를 갖는다. 카메라 프레임에서의 피처 위치는

$$ ^{C_i}p_{f_j}=\begin{bmatrix}^{C_i}X_{f_j} \\ ^{C_i}Y_{f_j} \\ ^{C_i}Z_{f_j}\end{bmatrix}=\displaystyle C(^{C_i}_G\bar{q})(^{G}p_{f_j}-^{G}p_{C_i}) $$

$^{G}p_{f_j}$는 G프레임에서의 피처 위치이다. 이 위치를 찾기 위해 least-square 방법으로 추정치인 $^{G}\hat{p}_{f_j}$를 구한다. (Appendix 참고)

이제 Residual

$$ r^{(j)}_i=z^{(j)}_i-\hat{z}^{(j)}_i $$

을 계산할 수 있고, 이를 선형화하면

$$ r^{(j)}_i\approx H^{(j)}_{X_i}\tilde{X}+H^{(j)}_{f_i} \ {^G\tilde{p}_{f_j}}+n^{(j)}_i $$

인데 이때 $H^{(j)}_{X_i}\tilde{X}$와 $H^{(j)}_{f_i}$는 각각 스테이트와 피처 위치에 대한 측정치 자코비안이다. 이제 모든 $M_j$ 측정치에 대해 residual을 쌓으면

$$ r^{(j)}\approx H^{(j)}_{X}\tilde{X}+H^{(j)}_{f} \ {^G\tilde{p}_{f_j}}+n^{(j)} $$

을 얻을 수 있다.

사실 ${^G\tilde{p}_{f_j}}$는 스테이트 추정치 $X$를 사용해 계산하기 때문에 $\tilde{X}$와 독립이 아니다. 따라서 EKF 측정치 방정식을 그대로 사용할 수 없다. 이를 해결하기 위해서 새로운 residual $r_o^{(j)}$를 정의한다. $r^{(j)}$를 $H_f^{(j)}$의 left nullspace로 projection하면 된다. $A$가 left nullspace의 basis를 column으로 하는 unitary 행렬일 때,

$$ \begin{align} r_o^{(j)}&=A^T(z^{(j)}-\hat{z}^{(j)})\approx A^TH_{X}^{(j)}\tilde{X}+A^Tn^{(j)}\\\\\\ &=H_{o}^{(j)}\tilde{X}^{(j)}+n_{o}^{(j)} \end{align} $$

를 얻을 수 있다. 왜냐하면 $2M_j\times3$ 행렬 $H_f^{(j)}$는 full column rank기 때문에 left nullspace는 $2M_j-3$의 크기를 가진다. 따라서 $r_o^{(j)}$도 같은 크기를 갖는다.

이제 피처 위치 오차가 스테이트 오차와 독립이므로, $M_j$로부터 얻어지는 측정치 $z_i^{(j)}$의 모든 정보를 사용할 수 있고, 따라서 EKF가 sub optimal이 된다.(원문에서는 선형 오차를 제외한 optimal이라고 표현함)

5. EKF 업데이트

이전 섹션에서 여러 시점의 카메라 측정치로부터 얻어지는 기하 관계를 이용해 정적 피처를 관찰하는 경우의 측정치 방정식을 다루었다. EKF 업데이트는 다음 두 경우에 이루어진다.

여러 이미지에서 관측하고 있는 피처가 더 이상 추적 불가능할 경우. 섹션4에서 설명한 방법으로 측정치가 처리된다. 피처가 카메라 FOV에서 벗어나는 경우가 이에 해당하기 때문에 가장 빈번하게 일어나는 경우이다.
섹션 3에서 다루었듯이 새로운 이미지가 입력될 때마다 카메라 시점 추정치가 스테이트 벡터에 포함된다. 만일 최대 카메라 시점 개수 $N_{MAX}$에 도달한 경우 최소 1개 이상의 이전 시점은 삭제된다. 삭제되기 전 모든 피처 관찰 정보가 사용된다. 본 논문에서는 전체 개수 중 1/3의 오래된 시점을 업데이트한 다음 삭제한다. *가장 오래된 시점 1개는 삭제하지 않고 남겨두어 긴 baseline을 가지도록 하고 있다.

어떤 time step에 L개의 피처가 추적되고 있고, 위의 두 조건에 의해 이 피처 정보를 처리해야 한다고 하자. 이제 $j=1,\cdots,L$일 때, Residual $r_o^{(j)}$과 측정치 자코비안 $H_o^{(j)}$을 각 피처에 대해서 계산해야 한다. 이 Residual을 stacking해서
$$
r_o=H_X\tilde{X}+n_o
$$
를 만들 수 있다.

이때 각 피처 측정치는 확률적으로 독립이므로 노이즈 벡터의 공분산은 $R_o=\sigma^2_{im}I_d$와 같이 나타난다. 문제는 $d=\sum_{j=1}^L(2M_j-3)$가 매우 크다는 것이다. 예를 들어 피처 10개가 시점 5개에 관찰되는 경우, $d=10\cdot(2\cdot5-3)=70$이 된다. 이를 해결하기 위해 자코비안 $H_X$에 QR decomposition을 사용한다.

$$ H_X=\begin{bmatrix} Q_1 & Q_2 \end{bmatrix} \begin{bmatrix} T_H \\ 0 \end{bmatrix} $$

$Q_1$과 $Q_2$는 $H_X$의 column space(원문에서는 range of $H_X$)와 nullspace의 basis로 이루어진 unitrary 행렬이다. $T_H$는 upper-triangle 행렬이다. $Q_1$이 unitary기 때문에 위 식을 전개하고 stacked residual에 대입하면 $$ r_o=Q_1T_H\tilde{X}+n_o\\\\\\ Q_1^Tr_o=T_H\tilde{X}+Q_1^Tn_o\\\\\\ \therefore r_n:=Q_1^Tr_o=T_H\tilde{X}+n_n $$ 와 같이 정리할 수 있다. $n_n$의 공분산은 $$ R_n=Q_1^TR_oQ_1=\sigma_{im}^2I_r $$ r은 $Q_1$의 column 수이다. 이제 칼만 게인은 $$ K=PT_H^T(T_HPT_H^T+R_n)^{-1} $$ 이며, correction은 $$ \Delta X=Kr_n $$ 이다. 마지막으로 스테이트 공분산 행렬은 $$ P_{k+1|k+1}=(I_\xi-KT_H)P_{k+1|k}(I_\xi-KT_H)^T+KR_nK^T $$ 와 같이 업데이트할 수 있다. 이때 $\xi=6N+15$는 공분산 행렬의 차원 이다.

계산량 부분 읽어봐야함!