[SLAM] VINS-mono 논문 리뷰 (+ IMU preintegration)

본 포스팅은 Yibin Wu의 "Formula Derivation and Analysis of the VINS-Mono" 페이퍼를 주로 참고하여 작성하였다.

1. Introduction

카메라와 IMU 좌표계를 시각화하면 다음과 같다. 두 센서는 단일 보드에 고정되어 있으며 보드를 캘리브레이션하면 두 센서 사이의 상대 포즈인 외부 파라미터(extrinsic paramter) $\mathbf{T}^{b}_{C}$를 구할 수 있다.

 
NOMENCLATURE

• 행렬은 굵은(bold) 대문자로 표기한다 e.g., $\mathbf{R}$
• 벡터는 굵은 소문자로 표기한다 e.g., $\mathbf{a}$
• 스칼라는 일반 소문자로 표기한다 e.g., a
• 벡터 변환과 관련된 좌표계는 위첨자$^{a}$와 아래첨자$_{a}$로 표기한다 e.g., $\mathbf{R}^{b}_{w}$ (from world to body frame). $\mathbf{R}^{b}_{w}\mathbf{v}^{w} = \mathbf{v}^{b}$
• 벡터의 경우 위첨자$^{a}$는 해당 좌표계에서 본 벡터를 의미한다 e.g., $\mathbf{g}^{w}$
• $\hat{*}$은 추정(estimated)한 값은 의미한다 e.g., $\hat{\mathbf{p}}$
• $\tilde{*}$은 관측(observed)한 값을 의미한다 $\tilde{\mathbf{p}}$
• $\mathbf{a}_{x}$는 벡터의 x 성분을 의미한다     
• $[*]_{\times}$는 벡터 $*$의 반대칭행렬(skew-symmetric matrix)를 의미한다 e.g., $\mathbf{x} = \begin{bmatrix} x,y,z \end{bmatrix}^{\intercal}$일 때 $[\mathbf{x}]_{\times} = \begin{bmatrix} 0 & -z & y \\ z & 0 & -x \\ -y & x & 0 \end{bmatrix}$
• $\mathbf{R}\{*\}$은 $*$을 회전행렬로 변환하는 것을 의미한다 e.g., $\mathbf{R}\{\mathbf{I} + [\delta \boldsymbol{\theta}]_{\times}\}$
• $\otimes$는 쿼터니언 곱을 의미한다 e.g., $\mathbf{q}_{1} \otimes \mathbf{q}_{2}$       
• $\Omega_{L}(\boldsymbol{\omega})$은 순수 쿼터니언 $\boldsymbol{\omega} = [0, \omega_{x}, \omega_{y}, \omega_{z}]$가 주어졌을 때 쿼터니언 좌측(left) 곱셈 연산자를 의미한다. 즉, $\Omega_{L} = \begin{bmatrix} 0 & -\boldsymbol{\omega}^{\intercal} \\ \boldsymbol{\omega} & [\boldsymbol{\omega}]_{\times} \end{bmatrix}$이다. 이와 반대로, $\Omega_{R}(\boldsymbol{\omega})$은 쿼터니언 우측(right) 곱셈 연산자를 의미하며 $\Omega_{R}(\boldsymbol{\omega}) = \begin{bmatrix} 0 & -\boldsymbol{\omega}^{\intercal} \\ \boldsymbol{\omega} & -[\boldsymbol{\omega}]_{\times} \end{bmatrix}$이다. 자세한 내용은 \ref{ref:3}의 (19)를 참조하면 된다.
• $\mathbf{b}_{g}$는 $\mathbf{b}_{w}$와 동일하게 gyro bias를 의미한다. 본 자료에서는 통일성을 위해 $\mathbf{b}_{g}$만 사용하였다. 
• $\mathbf{I} \in \mathbb{R}^{3\times3}$은 3x3 항등행렬을 의미한다.

 

2. Imu Preintegration

2.1. IMU Preintegration in Continuous Time
SLAM에 사용하는 IMU 센서는 일반적으로 카메라 센서보다 훨씬 빠른 속도로 데이터를 취득한다. 카메라가 15-30 [Hz]의 속도로 데이터를 취득하면 IMU 센서는 100-1000 [Hz]의 속도로 데이터를 취득한다. 따라서 일반적으로 두 개의 이미지 좌표계 사이에는 수 십개의 IMU 데이터가 존재한다. 이 때, 모든 IMU 데이터를 tightly-coupled nonlinear optimization에 사용하는 경우 엄청난 연산량 부하가 발생한다. 따라서 최근 들어 두 개의 이미지 좌표계(또는 키프레임) 사이에 존재하는 여러 IMU 데이터를 하나의 factor로 변환하여 연산량 부하를 줄이는 방법이 연구되었는데 이러한 방법을 IMU Preintegration이라고 한다.

3차원 공간 상에 IMU 센서가 존재한다고 하자. IMU 센서는 중력의 영향을 지속적으로 받기 때문에 IMU 가속도계에서 측정되는 값은 중력을 제거해야 실제로 센서가 움직이는 가속도가 된다.

\begin{equation}
  \begin{aligned}
\mathbf{f}^{b} = \mathbf{a}^{b} - \mathbf{g}^{b}
  \end{aligned}
\end{equation}
- $b$ : 바디 좌표계 (= IMU 좌표계) 
- $\mathbf{f}$ : 최종 IMU 가속도 
- $\mathbf{a}$ : 측정된 가속도 
- $\mathbf{g}$ : 중력 벡터 
 

2.1.1. IMU noise and bias

IMU 좌표계에서 측정되는 IMU 데이터는 센서의 움직임에 따른 가속도, 각속도의 변화와 중력의 성분을 포함하고 있으며 IMU 제조 과정에서 필연적으로 발생하는 bias의 영향과 센서 자체의 noise의 영향을 받는다. 따라서 IMU 센서를 통해 측정되는 값에는 다음 성분들이 포함되어 있다.

\begin{equation}
  \begin{aligned}
& \hat{\mathbf{a}}_{t} = \mathbf{a}_{t} + \mathbf{b}_{a_{t}} + \mathbf{R}^{t}_{w}\mathbf{g}^{w} + \mathbf{n}_{a} \\
& \hat{\boldsymbol{\omega}}_{t} = \boldsymbol{\omega}_{t} + \mathbf{b}_{g_{t}} + \mathbf{n}_{g}
  \end{aligned}
\end{equation}
- $\hat{\mathbf{a}}_{t}$ : $t$ 시간의 측정된 가속도
- $\hat{\boldsymbol{\omega}}_{t}$ : $t$ 시간의 측정된 각속도
- $\mathbf{a}_{t}$ : $t$ 시간의 실제 가속도
- $\boldsymbol{\omega}_{t}$ : $t$ 시간의 실제 각속도 
- $\mathbf{g}^{w} = [0,0,9.8]^{\intercal}$ : 월드 좌표계에서 본 중력 벡터
- $\mathbf{R}^{t}_{w}$ : (from world to t) 회전행렬 
- $\mathbf{b}_{at}, \mathbf{b}_{gt}$ : $t$ 시간의 가속도 bias와 각속도 bias 값.

• $\dot{\mathbf{b}}_{at} = \mathbf{n}_{b_{a}}$, $\dot{\mathbf{b}}_{gt} = \mathbf{n}_{b_{g}}$
• $\mathbf{n}_{b_{a}} \sim \mathcal{N}(0, \sigma^{2}_{b_{a}})$, $\mathbf{n}_{b_{g}} \sim \mathcal{N}(0, \sigma^{2}_{b_{g}})$.

- $\mathbf{n}_{a}, \mathbf{n}_{g}$ : 가속도와 각속도의 노이즈.

• $\mathbf{n}_{a} \sim \mathcal{N}(0, \sigma^{2}_{a})$, $\mathbf{n}_{g} \sim \mathcal{N}(0, \sigma^{2}_{g})$

따라서 IMU 센서를 통해 들어오는 중력, 노이즈, bias를 포함한 가속도와 각속도 $\hat{\mathbf{a}}_{t}, \hat{\boldsymbol{\omega}}_{t}$이므로 실제 IMU 센서의 가속도와 각속도 값을 별도로 구해야 한다. 실제 구하고자 하는 값 $\mathbf{a}_{t}, \boldsymbol{\omega}_{t}$를 기준으로 식을 정리하면 다음과 같다.

\begin{equation}
  \begin{aligned}
    & \mathbf{a}_{t} = \hat{\mathbf{a}}_{t} - \mathbf{b}_{a_{t}}  - \mathbf{n}_{a} - \mathbf{R}^{t}_{w}\mathbf{g}^{w}\\
& \boldsymbol{\omega}_{t} = \hat{\boldsymbol{\omega}}_{t} - \mathbf{b}_{g_{t}} - \mathbf{n}_{g}
  \end{aligned}
\end{equation}

2.1.2. IMU Preintegration

두 개의 이미지 좌표계에 대응하는 IMU 좌표계 $b_{k}, b_{k+1}$이 존재할 때, IMU로부터 위치(position), 속도, 각도 값들은 다음과 같이 구할 수 있다.

\begin{equation}
  \label{eq:0}
  \begin{aligned}
    & \mathbf{p}^{\omega}_{b_{k+1}} && = \mathbf{p}^{\omega}_{b_{k}} + \mathbf{v}^{\omega}_{b_{k}} \Delta t_{k} + \iint_{t \in [k,k+1]} \mathbf{R}^{\omega}_{t}\mathbf{a}_{t} \  dt^{2} \\
    & \mathbf{v}^{\omega}_{b_{k+1}} && = \mathbf{v}^{\omega}_{b_{k}} + \int_{t \in [k,k+1]} \mathbf{R}^{\omega}_{t}\mathbf{a}_{t} \ dt \\
    & \mathbf{q}^{\omega}_{b_{k+1}} && = \mathbf{q}^{\omega}_{b_{k}} \otimes \int_{t \in [k,k+1]} \dot{\mathbf{q}} \ dt \\
    & && = \mathbf{q}^{\omega}_{b_{k}} \otimes \int_{t \in [k,k+1]} \frac{1}{2} \Omega _{R}(\boldsymbol{\omega}_{t})\mathbf{q}^{b_{k}}_{t} dt
  \end{aligned}
\end{equation}
 
앞서 유도한 공식으로 $\mathbf{a}_{t}, \boldsymbol{\omega}_{t}$를 치환하여 다시 표현하면 다음과 같다.

\begin{equation}
  \label{eq:1}
  \begin{aligned}
    & \mathbf{p}^{\omega}_{b_{k+1}} = \mathbf{p}^{\omega}_{b_{k}} + \mathbf{v}^{\omega}_{b_{k}} \Delta t_{k} + \iint_{t \in [k,k+1]} (\mathbf{R}^{\omega}_{t}(\hat{\mathbf{a}}_{t} - \mathbf{b}_{at} - \mathbf{n}_{a}) - \mathbf{g}^{w}) dt^{2} \\
    & \mathbf{v}^{\omega}_{b_{k+1}} = \mathbf{v}^{\omega}_{b_{k}} + \int_{t \in [k,k+1]} (\mathbf{R}^{\omega}_{t}(\hat{\mathbf{a}}_{t} - \mathbf{b}_{at} - \mathbf{n}_{a}) - \mathbf{g}^{w}) dt \\
    & \mathbf{q}^{\omega}_{b_{k+1}} = \mathbf{q}^{\omega}_{b_{k}} \otimes \int_{t \in [k,k+1]} \frac{1}{2} \Omega_{R} (\hat{\boldsymbol{\omega}}_{t} - \mathbf{b}_{gt} - \mathbf{n}_{g})\mathbf{q}^{b_{k}}_{t} dt
  \end{aligned}
\end{equation}
- $\mathbf{p}^{\omega}_{b_{k+1}}$ : 월드 좌표계에서 바라본 $b_{k+1}$ 좌표계의 위치
- $\mathbf{v}^{\omega}_{b_{k+1}}$ : 월드 좌표계에서 바라본 $b_{k+1}$ 좌표계의 속도 
- $\mathbf{q}^{\omega}_{b_{k+1}}$ : 월드 좌표계에서 바라본 $b_{k+1}$ 좌표계의 방향(orientation) 
- $\Omega_{R}(\boldsymbol{\omega}) = [\boldsymbol{\omega}]_{R} = \begin{bmatrix} 0 & -\boldsymbol{\omega}^{\intercal} \\ \boldsymbol{\omega} & -[\boldsymbol{\omega}]_{\times} \end{bmatrix}$ where,  $\quad [\boldsymbol{\omega}]_{\times} = \begin{bmatrix} 0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & -\omega_{x} \\ -\omega_{y} & \omega_{x} & 0 \end{bmatrix}$ 
- $\otimes$ : 쿼터니언 곱셈 연산자 (e.g., $\mathbf{q} = \mathbf{q}_{1} \otimes \mathbf{q}_{2}$)

NOTICE: 이 때, $\mathbf{g}^{w}$는 실제 월드 좌표계 상의 중력을 나타내는 것이 아닌 IMU 시스템의 초기 좌표계 $\mathbf{f}^{b}_{0}$에서 중력 벡터를 의미한다.

(\ref{eq:1}) 쿼터니언 부분에서 $\int_{t \in [k,k+1]} \frac{1}{2} \Omega (\hat{\boldsymbol{\omega}}_{t} - \mathbf{b}_{gt} - \mathbf{n}_{g})\mathbf{q}^{b_{k}}_{t} dt$ 부분은 $\int \dot{\mathbf{q}} \ dt$ 형태이고 이는 다음과 같이 유도할 수 있다.

\begin{equation}
  \begin{aligned}
    \dot{\mathbf{q}} & = \lim_{\delta t \rightarrow 0} \frac{\mathbf{q}(t+\delta t) - \mathbf{q}(t)}{\delta t} \\
    & = \lim_{\delta t \rightarrow 0} \frac{\mathbf{q}(t) \otimes \delta \mathbf{q} - \mathbf{q}(t)}{\delta t} \\
    & = \lim_{\delta t \rightarrow 0} \frac{\mathbf{q}(t) \otimes (\begin{bmatrix} 1 \\ \frac{1}{2}\boldsymbol{\omega}_{t} \end{bmatrix} - \begin{bmatrix} 1 \\ 0 \end{bmatrix})}{\delta t} \\
    & = \frac{1}{2} \mathbf{q} \otimes \boldsymbol{\omega}_{t} = \frac{1}{2} \Omega_{R}(\boldsymbol{\omega}_{t})\mathbf{q}
  \end{aligned}
\end{equation}
- 충분히 작은 쿼터니언 $\delta \mathbf{q}$에 대해 근사적으로 $\delta \mathbf{q} \approx \begin{bmatrix} 1 \\ \frac{1}{2}\delta \boldsymbol{\theta} \end{bmatrix} = \begin{bmatrix} 1 \\ \frac{1}{2}\boldsymbol{\omega}_{t} \end{bmatrix}$가 성립한다 

