元論文

https://www-users.cs.umn.edu/~stergios/papers/ICRA07-MSCKF.pdf

要約

N個のカメラとIMUがある運動物体の位置を推定する。特徴点の座標や昔の測定情報はカルマンフィルターの状態には含めない一方で、各カメラのpose情報は状態として含める。

カルマンフィルターの状態の定義

まずIMU関係の状態を定義する。

$\begin{equation} X_{IMU} = ({}^{I}_{G}\theta, b_{g}, {}^{G}v_{I}, b_{a}, {}^{G}p_{I}) \end{equation}$

ここで系GはVIOのodometry系のことであり系IはIMU系である。 $b_{a}$ と $b_{g}$ はそれぞれ加速度と角速度に関するバイアス項である。
ここにNこのカメラ $C_{1}, C_{2}, ... C_{N}$ の位置情報も加えたものがカルマンフィルターの状態である。

$\begin{equation} X = (X_{IMU}, {}^{C_{1}}_{G}\theta, {}^{G}p_{C_{1}}, {}^{C_{2}}_{G}\theta, \dots, {}^{G}p_{C_{2}}, {}^{C_{N}}_{G}\theta, {}^{G}p_{C_{N}}) \end{equation}$

測定値の取り扱い

角速度

IMUで測定した角速度を $\omega_{m}$ とし、実際のIMUのpureの角速度を $\omega$ とすると、

$\begin{eqnarray} \omega_{m} &=& \omega + {}^{I}\omega_{E} + b_{g} + n_{g} \\\ &=& \omega + {}^{I}_{G}{}^{G}\omega_{E} + b_{g} + n_{g} \\\ \end{eqnarray}$

ここで ${}^{G}\omega_{E}$ は地球の自転による回転の効果であるがこれは小さい（1日で $2\pi$ 回転）なので通常無視できる。ここから我々が知りたい値であるIMU角速度の推定値を表現する。

$\begin{equation} \omega = \omega_{m} - {}^{I}_{G}R{}^{G}\omega_{E} - b_{g} - n_{g} \end{equation}$

期待値をとると、 $\hat{n}_{g} = 0$ であるから

$\begin{equation} \hat{\omega} = \omega_{m} - {}^{I}_{G}R{}^{G}\omega_{E} - \hat{b}_{g} \end{equation}$

すなわち

$\begin{equation} \omega = \hat{\omega} - \tilde{b}_{g} - n_{g} \end{equation}$

加速度

IMUで測定した加速度を $a_{m}$ としpureのIMU系における加速度を ${}^{I}a$ とすると

$\begin{eqnarray} a_{m} &\sim& {}^{I}a + {}^{I}g + b_{a} + n_{a} \\\ &=& {}^{I}_{G}R ({}^{G}a - {}^{G}g) + b_{a} + n_{a} \\\ \end{eqnarray}$

${}^{G}g$ の項がマイナスであるのは、力が働いているのと逆方向に加速しているとIMUが考えるからである。車が重力方向に自由落下（ ${}^{G}a = {}^{G}g$ ）しているときにIMUの測定量が0になると考えると納得できる。また ${}^{G}\omega_{E}$ による項は小さいので無視した。ここから我々が知りたい値であるIMU加速度の推定値を表現する。

$\begin{eqnarray} {}^{G}a &=& {}^{G}_{I}R(a_{m} - b_{a} - n_{a}) - {}^{G}g \\\ {}^{G}\hat{a} &=& {}^{G}_{I}R(a_{m} - \hat{b}_{a}) - {}^{G}g \end{eqnarray}$

伝搬ステップ

GPS-VIOの論文と同じようにerror stateからヤコビアンを導出する。GPS-VIOの論文では $t=t_{k+1}$ が $t=t_{k}$ を使ってどう表されるかという関係を導いていたが、この論文では状態の微分を考えている。本質的には $x_{k+1} = x_{k} + \dot{x}_{k}\Delta t$ なのでどちらも同じ情報であり、 $\Delta t$ をかけて単位行列を加算すれば同じものになることがわかる。

測定値の取り扱い

姿勢に関する伝搬ステップ

一般的にクォータニオンの微分を $\theta$ 微分に表現し直すと

