視覺-慣性組合導航

單目視覺導航技術存在的一個很大的問題就是當系統只有相機一個外部傳感器時無法很好的恢復尺度。除了借助常用的視覺手段如RGB-D相機或者雙目相機之外，還有一些其他的方法，但也各有缺點。例如激光測距器傳感器太重，紅外線傳感器又對太陽光太敏感，聲吶的測距范圍又受到限制。於是只剩下三種比較好的解決方案：雙目相機、壓力傳感器和IMU。然而雙目相機由於其基線太短而無法准確的測量遠處的距離，壓力傳感器在室內又不太可靠（突然開關門或空調都會使壓力發生突變）。於是似乎只有IMU成了一個比較合理的方案。而融合IMU數據和視覺導航系統輸出可以使用最小二乘或者EKF來實現。（UKF計算量太大不考慮）然而最小二乘對粗差又不太敏感，而且通過最小二乘只能得到scale的估計值，不能得到載體速度和位置的最優估計值 ^[1][2]，於是使用EKF來作為數據融合的框架。

EKF框架

單目相機可以用來補償IMU的暫時漂移，而磁力計、GPS又可以用於補償單目相機的空間漂移。下圖為幾種(non-exhaustive)傳感器的一個簡單分類。


上圖為幾種傳感器的分類。單目相機或者帶用激光掃描器的相機基於ICP進行姿態估計可以用於降低IMU的臨時性漂移。而全局性傳感器如激光跟蹤器或GPS則可以用於削弱相機或者激光掃描器的空間誤差。我們認為統一的自標定(self-calibrating)姿態估計方法非常重要，上述傳感器為作者最終使用的傳感器，它們的使用效果和局限將在下文介紹。

傳統的緊組合方案使用EKF SLAM的方法，其計算復雜度為\(O(M^2)\) ，其中M為特征數目。相反，本文中提出的方法將計算復雜度降為常數。而且，因為將姿態估計當作一個黑盒子，我們的方法結果與姿態估計采用的方法無關。最重要的是，我們的方法可以檢測姿態估計的錯誤和漂移。整體結構如下圖所示


傳感器安裝與傳感器模型

下面先討論系統中最核心的兩大傳感器：用於預測更新狀態的IMU和用於校正更新狀態的單目相機。IMU可以提供三軸的加速度和角速率，而單目相機經過VO的解算后可以得到在視覺框架(visual frame 其實就是VO里的world frame，由第一個關鍵幀確定)下缺少尺度信息的3D位置以及與尺度無關的姿態。下圖中顯示了單目相機和IMU的安裝以及相應的坐標系。其中，綠線表示的是由IMU框架到單目相機框架的系統標定參數，去取決於系統的變形，一般為常數；紅線表示的是世界坐標系到IMU框架的轉換參數，也是我們關注的重點；藍線表示的是從視覺框架到相機坐標系的轉換參數，由單目VO解算得到；灰線表示的是從視覺框架到世界坐標系的轉換參數，在短期內也是不變的，但我們認為，其中的旋轉參數（\(q_{v}^w\)）和平移參數（\(p_{v}^w\)）會隨着時間不斷漂移。引入這些漂移參數和建立世界坐標系的原因來源於兩個方面

• 姿態輸入尺度未知且在空間上漂移
• 多傳感器需要一個獨立的參考系來確定其位置姿態


各個傳感器使用的坐標系如上圖所示，每兩個坐標系之間都存在一個旋轉\(q\)和平移\(p\) 。基本不變的參數用綠線表示，如IMU-cam的轉換參數。紅線表示的數據是我們最終需要的位姿。藍線表示的是相機坐標系與是視覺框架的轉換關系。灰線表示的是視覺框架與世界坐標系的轉換參數，這些參數在短期內保持不變。注意\(q^w_{v}\)中微小的變化即為系統角度的漂移，\(p^w_{v}\)中微小的變化即為系統位置的漂移。

我們假定IMU測量值中包括一項零偏\(b\)和一項高斯隨機噪聲\(n\)，於是，對於IMU框架下的真實的角速度\(\omega\)和加速度\(a\)有

\[\omega=\omega_{m}-b_{\omega}-n_{\omega} ,\space a=a_{m}-b_{a}-n_{a}\tag{1} \]

上市中下標m表示測量值，並且將零偏建模為動態隨機過程，即