다음으로 레퍼런스 좌표계를 월드 좌표계 $\{w\}$에서 IMU 좌표계 $\{b_{k}\}$로 변경한다. 레퍼런스 좌표계를 $\{b_{k}\}$로 변경해야 $\{b_{k}\}$와 $\{b_{k+1}\}$ 사이의 위치, 속도, 각도를 적분한 preintegration 값을 구할 수 있다.

\begin{equation}
  \label{eq:2}
  \begin{aligned}
    & \mathbf{R}^{b_{k}}_{w} \mathbf{p}^{w}_{b_{k+1}} = \mathbf{R}^{b_{k}}_{w} (\mathbf{p}^{w}_{b_{k}} + \mathbf{v}^{w}_{b_{k}}\Delta t - \frac{1}{2}\mathbf{g}^{w} \Delta t_{k}^{2}) + \alpha^{b_{k}}_{b_{k+1}} \\
    & \mathbf{R}^{b_{k}}_{w}\mathbf{v}^{w}_{b_{k+1}} = \mathbf{R}^{b_{k}}_{w} (\mathbf{v}^{w}_{b_{k}} - \mathbf{g}^{w} \Delta t_{k}) + \beta^{b_k}_{b_{k+1}} \\
    & \mathbf{q}^{b_{k}}_{w} \otimes \mathbf{q}^{w}_{b_{k+1}} = \gamma^{b_{k}}_{b_{k+1}}
  \end{aligned} 
\end{equation}
- $\mathbf{R}^{b_{k}}_{w}$ : (from world to $b_{k}$) 회전행렬

위 그림에서 빨간색과 파란색으로 하이라이팅한 부분은 $b_{k}, b_{k+1}$ 두 시점에서 IMU 값을 의미하며 초록색으로 하이라이팅한 부분이 $[b_{k},b_{k+1}]$ 시간 사이에 preintegration되는 $\alpha^{b_{k}}_{b_{k+1}}, \beta^{b_{k}}_{b_{k+1}}, \gamma^{b_{k}}_{b_{k+1}}$ 값을 의미한다. 이를 자세히 풀어쓰면 다음과 같다.

\begin{equation}
  \label{eq:3}
  \begin{aligned}
    & \alpha^{b_{k}}_{b_{k+1}} = \iint_{t \in [k,k+1]} \mathbf{R}^{b_{k}}_{t} (\hat{\mathbf{a}}_{t} - \mathbf{b}_{at} - \mathbf{n}_{a}) dt^{2} \\
    & \beta^{b_{k}}_{b_{k+1}} = \int_{t \in [k,k+1]} \mathbf{R}^{b_{k}}_{t} (\hat{\mathbf{a}}_{t} - \mathbf{b}_{at} -\mathbf{n}_{a}) dt \\
    & \gamma^{b_{k}}_{b_{k+1}} = \int_{t \in [k,k+1]} \frac{1}{2} \Omega_{R}(\hat{\boldsymbol{\omega}}_{t} - \mathbf{b}_{gt} - \mathbf{n}_{g})\gamma^{b_{k}}_{t} dt
  \end{aligned}
\end{equation}
 

preintegration을 수행하면 VIO로 인해 IMU 포즈가 업데이트될 때마다 $t \in [k,k+1]$ 사이의 수많은 IMU 데이터에 일일히 repropagation을 수행할 필요없이 preintegration된 IMU 값들만 업데이트하면 되므로 연산량이 감소하는 효과가 있다.

2.2. IMU Preintegration in Discrete Time

