原論文 http://udel.edu/~ghuang/papers/tr_gps-vio.pdf

回転の表現はJPL記法を使っていることに注意（JPL記法についてはクォータニオンの基礎を参照）

数学的準備

こちらを参照すること。途中で出てくる式番号はこちらに対応する。

数学公式まとめ - salpikのブログ

VIO状態定義

IMUセンサーは3軸方向の加速度 $a_{m}$ と3軸回転方向の角速度 $\omega_{m}$ が測定できる。時刻 $t=t_{k}$ でのIMUに関連した状態量をVIO状態として定義する。

$\begin{equation} ^{V}x_{I_{k}} \equiv \left( ^{I_{k}}_{V}q,\ \ ^{V}p_{I_{k}}, ^{V}v_{I_{k}}, \right) \end{equation}$

論文にはバイアス項 $b_{\omega}, b_{a}$ も状態として含まれているが、議論ではこの項を無視しているのでこの段階で状態から省いておく。

IMUセンサー値を使った状態遷移

センサー測定量 $a_{m}$ 、 $\omega_{m}$ とIMU状態量 ${}^{I}a_{I}$ 、 ${}^{I}w_{I}$ の関係

$\begin{eqnarray} a_{m}(t) &=& {}^{I}a_{I}(t) + {}^{I}_{V}R\ {}^{V}g \\\ w_{m}(t) &=& {}^{I}w_{I}(t) \end{eqnarray}$

クォータニオンの状態遷移

時刻 $t=t_{k}$ から $t_{k+1}$ までのクォータニオン状態遷移 $\Theta(t_{k+1}, t_{k})$ を求めたい。

$\begin{equation} ^{I_{k+1}}_{V}q = \Theta(t_{k+1}, t_{k}) ^{I_{k}}_{V}q \end{equation}$

となるから、時刻を $t_{k} \rightarrow t$ にして

$\begin{equation} \Theta(t, t_{k}) = {}^{I_{t}}_{V}q \ \ {}^{I_{k}}_{V}q^{-1} \end{equation}$

したがって、微分をとると

$\begin{equation} \dot{\Theta}(t, t_{k}) = {}^{I_{t}}_{V}\dot{q} \ \ {}^{I_{k}}_{V}q^{-1} \end{equation}$

となる。

クォータニオンの微分についてはこちらを参照して

$\begin{equation} \dot{q}(t) = \frac{1}{2} \Omega(\vec{w}) q(t) \end{equation}$

なので、元の微分方程式は

$\begin{eqnarray} \dot{\Theta}(t, t_{k}) &=& \frac{1}{2} \Omega(\vec{\omega}) {}^{I_{t}}_{V}q \ \ {}^{I_{k}}_{V}q^{-1} \\\ &=& \frac{1}{2} \Omega(\vec{\omega}) \Theta(t, t_{k})\\\ \end{eqnarray}$

この微分方程式を解くと

$\begin{equation} \Theta(t, t_{k}) = \exp(\frac{1}{2}\Omega(\vec{\omega})) \end{equation}$

となるので、クォータニオンの状態遷移は

$\begin{equation} ^{I_{k+1}}_{V}q = \exp(\frac{1}{2}\Omega(\vec{\omega}))\ \ {}^{I_{k}}_{V}q \end{equation}$

と表される。

位置の状態遷移