$\begin{eqnarray} q &=& \tilde{q}\hat{q} \\\ \dot{q} &=& \dot{\tilde{q}}\hat{q} + \tilde{q}\dot{\hat{q}} \\\ &=& \frac{1}{2} (\omega, 0) = \dot{\tilde{q}}\hat{q} + \frac{1}{2}\tilde{q}(\hat{\omega}, 0)\hat{q} \ \ \ \ \ (クォータニオンの微分) \\\ \dot{\tilde{q}}\hat{q} &=& \frac{1}{2}\left[ (\omega, 0)q - \tilde{q}(\omega{w}, 0)\hat{q}\right] \\\ &=& \frac{1}{2}\left[ (\omega, 0)\tilde{q}\hat{q} - \tilde{q}(\omega{w}, 0)\hat{q}\right] \\\ &=& \frac{1}{2}\left[ (\omega, 0)\tilde{q} - \tilde{q}(\omega{w}, 0)\right] \hat{q} \\\ \dot{\tilde{q}} &=& \frac{1}{2}\left[ (\omega, 0)\tilde{q} - \tilde{q}(\omega{w}, 0)\right] \\\ \dot{\tilde{q}} &=& \frac{1}{2}\left[ (\hat{\omega} - \tilde{b}_{g} - n_{g}, 0)\tilde{q} - \tilde{q}(\omega{w}, 0)\right] \\\ &=& \frac{1}{2}\left[ \left( \begin{array}{cc} - \lfloor \hat{\omega} \rfloor & \hat{\omega} \\ -\hat{\omega} & 0\end{array}\right) \tilde{q} - \left( \begin{array}{cc} + \lfloor \hat{\omega} \rfloor & \hat{\omega} \\ -\hat{\omega} & 0\end{array}\right) \tilde{q} \right] - \frac{1}{2}(\tilde{b}_{g}+n_{g}, 0) \tilde{q} \ \ \ \ \ (JPL表記でのクォータニオンの行列表現)\\\ &=& \left( \begin{array}{cc} - \lfloor \hat{\omega} \rfloor & 0 \\ 0 & 0\end{array} \right) \left( \begin{array}{c} \tilde{\theta}_{x} \\ \tilde{\theta}_{y} \\ \tilde{\theta}_{z} \\ 1 \end{array} \right) - \frac{1}{2}\left( \begin{array}{cc} - \lfloor \tilde{b}_{g} - n_{g} \rfloor & \tilde{b}_{g}+n_{g} \\ -(\tilde{b}_{g}+n_{g})^\mathsf{T} & 0\end{array}\right) \left( \begin{array}{c} \tilde{\theta}_{x} \\ \tilde{\theta}_{y} \\ \tilde{\theta}_{z} \\ 1 \end{array} \right) \\\ \frac{1}{2} \left( \begin{array}{c} \dot{\tilde{\theta}}_{x} \\ \dot{\tilde{\theta}}_{y} \\ \dot{\tilde{\theta}}_{z} \end{array} \right) &\sim& \frac{1}{2} \left( \begin{array}{c} -\lfloor \hat{\omega} \rfloor \left( \begin{array}{c} \tilde{\theta}_{x} \\ \tilde{\theta}_{y} \\ \tilde{\theta}_{z} \end{array} \right) - (\tilde{b}_{g} + n_{g}) \\ 0 \end{array} \right) \\\ \dot{\tilde{\theta}} &\sim & -\lfloor \hat{\omega} \rfloor \tilde{\theta} - \tilde{b}_{g} - n_{g} \end{eqnarray}$

である。途中クォータニオンの微分の結果を使った。したがって今回の場合は

$\begin{equation} {}^{I}_{G}\dot{\tilde{\theta}} \sim -\lfloor \omega_{m} - \hat{b}_{g} - {}^{I}_{G}R{}^{G}\omega_{E} \rfloor {}^{I}_{G}{\tilde{\theta}} - \tilde{b}_{g} - n_{g} \end{equation}$

速度に関する伝搬ステップ

$\begin{eqnarray} {}^{G}\dot{v}_{I} &=& {}^{G}a_{I} \\\ &=& {}^{I}_{G}R^\mathsf{T}(a_{m} - b_{a} - n_{a}) - {}^{G}g \\\ {}^{G}\dot{\hat{v}}_{I} + {}^{G}\dot{\tilde{v}}_{I} &=& {}^{G}_{I}\hat{R}^\mathsf{T}(I + \lfloor {}^{G}_{I}\tilde{\theta} \rfloor)(a_{m} - \hat{b}_{a} - \tilde{b}_{a} - n_{a}) - {}^{G}g \\\ &\sim& {}^{G}_{I}\hat{R}^\mathsf{T}(a_{m} - \hat{b}_{a}) - {}^{G}g - {}^{G}_{I}\hat{R}^\mathsf{T}(\tilde{b}_{a} + n_{a}) + {}^{G}_{I}\hat{R}^\mathsf{T} \lfloor {}^{G}_{I}\tilde{\theta} \rfloor (a_{m} - \hat{b}_{a}) \ \ \ \ \ (高次の項は捨てた) \\\ &=& {}^{G}_{I}\hat{R}^\mathsf{T}(a_{m} - \hat{b}_{a}) - {}^{G}g - {}^{G}_{I}\hat{R}^\mathsf{T}(\tilde{b}_{a} + n_{a}) - {}^{G}_{I}\hat{R}^\mathsf{T} \lfloor a_{m} - \hat{b}_{a} \rfloor {}^{G}_{I}\tilde{\theta} \end{eqnarray}$