지금까지 유도한 (\ref{eq:3})은 연속 신호(continuous signal)에서 사용 가능한 공식이다. 하지만 실제 IMU 신호는 이산 신호(discrete signal)로 들어오므로 미분 방정식(differential equation)을 차분 방정식(difference equation)으로 표현해야 한다. 해당 과정에서 미분방정식의 수치 해법들이 사용되는데, 대표적으로 zero-order hold(Euler), first-order hold(Midpoint), higher order (RK4) 등이 존재한다. 이 중 VINS-mono에서 사용한 Mid-point method를 사용해 차분 방정식을 표현하면 다음과 같다.
\begin{equation}
  \begin{aligned}
    \hat{\alpha}^{b_{k}}_{i+1} & = \hat{\alpha}^{b_{k}}_{i} + \frac{1}{2}(\hat{\beta}^{b_{k}}_{i} + \hat{\beta}^{b_{k}}_{i+1})\delta t \\
    & = \hat{\alpha}^{b_{k}}_{i} + \hat{\beta}^{b_{k}}_{i} \delta t + \frac{1}{4}[\mathbf{R}\{\hat{\gamma}^{b_{k}}_{i}\}(\hat{\mathbf{a}}_{i} - \mathbf{b}_{ai}) + \mathbf{R}\{\hat{\gamma}^{b_{k}}_{i+1}\}(\hat{\mathbf{a}}_{i+1} - \mathbf{b}_{ai})]\delta t^{2} \\  
    \hat{\beta}^{b_{k}}_{i+1} & = \hat{\beta}^{b_{k}}_{i} + \frac{1}{2}[\mathbf{R}\{\hat{\gamma}^{b_{k}}_{i}\}(\hat{\mathbf{a}}_{i} - \mathbf{b}_{ai}) + \mathbf{R}\{\hat{\gamma}^{b_{k}}_{i+1}\}(\hat{\mathbf{a}}_{i+1} - \mathbf{b}_{ai})]\delta t \\
    \hat{\gamma}^{b_{k}}_{i+1} & = \hat{\gamma}^{b_{k}}_{i} \otimes \hat{\gamma}^{b_{k}}_{i, i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \begin{bmatrix} 1 \\ 1/4(\hat{\boldsymbol{\omega}}_{i} + \hat{\boldsymbol{\omega}}_{i+1} - 2 \mathbf{b}_{gi}) \delta t \end{bmatrix} \\
  \end{aligned} \label{eq:11}
\end{equation}
- $\hat{\alpha}, \hat{\beta}, \hat{\gamma}$: 이산 변환한 pretinegration 값들은 수치 해법에 의해 근사되므로 $\hat{(\cdot)}$으로 표기한다 
- $i$ : $[t_{k}, t_{k+1}]$ 시간 사이에서 이산 시간 스텝
- $\delta t$ : $i$와 $i+1$ 사이의 시간 간격 
- $\mathbf{R}\{\gamma^{b_{k}}_{i+1}\}$: $\gamma^{b_{k}}_{i+1}$를 회전행렬로 변환하는 연산자 

비교를 위해 Euler method을 통해 표현하면 다음과 같다.
\begin{equation}
  \begin{aligned}
    \hat{\alpha}^{b_{k}}_{i+1} & = \hat{\alpha}^{b_{k}}_{i} + \hat{\beta}^{b_{k}}_{i} \delta t + \frac{1}{2}\mathbf{R}\{\hat{\gamma}^{b_{k}}_{i+1}\}(\hat{\mathbf{a}}_{i} - \mathbf{b}_{ai})\delta t^{2}    \\
    \hat{\beta}^{b_{k}}_{i+1} & = \hat{\beta}^{b_{k}}_{i} + \mathbf{R}\{\hat{\gamma}^{b_{k}}_{i+1}\}(\hat{\mathbf{a}}_{i} - \mathbf{b}_{ai})\delta t \\
    \hat{\gamma}^{b_{k}}_{i+1} & = \hat{\gamma}^{b_{k}}_{i} \otimes \hat{\gamma}^{b_{k}}_{i, i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \begin{bmatrix} 1 \\  \frac{1}{2}(\hat{\boldsymbol{\omega}}_{i} - \mathbf{b}_{gi}) \delta t \end{bmatrix} \\
  \end{aligned} 
\end{equation}
 

NOTICE: VINS-mono의 저자는 노이즈 $\mathbf{n}_{a}, \mathbf{n}_{g}$ 값을 모르기 때문에 실제 코드 구현 상에서 0으로 놓고 진행하였다. (VINS-mono 논문에 언급됨)
NOTICE: 엄밀하게 말하면 preintegration의 위치 $\alpha^{b_{k}}_{b_{k+1}}$와 속도 $\beta^{b_{k}}_{b_{k+1}}$ 값은 중력을 포함하고 있지 않기 때문에 무중력 공간에서 IMU 센서를 움직일 때 측정되는 위치와 속도로 이해하면 된다.
 
실제 VINS-mono 코드에는 Mid-point method가 구현되어 있다. 따라서 $b_{k}$ 시점부터 $b_{k+1}$ 시점까지 IMU가 입력되는 매 순간 $t \in [k, k+1]$마다 위 (\ref{eq:11}) 공식에 의해 $\hat{\alpha}, \hat{\beta}, \hat{\gamma}$ 값이 업데이트된다. 

2.3. Error-state Kinematics in Continuous Time

다음으로 tightly-coupled VIO에 의해 bias $\mathbf{b}_{a}, \mathbf{b}_{g}$가 최적화되었을 때 이를 기존의 preintegration $\alpha, \beta, \gamma$에 업데이트하기 위한 자코비안 $\mathbf{J}^{b_{k}}_{b_{k+1}}$을 구해야한다. 또한 추후 VIO 과정에 사용하기 위한 preintegration 상태 변수들의 공분산 $\mathbf{P}^{b_{k}}_{b_{k+1}}$을 구해야한다.  이러한 $\mathbf{J}, \mathbf{P}$를 구하기 위해서는 에러 상태 방정식(error-state equation)으로 IMU 상태 변수들을 표현해야 한다.
 
\ref{ref:3} 페이퍼에서 설명한 내용을 참고하여 IMU preinegration의 에러 상태 방정식을 만들어보자. 일반적인 에러 상태 방정식은 다음과 같이 표현할 수 있다.
\begin{equation}
  \begin{aligned}
    \hat{\mathbf{x}} = \mathbf{x} + \delta \mathbf{x}
  \end{aligned}
\end{equation}
- $\hat{\mathbf{x}}$ : true 상태 
- $\mathbf{x}$ : nominal 상태 
- $\delta \mathbf{x}$ : error 상태

nominal 상태는 다음과 같이 자세하게 나타낼 수 있다.
\begin{equation}
  \begin{aligned}
    & \dot{\alpha} = \beta \\
    & \dot{\beta} = \mathbf{R}(\mathbf{a}_{m} - \mathbf{b}_a) \\
    & \dot{\gamma} = \frac{1}{2} \gamma \otimes (\boldsymbol{\omega}_{m} - \mathbf{b}_{g}) \\
    & \dot{\mathbf{b}}_{a} = 0 \\
    & \dot{\mathbf{b}}_{g} = 0 
  \end{aligned}
\end{equation}

이 때, $\mathbf{R}$은 쿼터니언 $\gamma$를 회전행렬로 변환한 행렬을 의미한다. 즉,   $\mathbf{R} \triangleq \mathbf{R}\{\gamma\}$이다. 다음으로 true 상태는 다음과 같이 자세하게 나타낼 수 있다.

\begin{equation}
  \begin{aligned}
    & \dot{\hat{\alpha}} = \hat{\beta} \\
    & \dot{\hat{\beta}} = \hat{\mathbf{R}}(\mathbf{a}_{m}  - \mathbf{n}_{a} - \hat{\mathbf{b}}_a) \\
    & \dot{\hat{\gamma}} = \frac{1}{2} \hat{\gamma} \otimes (\boldsymbol{\omega}_{m} - \mathbf{n}_{g} - \hat{\mathbf{b}}_{g} ) \\
    & \dot{\hat{\mathbf{b}}}_{a} = \mathbf{n}_{b_{a}} \\
    & \dot{\hat{\mathbf{b}}}_{g} = \mathbf{n}_{b_{g}} 
  \end{aligned}
\end{equation}

이 때, 가속도 bias와 각속도 bias는 random walk 모델을 따르며 이들의 미분은 white gaussian noise라고 가정한다. 즉, $\mathbf{n}_{b_{a}} \sim \mathcal{N}(0, \sigma^{2}_{b_{a}}), \mathbf{n}_{b_{g}} \sim \mathcal{N}(0, \sigma^{2}_{b_{g}})$를 만족한다. 마지막으로 error 상태를 자세히 나타내면 다음과 같다.
\begin{equation}
  \begin{aligned}
    & \dot{\delta \alpha} = \delta \beta \\
    & \dot{\delta \beta} = -\mathbf{R}[\mathbf{a}_{m} - \mathbf{b}_{a}]_{\times} \delta \boldsymbol{\theta} - \mathbf{R}\mathbf{n}_{a} - \mathbf{R}\delta \mathbf{b}_{a} \\
    & \dot{\delta \boldsymbol{\theta}} = -[\boldsymbol{\omega}_{m} - \mathbf{b}_{g}]_{\times} \delta \boldsymbol{\theta} - \delta \mathbf{b}_{g} - \mathbf{n}_{g} \\
    &\dot{\delta \mathbf{b}_{a}} = \mathbf{n}_{b_{a}} \\
    & \dot{\delta \mathbf{b}_{g}} = \mathbf{n}_{b_{g}}
  \end{aligned}
\end{equation}
- $\dot{\delta \boldsymbol{\theta}}$: error 상태의 각도 에러는 $\dot{\delta \gamma}$가 아닌 $\dot{\delta \boldsymbol{\theta}}$를 사용하여 표현한 것에 유의한다 

지금까지 정리한 에러 상태 방정식을 정리하면 아래 그림과 같다.

위 차등방정식에서 $\dot{\delta \beta}, \dot{\delta \boldsymbol{\theta}}$은 아래와 같이 유도할 수 있다. 2차항 이상의 값들은 상대적으로 값이 작으므로 무시한다. 우선 $\dot{\delta \beta}$는 다음과 같이 구할 수 있다.

\begin{equation}
  \begin{aligned}
    \dot{\beta} + \dot{\delta \beta} & = \boxed{\dot{\hat{\beta}}} && = \hat{\mathbf{R}}(\mathbf{a}_{m} -\mathbf{n}_{a} - \hat{\mathbf{b}_{a}}) \\
    \mathbf{R}(\mathbf{a}_{m} - \mathbf{b}_{a}) + \dot{\delta \beta} & = && = \mathbf{R}\{\mathbf{I} + [\delta \boldsymbol{\theta}]_{\times}\}(\mathbf{a}_{m} - \mathbf{n}_{a} - \mathbf{b}_{a} - \delta \mathbf{b}_{a}) \\
    \dot{\delta \beta} & = && = -\mathbf{R}[\mathbf{a}_{m} - \mathbf{b}_{a}]_{\times} \delta \boldsymbol{\theta} - \mathbf{R}\mathbf{n}_{a} - \mathbf{R}\delta \mathbf{b}_{a}
  \end{aligned}
\end{equation}
\begin{equation}
  \begin{aligned}
    \boxed{\dot{\delta \beta} = -\mathbf{R}[\mathbf{a}_{m} - \mathbf{b}_{a}]_{\times} \delta \boldsymbol{\theta} - \mathbf{R}\mathbf{n}_{a} - \mathbf{R}\delta \mathbf{b}_{a}}
  \end{aligned}
\end{equation}

다음으로 $\dot{\delta \boldsymbol{\theta}}$는 다음과 같이 구할 수 있다.
\begin{equation}
  \begin{aligned}
    \dot{(\gamma \otimes \delta \gamma)} & = \boxed{ \dot{\hat{\gamma}} } && = \frac{1}{2} \hat{\gamma} \otimes (\boldsymbol{\omega}_{m} - \mathbf{n}_{g} - \hat{\mathbf{b}}_{g}) \\
    \dot{\gamma} \otimes \delta \gamma +\gamma \otimes \dot{\delta \gamma}  & = && = \frac{1}{2}\gamma \otimes \delta \gamma \otimes (\boldsymbol{\omega}_{m} - \mathbf{n}_{g} - \mathbf{b}_{g} - \delta \mathbf{b}_{g}) 
  \end{aligned}
\end{equation}
\begin{equation}
  \begin{aligned}
    \frac{1}{2} \gamma \otimes (\boldsymbol{\omega}_{m} - \mathbf{b}_{g}) \otimes \delta \gamma + \gamma \otimes \dot{\delta \gamma} = \frac{1}{2} \gamma \otimes \delta \gamma \otimes (\boldsymbol{\omega}_{m} - \mathbf{n}_{g} - \mathbf{b}_{g} - \delta \mathbf{b}_{g})
  \end{aligned}
\end{equation}
- $\hat{\boldsymbol{\omega}} = \boldsymbol{\omega}_{m} -\mathbf{n}_{g} - \mathbf{b}_{g} - \delta \mathbf{b}_{g}$ 
- $\boldsymbol{\omega} = \boldsymbol{\omega}_{m} - \mathbf{b}_{g}$ 
라고 치환하여 표현하면 다음과 같다.

\begin{equation}
  \begin{aligned}
    \frac{1}{2} \gamma \otimes \boldsymbol{\omega} \otimes \delta \gamma + \gamma \otimes \dot{\delta \gamma} = \frac{1}{2} \gamma \otimes \delta \gamma \otimes \hat{\boldsymbol{\omega}}
  \end{aligned}
\end{equation}

양변에 공통된 $\gamma \otimes$를 생략하고 정리하면 다음과 같다.
\begin{equation}
  \begin{aligned}
    2 \dot{\delta \gamma}  & = \delta \gamma \otimes \hat{\boldsymbol{\omega}} - \boldsymbol{\omega} \otimes \delta \gamma \\
    & = \Omega_{R}(\hat{\boldsymbol{\omega}}) \delta \gamma - \Omega_{L}(\boldsymbol{\omega})\delta \gamma \\
    & = \begin{bmatrix} 0 & (\boldsymbol{\omega} - \hat{\boldsymbol{\omega}})^{\intercal} \\
      \hat{\boldsymbol{\omega}} - \boldsymbol{\omega} & - [\boldsymbol{\omega} + \hat{\boldsymbol{\omega}}]_{\times}
    \end{bmatrix} \delta \gamma
  \end{aligned}
\end{equation}

위 식을 행렬로 풀어서 표기하면 다음과 같다.
\begin{equation}
  \begin{aligned}
    \begin{bmatrix} 0 \\ \dot{\delta \boldsymbol{\theta}} \end{bmatrix} =
    \begin{bmatrix} 0 & (\delta \mathbf{b}_{g} + \mathbf{n}_{g})^{\intercal} \\
      -\delta \mathbf{b}_{g} - \mathbf{n}_{g} & - [2 \boldsymbol{\omega}_{m} - \mathbf{n}_{g} - 2 \mathbf{b}_{g} - \delta \mathbf{b}_{g}]_{\times}
    \end{bmatrix}
    \begin{bmatrix} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta} \end{bmatrix}
  \end{aligned}
\end{equation}

촤종적으로 $\dot{\delta \boldsymbol{\theta}}$는 다음과 같다.
\begin{equation}
  \boxed{
  \begin{aligned}
     \dot{\delta \boldsymbol{\theta}} = -[\boldsymbol{\omega}_{m} - \mathbf{b}_{g}]_{\times} \delta \boldsymbol{\theta} - \delta \mathbf{b}_{g} - \mathbf{n}_{g}
  \end{aligned} } 
\end{equation}

에러 상태 방정식을 한 번에 표기하면 다음과 같다.
\begin{equation}
  \begin{aligned}
    \begin{bmatrix}
      \dot{\delta \alpha}^{b_{k}}_{t} \\
      \dot{\delta \beta}^{b_{k}}_{t} \\
      \dot{\delta \boldsymbol{\theta}}^{b_{k}}_{t} \\
      \dot{\delta \mathbf{b}_{a_{t}}} \\
      \dot{\delta \mathbf{b}_{g_{t}}} \\
    \end{bmatrix} =
    \begin{bmatrix}
      & \mathbf{I} & & & & \\
      & & -\mathbf{R}^{b_{k}}_{t}[\mathbf{a}_{m}-\mathbf{b}_{a}]_{\times} & -\mathbf{R}^{b_{k}}_{t} &  \\
      & & -[\boldsymbol{\omega}_{m} - \mathbf{b}_{g}]_{\times} & & -\mathbf{I} \\
      & & & & & \\
      & & & & & 
    \end{bmatrix}
    \begin{bmatrix}
      \delta \alpha^{b_{k}}_{t} \\
      \delta \beta^{b_{k}}_{t} \\
      \delta \boldsymbol{\theta}^{b_{k}}_{t} \\
      \delta \mathbf{b}_{a_{t}} \\
      \delta \mathbf{b}_{g_{t}} \\
    \end{bmatrix}  +
    \begin{bmatrix}
      &&&\\
      -\mathbf{R}^{b_{k}}_{t} && \\
      &-\mathbf{I} && \\
      &&\mathbf{I}& \\
      &&&\mathbf{I}
    \end{bmatrix}
    \begin{bmatrix} \mathbf{n}_{a} \\ \mathbf{n}_{g} \\ \mathbf{n}_{b_{a}} \\ \mathbf{n}_{b_{g}} \end{bmatrix}
  \end{aligned}
\end{equation}

이는 다음과 같이 컴팩트하게 나타낼 수 있다.
\begin{equation}
  \begin{aligned}
    \dot{\delta \mathbf{x}}_{t} = \mathbf{F}_{t} \delta \mathbf{x}_{t} + \mathbf{G}_{t}\mathbf{n}_{t}
  \end{aligned}
\end{equation}

함수의 미분은 다음과 같이 나타낼 수 있다.
\begin{equation}
  \begin{aligned}
    \dot{\mathbf{x}} = \lim_{\delta t \rightarrow 0} \frac{\mathbf{x}(x + \delta t) - \mathbf{x}(t)}{\delta t}
  \end{aligned}
\end{equation}

충분히 작은 $\delta t$에 대해 lim을 생략하고 $\mathbf{x} \rightarrow \delta \mathbf{x}$로 표기하면 $\dot{\delta \mathbf{x}} = \frac{\delta \mathbf{x}(t +\delta t) - \delta \mathbf{x}(t)}{\delta t}$와 같이 나타낼 수 있고 이를 풀어서 쓰면 다음과 같다. 이 때, 편의를 위해 $\mathbf{x}(t) \rightarrow \mathbf{x}_{t}$로 표기한다.
\begin{equation}
  \begin{aligned}
    & \delta \mathbf{x}_{t+\delta t} = \dot{\delta \mathbf{x}}\delta t + \delta \mathbf{x}_t \\
    & \delta \mathbf{x}_{t+\delta t} = \mathbf{F}_{t}\delta \mathbf{x}_{t} + \mathbf{G}_{t}\mathbf{n}_{t}\delta t + \delta \mathbf{x}_{t} \\
    & \boxed {\delta \mathbf{x}_{t+\delta t} = (\mathbf{I} + \mathbf{F}_{t}\delta t)\delta \mathbf{x}_{t} + \mathbf{G}_{t}\mathbf{n}_{t} \delta t}
  \end{aligned} \label{eq:4}
\end{equation}

위 식에 따라 공분산 $\mathbf{P}^{b_{k}}_{b_{k+1}}$은 초기값 $\mathbf{P}^{b_{k}}_{b_{k}}=0$을 시작으로 다음과 같이 업데이트 된다. 이 때, $cov(\mathbf{x}) = \Sigma$ 일 때, $cov(\mathbf{Ax})=\mathbf{A}\Sigma \mathbf{A}^{\intercal}$의 성질을 이용한다.
\begin{equation}
  \begin{aligned}
    \mathbf{P}^{b_{k}}_{t+\delta t} = (\mathbf{I} + \mathbf{F}_{t}\delta t)\mathbf{P}^{b_{k}}_{t}(\mathbf{I}+\mathbf{F}_{t}\delta t)^{\intercal} + (\mathbf{G}_{t}\delta t) \mathbf{Q} (\mathbf{G}_{t}\delta t)^{\intercal}
  \end{aligned} \label{eq:12}
\end{equation}
- $\delta t$: IMU의 샘플링 시간 
- $\mathbf{Q}$: 노이즈에 대한 공분산 값 $\mathbf{Q} = diag(\sigma^{2}_{a}, \sigma^{2}_{g}, \sigma^{2}_{b_{a}}, \sigma^{2}_{b_{g}})$ 

다음으로 1차 근사 자코비안 $\mathbf{J}^{b_{k}}_{b_{k+1}} = \frac{\partial \delta \mathbf{x}^{b_{k}}_{b_{k+1}}}{\partial \delta \mathbf{x}^{b_{k}}_{b_{k}}}$ 값은 초기값 $\mathbf{J}^{b_{k}}_{b_{k}}=\mathbf{I}$을 시작으로 다음과 같이 업데이트 된다.
\begin{equation}
  \begin{aligned}
    \mathbf{J}_{t+\delta t} = (\mathbf{I} + \mathbf{F}_{t}\delta t)\mathbf{J}_{t}, \ \ t \in [k,k+1]
  \end{aligned} \label{eq:13}
\end{equation}

따라서 preintegration의 변수($\alpha^{b_{k}}_{b_{k+1}}, \beta^{b_{k}}_{b_{k+1}}, \gamma^{b_{k}}_{b_{k+1}}$)의 1차 근사로 표현하면 다음과 같다.
\begin{equation}
  \begin{aligned}
    & \alpha^{b_{k}}_{b_{k+1}} \approx \hat{\alpha}^{b_{k}}_{b_{k+1}} + \mathbf{J}^{\alpha}_{b_{a}} \delta \mathbf{b}_{ak} + \mathbf{J}^{\alpha}_{b_{k}}\delta \mathbf{b}_{gk} \\
    & \beta^{b_{k}}_{b_{k+1}} \approx \hat{\beta}^{b_{k}}_{b_{k+1}} + \mathbf{J}^{\beta}_{b_{a}} \delta \mathbf{b}_{ak} + \mathbf{J}^{\beta}_{b_{k}}\delta \mathbf{b}_{gk} \\
    & \gamma^{b_{k}}_{b_{k+1}} = \hat{\gamma}^{b_{k}}_{b_{k+1}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \mathbf{J}^{\gamma}_{b_{g}} \delta \mathbf{b}_{gk} \end{bmatrix}
  \end{aligned} \label{eq:14}
\end{equation}

위 식에서 $\mathbf{J}^{\alpha}_{b_{a}}$는 $\mathbf{J}_{b_{k+1}}$의 $\frac{\partial \delta \mathbf{x}^{b_{k}}_{b_{k+1}}}{\partial \delta \mathbf{b}_{a}}$ 블록을 의미하는 부분행렬이며 다른 행렬도 동일한 의미의 부분행렬이다. 이를 사용하면 VIO로 인해 충분히 작은 bias 값이 업데이트 되었을 때 모든 preintegration에 repropagation을 수행하는 대신 위 식을 통해 근사적으로 업데이트할 수 있다. 이는 많은 연산량이 감소되는 이점을 가진다.
 

2.4. Error-state Kinematics in Discrete Time

지금까지 유도한 에러 상태 방정식은 연속 신호(continuous signal)에 대한 방정식이었다. 하지만 이론과 달리 실제 IMU 데이터는 샘플링 시간 $\delta t > 0$ 간격으로 들어오는 이산 신호(discrete signal)이므로 앞서 설명한 미분 방정식(differential equation)을 차분 방정식(difference equation)으로 변환하는 작업이 필요하다. 이산 변환을 위해서는 Euler method, Midpoint method, RK method 등 다양한 수치 해법들이 필요하다. 본 섹션에서는 실제 VINS-fusion에서 사용한 방법인 Midpoint 방법을 통해 앞서 설명한 에러 상태방정식을 변환한다.

Midpoint numerical integration method: 
Midpoint 수치 해법은 기존의 변수들을 다음과 같이 변환한다.
\begin{equation}
  \begin{aligned}
    & \mathbf{n}_{g_{t}} \rightarrow \frac{\mathbf{n}_{g_{t}} + \mathbf{n}_{g_{t+1}}}{2} \\
    & \boldsymbol{\omega}_{t} \rightarrow \frac{\boldsymbol{\omega}_{t} + \boldsymbol{\omega}_{t+1}}{2} \\
    & \mathbf{R}^{b_{k}}_{t} \rightarrow \frac{1}{2}(\mathbf{R}^{b_{k}}_{t} + \mathbf{R}^{b_{k}}_{t+1}) \\
    & \delta \beta^{b_{k}}_{t} \rightarrow  \frac{\delta \beta^{b_{k}}_{t} +\delta \beta^{b_{k}}_{t+1}}{2}
  \end{aligned}
\end{equation}

위 변환과 앞서 설명했던 (\ref{eq:4})를 활용하여 기존의 에러 상태 방정식을 이산화하면 다음과 같다. 우선 $\dot{\delta \boldsymbol{\theta}}$를 이산화하면 다음과 같다.
\begin{equation}
  \begin{aligned}
    \dot{\delta \boldsymbol{\theta}} = -[\frac{\boldsymbol{\omega}_{t} + \boldsymbol{\omega}_{t+1}}{2} - \mathbf{b}_{g_{t}}]_{\times} \delta \boldsymbol{\theta}^{b_{k}}_{t} -\delta \mathbf{b}_{g_{t}} - \frac{\mathbf{n}_{g_{t}} + \mathbf{n}_{g_{t+1}}}{2}
  \end{aligned}
\end{equation}

위 식을 (\ref{eq:4})에 넣은 후 전개하면 다음과 같다.

\begin{equation}
  \begin{aligned}
    \boxed{ \delta \boldsymbol{\theta}^{b_{k+1}}_{t+1} = \bigg( \mathbf{I} - \delta t [\frac{\boldsymbol{\omega}_{t} + \boldsymbol{\omega}_{t+1} - \mathbf{b}_{g_{t}}}{2}]_{\times} \bigg) \delta \boldsymbol{\theta}^{b_{k}}_{t} - \delta \mathbf{b}_{g_{t}} \delta t - \frac{\mathbf{n}_{g_{t}} + \mathbf{n}_{g_{t+1}}}{2} \delta t }
  \end{aligned} \label{eq:10}
\end{equation}

다음으로 $\dot{\delta \beta^{b_{k}}_{t}}$를 이산화하면 다음과 같다.
\begin{equation}
  \begin{aligned}
    \dot{\delta \beta^{b_{k}}_{t}} = & -\frac{1}{2}\mathbf{R}^{b_{k}}_{t}[\mathbf{a}_{t} - \mathbf{b}_{a_{t}}]_{\times} \delta \boldsymbol{\theta}^{b_{k}}_{t} -\frac{1}{2}\mathbf{R}^{b_{k}}_{t+1}[\mathbf{a}_{t+1} - \mathbf{b}_{a_{t}}]_{\times} \delta \boldsymbol{\theta}^{b_{k}}_{t+1} - \frac{1}{2}(\mathbf{R}^{b_{k}}_{t} + \mathbf{R}^{b_{k}}_{t+1}) \delta \mathbf{b}_{a_{t}} \\ & - \frac{1}{2}(\mathbf{R}^{b_{k}}_{t}  + \mathbf{R}^{b_{k}}_{t+1})\mathbf{n}_{a_{t}}
  \end{aligned}
\end{equation}

위 식에 앞서 계산한 $\delta \boldsymbol{\theta}^{b_{k+1}}_{t+1}$를 치환하고 전개하면 다음과 같은 식을 얻을 수 있다.
\begin{equation}
    \boxed{
  \begin{aligned}
    \delta \beta^{b_{k}}_{t+1} = & \ \delta \beta^{b_{k}}_{t} \\
    & +  \bigg( -\frac{\delta t}{2}\mathbf{R}^{b_{k}}_{t}[\mathbf{a}_{t} - \mathbf{b_{a_t}}]_{\times} - \frac{\delta t}{2} \mathbf{R}^{b_{k}}_{t+1}[\mathbf{a}_{t+1} - \mathbf{b}_{a_{t}}]_{\times} \delta \boldsymbol{\theta}^{b_{k}}_{t+1} \big( \mathbf{I} - \delta t[\frac{\boldsymbol{\omega}_{t} + \boldsymbol{\omega}_{t+1}}{2} - \mathbf{b}_{g_{t}}]_{\times} \big) \bigg) \delta \boldsymbol{\theta}^{b_{k}}_{t} \\
      & + \frac{\delta t^{2}}{2} \mathbf{R}^{b_{k}}_{t+1}(\mathbf{a}_{t+1} - \mathbf{b}_{a_{t}}) - \frac{\delta t^{2}}{4}\mathbf{R}^{b_{k}}_{t+1}(\mathbf{a}_{t+1}-\mathbf{b_{a_{t}}}) (\mathbf{n}_{g_{t}} + \mathbf{n}_{g_{t+1}}) - \frac{\delta t}{2}(\mathbf{R}^{b_{k}}_{t} + \mathbf{R}^{b_{k}}_{t+1})\delta \mathbf{b}_{a_{t}} \\
      & - \frac{\delta t}{2} \mathbf{R}^{b_{k}}_{t} \mathbf{n}_{a_{t}}  - \frac{\delta t}{2} \mathbf{R}^{b_{k}}_{t+1} \mathbf{n}_{a_{t+1}} 
  \end{aligned} }
\end{equation}

마지막으로 $\dot{\delta \alpha^{b_{k}}_{t}}$를 이산화하면 다음과 같다.
\begin{equation}
  \begin{aligned}
    \dot{\delta \alpha^{b_{k}}_{t}} = \frac{1}{2} (\delta \beta^{b_{k}}_{t} + \delta \beta^{b_{k}}_{t+1})
  \end{aligned}
\end{equation}

위 식과 (\ref{eq:4})를 활용하여 $\delta \alpha^{b_{k}}_{t+1}$을 구하면 다음과 같다.
\begin{equation}
  \begin{aligned}
    \boxed{ \delta \alpha^{b_{k}}_{t+1} = \delta \alpha^{b_{k}}_{t}  + \frac{1}{2} \Big(\delta \beta^{b_{k}}_{t} + \delta \beta^{b_{k}}_{t+1} \Big)\delta t   }
  \end{aligned}
\end{equation}

지금까지 구한 에러 상태 방정식을 행렬 형태로 나타내면 다음과 같다.
\begin{equation}
  \begin{aligned}
    \begin{bmatrix}
      \delta \alpha_{k+1} \\
      \delta \boldsymbol{\theta}_{k+1} \\
      \delta \beta_{k+1} \\
      \delta \mathbf{b}_{a_{k+1}} \\
      \delta \mathbf{b}_{g_{k+1}} \\
    \end{bmatrix}
    = \begin{bmatrix}
      \mathbf{I} & \mathbf{F}_{01} & \delta t \mathbf{I} & \mathbf{F}_{03} & \mathbf{F}_{04} \\
      & \mathbf{F}_{11} &  &  & -\delta t \mathbf{I} \\
       & \mathbf{F}_{21} & \mathbf{I} & \mathbf{F}_{23} & \mathbf{F}_{24} \\
      &&&\mathbf{I}& \\
      &&&&\mathbf{I}
    \end{bmatrix} 
    \begin{bmatrix}
      \delta \alpha_{k} \\
      \delta \boldsymbol{\theta}_{k} \\
      \delta \beta_{k} \\
      \delta \mathbf{b}_{a_{k}} \\
      \delta \mathbf{b}_{g_{k}} \\
    \end{bmatrix} +
    \begin{bmatrix}
      \mathbf{G}_{00} & \mathbf{G}_{01} & \mathbf{G}_{02} & \mathbf{G}_{03} &  &  \\
      & -\delta t /2 \mathbf{I} &  & -\delta t /2 \mathbf{I} &  &  \\
      -\frac{\mathbf{R}_{k}\delta t}{2} & \mathbf{G}_{21} & -\frac{\mathbf{R}_{k+1}\delta t}{2} & \mathbf{G}_{23} &  &  \\
      &&&&\delta t \mathbf{I} &  \\
      &&&& & \delta t \mathbf{I}
    \end{bmatrix}
    \begin{bmatrix}
      \mathbf{n}_{a_{k}} \\
      \mathbf{n}_{g_{k}} \\
      \mathbf{n}_{a_{k+1}} \\
      \mathbf{n}_{g_{k+1}} \\
      \mathbf{n}_{b_{a}} \\
      \mathbf{n}_{b_{g}} \\
    \end{bmatrix} 
  \end{aligned}
\end{equation}

각각 부분행렬은 다음과 같다.
\begin{equation}
  \begin{aligned}
    & \mathbf{F}_{01} = -\frac{\delta t^{2}}{4} \mathbf{R}_{k}[\hat{\mathbf{a}}_{k} -\mathbf{b}_{a_{k}}]_{\times} - \frac{\delta t^{2}}{4} \mathbf{R}_{k+1} [\hat{\mathbf{a}}_{k+1} - \mathbf{b}_{a_{k}}]_{\times} \bigg( \mathbf{I} - [\frac{\hat{\boldsymbol{\omega}}_{k} + \hat{\boldsymbol{\omega}}_{k+1}}{2} - \mathbf{b}_{g_{k}}]_{\times} \delta t \bigg) \\
    & \mathbf{F}_{03} = -\frac{\delta t^{2}}{4}(\mathbf{R}_{k} + \mathbf{R}_{k+1}) \\
    & \mathbf{F}_{04} = \frac{\delta t^{3}}{4} \mathbf{R}_{k+1}[\hat{\mathbf{a}}_{k+1} -\mathbf{b}_{a_{k}}]_{\times} \\
    & \mathbf{F}_{11} = \mathbf{I} - [\frac{\hat{\boldsymbol{\omega}}_{k} + \hat{\boldsymbol{\omega}}_{k+1}}{2} - \mathbf{b}_{g_{k}}]_{\times} \delta t \\
    & \mathbf{F}_{21} = -\frac{\delta t}{2} \mathbf{R}_{k}[\hat{\mathbf{a}}_{k} -\mathbf{b}_{a_{k}}]_{\times} - \frac{\delta t}{2} \mathbf{R}_{k+1} [\hat{\mathbf{a}}_{k+1} - \mathbf{b}_{a_{k}}]_{\times} \bigg( \mathbf{I} - [\frac{\hat{\boldsymbol{\omega}}_{k} + \hat{\boldsymbol{\omega}}_{k+1}}{2} - \mathbf{b}_{g_{k}}]_{\times} \delta t \bigg) \\
    & \mathbf{F}_{23} = -\frac{\delta t}{2} (\mathbf{R}_{k} + \mathbf{R}_{k+1}) \\
    & \mathbf{F}_{24} = \frac{\delta t^{2}}{2} \mathbf{R}_{k+1}[\hat{\mathbf{a}}_{k+1} - \mathbf{b}_{a_{k}}]_{\times} \\ \\ 
    & \mathbf{G}_{00} = -\frac{\delta t^{2}}{4} \mathbf{R}_{k} \\
    & \mathbf{G}_{01} = \mathbf{G}_{03} = \frac{\delta t^{3}}{8} \mathbf{R}_{k+1} [\hat{\mathbf{a}}_{k+1} - \mathbf{b}_{a_{k}}]_{\times} \\
    & \mathbf{G}_{02} = -\frac{\delta t^{2}}{4} \mathbf{R}_{k+1} \\
    & \mathbf{G}_{21} = \mathbf{G}_{23} = \frac{\delta t^{2}}{4} \mathbf{R}_{k+1}[\hat{\mathbf{a}}_{k+1} - \mathbf{b}_{a_{k}}]_{\times}
  \end{aligned}
\end{equation}

위 자코비안 $\mathbf{F}, \mathbf{G}$은 VINS-mono 코드 중 integration_base.h :: midPointIntegration() 함수에 구현되어 있다.

VINS-mono에서는 지금까지 구한 에러 상태 방정식을 활용하여 다음과 같은 작업을 수행한다.

• $[b_{k}, b_{k+1}]$ 구간 사이의 매 $t$ 순간마다 공분산 $\mathbf{P}^{b_{k}}_{t}$를 구한다(\ref{eq:12}). 이 때 앞서 구한 이산화된 $\mathbf{F}, \mathbf{G}$ 행렬이 사용된다. 이는 다음 키프레임 생성 시점 $b_{k+1}$까지 지속적으로 계산되며 최종적으로 구해진 $\mathbf{P}^{b_{k}}_{b_{k+1}}$은 VIO를 최적화할 때 사용된다. 
• $[b_{k}, b_{k+1}]$ 구간 사이의 매 $t$ 순간마다 공분산 $\mathbf{J}^{b_{k}}_{t}$를 구한다(\ref{eq:13}). 이 때 앞서 구한 이산화된 $\mathbf{F}, \mathbf{G}$ 행렬이 사용된다. VIO에 의해 bias $\mathbf{b}_{a}, \mathbf{b}_{g}$ 값이 최적화되면 (\ref{eq:14})를 통해 기존 preintegration 값에 업데이트한다.

 

3. Initialization

VINS-mono의 초기화(initialization) 과정은 다음과 같이 4개의 과정으로 구성되어 있다.

3.1. Vision-Only SfM in Sliding Window

초기화 과정은 슬라이딩 윈도우 내에서 vision-only SFM을 수행하는 것부터 시작한다. 우선 두 좌표계 $F_{t}, F_{t+1}$가 시간 순서로 주어졌다고 하자.

1. $F_{t}$에서 30개 이상의 특징점이 추출되고 동시에 $F_{t} \leftrightarrow F_{t+1}$ 사이의 평균 parallex가 20 픽셀 이상 차이가 나면 두 좌표계 사이의 3차원 상대 포즈(relative pose)를 Five-point 방법을 통해 계산한다. 이 때, parallex는 2차원 이미지 평면 상에서 두 픽셀의 거리 차이를 의미한다. 상대 포즈는 스케일 값을 제외하고(up to scale) 추정이 가능하다.
2. 다음으로 $F_{t}$를 레퍼런스 좌표계 $c_0$로 설정한다. $F_{t}$는 항상 첫번째 좌표계를 의미하는 것이 아닌 두 좌표계 $F_{t}, F_{t+1}$ 중 이전 좌표계를 의미한다.
3. 다음으로 두 좌표계 내의 모든 특징점을 triangulation한다.
4. triangulation된 3차원 점들을 사용하여 슬라이드 윈도우 내의 다른 좌표계들의 포즈를 구하기 위해 EPnP를 수행한다.
5. 슬라이드 윈도우 내의 모든 포즈를 최적화하기 위해 bundle adjustment를 수행한다.

 

3.2. Visual-Inertial Alignment

1) Gyroscope Bias Calibration:  vision-only SFM이 모두 수행되고 나면 IMU preinegration된 값과 SFM된 값을 사용하여 여러 파라미터들을 초기화할 수 있다. 우선, 두 키프레임 좌표계 $\{b_{k}\}, \{b_{k+1}\}$이 주어졌을 때 SFM을 통해 구한 $\mathbf{q}^{c_{0}}_{b_{k+1}}, \mathbf{q}^{c_{0}}_{b_{k}}$와 IMU preintegration을 통해 구한 $\gamma^{b_{k}}_{b_{k+1}}$을 사용하여 gyroscope bias 값을 업데이트할 수 있다.
\begin{equation}
  \begin{aligned}
    & \min_{\delta \mathbf{b}_{g}} \sum_{k \in \mathcal{B}} \left\| (\mathbf{q}^{c_{0}}_{b_{k+1}})^{-1} \otimes \mathbf{q}^{c_{0}}_{b_{k}} \otimes \gamma^{b_{k}}_{b_{k+1}} \right\|^{2} \\
    & = \min_{\delta \mathbf{b}_{g}} \sum_{k \in \mathcal{B}} \left\| \mathbf{q}^{b_{k+1}}_{b_{k}} \otimes \gamma^{b_{k}}_{b_{k+1}} \right\|^{2} \\
    & = \min_{\delta \mathbf{b}_{g}} \sum_{k \in \mathcal{B}} \left\| \mathbf{r}_{gyr} \right\|^{2}
  \end{aligned}
\end{equation}

(\ref{eq:10})에서 자코비안 $\mathbf{J}^{\gamma}_{\mathbf{b}_{g}} = -\delta t \mathbf{I}$이므로 이를 통해 다음 식을 업데이트하면서 최적의 $\delta \mathbf{b}_{g}$를 업데이트한다. \begin{equation}
  \begin{aligned}
\gamma^{b_{k}}_{b_{k+1}} \approx \hat{\gamma}^{b_{k}}_{b_{k+1}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \mathbf{J}^{\gamma}_{\mathbf{b}_{g}} \delta \mathbf{b}_{g}  \end{bmatrix}
  \end{aligned} 
\end{equation}

코드 구현에서는 모든 키프레임들에 대해 $\mathbf{J}^{\intercal}\mathbf{J} \delta \mathbf{b}_{g}^{*} = \mathbf{J}^{\intercal}\mathbf{r}_{gyr}$ 형태로 식을 만든 후 cholesky decomposition을 통해 한 번에 최적의 값을 구한다.

2) Velocity, Gravity Vector, and Metric Scale Initialization : 캘리브레이션 된 외부 파라미터(extrinsic paramter) ${\color{red}\mathbf{p}^{b}_{c}}, {\color{blue} \mathbf{q}^{b}_{c}}$를 사용하여 카메라와 IMU 사이의 상대 포즈를 구하면 다음과 같다.
\begin{equation}
  \begin{aligned}
    & s \bar{\mathbf{p}}^{c_{0}}_{b_{k}} = s \bar{\mathbf{p}}^{c_{0}}_{c_{k}} - \mathbf{R}^{c_{0}}_{b_{k}} {\color{red}\mathbf{p}^{b}_{c}}\
    & \mathbf{q}^{c_0}_{b_{k}} = \mathbf{q}^{c_{0}}_{c_{k}} \otimes ({\color{blue}\mathbf{q}^{b}_{c}})^{-1} \
  \end{aligned} \label{eq:5}
\end{equation}
- $c_{0}$: 초기화가 시작된 시점의 가장 첫번째 좌표계 
- $\bar{\mathbf{p}}$: 스케일을 제외한(up to scale) 위치 벡터 $\mathbf{p}$ 
- $s$: 스케일 값

(\ref{eq:2})에서 모든 좌표계의 레퍼런스 좌표계를 월드 좌표계 $\{w\}$에서 $\{c_0\}$ 좌표계로 변환하고 preintegation 항을 이항하여 정리하면 다음과 같이 나타낼 수 있다. 
\begin{equation}
  \begin{aligned}
    & \hat{\alpha}^{b_{k}}_{b_{k+1}} = \mathbf{R}^{b_{k}}_{c_{0}} \bigg( s\Big( \bar{\mathbf{p}}^{c_{0}}_{b_{k+1}} - \bar{\mathbf{p}}^{c_{0}}_{b_{k}} \Big) + \frac{1}{2}\mathbf{g}^{c_{0}}\delta t^{2} - \mathbf{R}^{c_{0}}_{b_{k}} \mathbf{v}^{b_{k}}_{b_{k}} \delta t \bigg) \\
    & \hat{\beta}^{b_{k}}_{b_{k+1}} = \mathbf{R}^{b_{k}}_{c_{0}} \Big( \mathbf{R}^{c_{0}}_{b_{k+1}}\mathbf{v}^{b_{k+1}}_{b_{k+1}} + \mathbf{g}^{c_{0}}\delta t - \mathbf{R}^{c_{0}}_{b_{k}}\mathbf{v}^{b_{k}}_{b_{k}} \Big)
  \end{aligned} \label{eq:6}
\end{equation}

(\ref{eq:5}), (\ref{eq:6})로부터 선형 방정식으로 구성된 선형 시스템을 얻을 수 있다.
\begin{equation}
  \begin{aligned}
    \hat{\alpha}^{b_{k}}_{b_{k+1}} & = \mathbf{R}^{b_{k}}_{c_{0}} \bigg( s\Big( \bar{\mathbf{p}}^{c_{0}}_{b_{k+1}} - \bar{\mathbf{p}}^{c_{0}}_{b_{k}} \Big) + \frac{1}{2}\mathbf{g}^{c_{0}}\delta t^{2} - \mathbf{R}^{c_{0}}_{b_{k}} \mathbf{v}^{b_{k}}_{b_{k}} \delta t \bigg) \\
    & = \mathbf{R}^{b_{k}}_{c_{0}} \bigg( s \bar{\mathbf{p}}^{c_{0}}_{c_{k+1}} - \mathbf{R}^{c_{0}}_{b_{k+1}}\mathbf{p}^{b}_{c} - (s \bar{\mathbf{p}}^{c_{0}}_{c_{k}} - \mathbf{R}^{c_{0}}_{b_{k}}\mathbf{p}^{b}_{c})  + \frac{1}{2}\mathbf{g}^{c_{0}}\delta t^{2} - \mathbf{R}^{c_{0}}_{b_{k}} \mathbf{v}^{b_{k}}_{b_{k}} \delta t  \bigg) \\
    & = \mathbf{R}^{b_{k}}_{c_{0}} (\bar{\mathbf{p}}^{c_{0}}_{c_{k+1}} - \bar{\mathbf{p}}^{c_{0}}_{c_{k}})s  + \frac{1}{2} \mathbf{R}^{b_{k}}_{c_{0}}\mathbf{g}^{c_{0}}\delta t^{2} - \mathbf{v}^{b_{k}}_{b_{k}} \delta t + \mathbf{p}^{b}_{c} - \mathbf{R}^{b_{k}}_{c_{0}} \mathbf{R}^{c_{0}}_{b_{k+1}} \mathbf{p}^{b}_{c}
  \end{aligned} \label{eq:66}
\end{equation}

선형 시스템은 다양한 행렬 분해(decomposition) 방법을 통해 비교적 쉽게 해를 구할 수 있는 이점이 있다. 위 식을 선형시스템 $\mathbf{Ax} = \mathbf{b}$ 형태로 정리하면 다음과 같다.
\begin{equation}
  \begin{aligned}
    \begin{bmatrix} \hat{\alpha}^{b_{k}}_{b_{k+1}} - \mathbf{p}^{b}_{c} + \mathbf{R}^{b_{k}}_{c_{0}}\mathbf{R}^{c{0}}_{b_{k+1}} \mathbf{p}^{b}_{c} \\
      \hat{\beta}^{b_{k}}_{b_{k+1}}
    \end{bmatrix} = \begin{bmatrix}
      \delta t \mathbf{I} & 0 & \frac{1}{2}\mathbf{R}^{b_{k}}_{c_{0}}\delta t^{2} & \mathbf{R}^{b_{k}}_{c_{0}}(\bar{\mathbf{p}}^{c_{0}}_{c_{k+1}} - \bar{\mathbf{p}}^{c_{0}}_{c_{k}}) \\
      -\mathbf{I} & \mathbf{R}^{b_{k}}_{c_{0}} \mathbf{R}^{c_{0}}_{b_{k+1}} & \delta t \mathbf{R}^{b_{k}}_{c_{0}} & 0 
    \end{bmatrix}
    \begin{bmatrix}
        \mathbf{v}^{b_{k}}_{b_{k}} \\ \mathbf{v}^{b_{k+1}}_{b_{k+1}} \\ \mathbf{g}^{c_{0}} \\ s
    \end{bmatrix} \\
  \end{aligned}
\end{equation}

\begin{equation}
  \begin{aligned}
    \hat{\mathbf{z}}^{b_{k}}_{b_{k+1}} = \mathbf{H}^{b_{k}}_{b_{k+1}} \mathcal{X}_{I} + \mathbf{n}^{b_{k}}_{b_{k+1}} 
  \end{aligned}
\end{equation}
- $\mathcal{X}_{I} = [\mathbf{v}^{b_{0}}_{b_{0}}, \mathbf{v}^{b_{1}}_{b_{1}}, \cdots, \mathbf{v}^{b_{n}}_{b_{n}}, \mathbf{g}^{c_{0}}, s]$ 
- $\mathbf{n}^{b_{k}}_{b_{k+1}}$: $[b_{k}, b_{k+1}]$ 좌표계 사이의 모든 노이즈 항 

이는 VINS-mono의 (10),(11) 식과 동일하다. 위 식 중 $\mathbf{R}^{c_{0}}_{b_{k}}, \mathbf{R}^{c_{0}}_{b_{k+1}}, \bar{\mathbf{p}}^{c_{0}}_{c_{k}}, \bar{\mathbf{p}}^{c_{0}}_{c_{k+1}}$은 SFM으로 부터 얻은 값이다.
따라서 최종적으로 계산해야 하는 최적화 공식은 다음과 같이 나타낼 수 있다.
\begin{equation}
  \begin{aligned}
    \min_{\mathcal{X}_{I}} \sum_{k \in \mathcal{B}} \left\|  \hat{\mathbf{z}}^{b_{k}}_{b_{k+1}} - \mathbf{H}^{b_{k}}_{b_{k+1}} \mathcal{X}_{I} \right\|^{2}
  \end{aligned}
\end{equation}

3) Gravity Refinement:  위 과정에서 중력 벡터 값이 1차적으로 추정되고 이후 다시 한 번 미세 조정되는 최적화를 거친다. 중력은 크기가 고정되어 있는 상수이므로 ($9.8m/s^2$) 만약 추정된 중력의 크기가 9.8보다 크다면 작게 만들고 9.8보다 작다면 크게 만들 수 있다. 크기를 알고 있는 중력 벡터는 자유도가 2이므로 우리가 세 개의 벡터 값 중 두 개를 알고 있으면 나머지 값은 자동으로 결정된다. 따라서 중력 벡터는 2차원 접평면을 가지는 매니폴드로 파라미터화 할 수 있다.
\begin{equation}
  \begin{aligned}
    \mathbf{g}^{c_0} = \left\| \mathbf{g} \right\| \frac{\mathbf{g}^{c_{0}}}{\left\| \mathbf{g}^{c_{0}} \right\| } + \begin{bmatrix} \mathbf{b}_{1} & \mathbf{b}_{2} \end{bmatrix} \begin{bmatrix} w_{1} \\ w_{2} \end{bmatrix}
  \end{aligned}
