imu_tk代碼地址
https://bitbucket.org/alberto_pretto/imu_tk
II. S ENSOR E RROR M ODEL
對於理想的IMU，加速度計三元組的3個軸和陀螺儀三元組的3個軸定義單個共享的正交3D幀。 每個加速度計檢測沿一個不同軸的加速度，而每個陀螺儀測量圍繞同一軸的角速度。 不幸的是，在實際IMU中，由於組裝不准確，兩個三元組形成兩個不同的（即，未對准的）非正交的幀。 此外，單個傳感器並不完美：通常，用於以實際物理量轉換傳感器的數字輸出的比例因子對於相同傳感器的不同實例是不同的，而制造商僅提供默認的標稱比例因子。 此外，輸出信號幾乎總是受非零可變偏差的影響。
如上所述，加速度計幀（AF）和陀螺儀幀（GF）通常都是非正交的。 我們可以通過以下方式定義兩個相關的正交理想幀（分別為AOF和GOF）：
•AOF的x軸和AF的x軸重合
•AOF的y軸位於AF的x軸和y軸所跨越的平面中。
對於陀螺儀情況，分別用GF和GOF替換AF和AOF縮寫詞就足夠了。 最后，我們定義一個主體框架（BF），它是一個正交框架，表示例如IMU底盤的坐標系。 車身框架通常與AF和GF框架的角度不同，但通常它們之間沒有直接關系。 對於小角度，非正交幀（AF或GF）中的測量s S可以在正交體幀中變換為（對於推導的細節，參見[18]）：
其中s B和s S分別表示車身坐標和加速度計（或陀螺儀）坐標中的特定力（加速度）或等效的旋轉速度。 這里βij是圍繞第j個BF軸的第i個加速度計或陀螺儀軸的旋轉，參見圖2。
另一方面，兩個正交幀BF和AOF（以及等效地，BF和GOF）通過純旋轉相關。
在所提出的校准方法中，我們假設車身框架BF與加速度計正交框架AOF一致：在這種情況下，角度βxz，βxy，βyx變為零，因此在加速度計情況下，等式1變為：
我們改變字母β，參考一般情況，字母α，指加速度計情況，而O和S分別表示AOF和AF中的特定加速度3。
如前所述，陀螺儀和加速度計測量應參考相同的參考系，在我們的例子中是AOF。 然后，使用Eq 1，對於陀螺儀，我們有：
其中ωO和ωS分別表示AOF和GF中的特定角速度。
加速度計和陀螺儀都受到偏差和尺度誤差的影響。 引入了兩個縮放矩陣
我們還介紹了兩個偏向量
完整的傳感器誤差模型是
對於加速度計
對於陀螺儀，其中νg和νg分別是加速度計測量噪聲和陀螺儀測量噪聲。
III. B ASIC C ALIBRATION F RAMEWORK
為了校准加速度計三元組，我們需要估計以下未知參數向量：
我們定義以下函數：
在這里，我們可以忽略測量噪聲，因為在我們的校准過程中，我們在每個靜態間隔中應用信號平均。
與傳統的多位置方案一樣，我們將IMU移動到一組M個不同的，暫時穩定的旋轉中。 我們可以提取M個加速度矢量Sk（在非正交AF中測量），在每個靜態間隔內的時間窗口中對加速度計讀數求平均。 我們用來估算加速度計參數的成本函數是：
其中|| g || 是可以從特定公共表格中容易地恢復的局部重力矢量的實際大小（例如，知道我們正在執行校准的位置的緯度，經度和高度）。 為了最小化Eq 10，我們采用Levenberg-Marquardt（LM）算法。
為了校准陀螺儀三元組，我們可以假設系統無偏置，在適當的無運動初始階段簡單地平均靜態陀螺儀信號。 以下關於艾倫方差的討論證明了這一點（參見第IV-C節）。 此外，由於我們需要使用加速度計作為已知參考，我們使用上面計算的校准參數θacc，用等式9校正加速度計讀數。
我們定義運算符Ψ，它將n個陀螺儀讀數的序列ωSi和由校准的加速度計給出的初始重力值ua，k-1（即表示重力方向的單位矢量）作為輸入，並返回最終的重力 或者ug，k，使用第k-1和第k個靜態區間之間的陀螺儀測量值計算：
Ψ可以是通過積分輸入角速度來計算最終方向的任何積分算法。我們需要估計以校准陀螺儀的未知參數矢量是：
在這種情況下，我們可以將成本函數定義為：
其中M是靜態區間的數量，ua，k是在時間窗口中對第k個靜態區間中的校准加速度計讀數進行平均測量的加速度，並且ug，k是使用等式11計算的加速度對數（即，積分 第k-1和第k個靜態區間之間的角速度）。 我們獲得θ陀螺，用LM最小化Eq 13。
IV. C ALIBRATION P ROCEDURE
如第二節所述。 I，所提出的校准框架需要收集具有原始加速度計和陀螺儀讀數的流的數據集，當操作者在不同的靜態位置移動IMU時獲取該數據集，以便產生一組不同的，暫時穩定的旋轉。 我們的校准協議的簡單圖表如圖1所示。 III，為了減輕Eq 10和Eq 13最小化時的噪聲影響，我們需要在適當的時間間隔內對信號求平均值。 這對靜態間隔的長度施加了下限（圖1中的t等待）。沒有運動的初始化周期（圖1中的T init）也是必不可少的：這將被用來表征陀螺儀的偏差（ 第IV-C節）和靜態探測器操作員（第IV-A節）。
A. Static Detector
校准的准確性很大程度上取決於靜態和運動間隔之間分類的可靠性：校准加速度計我們使用靜態間隔，而對於陀螺儀校准，我們還包括兩個連續靜態間隔之間的運動間隔。根據我們的經驗， 傳遞基於濾波器的算子，如[8]中使用的准靜態檢測器，對真實數據集表現不佳：檢測到的靜態間隔通常包括一小部分運動。 而且，它們需要微調，因為它們取決於三個參數。
我們建議使用基於方差的靜態檢測器算子，它利用上面介紹的靜態區間長度的下限。 我們將探測器基於加速度計信號：給定時間間隔長度為t秒（參見圖1），對於每個加速度計樣本（tx，a ty，tz），在時間t，我們計算方差幅度，即 方差的大小如下：
其中var t w（a t）是在以t為中心的長度為t w秒的時間間隔內計算一般信號a t的方差的算子。 我們在靜態和運動間隔之間進行分類，只需檢查ζ（t）的平方是否低於或大於閾值。
作為閾值，我們考慮在所有初始化時段T init上計算的方差幅度ζinit的平方的整數倍。 在所有實驗中，我們使用t w = 2秒，而使用Allan方差估計T init（參見第IV-C節）。 值得注意的是，我們的靜態探測器不需要任何參數調整：分類中使用的整數乘數由我們的校准算法自動估算（參見第IV-D節）。 圖3報告了我們的靜態濾波器如何處理實際數據的示例：在這種情況下，估計的整數乘數為6。
B. Runge-Kutta Integration
D. Complete Procedure
為了避免校准參數估計中的不可觀察性，必須收集至少九種不同的高度[15]（例如，圖1）。 根據我們的經驗，需要更多N個不同的高度來獲得更好的校准結果，同時保持減少每個靜態間隔的持續時間，以保持陀螺儀偏差的時間穩定性的假設。當36≤N≤50且1秒≤t等於≤4秒時，我們在校准精度，偏差穩定性和降噪之間取得了良好的平衡。
初始化時段T init的持續時間由Allan方差分析給出（參見第IV-C節）。 校准協議總結在圖1中，而在算法1中，報告校准算法的偽代碼。