\[\dot b_{\omega}=n_{b_{\omega}},\space\dot b_{a}=n_{b_{a}}\tag{2} \]

因此，我們可以總結噪聲模型特性如下

\[E(n_{a})=E(n_{\omega})=E(n_{b_{a}})=E(n_{b_{\omega}})=0 \\E(n_{\omega}(t+\tau)*n^T_{\omega}(t))=\sigma^2_{\omega}\delta(\tau) \\E(n_{b_{\omega}}(t+\tau)*n^T_{b_{\omega}}(t))=\sigma^2_{b_{\omega}}\delta(\tau) \\E(n_{a}(t+\tau)*n^T_{a}(t))=\sigma^2_{a}\delta(\tau) \\E(n_{b_{a}}(t+\tau)*n^T_{b_{a}}(t))=\sigma^2_{b_{a}}\delta(\tau) \tag{3} \]

定義\([\delta(x)]=\frac{1}{[x]}\)(式中\([x]\)表示\(x\)的單位)，同時高斯白噪聲的單位和傳感器測量值的單位應當是一致的，於是在連續時間下，我們可以得到各個噪聲的單位

\[\\ [\sigma_\omega^2]=\frac{\sigma_\omega^2\delta(\tau)}{[\delta(\tau)]}=\frac{rad^2}{sec} \\ [\sigma_{b_\omega}^2]=\frac{\sigma_{b_\omega}^2\delta(\tau)}{[\delta(\tau)]}=\frac{rad^2}{sec^3} \\ [\sigma_a^2]=\frac{\sigma_a^2\delta(\tau)}{[\delta(\tau)]}=\frac{m^2}{sec^3} \\ [\sigma_{b_a}^2]=\frac{\sigma_{b_a}^2\delta(\tau)}{[\delta(\tau)]}=\frac{m^2}{sec^5}\tag{4} \]

這些值應當由廠商提供，對於離散時間系統來說，因為需要在濾波器中使用這些值，我們需要根據它們的單位分別進行轉換

\[d\sigma^2_\omega=\frac{\sigma^2_\omega}{\Delta t}, \space d\sigma^2_{b_{\omega}}=\sigma^2_{b_{\omega}}*\Delta t \\ d\sigma^2_a=\frac{\sigma^2_a}{\Delta t}, \space d\sigma^2_{b_{a}}=\sigma^2_{b_{a}}*\Delta t\tag{5} \]

使用廠商提供的參數並結合上式進行轉換，我們就能得到一個很好的離散濾波器噪聲模型。

狀態的表示

濾波器的狀態有

• IMU在世界坐標系W下的位置\(p^i_w\),速度\(v^i_w\)和姿態\(q^i_w\)
• 陀螺儀零偏\(b_\omega\)和加速度計零偏\(b_a\)以及尺度\(\lambda\)
• 內部自標定參數：IMU框架到相機坐標系的旋轉參數\(q_i^c\) 以及相機中心在IMU框架的坐標\(p_i^c\)
• 視覺框架V與世界坐標系轉換參數的漂移表現為從v系到w系的旋轉\(q_v^w\)和平移\(p_v^w\)的變化

以上符號當表示一般的量時上標表示某一框架，下標表示該量的參考框架，比如\(p^i_w\)表示i系在W系下的位置；而當表示轉換關系時，下標為源坐標系，上標為目標坐標系，比如\(q^i_w\) 表示W系轉換到IMU系的旋轉參數。

此外需要說明的是，某一框架在某一參考框架下的姿態，是指參考框架的坐標軸（基）按照該姿態進行旋轉后可得到該框架的坐標軸，而相反的該框架下的向量經過該姿態的旋轉，得到參考框架下的向量。以\(q^i_w\) 為例，其表示W系下向量轉換到IMU系下向量的旋轉參數，同時也表示i系在W系下的姿態，即\(X_w=R_i^wX_i+p_w^i\)，式中\(R_i^w\)為\(q^w_i\)對應的旋轉矩陣。

狀態變量中並沒有包括重力向量是因為（初始時）世界坐標系已經和水平面對齊。，最終我們得到包含31個元素的狀態變量

\[X=\{p_w^{i^T},v_w^{i^T},q_w^{i^T},b_\omega^{T},b_a^{T},\lambda,p_i^c,q_i^c,p_v^w,q_v^w\}\tag{6} \]

相應的狀態微分方程為