\end{equation}

위 식에서 $\mathbf{g}^{c_{0}}, \mathbf{b}_{1}, \mathbf{b}_{2}$는 서로 직교(orthogonal)하며 이 중 $\mathbf{b}_{1}, \mathbf{b}_{2}$는 접평면을 span하는 두 basis 벡터이다. 그리고 $w_{1}, w_{2}$는 섭동(perturbation) 또는 에러를 의미하며 1차적으로 추정된 중력 벡터에 의해 결정되는 값이다. 이러한 $w_{1}, w_{2}$ 최적화를 통해 0에 가깝게 만드는 것이 gravity refinement의 목적이다.

4) Completing Initialization:  중력 벡터까지 미세 조정이 완료되었으면 다음으로 중력 벡터를 z축으로 하는 월드 좌표계 $(\cdot)^{w}$를 생성하여 기존의 $(\cdot)^{c_0}$ 좌표계의 파라미터를 모두 월드 좌표계 $(\cdot)^{w}$으로 회전시킨다. 이 때 IMU 좌표계의 속도 또한 월드 좌표계로 회전시킨다. 다음으로 visual SFM으로 생성한 값에 스케일을 적용하며 metric 단위를 갖도록 한다. 해당 시점에서 모든 초기화 과정이 마무리되고 모든 파라미터는 VIO의 입력으로 들어간다.