$\begin{eqnarray} {}^{V}p_{I_{k+1}} &=& {}^{V}p_{I_{k}} + \int_{t_{k}}^{t_{k+1}} d\tau {}^{V}v_{I_{k}}(\tau) \\\ &=& {}^{V}p_{I_{k}} + \int_{t_{k}}^{t_{k+1}} d\tau \left( {}^{V}v_{I_{k}} + \int_{t_{k}}^{\tau} ds {}^{V}a_{I_{s}}(s) \right) \\\ &=& {}^{V}p_{I_{k}} + \int_{t_{k}}^{t_{k+1}} d\tau \left( {}^{V}v_{I_{k}} \int_{t_{k}}^{\tau} ds (-{}^{V}g + {}^{V}_{I_{s}}R a_{m}) \right) \\\ &=& {}^{V}p_{I_{k}} + {}^{V}v_{I_{k}}\Delta t - \frac{1}{2}{}^{V}g\Delta t^{2} + \int_{t_{k}}^{t_{k+1}} d\tau \int_{t_{k}}^{s} ds {}^{V}_{I_{\tau}}R a_{m} \\\ &=& {}^{V}p_{I_{k}} + {}^{V}v_{I_{k}}\Delta t - \frac{1}{2}{}^{V}g\Delta t^{2} + {}_{V}^{I_{k}}R^\mathsf{T} \int_{t_{k}}^{t_{k+1}} d\tau \int_{t_{k}}^{\tau} ds\ \ {}^{I_{k}}_{I_{s}}R a_{m} \\\ &\equiv& {}^{V}p_{I_{k}} + {}^{V}v_{I_{k}}\Delta t - \frac{1}{2}{}^{V}g\Delta t^{2} + {}_{V}^{I_{k}}R^\mathsf{T} \vec{\alpha} \ \ \ (\vec{\alpha} を定義) \end{eqnarray}$

ここで $\Delta t \equiv t_{k}-t_{k-1}$ である。

速度の状態遷移

$\begin{eqnarray} {}^{V}v_{I_{k+1}} &=& {}^{V}v_{I_{k}} + \int_{t_{k}}^{t_{k+1}} d\tau {}^{V}a_{I_{k}}(\tau) \\\ &=& {}^{V}v_{I_{k}} + \int_{t_{k}}^{t_{k+1}} d\tau (-{}^{V}g + {}^{V}_{I_{\tau}}R a_{m}) \\\ &=& {}^{V}v_{I_{k}} -g\Delta t + {}_{V}^{I_{k}}R^\mathsf{T} \int_{t_{k}}^{t_{k+1}} d\tau \ {}^{I_{k}}_{I_{\tau}}R a_{m} \\\ &\equiv& {}^{V}v_{I_{k}} -g\Delta t + {}_{V}^{I_{k}}R^\mathsf{T} \vec{\beta} \ \ \ (\vec{\beta} を定義) \end{eqnarray}$

これで状態遷移は記述できた。

状態遷移に関する共分散行列の導出

error stateでの表現方法

上記推定後の状態に対する誤差共分散行列を求めるために、推定後の状態と推定前状態（前周期推定状態）の偏微分行列を求めたい。この場合はerror stateを考えるとよい。error stateというのは誤差のことで、真値を $x$ 、予測値を $\hat{x}$ 、誤差を $\tilde{x}$ としたときに $x=\hat{x}+\tilde{x}$ という関係が成立する。

簡単な例

$\begin{eqnarray} x_{t+1} &=& f(x_{t}, y_{t}) \\\ y_{t+1} &=& g(x_{t}, y_{t}) \end{eqnarray}$

により予測が更新される場合、求めたいのは $\frac{\partial f}{\partial x}$ 、 $\frac{\partial f}{\partial y}$ 、 $\frac{\partial g}{\partial x}$ 、 $\frac{\partial g}{\partial y}$ である。元の式をerror stateで表すと

$\begin{eqnarray} \hat{x}_{t+1} + \tilde{x}_{t+1} &=& f(\hat{x}_{t}, \hat{y}_{t}) + \frac{\partial f}{\partial x} \tilde{x}_{t} + \frac{\partial f}{\partial y} \tilde{y}_{t} \\\ \hat{y}_{t+1} + \tilde{y}_{t+1} &=& g(\hat{x}_{t}, \hat{y}_{t}) + \frac{\partial g}{\partial x} \tilde{x}_{t} + \frac{\partial g}{\partial y} \tilde{y}_{t} \end{eqnarray}$

整理すると

$\begin{eqnarray} \tilde{x}_{t+1} &=& \frac{\partial f}{\partial x} \tilde{x}_{t} + \frac{\partial f}{\partial y} \tilde{y}_{t} \\\ \tilde{y}_{t+1} &=& \frac{\partial g}{\partial x} \tilde{x}_{t} + \frac{\partial g}{\partial y} \tilde{y}_{t} \end{eqnarray}$

よって、error stateで状態を表せばその係数が求めたい偏微分相当になっていることがわかった。