\[\begin{array} \dot p^i_w=v^i_w \\ \dot v^i_w=C^T_{q_w^i}(a_m-b_a-n_a)-g \\ \dot q_w^i = \frac{1}{2}\Omega(\omega_m-b_\omega-n_\omega)q_w^i \end{array}\tag{7} \]

\[\dot b_\omega=n_{b_\omega} \quad \dot b_a=n_{b_a}\quad \dot \lambda=0 \\ \dot p_i^c=0 \quad \dot q_i^c=0 \quad \dot p^w_v =0 \quad \dot q^w_v=0\tag{8} \]

上式中\(C_{(q)}\)為四元數\(q\)對應的旋轉矩陣，\(g\)為世界坐標系下的重力向量，\(\Omega(\omega)\)為\(\omega\)的四元數更新矩陣（是相應的斜對稱矩陣skew？）。我們假設尺度和視覺框架V的漂移是空間上的，並且隨時間變化(We assume the scale and visual frame V to drift spatially and not temporally???)。因此我們可以使用靜止模型(zero motion model)。有一些狀態需要額外的注意因為它們的確定性需要根據具體的空間事件動態調整。比如，當使用基於關鍵幀的VO算法計算相機的姿態時，尺度\(\lambda\)和視覺框架漂移參數\(p^w_v,q_v^w\)的方差只能在關鍵幀創建時改正並且和算法具體的性能有關。考慮以上因素以及前文提到的濾波器狀態估計噪聲模型，我們得到

\[\begin{array} \\ \hat{\dot {p} }^i_w=\hat v^i_w \\ \hat{\dot v}^i_w=C^T_{\hat q_w^i}(a_m-\hat b_a)-g \\ \hat{\dot q} _w^i = \frac{1}{2}\Omega(\omega_m-\hat b_\omega)\hat q_w^i \end{array}\tag{9} \]

\[\hat {\dot b}_\omega=0 \quad \hat {\dot b}_a=0 \quad \hat{ \dot \lambda}=0 \\ \hat{\dot p}_i^c=0 \quad \hat{\dot q}_i^c=0 \quad \hat{\dot p}^w_v =0 \quad \hat{\dot q}^w_v=0\tag{10} \]

誤差狀態表示

在上述的系統狀態表示中，我們使用四元數來描述姿態。表示姿態誤差和相應的方差時，一般使用四元數的誤差形式，而不直接進行算數上的作差。這能夠提高系統的數值穩定性並且用最少的狀態來描述姿態。因此，我們可以得到28元素的誤差狀態向量\(\tilde x\)

\[\tilde x =\{\Delta p_w^{i^T}\space \Delta v_w^{i^T} \space \delta \theta_w^{i^T} \space \Delta b_\omega^{T} \space \Delta b_a^{T} \space \Delta \lambda \space \Delta p_i^{c^T} \space \delta \theta_i^{c^T} \space \Delta p_v^{w^T} \space \Delta \theta_v^{w^T}\}\tag{11} \]

以上除了四元數之外的誤差變量均滿足\(\tilde x=x-\hat x\)，而四元數的誤差變量則滿足

\[\delta q^i_w=q^i_w \otimes \hat q^{i^{-1}}_w \approx[\frac{1}{2} \delta \theta_w^{i^T}\space1]^T \\ \delta q^c_i=q^c_i \otimes \hat q^{c^{-1}}_i \approx[\frac{1}{2} \delta \theta_i^{c^T}\space1]^T \\ \delta q^v_w=q^w_v \otimes \hat q^{w^{-1}}_v \approx[\frac{1}{2} \delta \theta_v^{w^T}\space1]^T\tag{12} \]

連續時間下的誤差狀態微分方程為

\[\begin{array} \\ \Delta \dot p^i_w=\Delta v^i_w \\ \Delta \dot v^i_w = -C^T_{\hat q^i_w}[\hat a_\times]\delta \theta-C^T_{\hat q^i_w}\Delta b_a-C^T_{\hat q^i_w}n_a \\ \delta \dot \theta^i_w = -[\hat \omega_\times]\delta \theta-\Delta b_\omega-n_\omega \\ \end{array} \\ \Delta \dot b_\omega=n_{b_\omega} \quad \Delta \dot b_a=n_{b_a} \quad \Delta \dot \lambda=0 \\ \Delta \dot p^c_i = 0 \quad \delta \dot q^c_i=0 \quad \Delta \dot p^w_v=0 \quad \delta \dot q^w_v=0\tag{13} \]