4. Tightly-Coupled Nonlinear Optimization

4.1. Basic of the State Estimation

상태 추정 문제는 일반적으로 베이지안 확률의 Maximum A Posteriori(MAP)를 구하는 문제로 해석할 수 있다. 이는 관측 값이 있는 상태에서 로봇의 상태의 조건부 확률분포를 계산하는 것과 동일하다.
\begin{equation}
  \begin{aligned}
    P(\mathbf{x} | \mathbf{z}) 
  \end{aligned}
\end{equation}
- $\mathbf{x}$: 로봇의 상태 
- $\mathbf{z}$: 관측된 센서 데이터 

위 식에 베이지안 법칙을 적용하면 다음 식을 얻을 수 있다.
\begin{equation}
  \begin{aligned}
    P(\mathbf{x} | \mathbf{z}) = \frac{P(\mathbf{z} | \mathbf{x}) P(\mathbf{x})}{P(\mathbf{z})} \propto P(\mathbf{z} | \mathbf{x})P(\mathbf{x})
  \end{aligned}
\end{equation}

위 식에서 $P(\mathbf{x}|\mathbf{z})$를 posterior라고 하고 $P(\mathbf{x})$를 priori라고 하며 $P(\mathbf{z} | \mathbf{x})$를 likelihood라고 한다. 일반적으로 posterior를 바로 구하는 것은 어렵다고 알려져 있다. 따라서 priori와 likelihood를 변경하여 posterior가 최대가 되는 최적의 상태 변수를 구하는 방식이 널리 사용되는데 이를 Maximum A Posteriori(MAP)라고 한다. 대부분의 경우 priori 정보는 모르는 상황에서 상태 추정을 진행하므로 수식은 다음과 같이 변형된다.
\begin{equation}
  \begin{aligned}
    \mathcal{X}^{*} = \arg\max P(\mathbf{x} | \mathbf{z}) \propto \arg\max P(\mathbf{z} | \mathbf{x})P(\mathbf{x})\propto \arg\max P(\mathbf{z} | \mathbf{x})
  \end{aligned} \label{eq:7}
\end{equation}

위 식에서 보듯이 MAP 문제는 관측치를 최대로하는 상태 $\arg\max P(\mathbf{z} | \mathbf{x})$를 구하는 문제로 변형되었다. 이렇게 priori 없이 likelihood의 최대값을 추정하는 문제를 Maxium Likelihood Estimator (MLE)라고 한다. 만약 관측치 $\mathbf{z}$가 가우시안 분포를 따른다고 가정하면 $\mathbf{z} \sim \mathcal{N}(\bar{\mathbf{z}}, \mathbf{Q})$와 같이 나타낼 수 있으며 (\ref{eq:7})를 다음과 같이 나타낼 수 있다.
\begin{equation}
  \begin{aligned}
    \mathcal{X}^{*} = \arg\max P(\mathbf{z} | \mathbf{x}) = \arg\min \sum \left\| \mathbf{z} - h(\mathbf{x}) \right\|_{\mathbf{Q}}
  \end{aligned} \label{eq:8}
\end{equation}

이 때, $h(\cdot)$은 상태 함수를 나타내며 $\left\| \mathbf{x} \right\|_{\mathbf{Q}} = \mathbf{x}^{\intercal}Q^{-1}\mathbf{x}$는 마할라노비스 놈(mahalanobis norm)을 의미한다.

NOTICE:  다차원 가우시안 분포가 $\mathbf{x} \sim \mathcal{N}(\mu, \mathbf{Q})$를 만족할 때 $\mathbf{x}$의 확률밀도함수(probability density function; pdf)는 다음과 같이 쓸 수 있다.
\begin{equation}
  \begin{aligned}
    & P(\mathbf{x}) = \frac{1}{\sqrt{(2\pi)^{N} \det(\mathbf{Q})}} \exp \bigg(-\frac{1}{2}(\mathbf{x} - \mu)^{\intercal} \mathbf{Q}^{-1} (\mathbf{x} - \mu) \bigg) \\
    & -\ln P(\mathbf{x}) = \cancel{\ln \sqrt{ (2\pi)^{N} \det(\mathbf{Q}) }} + \frac{1}{2}(\mathbf{x}-\mu)^{\intercal} \mathbf{Q}^{-1} (\mathbf{x}- \mu)
  \end{aligned}