姿勢のerror stateによる状態遷移表現

さきほどはクォータニオン ${}^{I_{k}}_{V}q$ で姿勢を表現していたが角度ベクトル ${}^{I_{k}}_{V} \theta$ で表現することを目指す。JPLとHamiltonの表記によると、（本論文の記法である）JPLでは

$\begin{equation} {}^{I_{k}}_{V}R = (I - \lfloor \vec{\theta} \rfloor )\ {}^{I_{k}}_{V}\hat{R} \end{equation}$

一方

$\begin{eqnarray} {}^{I_{k+1}}_{I_{k}}R &=& \exp({}^{I_{k+1}}_{I_{k}}\theta) \\\ &=& \exp({}^{I_{k+1}}_{I_{k}}\hat{\theta} + {}^{I_{k+1}}_{I_{k}}\tilde{\theta}) \\\ &\sim& \exp({}^{I_{k+1}}_{I_{k}}\hat{\theta})\exp(J_{r}(\theta){}^{I_{k+1}}_{I_{k}}\tilde{\theta})\ \ \ \ \ (1)より \\\ &\sim& \exp({}^{I_{k+1}}_{I_{k}}\hat{\theta})\ (I+\lfloor J_{r}(\theta){}^{I_{k+1}}_{I_{k}}\tilde{\theta} \rfloor) \ \ \ \ \ (2)より \\\ &\sim& {}^{I_{k+1}}_{I_{k}}\hat{R}(I-\lfloor J_{r}(\theta){}^{I_{k+1}}_{I_{k}}\tilde{\theta} \rfloor) \ \ \ \ TODO: なぜ負号になるか \end{eqnarray}$

ここで求めた関係式を、回転行列の伝搬式

$\begin{equation} {}^{I_{k+1}}_{V}R = {}^{I_{k+1}}_{I_{k}}R\ {}^{I_{k}}_{V}R \end{equation}$

に代入すると