上式中\(\hat \omega=\omega_m-\hat b_\omega\)，\(\hat a = a_m-\hat b_a\)，於是可以得到連續時間下的線性誤差狀態方程。

\[\dot {\tilde x}=F_c \tilde x+G_cn\tag{14} \]

上式中\(n\)為噪聲向量即\(n=[n_a^T,n^T_{b_a},n^T_\omega,n^T_{b_\omega}]\)。為了加快計算速度，認為\(F_c\)和\(G_c\)在積分周期中均為常值，對上述狀態方程進行離散化后可得

\[F_d=\exp(F_c\Delta t)=\mathbf{I_d}+F_c \Delta t+ \frac{1}{2!}F^2_c\Delta t^2\tag{15} \]

經過分析和計算可以得到\(F_d\)的結構如下

\[F_d= \begin{bmatrix} \mathbf{I_{d_3}}&\Delta t&A&B&-C^T_{\hat q^i_w}\frac{\Delta t^2}{2}& \mathbf{0_{3\times13}}\\ \mathbf{0_{3}}&\mathbf{I_{d_3}}&C&D&-C^T_{\hat q^i_w}\Delta t& \mathbf{0_{3\times13}} \\ \mathbf{0_{3}}&\mathbf{0_{3}} &E&F& \mathbf{0_{3}} & \mathbf{0_{3\times13}} \\ \mathbf{0_{3}}& \mathbf{0_{3}}& \mathbf{0_{3}}&\mathbf{I_{d_3}}& \mathbf{0_{3}}& \mathbf{0_{3\times13}} \\ \mathbf{0_{3\times13}}&\mathbf{0_{3\times13}}&\mathbf{0_{3\times13}}&\mathbf{0_{3\times13}}&\mathbf{0_{3\times13}}&\mathbf{I_{d_3}} \end{bmatrix}\tag{16} \]

使用小角度近似即\(|\omega|\to0\)，並使用洛必達法則可以得到\(A,B,C,D,E,F\)的解^[3]

\[\begin{array} \\ A=-C^T_{\hat q^i_w}[\hat a_\times](\frac{\Delta t^2}{2} - \frac{\Delta t^3}{3!}[\omega_\times]+ \frac{\Delta t^4}{4!}[\omega_\times]^2 ) \\ B=-C^T_{\hat q^i_w}[\hat a_\times](\frac{-\Delta t^3}{3!} + \frac{\Delta t^4}{4!}[\omega_\times]-\frac{\Delta t^5}{5!}[\omega_\times]^2 ) \\ C=-C^T_{\hat q^i_w}[\hat a_\times](\Delta t - \frac{\Delta t^2}{2!}[\omega_\times]+\frac{\Delta t^3}{3!}[\omega_\times]^2 ) \\ D=-A \\ E = \mathbf{I_d}-\Delta t[\omega_\times]+\frac{\Delta t^2}{2!}[\omega_\times]^2 \\ F = -\Delta t+ \frac{\Delta t^2}{2!}[\omega_\times]-\frac{\Delta t^3}{3!}[\omega_\times]^2 \end{array}\tag{17} \]

同時可以得到\(G_c\)為

\[G_c= \begin{bmatrix} \mathbf{0_3}&\mathbf{0_3}&\mathbf{0_3}&\mathbf{0_3} \\ -C^T_{\hat q^i_w}&\mathbf{0_3}&\mathbf{0_3}&\mathbf{0_3} \\ \mathbf{0_3}&\mathbf{0_3}&\mathbf{-I_{d_3}}&\mathbf{0_3} \\ \mathbf{0_3}&\mathbf{0_3}&\mathbf{0_3}&\mathbf{I_{d_3}} \\ \mathbf{0_3}&\mathbf{I_{d_3}} &\mathbf{0_3}&\mathbf{0_3}\\ \mathbf{0_{13\times3}}&\mathbf{0_{13\times3}}&\mathbf{0_{13\times3}}&\mathbf{0_{13\times3}}\\ \end{bmatrix}\tag{18} \]

於是通過積分就可以得到矩陣\(Q_d\)為

\[Q_d=\int_{\Delta t}{F_d(\tau)G_cQ_cG^T_cF_d(\tau)^Td\tau}\tag{19} \]