\end{equation}

위 식에서 $\ln\sqrt{\cdots}$ 부분은 $\mathbf{x}$를 포함하고 있지 않으므로 $\frac{1}{2}(\cdots)$ 부분만이 $\mathbf{x}$를 최적화시킬 때 사용한다.

이후 (\ref{eq:8})은 비선형 최소제곱법(nonlinear least squares)을 사용하여 최적해를 구할 수 있다. 이를 수행하는 구체적인 방법에는 크게 Gauss-Newton(GN) 방법과 Levenberg-Marquardt(LM) 방법이 존재한다. 이 중 GN 방법을 사용하여 최적화 공식을 풀어보자.
\begin{equation}
  \begin{aligned}
    \mathcal{X} = \arg\min_{\mathbf{x}} \left\| f(\mathbf{x}) \right\|_{\mathbf{P}}
  \end{aligned}
\end{equation}

위 식에서 $\mathbf{P}$는 에러의 공분산 행렬을 의미하며 $\left\| \cdot \right\|$은 L2 norm을 의미한다. $f(\mathbf{x})$를 1차 테일러 근사하여 표현하면 $f(\mathbf{x} +\delta \mathbf{x}) \approx f(\mathbf{x}) + \mathbf{J}(\mathbf{x})\delta \mathbf{x}$가 되고 이를 최적의 증분량 $\delta \mathbf{x}^{*}$에 대해 다음과 같이 식을 전개할 수 있다.
\begin{equation}
  \begin{aligned}
    \delta \mathbf{x}^{*} = \arg\min_{\delta \mathbf{x}} \left\| f(\mathbf{x}) + \mathbf{J}(\mathbf{x}) \delta \mathbf{x} \right\|_{\mathbf{P}} 
  \end{aligned}
\end{equation}

\begin{equation}
  \begin{aligned}
   \left\| f(\mathbf{x}) + \mathbf{J}(\mathbf{x}) \delta \mathbf{x} \right\|_{\mathbf{P}}  & = \Big( f(\mathbf{x}) + \mathbf{J}(\mathbf{x}) \delta \mathbf{x} \Big)^{\intercal} \mathbf{P}^{-1} \Big( f(\mathbf{x}) + \mathbf{J}(\mathbf{x}) \delta \mathbf{x} \Big) \\
    & = f(\mathbf{x})^{-1}\mathbf{P}^{-1}f(\mathbf{x}) + 2 \mathbf{J}(\mathbf{x})^{\intercal} \mathbf{P}^{-1} f(\mathbf{x}) \delta \mathbf{x} + \delta \mathbf{x}^{\intercal} \mathbf{J}(\mathbf{x})^{\intercal} \mathbf{P}^{-1}\mathbf{J}(\mathbf{x})\delta \mathbf{x}
  \end{aligned}
\end{equation}

위 식을 $\delta \mathbf{x}$에 대해 미분하고 $(\cdot)' = 0$로 최소값을 구하면 다음과 같은 식을 구할 수 있다.
\begin{equation}
  \begin{aligned}
    \mathbf{J}(\mathbf{x})^{\intercal} \mathbf{P}^{-1}\mathbf{J}(\mathbf{x}) \delta \mathbf{x}^{*}  & = -\mathbf{J}(\mathbf{x})^{\intercal} \mathbf{P}^{-1} f(\mathbf{x}) \\
    \mathbf{H}\delta \mathbf{x}^{*} & = \mathbf{b}
  \end{aligned} \label{eq:9}
\end{equation}

위 식은 SVD 또는 cholesky decomposition과 같은 다양한 행렬 분해(decomposition) 방법을 통해 최적해 $\delta \mathbf{x}^{*}$를 구할 수 있다. 최적해를 구한 다음 이를 기존의 상태에 업데이트함으로써 GN의 한 사이클이 마무리된다.
\begin{equation}
  \begin{aligned}
    \mathbf{x} \leftarrow \mathbf{x} + \delta \mathbf{x}^{*}
  \end{aligned}
\end{equation}

4.2. Cost Function

SLAM 초기화가 완료되었으면 다음으로 슬라이딩 윈도우 기반의 tightly-coupled monocular VIO를 수행한다. VIO에는 특징점의 inverse depth $\lambda$와 모든 좌표계의 속도 $\mathbf{v}_{k}$와 포즈 $\mathbf{p}_{k}, \mathbf{q}_{k}$, 그리고 IMU bias $\mathbf{b}_{a}, \mathbf{b}_{g}$, 외부 파라미터(extrinsic) $\mathbf{x}^{b}_{c}$ 값이 한 번에 최적화된다.
\begin{equation}
  \begin{aligned}
    & \mathcal{X} = [\mathbf{x}_{0}, \mathbf{x}_{1}, \cdots, \mathbf{x}_{n}, \mathbf{x}^{b}_{c}, \lambda_{0}, \lambda_{1}, \cdots, \lambda_{m}] \\
    & \mathbf{x}_{k} = [\mathbf{p}^{w}_{b_{k}}, \mathbf{v}^{w}_{b_{k}}, \mathbf{q}^{w}_{b_{k}}, \mathbf{b}_{a}, \mathbf{b}_{g}] \\
    & \mathbf{x}^{b}_{c} = [\mathbf{p}^{b}_{c}, \mathbf{q}^{b}_{c}]
  \end{aligned}
\end{equation}

VINS-mono에서 VIO의 최적화 함수는 다음과 같이 정의된다.
\begin{equation}
  \begin{aligned}
    \min_{\mathcal{X}} \bigg\{ \left\| \mathbf{r}_{p} - \mathbf{J}_{p}\mathcal{X} \right\|_{\mathbf{P}_{M}} + \sum_{k \in \mathcal{B}} \left\| \mathbf{r}_{\mathcal{B}} \Big( \hat{\mathbf{z}}^{b_{k}}_{b_{k+1}}, \mathcal{X} \Big) \right\|_{\mathbf{P}_{B}} + \sum_{(l,j) \in \mathcal{C}} \left\| \mathbf{r}_{\mathcal{C}} \Big( \hat{\mathbf{z}}^{c_{j}}_{l}, \mathcal{X} \Big) \right\|_{\mathbf{P}^{c_{j}}_{l}} \bigg\}
  \end{aligned}
\end{equation}

위 식에서 앞의 $\mathbf{r}_{p}, \mathbf{J}_{p}$ 부분은 marginalization으로 인한 priori 정보를 의미하며 $\mathbf{r}_{\mathcal{B}}$ 부분은 IMU의 residual을 의미하고 $\mathbf{r}_{\mathcal{C}}$는 카메라 이미지의 residual을 의미한다. $\mathcal{B}$는 슬라이딩 윈도우 내의 모든 IMU 측정값의 집합을 의미하고 $\mathcal{C}$는 슬라이딩 윈도우 내에서 두 번 이상 관측된 모든 특징점들의 집합을 의미한다. $\mathbf{P}_{M}, \mathbf{P}_{\mathcal{B}}, \mathbf{P}_{\mathcal{C}}$는 각각 prior, IMU, visual residual의 공분산 값을 의미한다.

4.3. IMU Model

(\ref{eq:2})에서 IMU 측정값의 residual을 다음과 같이 나타낼 수 있다. 이 때, residual은 에러와 동일한 의미를 지니며 관측값 - 예측값을 의미한다.
\begin{equation}
  \begin{aligned}
    \mathbf{r}_{\mathcal{B}} \Big( \hat{\mathbf{z}}^{b_{k}}_{b_{k+1}}, \mathcal{X} \Big) = \begin{bmatrix}
      \delta \alpha^{b_{k}}_{b_{k+1}} \\
      \delta \boldsymbol{\theta}^{b_{k}}_{b_{k+1}} \\
      \delta \beta^{b_{k}}_{b_{k+1}} \\
      \delta \mathbf{b}_{a}\\
      \delta \mathbf{b}_{g}\\
    \end{bmatrix} = \begin{bmatrix}
      \mathbf{R}^{b_{k}}_{w}(\mathbf{p}^{w}_{b_{k+1}} - \mathbf{p}^{w}_{b_{k}} - \mathbf{v}^{w}_{b_{k}}\Delta t_{k} + \frac{1}{2} \mathbf{g}^{w}\Delta t_{k}^{2}) - \hat{\alpha}^{b_{k}}_{b_{k+1}} \\
      2 \Big[ \Big( \hat{\gamma}^{b_{k}}_{b_{k+1}} \Big)^{-1} \otimes (\mathbf{q}^{w}_{b_{k}})^{-1} \otimes \mathbf{q}^{w}_{b_{k+1}}  \Big]_{xyz} \\
      \mathbf{R}^{b_{k}}_{w} ( \mathbf{v}^{w}_{b_{k+1}} - \mathbf{v}^{w}_{b_{k}} + \mathbf{g}^{w}\Delta t_{k}) - \hat{\beta}^{b_{k}}_{b_{k+1}} \\
      \mathbf{b}_{a_{k+1}} - \mathbf{b}_{a_{k}} \\
      \mathbf{b}_{g_{k+1}} - \mathbf{b}_{g_{k}} \\
    \end{bmatrix}
  \end{aligned}
\end{equation}
- $[\cdot]_{xyz}$: 쿼터니언에서 벡터 파트 $(x,y,z)$만 따로 추출하는 연산자 
- $\delta \boldsymbol{\theta}^{b_{k}}_{b_{k+1}}$: 쿼터니언의 에러 상태 표현법. $\mathbf{q}$가 아닌 $\boldsymbol{\theta}$로 각도 에러를 표기한다. 
- $(\delta \alpha^{b_{k}}_{b_{k+1}}, \delta \beta^{b_{k}}_{b_{k+1}}, \delta \gamma^{b_{k}}_{b_{k+1}})$: 두 이미지 사이에 preintegration된 IMU 측정값 

IMU residual에서 최적화해야 하는 상태 변수는 다음과 같다.
\begin{equation}
  \begin{aligned}
      & \mathbf{x}[0] = [\mathbf{p}^{w}_{b_{k}}, \mathbf{q}^{w}_{b_{k}}] \\
      & \mathbf{x}[1] = [\mathbf{v}^{w}_{b_{k}}, \mathbf{b}_{a_{k}}, \mathbf{b}_{g_{k}} ] \\
      & \mathbf{x}[2] = [\mathbf{p}^{w}_{b_{k+1}}, \mathbf{q}^{w}_{b_{k+1}} ] \\
      & \mathbf{x}[3] = [\mathbf{v}^{w}_{b_{k+1}}, \mathbf{b}_{a_{k+1}}, \mathbf{b}_{g_{k+1}} ]
  \end{aligned}
\end{equation}

위 상태 변수는 코드에서 구현된 내용을 기반으로 네 그룹으로 나누었다. 각 상태 변수의 차원은 각각 7,9,7,9이다.

NOTICE:  상태 변수를 최적화할 때 방향 표기법으로 쿼터니언 $\mathbf{q}$을 사용하는 경우 over-parameterization과 같은 문제로 인해 최적화에 사용하기 쉽지 않다. 따라서 $\mathbf{q}$의 실수부 $w$를 제외한 허수부 $(x,y,z)$만 최적화에 사용함으로써 별도의 제약조건이 없는 lie algebra so(3) 기반의 최적화를 진행할 수 있다. 이 때, 허수부만 사용하는 쿼터니언은 순수 쿼터니언이 되므로 회전 벡터 $\boldsymbol{\theta}$와 물리적 의미가 동일하다. 최적화가 된 다음에 포즈를 업데이트할 때는 다시 단위 쿼터니언 $\mathbf{q}$로 변환하여 업데이트한다. 이는 ceres solver의 ``LocalParameterization''에 구현되어 자동으로 변환 및 업데이트된다. 이는 IMU 뿐만 아니라 visual residual의 회전을 표현할 때도 동일하게 적용된다.

따라서 기존의 IMU residual 자코비안의 차원은 $<15\times7, \ 15\times9, \ 15\times 7, \ 15\times 9>$이지만 회전을 업데이트할 때 4개가 아닌 3개의 파라미터만 사용되므로 $\mathbf{J}[0], \mathbf{J}[2]$의 마지막 열은 항상 0이된다. 따라서 표현의 편의 상 $\mathbf{J}[0], \mathbf{J}[2]$은 $<15 \times 6>$ 크기의 행렬로 나타내었다.

따라서 자코비안은 다음과 같다. 자세한 자코비안의 유도 과정은 Appendix를 참고하면 된다.
\begin{equation}
  \begin{aligned}
    \mathbf{J}[0]_{15\times 6} = \frac{\partial \mathbf{r}_{\mathcal{B}} \Big( \hat{\mathbf{z}}^{b_{k}}_{b_{k+1}}, \mathcal{X} \Big)}{\partial [\mathbf{p}^{w}_{b_{k}}, \mathbf{q}^{w}_{b_{k}}]} = \begin{bmatrix}
      -\mathbf{R}^{b_{k}}_{w} & [\mathbf{R}^{b_{k}}_{w}(\mathbf{p}^{w}_{b_{k+1}} - \mathbf{p}^{w}_{b_{k}} - \mathbf{v}^{w}_{b_{k}}\Delta t_{k} + \frac{1}{2} \mathbf{g}^{w}\Delta t_{k}^{2})]_{\times} \\
       0 & [\gamma^{b_{k}}_{b_{k+1}}]_{R}[(\mathbf{q}^{w}_{b_{k+1}})^{-1} \otimes \mathbf{q}^{b_{k}}_{w}]_{L, 3\times3} \\
      0 & [\mathbf{R}^{b_{k}}_{w} (\mathbf{p}^{w}_{b_{k+1}} - \mathbf{p}^{w}_{b_{k}} + \mathbf{g}^{w} \Delta t_{k})]_{\times} \\
      0&0\\
      0&0
    \end{bmatrix}
  \end{aligned}
\end{equation}

\begin{equation}
  \begin{aligned}
    \mathbf{J}[1]_{15\times9} = \frac{\partial \mathbf{r}_{\mathcal{B}} \Big( \hat{\mathbf{z}}^{b_{k}}_{b_{k+1}}, \mathcal{X} \Big)}{\partial [\mathbf{v}^{w}_{b_{k}}, \mathbf{b}_{a_{k}}, \mathbf{b}_{g_{k}}]} = \begin{bmatrix}
      -\mathbf{R}^{b_{k}}_{w} \Delta t_{k} & -\mathbf{J}^{\alpha}_{b_{a}} & -\mathbf{J}^{\alpha}_{b_{g}} \\
      0&0& -[(\hat{\gamma}^{b_{k}}_{b_{k+1}})^{-1} \otimes (\mathbf{q}^{w}_{b_{k}})^{-1} \otimes \mathbf{q}^{w}_{b_{k+1}}]_{R,3\times3} \mathbf{J}^{\gamma}_{b_{g}} \\
      -\mathbf{R}^{b_{k}}_{w} & -\mathbf{J}^{\beta}_{b_{a}} & -\mathbf{J}^{\beta}_{b_{g}} \\
      0 & - \mathbf{I} & 0 \\
      0 & 0 &-\mathbf{I}
    \end{bmatrix}
  \end{aligned}