$\begin{eqnarray} (I - \lfloor {}^{I_{k+1}}_{V} \tilde{\theta} \rfloor )\ {}^{I_{k+1}}_{V}\hat{R} &\sim& {}^{I_{k+1}}_{I_{k}}\hat{R}(I-\lfloor J_{r}({}^{I_{k+1}}_{I_{k}}\tilde{\theta}) {}^{I_{k+1}}_{I_{k}}\tilde{\theta} \rfloor \ (I - \lfloor {}^{I_{k}}_{V} \tilde{\theta} \rfloor )\ {}^{I_{k}}_{V}\hat{R} \\\ - \lfloor {}^{I_{k+1}}_{V} \tilde{\theta} \rfloor {}^{I_{k+1}}_{V}\hat{R} &\sim& -{}^{I_{k+1}}_{I_{k}}\hat{R} \lfloor J_{r}({}^{I_{k+1}}_{I_{k}}\tilde{\theta}) {}^{I_{k+1}}_{I_{k}}\tilde{\theta} \rfloor {}^{I_{k}}_{V}\hat{R} - {}^{I_{k+1}}_{I_{k}}\hat{R} \lfloor {}^{I_{k}}_{V} \tilde{\theta} \rfloor {}^{I_{k}}_{V}\hat{R} \\\ - \lfloor {}^{I_{k+1}}_{V} \tilde{\theta} \rfloor {}^{I_{k+1}}_{V}\hat{R} &\sim& - \lfloor {}^{I_{k+1}}_{I_{k}}\hat{R} J_{r}({}^{I_{k+1}}_{I_{k}}\tilde{\theta}) {}^{I_{k+1}}_{I_{k}}\tilde{\theta} \rfloor {}^{I_{k+1}}_{I_{k}}\hat{R} {}^{I_{k}}_{V}\hat{R} - \lfloor {}^{I_{k+1}}_{I_{k}}\hat{R} {}^{I_{k}}_{V} \tilde{\theta} \rfloor {}^{I_{k+1}}_{I_{k}}\hat{R} {}^{I_{k}}_{V}\hat{R} \ \ \ \ \ (5)より \\\ \lfloor {}^{I_{k+1}}_{V} \tilde{\theta} \rfloor {}^{I_{k+1}}_{V}\hat{R} &\sim& \lfloor {}^{I_{k+1}}_{I_{k}}\hat{R} J_{r}({}^{I_{k+1}}_{I_{k}}\tilde{\theta}) {}^{I_{k+1}}_{I_{k}}\tilde{\theta} \rfloor {}^{I_{k+1}}_{V}\hat{R} + \lfloor {}^{I_{k+1}}_{I_{k}}\hat{R} {}^{I_{k}}_{V} \tilde{\theta} \rfloor {}^{I_{k+1}}_{V}\hat{R} \\\ {}^{I_{k+1}}_{V} \tilde{\theta} &\sim& {}^{I_{k+1}}_{I_{k}}\hat{R} J_{r}({}^{I_{k+1}}_{I_{k}}\tilde{\theta}) {}^{I_{k+1}}_{I_{k}}\tilde{\theta} + {}^{I_{k+1}}_{I_{k}}\hat{R} {}^{I_{k}}_{V} \tilde{\theta} \\\ &=& {}^{I_{k+1}}_{I_{k}}\hat{R} J_{r}({}^{I_{k+1}}_{I_{k}}\tilde{\theta}) {}^{I_{k+1}}_{I_{k}}\tilde{\theta} + {}^{I_{k+1}}_{V}\hat{R} {}^{I_{k}}_{V}\hat{R}^\mathsf{T} {}^{I_{k}}_{V} \tilde{\theta} \ \ \ \ \ (各t_{k}での測定量で表記し直した) \end{eqnarray}$

位置のerror stateによる表現

$\begin{equation} {}^{V}p_{I_{k+1}} = {}^{V}p_{I_{k}} + {}^{V}v_{I_{k}}\Delta t - \frac{1}{2}{}^{V}g\Delta t^{2} + {}_{V}^{I_{k}}R^\mathsf{T} \vec{\alpha} \\\ \end{equation}$

をベースとし、

$\begin{eqnarray} {}^{V}p_{I_{k}} &=& {}^{V}\hat{p}_{I_{k}} + {}^{V}\tilde{p}_{I_{k}} \\\ {}_{V}^{I_{k}}R &=& (I - \lfloor {}_{V}^{I_{k}}\theta \rfloor) {}_{V}^{I_{k}}\hat{R} \end{eqnarray}$

を使うと

$\begin{eqnarray} {}^{V}\hat{p}_{I_{k+1}} + {}^{V}\tilde{p}_{I_{k+1}} = {}^{V}\hat{p}_{I_{k}} + {}^{V}\tilde{p}_{I_{k}} + {}^{V}\hat{v}_{I_{k}}\Delta t + {}^{V}\tilde{v}_{I_{k}}\Delta t - \frac{1}{2}{}^{V}g\Delta t^{2} + {}_{V}^{I_{k}}\hat{R}^\mathsf{T}(I + \lfloor {}_{V}^{I_{k}}\theta \rfloor) \vec{\alpha} \end{eqnarray}$

予測値については

$\begin{equation} {}^{V}\hat{p}_{I_{k+1}} = {}^{V}\hat{p}_{I_{k}} + {}^{V}\hat{v}_{I_{k}}\Delta t - \frac{1}{2}{}^{V}g\Delta t^{2} + {}_{V}^{I_{k}}\hat{R}^\mathsf{T} \vec{\alpha} \end{equation}$

という関係性が成立するので、上の式は

$\begin{eqnarray} {}^{V}\tilde{p}_{I_{k+1}} &=& {}^{V}\tilde{p}_{I_{k}} + {}^{V}\tilde{v}_{I_{k}}\Delta t + {}_{V}^{I_{k}}\hat{R}^\mathsf{T} \lfloor {}_{V}^{I_{k}}\tilde{\theta} \rfloor \vec{\alpha} \\\ &=& {}^{V}\tilde{p}_{I_{k}} + {}^{V}\tilde{v}_{I_{k}}\Delta t - {}_{V}^{I_{k}}\hat{R}^\mathsf{T} \lfloor {}_{V}^{I_{k}}\vec{\alpha} \rfloor {}_{V}^{I_{k}} \tilde{\theta} \ \ \ \ \ (式(4)より) \\\ &=& {}^{V}\tilde{p}_{I_{k}} + {}^{V}\tilde{v}_{I_{k}}\Delta t - \lfloor {}_{V}^{I_{k}} \hat{R}^\mathsf{T} {}_{V}^{I_{k}}\vec{\alpha} \rfloor {}_{V}^{I_{k}} \hat{R}^\mathsf{T} {}_{V}^{I_{k}} \tilde{\theta} \ \ \ \ \ (式(5)より) \\\ &=& {}^{V}\tilde{p}_{I_{k}} + {}^{V}\tilde{v}_{I_{k}}\Delta t - \lfloor {}^{V}\hat{p}_{I_{k+1}} - {}^{V}\hat{p}_{I_{k}} - {}^{V}v_{I_{k}}\Delta t + \frac{1}{2}{}^{V}g\Delta t^{2} \rfloor {}_{V}^{I_{k}} \hat{R}^\mathsf{T} {}_{V}^{I_{k}} \tilde{\theta} \ \ \ \ \ (\vec{\alpha}の扱いが面倒なので削除) \\\ \end{eqnarray}$

速度のerror stateによる表現

$\begin{equation} {}^{V}v_{I_{k+1}} = {}^{V}v_{I_{k}} -g\Delta t + {}_{V}^{I_{k}}R^\mathsf{T} \vec{\beta} \end{equation}$

をベースとし、位置のerror state表現と同様にerror stateを含ませると

$\begin{eqnarray} {}^{V}\hat{v}_{I_{k+1}} + {}^{V}\tilde{v}_{I_{k+1}} = {}^{V}\hat{v}_{I_{k}} + {}^{V}\tilde{v}_{I_{k}} -g\Delta t + {}_{V}^{I_{k}}\hat{R}^\mathsf{T} (I + \lfloor {}_{V}^{I_{k}}\tilde{\theta} \rfloor)\vec{\beta} \end{eqnarray}$

予測値については

$\begin{equation} {}^{V}\hat{v}_{I_{k+1}} = {}^{V}\hat{v}_{I_{k}} -g\Delta t + {}_{V}^{I_{k}}\hat{R}^\mathsf{T} \vec{\beta} \end{equation}$

という関係性が成立するので、上の式は

$\begin{eqnarray} {}^{V}\tilde {v}_{I_{k+1}} &=& {}^{V}\tilde{v}_{I_{k}} + {}_{V}^{I_{k}}\hat{R}^\mathsf{T} \lfloor {}_{V}^{I_{k}}\tilde{\theta} \rfloor \vec{\beta} \\\ &=& {}^{V}\tilde{v}_{I_{k}} - {}_{V}^{I_{k}}\hat{R}^\mathsf{T} \lfloor \vec{\beta} \rfloor {}_{V}^{I_{k}}\tilde{\theta} \ \ \ \ \ (式(4)より) \\\ &=& {}^{V}\tilde{v}_{I_{k}} - \lfloor {}_{V}^{I_{k}}\hat{R}^\mathsf{T} \vec{\beta} \rfloor {}_{V}^{I_{k}}\hat{R}^\mathsf{T} {}_{V}^{I_{k}}\tilde{\theta} \ \ \ \ \ (式(5)より) \\\ &=& {}^{V}\tilde{v}_{I_{k}} - \lfloor {}^{V}\hat {v}_{I_{k+1}} - {}^{V}\hat {v}_{I_{k}} + g\Delta t \rfloor {}_{V}^{I_{k}}\hat{R}^\mathsf{T} {}_{V}^{I_{k}}\tilde{\theta} \ \ \ \ \ (\vec{\beta}の扱いが面倒なので削除) \end{eqnarray}$

伝搬行列まとめ

まとめると $t=t_{k}$ から $t_{k+1}$ までのpropagationを表す行列は

$\begin{equation} \left( \begin{array}{c} {}^{I_{k+1}}_{V}\tilde{\theta} \\ {}^{V}\tilde{p}_{I_{k+1}} \\ {}^{V}\tilde{v}_{I_{k+1}} \\ \tilde{b}^{\omega}_{k+1} \\ \tilde{b}^{a}_{k+1} \end{array} \right) = \left( \begin{array}{ccccc} {}^{I_{k+1}}_{V}\hat{R} {}^{I_{k}}_{V}\hat{R}^\mathsf{T} & 0_{3\times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\ - \lfloor {}^{V}\hat{p}_{I_{k+1}} - {}^{V}\hat{p}_{I_{k}} - {}^{V}v_{I_{k}}\Delta t + \frac{1}{2}{}^{V}g\Delta t^{2} \rfloor {}_{V}^{I_{k}} \hat{R}^\mathsf{T} & I_{3} & \Delta t I_{3} & 0_{3 \times 3} & 0_{3 \times 3} \\ - \lfloor {}_{V}^{I_{k}}\hat{R}^\mathsf{T} \vec{\beta} \rfloor {}_{V}^{I_{k}}\hat{R}^\mathsf{T} & 0_{3 \times 3} & I_{3} & 0_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & I_{3} & 0_{3 \times 3} \\ 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & I_{3} \end{array} \right) \left( \begin{array}{c} {}^{I_{k}}_{V}\tilde{\theta} \\ {}^{V}\tilde{p}_{I_{k}} \\ {}^{V}\tilde{v}_{I_{k}} \\ \tilde{b}^{\omega}_{k} \\ \tilde{b}^{a}_{k} \end{array} \right) \end{equation}$

カメラでの特徴点の観測

特徴点 $f$ の位置を $\vec{p}_{f}$ とする。カメラが向いている方向がx、横方向がy、上方向がzである。カメラの画像平面上での観測位置 $\vec{z}_{C}$ は

$\begin{equation} \vec{z}_{C} = \left( \begin{array}{c} \frac{{}^{C}x_{f}}{{}^{C}z_{f}} \\ \frac{{}^{C}y_{f}}{{}^{C}z_{f}} \end{array} \right) \end{equation}$

また、特徴点のカメラ座標系とVIO座標系の変換の関係は

$\begin{eqnarray} {}^{C}p_{f} &=& {}^{C}_{V}R({}^{V}p_{f} - {}^{V}p_{C}) \\\ &=& {}^{C}_{I}R {}^{I}_{V}R({}^{V}p_{f} - ({}^{V}p_{I} + {}^{V}_{C}R({}^{C}p_{C} - {}^{C}p_{I}))) \ \ \ \ \ (特徴点ベクトルをカメラ座標系ではなくVIO座標系で表現する) \\\ &=& {}^{C}_{V}R{}^{I}_{V}R({}^{V}p_{f} - {}^{V}p_{I}) + {}^{C}p_{I} \end{eqnarray}$

error stateで展開すると

$\begin{eqnarray} {}^{C}\hat{p}_{f} + {}^{C}\tilde{p}_{f} &=& {}^{C}_{V}\hat{R}(I - \lfloor {}^{I}_{V}\tilde{\theta} \rfloor){}^{I}_{V}\hat{R}({}^{V}\hat{p}_{f} + {}^{V}\tilde{p}_{f} - {}^{V}\hat{p}_{I} - {}^{V}\tilde{p}_{I}) + {}^{C}p_{I} \\\ {}^{C}\tilde{p}_{f} &=& {}^{C}_{V}\hat{R} {}^{I}_{V}\hat{R}({}^{V}\tilde{p}_{f} - {}^{V}\tilde{p}_{I}) - {}^{C}_{V}\hat{R} \lfloor {}^{I}_{V}\tilde{\theta} \rfloor {}^{I}_{V}\hat{R} ({}^{V}\tilde{p}_{f} - {}^{V}\tilde{p}_{I}) \\\ &=& {}^{C}_{V}\hat{R} {}^{I}_{V}\hat{R}({}^{V}\tilde{p}_{f} - {}^{V}\tilde{p}_{I}) + {}^{C}_{V}\hat{R} \lfloor {}^{I}_{V}\hat{R} ({}^{V}\tilde{p}_{f} - {}^{V}\tilde{p}_{I}) \rfloor {}^{I}_{V}\tilde{\theta} \\\ \end{eqnarray}$

VIO系での特徴位置 ${}^{V}p_{f}$ とIMUの位置 ${}^{V}p_{I}$ がカルマンフィルターの状態量となる。 ${}^{C}p_{I}$ はIMUとカメラの取り付けの関係（通常既知）である。ヤコビ行列を求める時は一度カメラ系での特徴点の座標を経由してから微分することになる。また、 ${}^{E}_{V}\tilde{\theta}$ と ${}^{E}\tilde{p}_{V}$ （VIO系とglobal系の変換）については当然特徴点観測には影響がないが、後のGPS測定に必要になるためここでヤコビ行列に加えておく。

$\begin{eqnarray} H_{C} &=& \left[ \frac{\partial \tilde{z}_{c_{k}}}{\partial {}^{I_{k}}_{V}\tilde{\theta}},\ \frac{\partial \tilde{z}_{c_{k}}}{\partial {}^{V}\tilde{p}_{I_{k}}},\ \frac{\partial \tilde{z}_{c_{k}}}{\partial {}^{V}\tilde{v}_{I_{k}}},\ \frac{\partial \tilde{z}_{c_{k}}}{\partial {}^{V}\tilde{p}_{f}},\ \frac{\partial \tilde{z}_{c_{k}}}{\partial {}^{E}_{V}\tilde{\theta}},\ \frac{\partial \tilde{z}_{c_{k}}}{\partial {}^{E}\tilde{p}_{V}} \right] \\\ &=& \left[ H_{\Pi}\frac{\partial {}^{C_{k}}\tilde{p}_{f}}{\partial {}^{I_{k}}_{V}\tilde{\theta}},\ H_{\Pi}\frac{\partial {}^{C_{k}}\tilde{p}_{f}}{\partial {}^{V}\tilde{p}_{I_{k}}},\ H_{\Pi}\frac{\partial {}^{C_{k}}\tilde{p}_{f}}{\partial {}^{V}\tilde{v}_{I_{k}}},\ H_{\Pi}\frac{\partial {}^{C_{k}}\tilde{p}_{f}}{\partial {}^{V}\tilde{p}_{f}},\ H_{\Pi}\frac{\partial {}^{C_{k}}\tilde{p}_{f}}{\partial {}^{E}_{V}\tilde{\theta}},\ H_{\Pi}\frac{\partial {}^{C_{k}}\tilde{p}_{f}}{\partial {}^{E}\tilde{p}_{V}} \right] \\\ &=& \left[ H_{\Pi}{}^{C}_{V}\hat{R} \lfloor {}^{I}_{V}\hat{R} ({}^{V}\tilde{p}_{f} - {}^{V}\tilde{p}_{I}) \rfloor,\ -H_{\Pi} {}^{C}_{V}\hat{R} {}^{I}_{V}\hat{R},\ 0_{2\times 3 },\ H_{\Pi}{}^{C}_{V}\hat{R} {}^{I}_{V}\hat{R},\ 0_{2\times1},\ 0_{2\times1} \right] \\\ H_{\Pi} &\equiv& \left( \begin{array}{ccc} \frac{1}{{}^{C_{k}}\hat{z}_{f}} & 0 & -\frac{{}^{C_{k}}\hat{x}_{f}}{{}^{C_{k}}\hat{z}_{f}^{2}} \\ 0 & \frac{1}{{}^{C_{k}}\hat{z}_{f}} & -\frac{{}^{C_{k}}\hat{y}_{f}}{{}^{C_{k}}\hat{z}_{f}^{2}} \\ \end{array} \right) \end{eqnarray}$

GPSでのIMU位置の測定

GPSはEarth座標系でGPSセンサーの位置 ${}^{E}p_{g_{k}}$ を測定することができる。我々が求めたいVIO座標系でのIMUの位置 ${}^{V}p_{I}$ を使った関係式にすると

$\begin{eqnarray} z_{g_{k}} \equiv {}^{E}p_{g_{k}} &=& {}^{E}p_{V} + {}^{E}_{V}R {}^{V}p_{g_{k}} \ \ \ \ \ (座標変換の合成)\\\ &=& {}^{E}p_{V} + {}^{E}_{V}R ({}^{V}p_{I_{k}}+ {}^{I_{k}}_{V}R^\mathsf{T}{}^{I_{k}}p_{g_{k}}) \end{eqnarray}$

となる。 ${}^{E}p_{V}$ と ${}^{E}_{V}R$ はVIOとEarth座標系の間の変換なので、trajectory alignmentなどで求める。GPS測定結果をIMU座標系の位置に直すためにGPS取り付け位置の関係 ${}^{I_{k}}p_{g_{k}}$ とIMUのVIO系での姿勢 ${}^{V}_{I_{k}}R$ を知っておく必要がある。この式を展開すると

$\begin{eqnarray} \hat{z}_{g_{k}} + \tilde{z}_{g_{k}} &\sim& {}^{E}\hat{p}_{V} + {}^{E}\tilde{p}_{V} (I_{3}-\lfloor {}^{E}_{V}\tilde{\theta} \rfloor ){}^{E}_{V}\hat{R} ({}^{V}\hat{p}_{I_{k}} + {}^{V}\tilde{p}_{I_{k}}) + (I_{3} - \lfloor {}^{E}_{V}\tilde{\theta} \rfloor ) {}^{E}_{V}\hat{R} {}^{I_{k}}_{V}\hat{R}^\mathsf{T} (I_{3} + \lfloor {}^{I_{k}}_{V}\theta \rfloor) {}^{I_{k}}p_{g_{k}} \\\ \tilde{z}_{g_{k}} &\sim& {}^{E}\tilde{p}_{V} + {}^{E}_{V}\hat{R} {}^{V}\tilde{p}_{I_{k}} - \lfloor {}^{E}_{V}\tilde{\theta} \rfloor {}^{E}_{V}\hat{R}{}^{V}\hat{p}_{I_{k}} - \lfloor {}^{E}_{V}\tilde{\theta} \rfloor {}^{E}_{V}\hat{R} {}^{I_{k}}_{V}\hat{R}^\mathsf{T} {}^{I}p_{g_{k}} + {}^{E}_{V}\hat{R} {}^{I_{k}}_{V}\hat{R}^\mathsf{T} \lfloor {}^{I_{k}}_{V}\tilde{\theta} \rfloor {}^{I}p_{g_{k}} \\\ \end{eqnarray}$

論文では $p_{g_{k}} \sim 0$ の近似を行っており、

$\begin{eqnarray} \tilde{z}_{g_{k}} &\sim& {}^{E}\tilde{p}_{V} + {}^{E}_{V}\hat{R} {}^{V}\tilde{p}_{I_{k}} - \lfloor {}^{E}_{V}\tilde{\theta} \rfloor {}^{E}_{V}\hat{R}{}^{V}\hat{p}_{I_{k}} \\\ &=& {}^{E}\tilde{p}_{V} + {}^{E}_{V}\hat{R} {}^{V}\tilde{p}_{I_{k}} + \lfloor {}^{E}_{V}\hat{R}{}^{V}p_{I_{k}} \rfloor {}^{E}_{V}\tilde{\theta} \\\ \end{eqnarray}$

したがってヤコビ行列は

$\begin{eqnarray} H_{G} &=& \left[ \frac{\partial \tilde{z}_{g_{k}}}{\partial {}^{I_{k}}_{V}\tilde{\theta}},\ \frac{\partial \tilde{g}_{c_{k}}}{\partial {}^{V}\tilde{p}_{I_{k}}},\ \frac{\partial \tilde{z}_{g_{k}}}{\partial {}^{V}\tilde{v}_{I_{k}}},\ \frac{\partial \tilde{z}_{g_{k}}}{\partial {}^{V}\tilde{p}_{f}},\ \frac{\partial \tilde{z}_{g_{k}}}{\partial {}^{E}_{V}\tilde{\theta}},\ \frac{\partial \tilde{g}_{c_{k}}}{\partial {}^{E}\tilde{p}_{V}} \right] \\\ &=& \left[ \vec{0}_{3\times 3},\ {}^{E}_{V}\hat{R},\ \vec{0}_{3\times 3},\ \vec{0}_{3\times 3},\ \lfloor {}^{E}_{V}\hat{R}{}^{V}\hat{p}_{I_{k}} \rfloor,\ I_{3} \right] \\\ \end{eqnarray}$

参考

https://arxiv.org/pdf/1512.02363.pdf