上式中\(Q_c\)為連續時間系統下的噪聲協方差矩陣\(Q_c=diag(\sigma^2_{n_a},\sigma^2_{n_{b_a}},\sigma^2_{n_\omega},\sigma^2_{n_{b_\omega}})\)。

得到離散時間系統下誤差狀態方程和誤差協方差陣后就可以按照下面的流程來對誤差狀態進行估計更新

1. 根據式(9)(10)進行IMU狀態更新，對四元數使用一階積分進行計算^[3]

2. 計算\(F_d\)和\(Q_d\)

3. 使用濾波迭代方程計算IMU狀態相應的協方差矩陣

4. \[P_{k+1|k}=F_dP_{k|k}F^T_d+Q_d \]

觀測校正

前文提到IMU的加速度和角速度用於狀態的預測更新，而接下來要介紹的位姿測量（相機）則用於狀態的校正更新。然而位姿測量值在空間上存在漂移。為了更好的理解校正過程，我們先忽略位置的漂移，並且假定僅存在姿態的漂移。即認為\(p^w_v\)為0並且在濾波器的方程中可以忽略。

協方差矩陣

在濾波器的校正過程中，我們首先對觀測狀態的測量噪聲的協方差矩陣進行估計。例如，EKF-SLAM中相機位姿的協方差矩陣就是實時估計的，為了計算姿態的協方差可以使用五點法或者其他類似的方法^[4]。而利用關鍵幀進行非線性優化得到的位姿將更加復雜：我們建議使用[4]中提供的利用初始兩個關鍵幀的估計方法。此外，我們還建議使用[5]中給出的定義N個相鄰的關鍵幀進行優化，剩下的關鍵幀固定的方法。我們假設真實位姿的不確定程度與N個關鍵幀經過加權平均的不確定度相同。注意N是根據快速計算需求而選取的參數。但當使用的VO算法完全閉源且不輸出精度信息那么就比較麻煩了。

我們將VO計算得到的位置\(p\)和旋轉\(q\)當作濾波器的觀測量，他們的噪聲為\(n_p\)\(n_q\)，於是可以得到具有6個元素的測量噪聲向量

\[n_m=[n_p \space n_q]^T \]

根據以上噪聲向量可以得到觀測量的協方差矩陣\(R\)。

觀測模型

我們將VO解算得到的位置\(p_v^c\)和旋轉\(q_v^c\)當作系統的觀測量，於是對於位置，有一下觀測模型

\[z_p=p_v^c=C_{q^w_v}^T(p^i_w+C^T_{q^i_w}p^c_i)\lambda+n_p \]

上式中\(C_{q^i_w}\)為IMU在世界坐標系下的姿態，而\(C_{q^w_v}\)則為從v系到W系的旋轉矩陣。

同時，我們定義姿態誤差為

\[\begin{align*} \tilde z_p&=z_p-\hat z_p \\ &=C^T_{q^w_v}(p^i_w+C^T_{q_w^i}p^c_i)\lambda+n_p -\\ &\quad C^T_{\hat q^w_v}(\hat p^i_w+C^T_{\hat q^i_w}\hat p^c_i)\hat \lambda \end{align*} \]

將上式寫為線性形式

\[\tilde z_{p_l}=H_p\tilde x \]

式中\(H_p\)為

\[H_p= \begin{bmatrix} C^T_{\hat q^w_v}\hat \lambda \\ -C^T_{\hat q_v^w}C^T_{\hat q^i_w}[\hat p^c_{i \times}]\hat \lambda \\ \mathbf{0_3} \\ \mathbf{0_3} \\ C^T_{\hat q^w_v}C^T_{\hat q^i_w}\hat p^c_i+C^T_{\hat q^w_v}\hat p \\ C^T_{\hat q^w_v}C^T_{\hat q^i_w}\hat \lambda \\ \mathbf{0_3} \\ -C^T_{\hat q ^w_v }[(p_w^i+C^T_{\hat q^i_w}p^c_i)\lambda _\times] \end{bmatrix}^T \]

上式中\(\hat p = \hat p ^i_w\)，又跟據四元數的誤差定義得到

\[q^i_w=\delta q^i_w \otimes \hat q^i_w \\ C_{q^i_w}=C_{q^i_{\hat i}}C_{q^{\hat i}_w} \\ C_{q^i_{\hat i}} \approx \mathbf{I_d}-[\delta \theta^i_{w \times}] \]