\end{equation}

\begin{equation}
  \begin{aligned}
    \mathbf{J}[2]_{15\times 6} = \frac{\partial \mathbf{r}_{\mathcal{B}} \Big( \hat{\mathbf{z}}^{b_{k}}_{b_{k+1}}, \mathcal{X} \Big)}{\partial [\mathbf{p}^{w}_{b_{k+1}}, \mathbf{q}^{w}_{b_{k+1}}]} = \begin{bmatrix}
      \mathbf{R}^{b_{k}}_{w} & 0 \\
      0 & [(\hat{\gamma}^{b_{k}}_{b_{k+1}})^{-1} \otimes (\mathbf{q}^{w}_{b_{k}})^{-1} \otimes \mathbf{q}^{w}_{b_{k+1}}]_{L} \\
      0&0\\
      0&0\\
      0&0
    \end{bmatrix}
  \end{aligned}
\end{equation}

\begin{equation}
  \begin{aligned}
    \mathbf{J}[3]_{15\times9} = \frac{\partial \mathbf{r}_{\mathcal{B}} \Big( \hat{\mathbf{z}}^{b_{k}}_{b_{k+1}}, \mathcal{X} \Big)}{\partial [\mathbf{v}^{w}_{b_{k+1}}, \mathbf{b}_{a_{k+1}}, \mathbf{b}_{g_{k+1}}]} = \begin{bmatrix}
      0&0&0\\
      0&0&0\\
      \mathbf{R}^{b_{k}}_{w} &0&0 \\
      0&\mathbf{I}&0 \\
      0&0&\mathbf{I}
    \end{bmatrix}
  \end{aligned}
\end{equation}

4.4. Vision Model

$i$번째 이미지에서 $l$번째 특징점이 처음 관측되었을 때, 해당 특징점을 $j$번째 이미지에서 관측한 visual residual은 다음과 같다. 
\begin{equation}
  \begin{aligned}
    & \hat{\bar{\mathbf{P}}}^{c_{j}}_{l} && = \pi^{-1}_{c} \bigg( \begin{bmatrix} \hat{u}^{c_{j}}_{l} \\ \hat{v}^{c_{j}}_{l} \end{bmatrix} \bigg) \\
    & && = \begin{bmatrix} \hat{x}^{c_{j}}_{l} & \hat{y}^{c_{j}}_{l} & \hat{z}^{c_{j}}_{l} \end{bmatrix}^{\intercal} \\
    & \tilde{\mathbf{P}}^{c_{j}}_{l} && = \mathbf{R}^{c}_{b} \bigg\{ \mathbf{R}^{b_{j}}_{w} \bigg[ \mathbf{R}^{w}_{b_{i}} \bigg( \mathbf{R}^{b}_{c} \frac{1}{\lambda_{l}} \pi^{-1}_{c}  \begin{bmatrix} \hat{u}^{c_{i}}_{l} \\ \hat{v}^{c_{i}}_{l} \end{bmatrix}  + \mathbf{P}^{b}_{c} \bigg) + \mathbf{P}^{w}_{b_{i}} \bigg] + {\color{red}\mathbf{P}^{b_{j}}_{w}} \bigg\} + {\color{blue}\mathbf{P}^{c}_{b}} \\
    & && = \mathbf{R}^{c}_{b} \bigg\{ \mathbf{R}^{b_{j}}_{w} \bigg[ \mathbf{R}^{w}_{b_{i}} \bigg( \mathbf{R}^{b}_{c} \frac{1}{\lambda_{l}} \pi^{-1}_{c}  \begin{bmatrix} \hat{u}^{c_{i}}_{l} \\ \hat{v}^{c_{i}}_{l} \end{bmatrix}  + \mathbf{P}^{b}_{c} \bigg) + \mathbf{P}^{w}_{b_{i}} {\color{red}- \mathbf{P}^{w}_{b_{j}}} \bigg] {\color{blue}- \mathbf{P}^{b}_{c}}  \bigg\}  \\
    & && = \begin{bmatrix} x^{c_{j}}_{l} & y^{c_{j}}_{l} & z^{c_{j}}_{l} \end{bmatrix}^{\intercal}
  \end{aligned}
\end{equation}
- $\hat{\bar{\mathbf{P}}}^{c_{j}}_{l} \in \mathbb{R}^{3}$: $j$번째 이미지에서 $l$번째 특징점의 관측값. 최적화를 위해 단위벡터의 크기를 가진다 
- $\tilde{\mathbf{P}}^{c_{j}}_{l} \in \mathbb{R}^{3}$: $j$번째 이미지에서 $l$번째 특징점의 예측값 
- $\pi^{-1}_{c}(\cdot)$: back projection 함수. 이미지 평면 상의 2D 포인트를 월드 상의 3D 포인트로 변환한다. VINS-mono 논문에서는 해당 함수를 통과한 벡터가 단위 벡터가 된다. 이는 단위 벡터로 변환해야 최적화를 수행할 수 있기 때문으로 보인다 
- $\begin{bmatrix} \hat{u}^{c_{j}}_{l} \\ \hat{v}^{c_{j}}_{l} \end{bmatrix}$: $j$번째 이미지에서 본 $l$번째 특징점의 픽셀 좌표 

위 식은 VINS-mono (17) 식과 동일하다. 위 식은 임의의 변환 행렬(transformation matrix) $\mathbf{T}^{a}_{b} = \begin{bmatrix} \mathbf{R}^{a}_{b} & \mathbf{P}^{a}_{b} \\ 0 & 1 \end{bmatrix}$이 주어졌을 때 $\mathbf{T}^{a}_{b}\mathbf{P}^{b} = \mathbf{P}^{a}$를 풀어 쓴 형태이다.
\begin{equation}
  \begin{aligned}
    \mathbf{R}^{a}_{b}\mathbf{P}^{b} + \mathbf{P}^{a}_{b} = \mathbf{P}^{a}
  \end{aligned}
\end{equation}

또한, 위 식에서 색칠한 부분은 변환 행렬의 역행렬 $(\mathbf{T}^{a}_{b})^{-1} = \mathbf{T}^{b}_{a}  = \begin{bmatrix} \mathbf{R}^{b}_{a} & -\mathbf{R}^{b}_{a}\mathbf{P}^{a}_{b} \\ 0 & 1 \end{bmatrix} =\begin{bmatrix} \mathbf{R}^{b}_{a} & \mathbf{P}^{b}_{a} \\ 0 & 1 \end{bmatrix}$인 성질을 참고하면 된다.
\begin{equation}
  \begin{aligned}
    -\mathbf{R}^{b}_{a}\mathbf{P}^{a}_{b} = \mathbf{P}^{b}_{a} \quad \Rightarrow \quad \mathbf{P}^{a}_{b} = -\mathbf{R}^{a}_{b}\mathbf{P}^{b}_{a}
  \end{aligned}
\end{equation}

VINS-mono의 필자는 visual residual 벡터가 이미지 픽셀 좌표의 특성 상 2 자유도를 갖는다는 점을 참고하여 중력 벡터 최적화 방법과 유사한 방법을 사용하였다. 따라서 $\hat{\bar{\mathbf{P}}}^{c_{j}}_{l}$을 매니폴드화 하여 임의의 두 직교하는 basis 벡터 $[\mathbf{b}_{1}, \mathbf{b}_{2}]$로 부터 span되는 접평면이 주어졌을 때 에러 값 $w_{1},w_{2}$의 크기를 최소화하는 방향으로 최적화를 진행한다. 
 
우선, basis 벡터를 사용하지 않은 경우 visual residual은 다음과 같다.
\begin{equation}
  \begin{aligned}
    \mathbf{r}_{c}(\hat{\mathbf{z}}^{c_{j}}_{l}, \mathcal{X}) & = \bigg( \frac{\tilde{\mathbf{P}}^{c_{j}}_{l}}{\tilde{\mathbf{P}}^{c_{j}}_{l}.z }  - \hat{\bar{\mathbf{P}}}^{c_{j}}_{l} \bigg)_{xy} \in \mathbb{R}^{2\times 1}
  \end{aligned}
\end{equation}

basis 벡터 $[\mathbf{b}_{1}, \mathbf{b}_{2}]$를 사용한 visual residual은 다음과 같이 나타낼 수 있다.
\begin{equation}
  \begin{aligned}
    \mathbf{r}_{c}(\hat{\mathbf{z}}^{c_{j}}_{l}, \mathcal{X}) & = \begin{bmatrix} \mathbf{b}_{1} & \mathbf{b}_{2} \end{bmatrix}^{\intercal} \cdot \bigg( \frac{\tilde{\mathbf{P}}^{c_{j}}_{l}}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|}  - \hat{\bar{\mathbf{P}}}^{c_{j}}_{l} \bigg) \\
  \end{aligned}
\end{equation}
- $\frac{\tilde{\mathbf{P}}^{c_{j}}_{l}}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|}$: 벡터의 크기를 단위 벡터로 만들기 위해 정규화한다

visual residual에서 최적화해야 하는 변수는 다음과 같다.
\begin{equation}
  \begin{aligned}
    & \mathbf{x}[0] = [\mathbf{p}^{w}_{b_{i}}, \mathbf{q}^{w}_{b_{i}}] \\
      & \mathbf{x}[1] = [\mathbf{p}^{w}_{b_{j}}, \mathbf{q}^{w}_{b_{j}}] \\
      & \mathbf{x}[2] = [\mathbf{p}^{b}_{c}, \mathbf{q}^{b}_{c}] \\
      & \mathbf{x}[3] = [\lambda_{l}]
  \end{aligned}
\end{equation}

자코비안은 다음과 같이 연쇄 법칙(chain rule)을 사용하여 계산할 수 있다.
\begin{equation}
  \begin{aligned}
    \frac{\partial \mathbf{r}_{c}}{\partial \mathbf{x}} = \frac{\partial \mathbf{r}_{c}}{\partial \tilde{\mathbf{P}}_{l}^{c_{j}}} \cdot \frac{\partial \tilde{\mathbf{P}}^{c_{j}}_{l}}{\partial \mathbf{x}}
  \end{aligned}
\end{equation}

basis를 사용하지 않는 visual residual의 자코비안은 $\frac{\partial \mathbf{r}_{c}}{\partial \tilde{\mathbf{P}}_{l}^{c_{j}}}$은 다음과 같다.
\begin{equation}
  \begin{aligned}
    \frac{\partial \mathbf{r}_{c}}{\partial \tilde{\mathbf{P}}_{l}^{c_{j}}} = \begin{bmatrix} \frac{1}{z^{c_{j}}_{l}} & 0 & -\frac{x^{c_{j}}_{l}}{(z^{c_{j}}_{l})^{2}} \\ 0 & \frac{1}{z^{c_{j}}_{l}} & -\frac{y^{c_{j}}_{l}}{(z^{c_{j}}_{l})^{2}} \end{bmatrix}
  \end{aligned}
\end{equation}
- $\mathbf{r}_{c} = \begin{bmatrix} \frac{x^{c_{j}}_{l}}{z^{c_{j}}_{l}} - \hat{u}^{c_{j}}_{l} & \frac{y^{c_{j}}_{l}}{z^{c_{j}}_{l}} - - \hat{v}^{c_{j}}_{l} \end{bmatrix}^{\intercal}$ 
- $\tilde{\mathbf{P}}_{l}^{c_{j}} = \begin{bmatrix} x^{c_{j}}_{l} & y^{c_{j}}_{l} & z^{c_{j}}_{l} \end{bmatrix}^{\intercal}$  

반면, basis $\mathbf{b}_{1}, \mathbf{b}_{2}$이 적용된 자코비안은 다음과 같이 구할 수 있다.
\begin{equation}
  \begin{aligned}
    \frac{\partial \mathbf{r}_{c}}{\partial \tilde{\mathbf{P}}_{l}^{c_{j}}} & = \begin{bmatrix} \mathbf{b}_{1} & \mathbf{b}_{2} \end{bmatrix}^{\intercal} \cdot \frac{\partial \bigg( \frac{\tilde{\mathbf{P}}^{c_{j}}_{l}}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|}  - \hat{\bar{\mathbf{P}}}^{c_{j}}_{l} \bigg)}{\partial \tilde{\mathbf{P}}_{l}^{c_{j}}} \\
    & = \frac{\partial \frac{\tilde{\mathbf{P}}^{c_{j}}_{l}}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|}}{\partial \tilde{\mathbf{P}}_{l}^{c_{j}}}
    = \frac{1}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|} \mathbf{I} -  \frac{\tilde{\mathbf{P}}^{c_{j}}_{l}  \frac{\partial \left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|}{\partial \tilde{\mathbf{P}}^{c_{j}}_{l}}}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|^{2}} \\
    & = \begin{bmatrix}
      \frac{1}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|} - \frac{x^{2}}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|^{3}} &  -\frac{xy}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|^{3}} & -\frac{xz}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|^{3}}\\
      -\frac{xy}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|^{3}} & \frac{1}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|} - \frac{y^{2}}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|^{3}} & -\frac{yz}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|^{3}} \\
      - \frac{xz}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|^{3}} & -\frac{yz}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|^{3}} & \frac{1}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|} - \frac{z^{2}}{\left\| \tilde{\mathbf{P}}^{c_{j}}_{l} \right\|^{3}}
    \end{bmatrix}
  \end{aligned}
\end{equation}

다음으로 $\frac{\partial \tilde{\mathbf{P}}_{l}^{c_{j}}}{\partial \mathbf{x}}$는 다음과 같이 구할 수 있다. 디테일한 자코비안 유도는 Appendix를 참조하면 된다. 
\begin{equation}
  \begin{aligned}
    \mathbf{J}[0]_{3\times 6} = \frac{\partial \tilde{\mathbf{P}}_{l}^{c_{j}}}{\partial \begin{bmatrix} \mathbf{p}^{w}_{b_{i}} & \mathbf{q}^{w}_{b_{i}} \end{bmatrix}} = \begin{bmatrix} \mathbf{R}^{c}_{b}\mathbf{R}^{b_{j}}_{w} & -\mathbf{R}^{c}_{b}\mathbf{R}^{b_{j}}_{w}\mathbf{R}^{w}_{b_{i}}\bigg(\Big[ \frac{1}{\lambda_{l}}\mathbf{R}^{b}_{c}\hat{\bar{\mathbf{P}}}^{c_{i}}_{l} + \mathbf{P}^{b}_{c} \Big]_{\times} \bigg) \end{bmatrix}
  \end{aligned}
\end{equation}