となり、

$\begin{equation} {}^{G}\dot{\hat{v}}_{I} = {}^{G}_{I}\hat{R}(a_{m} - \hat{b}_{a}) - {}^{G}g \end{equation}$

であるから

$\begin{equation} {}^{G}\dot{\tilde{v}}_{I} \sim - {}^{G}_{I}\hat{R}^\mathsf{T}(\tilde{b}_{a} + n_{a}) - {}^{G}_{I}\hat{R}^\mathsf{T} \lfloor a_{m} - \hat{b}_{a} \rfloor {}^{G}_{I}\tilde{\theta} \end{equation}$

位置に関する伝搬ステップ

$\begin{eqnarray} {}^{G}\dot{p}_{I} &=& {}^{G}v_{I} \\\ {}^{G}\dot{\hat{p}}_{I} + {}^{G}\dot{\tilde{p}}_{I} &=& {}^{G}\hat{v}_{I} + {}^{G}\tilde{v}_{I} \\\ {}^{G}\dot{\tilde{p}}_{I} &=& {}^{G}\tilde{v}_{I} \end{eqnarray}$

伝播行列まとめ

$\begin{eqnarray} \frac{d}{dt} \left( \begin{array}{c} {}^{I}_{G}\tilde{\theta} \\ \tilde{b}_{g} \\ {}^{G}\tilde{v}_{I} \\ \tilde{b}_{a} \\ {}^{G}\tilde{p}_{I} \end{array} \right) &=& \left( \begin{array}{ccccc} - \lfloor \omega_{m} - \hat{b}_{g} - {}^{I}_{G}R{}^{G}\omega_{E} \rfloor & -I_{3} & 0_{3\times 3} & 0_{3\times 3} & 0_{3\times 3} \\ 0_{3\times 3} & 0_{3\times 3} & 0_{3\times 3} & 0_{3\times 3} & 0_{3\times 3} \\ {}^{G}_{I}\hat{R}^\mathsf{T} \lfloor a_{m} - \hat{b}_{a} \rfloor & 0_{3\times 3} & 0_{3\times 3} & - {}^{G}_{I}\hat{R}^\mathsf{T} & 0_{3\times 3} \\ 0_{3\times 3} & 0_{3\times 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} \\ \end{array} \right) \left( \begin{array}{c} {}^{I}_{G}\tilde{\theta} \\ \tilde{b}_{g} \\ {}^{G}\tilde{v}_{I} \\ \tilde{b}_{a} \\ {}^{G}\tilde{p}_{I} \end{array} \right) \\\ && + \left( \begin{array}{cccc} -I_{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} & -{}^{G}_{I}\hat{R}^\mathsf{T} & 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} \\ \end{array} \right) \left( \begin{array}{c} n_{g} \\ \dot{b}_{g} \\ n_{a} \\ \dot{b}_{a} \\ \end{array} \right) \end{eqnarray}$

観測ステップ

$i$ 番目のセンサーで $j$ 番目の特徴点を検出した結果を $z_{i}^{(j)}$ とする。以降はある１つの特徴点 $j$ のみに着目して記述する。

$\begin{equation} \tilde{z}^{(j)} = (\tilde{z}_{1}^{(j)},\ \tilde{z}_{2}^{(j)},\ \dots \tilde{z}_{M}^{(j)})^\mathsf{T} \sim H_{X}^{(j)}\tilde{X} + H_{f}^{(j)} {}^{G}\tilde{p}_{f_{j}} + n_{i}^{(j)} \end{equation}$

ここではヤコビ行列の詳細までは記載しない。ここで、カルマンフィルターの状態とは無関係の ${}^{G}\tilde{p}_{f_{j}}$ があるので通常の拡張カルマンフィルターの定式化となっていない。これを拡張カルマンフィルターの形に持っていくため、 $H_{f}^{(j)}$ に対するleft null spaceの行列 $A$ を使う（left null spaceについてはこちらを参照）。 $H_{f}^{(j)}$ はカメラの数を $M$ とすると $2M\times 3$ であるのでleft null spaceの次元は $2M-3$ である。 $(2M-3)\times 2M$ の行列である $A$ を左からかけると、 $AH_{f}^{(j)} = O$ であることに注意すれば

$\begin{equation} A\tilde{z}_{i}^{(j)} \sim AH_{X}^{(j)}\tilde{X} + An_{i}^{(j)} \end{equation}$

ここで ${z}_{i}^{(j)}$ は $2M$ 次元であったが $A\tilde{z}_{i}^{(j)}$ は $2M-3$ 次元に減退していることに注意。これで通常の拡張カルマンフィルターの形となった。これは、普通観測結果はカメラがM個あるため $2M$ 次元であるが、拘束条件があるため観測次元が実は $2M-3$ に落ちていると理解すれば理解しやすい。