對於姿態測量，我們使用四元數的形式來表示姿態誤差。首先VO提供的姿態為從視覺框架轉換到相機坐標系的旋轉參數\(q_v^c\)，可寫為

\[z_q=q^c_v=q^c_i \otimes q^i_w \otimes q^w_v \]

於是可以得到相應的誤差方程（通過四元數的“除法”）

\[\begin{align*} \tilde z_q&=z_q-\hat z_q \\ & =q^c_i \otimes q^i_w \otimes q^w_v \otimes(q^c_i \otimes \hat q^i_w \otimes \hat q^w_v)^{-1} \\ & =H^{wi}_q\delta q^i_w = H^{ic}_q \delta q^c_i = H^{vw}_q \delta q^w_v \end{align*} \]

式中\(H^{wi}_q\)，\(H_q^{ic}\)和\(H^{vw}_q\)極為相應的線性轉換矩陣，最終可以得到觀測方程為

\[\begin{bmatrix} \tilde z_p \\ \tilde z_q \end{bmatrix}=\begin{bmatrix}& & H_p \\ \mathbf{0_{3\times6}} & \tilde H^{wi}_q & \mathbf{0_{3\times10}} & \tilde H^{ic}_q & \tilde H^{vw}_q \end{bmatrix} \tilde x \]

using the appximation....(這句話想說啥？)

測量校正更新

一旦我們計算的到測量矩陣\(\mathbf{H}\)后，我們就可以根據kalman濾波的流程對之前的估計狀態進行校正更新

1. 計算殘差\(\tilde z=z-\hat z\)
2. 計算新息矩陣\(S=HPH^T+R\)
3. 計算kalman濾波增益\(K=PH^TS^{-1}\)
4. 計算誤差的估計值\(\hat{\tilde x}=K \tilde z\)

利用誤差的估計值對狀態進行改正即可得到校正后的狀態值，而后對協方差矩陣進行更新

\[P_{k+1|k+1}=(\mathbf{I_d}-KH)P_{k+1|k}(\mathbf{I_d}-KH)^T+KRK^T \]

VO位姿估計故障檢測

前文提到我們認為短時間內世界坐標系和視覺框架的旋轉\(q_v^w\)不變，因此在每個濾波估計更新的時刻我們可以得到該旋轉為

\[q_v^w(k)=\hat q^{i^{-1}}_w(k) \otimes \hat q^{c^{-1}}_i (k)\otimes q^c_v(k) \]

因為姿態的漂移相對於濾波估計和校正的周期來說非常慢，因此我們可以對\(q^w_v(k)\)作平滑。我們建議使用一個中值濾波器對\(q^w_v(k)\)序列進行處理，並且識別出其中的粗差。於是，使用一個窗口大小為N的中值濾波器對世界坐標系和視覺框架之間的轉換參數進行濾波得到

\[\hat q^w_v(k)=med(q^w_v(i))\quad i=k-N\to k \]

因為漂移非常慢，所以我們可以識別出突變的異常值。識別到異常值后，不對偏差和尺度更新，但對IMU的位置、速度、姿態進行更新防止IMU的狀態發散。此外，相對與IMU的加速度計，其內部的陀螺儀測量比較可靠，對陀螺儀的角速度測量進行積分在長時間內可以得到較好的姿態。因此我們相應的減小VO位置測量的噪聲，增大VO姿態測量的噪聲。

[1]L. Kneip, A. Martinelli, S. Weiss, D. Scaramuzza, and R. Siegwart, Y. Closed-Form Solution for Absolute Scale Velocity Determination Combining Inertial Measurements and a Single Feature Correspondence. In International Conference on Robotics and Automation, Shanghai, China, May 2011. URL http://hal.inria.fr/hal-00641772/en/ .

[2]A. Martinelli, C. Troiani, and A. Renzaglia. Vision-aided inertial navigation: Closed-form determination of absolute scale, speed and attitude. In International Conference on Intelligent Robots and Systems, San Francisco, États-Unis, 2011. URL http://hal.inria.fr/hal-00641050 .

[3]N. Trawny and S. Roumeliotis. Indirect kalman filter for 3d attitude estimation. Technical report, University of Minnesota, Department of Computing Science and Engineering, 2005.

[4]C. Beder and R. Steffen. Determining an initial image pair for fixing the scale of a 3d reconstruction from an image sequence. Number 4174 in LNCS. Springer, 2006.