\begin{equation}
  \begin{aligned}
    \mathbf{J}[1]_{3\times 6} = \frac{\partial \tilde{\mathbf{P}}_{l}^{c_{j}}}{\partial \begin{bmatrix} \mathbf{p}^{w}_{b_{j}} & \mathbf{q}^{w}_{b_{j}} \end{bmatrix}} = \begin{bmatrix}
      -\mathbf{R}^{c}_{b} \mathbf{R}^{b_{j}}_{w} & 
      \mathbf{R}^{c}_{b} \bigg[ \mathbf{R}^{b_{j}}_{w} \bigg( \mathbf{R}^{w}_{b_{i}}\bigg( \mathbf{R}^{b}_{c} \frac{1}{\lambda_{l}}\hat{\bar{\mathbf{P}}}^{c_{i}}_{l} \bigg) + \mathbf{P}^{w}_{b_{i}}  - \mathbf{P}^{w}_{b_{j}}\bigg) \bigg]_{\times}
    \end{bmatrix}
  \end{aligned}
\end{equation}

\begin{equation}
  \begin{aligned}
    \mathbf{J}[2]_{3\times6}  = \frac{\partial \tilde{\mathbf{P}}_{l}^{c_{j}}}{\partial \begin{bmatrix} \mathbf{p}^{b}_{c} & \mathbf{q}^{b}_{c} \end{bmatrix}} = \begin{bmatrix}
      \mathbf{R}^{c}_{b}(\mathbf{R}^{b_{j}}_{w}\mathbf{R}^{w}_{b_{i}}-\mathbf{I}) & \bigg[ \mathbf{R}^{c}_{b}\bigg(\mathbf{R}^{b_{j}}_{w} \bigg( \mathbf{R}^{w}_{b_{i}}\mathbf{P}^{b}_{c} + \mathbf{P}^{w}_{b_{i}} - \mathbf{P}^{w}_{b_{j}} \bigg) -\mathbf{P}^{b}_{c} \bigg) \bigg]_{\times} + \\
      &  \bigg[ \mathbf{R}^{c}_{b}\mathbf{R}^{b_{j}}_{w}\mathbf{R}^{w}_{b_{i}} \mathbf{R}^{b}_{c} \frac{1}{\lambda_{l}} \hat{\bar{\mathbf{P}}}^{c_{i}}_{l} \bigg]_{\times} - \mathbf{R}^{c}_{b}\mathbf{R}^{b_{j}}_{w}\mathbf{R}^{w}_{b_{i}}\mathbf{R}^{b}_{c} \bigg( \frac{1}{\lambda_{l}} [\hat{\bar{\mathbf{P}}}^{c_{i}}_{l}]_{\times} \bigg)
    \end{bmatrix}
  \end{aligned}
\end{equation}

\begin{equation}
  \begin{aligned}
    \mathbf{J}[3]_{3\times1} =  \frac{\partial \tilde{\mathbf{P}}_{l}^{c_{j}}}{\partial \lambda_{l}} = \begin{bmatrix} -\frac{1}{\lambda_{l}^{2}} \mathbf{R}^{c}_{b}\mathbf{R}^{b_{j}}_{w}\mathbf{R}^{w}_{b_{i}}\mathbf{R}^{b}_{c} \hat{\bar{\mathbf{P}}}^{c_{i}}_{l} \end{bmatrix}
  \end{aligned}
\end{equation}

5. Marginalization

시간이 지남에 따라 VIO의 상태 변수는 증가하게 되어 이에 따른 연산량 또한 기하급수적으로 증가하게 된다. 정보의 손실없이 연산량을 줄이기 위해서는 이전의(또는 오래된) 관측값들의 정보를 prior 항으로 넘겨주고 이를 제거하는 작업이 필요한데 이를 marginalization이라고 한다. marginalization이 적용되어 사라지는 상태를 $\mathcal{X}_{m}$, 사라지지 않고 남겨지는(remain) 상태를 $\mathcal{X}_{r}$이라고 하자.

(\ref{eq:9}) 에서 오래된 관측값들을 별도로 마킹하여 marginalization 파트로 설정한 뒤  marginalization 파트와 remain 파트로 아래와 같이 정렬한다.
\begin{equation}
  \begin{aligned}
    \begin{bmatrix} \mathbf{H}_{mm} & \mathbf{H}_{mr} \\ \mathbf{H}_{rm} & \mathbf{H}_{rr} \end{bmatrix}
    \begin{bmatrix} \delta \mathbf{x}_{m} \\ \delta \mathbf{x}_{r} \end{bmatrix} =
    \begin{bmatrix} \mathbf{b}_{m} \\ \mathbf{b}_{r} \end{bmatrix}
  \end{aligned}
\end{equation}

marginalization을 수행하기 위해서 위 행렬에 schur complement를 수행하면 다음과 같이 행렬이 변형된다.
\begin{equation}
  \begin{aligned}
    \begin{bmatrix} \mathbf{I} & 0 \\ -\mathbf{H}_{rm}\mathbf{H}_{mm}^{-1} & \mathbf{I} \end{bmatrix} \begin{bmatrix} \mathbf{H}_{mm} & \mathbf{H}_{mr} \\ \mathbf{H}_{rm} & \mathbf{H}_{rr} \end{bmatrix} \begin{bmatrix} \delta \mathbf{x}_{m} \\ \delta \mathbf{x}_{r} \end{bmatrix} =
    \begin{bmatrix} \mathbf{I} & 0 \\ -\mathbf{H}_{rm}\mathbf{H}_{mm}^{-1} & \mathbf{I} \end{bmatrix}
    \begin{bmatrix} \mathbf{b}_{m} \\ \mathbf{b}_{r} \end{bmatrix}
  \end{aligned}
\end{equation}

\begin{equation}
  \begin{aligned}
    \begin{bmatrix} \mathbf{H}_{mm} & \mathbf{H}_{mr} \\ 0 & \underbrace{\mathbf{H}_{rr} -\mathbf{H}_{rm}\mathbf{H}_{mm}^{-1}\mathbf{H}_{mr} }_{\mathbf{H}_{p}}  \end{bmatrix}
    \begin{bmatrix} \delta \mathbf{x}_{m} \\ \delta \mathbf{x}_{r} \end{bmatrix} =
    \begin{bmatrix} \mathbf{b}_{m} \\ \underbrace{\mathbf{b}_{r}-\mathbf{H}_{rm}\mathbf{H}_{mm}^{-1}\mathbf{b}_{m} }_{\mathbf{b_{p}}} \end{bmatrix}
  \end{aligned}
\end{equation}

\begin{equation}
  \begin{aligned}
    \therefore  ( \mathbf{H}_{rr}-\mathbf{H}_{rm}\mathbf{H}_{mm}^{-1}\mathbf{H}_{mr} )\delta \mathbf{x}_{r} & = \mathbf{b}_{r}-\mathbf{H}_{rm}\mathbf{H}_{mm}^{-1}\mathbf{b}_{m}  \\
     \mathbf{H}_{p} \delta \mathbf{x}_{r} & = \mathbf{b}_{p}
  \end{aligned}
\end{equation}

슬라이딩 윈도우의 키프레임 개수를 유지하기 위해 인스턴스를 $[m,n]$개만 유지하는 경우, $m$ 이전의 상태는 marginalized되어 prior 항이 된다. 따라서 MAP 문제는 다음과 같이 쓸 수 있다.
\begin{equation}
  \begin{aligned}
    \mathcal{X}_{m:n}^{*} = \arg\min_{\mathcal{X}_{m:n}} \sum^{n}_{t=m}\sum_{k \in \mathcal{S}} \bigg\{ \left\| \mathbf{z}^{k}_{t} - h^{k}_{t}(\mathcal{X}_{m:n}) \right\|^{2}_{\Omega^{k}_{t}}  + (\mathbf{H}_{p} \delta \mathbf{x}_{r} - \mathbf{b}_{p}) \bigg\}
  \end{aligned}
\end{equation}
- $\mathcal{S}$: 관측값들의 집합 
- $\mathcal{X}_{m:n}$: $m$부터 $n$ 인스턴스 사이의 상태 변수 

6. Global Optimization in the VINS-Fusion

2019년도에 VINS-mono의 프레임워크를 기반으로 사용하면서 전역 포즈를 추정(global pose estimation)하는 VINS-fusion이 제안되었다. 지역(local)적으로 추정되는 VIO 포즈는 pose graph optimization(PGO)에 의해 GPS와 같은 전역 센서 데이터와 퓨전된다. PGO를 수행하면 지역 좌표계 $\{L\}$와 전역 좌표계 $\{G\}$ 사이의 변환 행렬 $\mathbf{T}^{G}_{L}$이 추정되어 지역 상태 변수들이 전역 좌표계로 변환된다. PGO에서 추정하는 상태 변수는 다음과 같다.
\begin{equation}
  \begin{aligned}
    \mathcal{X} = \begin{bmatrix} \mathbf{q}^{G}_{i} & \mathbf{P}^{G}_{i} & \cdots & \mathbf{q}^{G}_{n} & \mathbf{P}^{G}_{n} \end{bmatrix}
  \end{aligned}
\end{equation}

VINS-fusion에서 모든 노드는 전역 좌표계(GPS 좌표계)에서 로봇의 포즈를 가지고 있으며 두 연속적인 노드 사이의 엣지는 상대 포즈를 의미하며 VIO를 통해 추정된다. 지역 포즈와 전역 포즈는 다음과 같이 나타낼 수 있다.
\begin{equation}
  \begin{aligned}
    & \mathbf{T}^{L}_{i} = \begin{bmatrix} \mathbf{R}^{L}_{i} & \mathbf{P}^{L}_{i} \\ 0 &1 \end{bmatrix} \\
    & \mathbf{T}^{G}_{i} = \begin{bmatrix} \mathbf{R}^{G}_{i} & \mathbf{P}^{G}_{i} \\ 0 & 1 \end{bmatrix}
  \end{aligned}
\end{equation}

이 때, $i$는 $i$번째 노드를 의미하며 $L$은 지역 레퍼런스 좌표계(local reference frame)을 의미한다. $G$는 전역 레페런스 좌표계를 의미한다. 연속적인 두 노드 $i,j$가 주어졌을 때 두 노드 사이의 상대 포즈는 다음과 같이 두 가지 방법으로 유도할 수 있다.

\begin{equation}
  \begin{aligned}
    & _{L}\mathbf{T}^{i}_{j} = \begin{bmatrix} \mathbf{R}^{L}_{i} & \mathbf{P}^{L}_{i} \\ 0 & 1 \end{bmatrix}^{-1} \begin{bmatrix} \mathbf{R}^{L}_{j} & \mathbf{P}^{L}_{j} \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} _{L}\mathbf{R}^{i}_{j} & _{L}\mathbf{P}^{i}_{ij} \\ 0 &1 \end{bmatrix} \\
    & _{G}\mathbf{T}^{i}_{j} = \begin{bmatrix} \mathbf{R}^{i}_{G} \mathbf{R}^{G}_{j} & \mathbf{R}^{i}_{G}(\mathbf{P}^{G}_{j} - \mathbf{P}^{G}_{i}) \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} _{G}\mathbf{R}^{i}_{j} & _{G}\mathbf{P}^{i}_{ij} \\ 0 & 1 \end{bmatrix}
  \end{aligned}
\end{equation}

이 때,VINS-fusion 저자는 첫 번째 GPS 신호가 잡히는 지점을 전역 좌표계의 원점으로 설정하였으며 ENU(East-North-Up) 좌표계를 사용하여 방향을 설정하였다. 위 식에서 residual은 다음과 같다.
\begin{equation}
  \begin{aligned}
    \begin{bmatrix} \delta \mathbf{P}_{ij} \\ \delta \boldsymbol{\theta}_{ij} \end{bmatrix} = \begin{bmatrix} _{G}\mathbf{P}^{i}_{ij} - \ _{L}\mathbf{P}^{i}_{ij} \\ 2(_{L}\mathbf{q}^{i}_{j})^{-1} (_{G}\mathbf{q}^{i}_{j}) \end{bmatrix}
  \end{aligned}
\end{equation}
- $_{L}\mathbf{T}^{i}_{j}$: 지역 좌표계에서 바라본 $i$ 노드와 $j$ 노드의 상대 포즈 
- $_{G}\mathbf{T}^{i}_{j}$: 전역 좌표계에서 바라본 $i$ 노드와 $j$ 노드의 상대 포즈 

또한 GPS 위치 제약 조건에 의해 다음 공식이 성립한다.
\begin{equation}
  \begin{aligned}
    \delta \mathbf{P}_{i} = \begin{bmatrix} \hat{\mathbf{P}}^{G}_{i} - \tilde{\mathbf{P}}^{G}_{i} \end{bmatrix}
  \end{aligned}
\end{equation}
- $\hat{\mathbf{P}}^{G}_{i}$: 전역 좌표계에서 바라본 $i$ 노드의 위치 추정값 
- $\tilde{\mathbf{P}}^{G}_{i}$: 전역 좌표계에서 바라본 $i$ 노드의 위치 관측값

전역 위치 에러에 대한 표준편차(stdev) 값은 GPS 위치 추정 알고리즘에 의해 구할 수 있다. 지역 위치와 회전에 대한 표준편차는 VINS-fusion 코드 상에서 각각 0.1 [m]와 0.01 [rad]로 고정하여 사용하였다.

PGO는 ceres-solver를 사용하여 최적화하였으며 VIO에 비해 상당히 간헐적인(quite sparse) 주기로 업데이트 된다. 이에 따라 윈도우 크기를 크게하여 drift-free한 포즈를 추정한다고 논문에 언급하였으나 코드를 분석해봤을 때는 구현되지 않은 것으로 보인다(not implemented in the code). PGO에서 많은 노드가 한 번에 업데이트되지만 지역 좌표계와 전역 좌표계 사이의 변환 행렬 $\mathbf{T}^{G}_{L}$은 가장 최근 지역 좌표계에 대해서만 추정된다.

본 페이퍼의 저자(Yibin Wu)는 모든 노드에 대해서 변환 행렬을 추정하는게 에러를 줄이는데 효과적일 것으로 예상한다. 그리고 VIO와 GPS 측정값에 대한 시간 동기화(time synchronization)이 제대로 구현되어 있지 않다고 한다. 이러한 시간 동기화 문제는 심각한 포즈 에러를 야기할 수 있으며 또한, VIO와 GPS는 loosely-coupled 방식으로 동작하기 때문에 GPS 측정값이 IMU 센서의 값을 보정해주지 못하는 한계점을 가지고 있다.

7. References

[1] Qin, Tong, Peiliang Li, and Shaojie Shen. "Vins-mono: A robust and versatile monocular visual-inertial state estimator." IEEE Transactions on Robotics 34.4 (2018): 1004-1020. 
[2] Wu, Yibin. "Formula Derivation and Analysis of the VINS-Mono." arXiv preprint arXiv:1912.11986 (2019). 
[3] Sola, Joan. "Quaternion kinematics for the error-state Kalman filter." arXiv preprint arXiv:1711.02508 (2017).