分享基於高斯牛頓法的加速度計校正方法與驗證
本帖最后由 john800422 于 2016-2-10 13:13 编辑分享最近自己做的 IMU 校正，並配合 MATLAB 來做驗證，有錯歡迎校正：）
http://kitsprout.logdown.com/posts/464365
主要使用了高斯牛頓法來做感測器的校正，這種校正方法頗實用，算是是蠻常見的一種方法，精度也高，
不過缺點是需要計算反矩陣，運算量偏大，實際使用 ST 的芯片 STM32F411C CortexM4 100MHz 來實現大概花 1s 左右的時間，但在實際校正應用上影響應該不大。
這裡的感測器校正不探討溫度對於感測器在誤差方面的影響，因為沒有特別用溫度來校正過 IMU 相關感測器，不確定在溫度產生的誤差方面對於校正是否有顯著或是影響大的結果，之後如果有空來測量不同的工作時間長度下，芯片工作溫度的變化後，或許可以評估一下是否有這些必要，但目前就先假設感測器的溫度固定，不會因為外在或內部的溫度而影響噪音的大小。
## 陀螺儀（Gyroscope or Gyro）校正
對陀螺儀校正的部分主要校正偏差（Bias）的誤差，不對安裝誤差做校正，因為對於安裝物差的校正需要一些＂絕對＂的測量數據，這些數據通常都需要外部的儀器來輔助校正與測量，舉一個簡單的例子，今天我想要找出下列方程式中的 R_gyro 矩陣，找出這一個矩陣的一個方法就是在不同的角速度下測量，外部的裝置可以產生一個固定的角速度給予感測器，或是透過另一個可靠的感測器來做提供實際的結果（可能有些人會想為什麼不直接用感測器的輸出？因為感測器的輸出是包含誤差的，所以才需要被校正，如果感測器沒有誤差當然沒有問題，可以使用，不過就不需要做校正了...），而感測器將測量到的結果與實際的結果作比較，就可以得出 R_gyro 矩陣，那不用外部的儀器可做嗎？可以，如果有人可以產生一個固定或是準確的角速度，就可以透過這個角速度來校正陀螺儀，不過個人是沒辦法實現這高難度的動作，只能先假設 R_gyro 矩陣是一個單位矩陣或是一個對角矩陣（Diagonal Matrix）。
在假設 R_gyro 是一個對角矩陣後，問題就簡化了許多，在 n_noise 為高斯噪音（Gaussian Noise）下，平均值為 0，靜止下對陀螺儀取樣 w_ideal = 0，所以這時測量到的就是 w_bias，只要在每次測量的結果減掉 w_bias，並乘上一個放大縮小的變數，就可以得出理想的陀螺儀資料了，說的簡單一點就是在靜止下取平均值就可以得出 w_bias。
## 加速度計（Accelerometer or G-Sensor）校正
首先先放上感測器的模型
對加速度計校正的部分除了 B_bias 矩陣的偏差外，還會找出 S 矩陣來校正比例誤差與焊接、安裝產生的未對準誤差，而三軸間的正交誤差不打算特別做校正，因為使用的感測器是三軸集成於一顆芯片的，所以直接假設三軸互相正交，則 S 的反矩陣（K 矩陣）會是一個對證矩陣，並且表示成無偏差的形式，為的是減少展開的複雜度，所以問題可以化簡成下面式子
把右式展開則可以表示如下，其中 k 表示第 k 次的加速度計資料
再來就是透過在沒有運動加速度的條件下，加速度計三軸輸出平方和會等於重力加速度的關係來做校正，此種方法不需要依預先設定好的角度與位置來校正，校正的自由度較大
將上式表示成誤差函數，並展開與整理，結果如下
定義 E 函數為誤差的平方和，若可以找出 E 函數的極值（最小值）時，就可以得出 K 矩陣與 B 矩陣了
因為 E 函數可以表示成 e'*e 的形式，所以這裡解極值的方法我們採用高斯牛頓法（Gauss-Newton Method），詳細的演算法說明請自行參考 (https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm) ，迭代過程如下式，對誤差的 Jocabian 與 Hessian 反矩陣相乘，其中 Hessian 矩陣中省略掉了二次微分項，自己實際用 MATLAB 模擬過，有無省略結果其實差不多，有時好、有時差，但在運算量上差別還頗大的，所以這裡把二次為紛向給忽略掉。
而 X 就是我們想要求得的 K 矩陣與 B 矩陣，透過不斷的迭代而得出來，如果要校正正交誤差就在加入 3 項 k 矩陣的沒有對稱的變數即可，但實際效果如何我倒是沒有試過，有興趣可以自己試看看
## **MATLAB 的模擬結果**
詳細的微分結果與程式都放在 Github 上
C 語言驗證版本 https://github.com/Hom-Wang/C-Language/tree/master/newtonMethod
MATLAB 驗證版本 https://github.com/Hom-Wang/MATLAB/tree/master/gauss-Newton_Methods
↑ 修正量，也就是 Jocabian 與 Hessian 反矩陣的相乘，最後卻趨近 0，表示收斂。
↑ 校正參數，在迭代的過程中不斷的更新，最後卻趨於穩定的一個值。
↑ 誤差平方和。
↑ 最後整裡的一張實際數值與迭代結果得比較表格，誤差大概都在 0.01% 以下。
## 磁力計（Magnetometer or Compass）校正
磁力計的校正之前是使用橢圓擬合，詳細可以參考(https://github.com/Hom-Wang/Quadcopter/blob/master/%E6%8E%A8%E5%B0%8E_%E6%A9%A2%E5%9C%93%E6%93%AC%E5%90%88.pdf)，不過橢圓擬合僅適用於平面的校正，離開該平面，像是傾斜等情況，容易產生誤差，但理論上電子羅盤也可以用上述的高斯牛頓法來求得校正參數，把 g 改成 1 即可，實現歸一化的作用，因為最後在求航向角時，僅需要知道比例關係，不需要知道實際的磁場強度。
***更正
此方法主要以重力來校正，若待校正裝置與重力有特定夾角的話，此方法無法將裝置校正至水平，
若要校正上述問題可以用 Least Squares 的方法。 顶一个，虽然看不懂多少{:smile:} 前排支持！ 来顶一个，准备好好学习一下 楼主威武，现在毕业了吗？ 支持一个 后面试试~ 过来学习 这个方法无法矫正安装误差，所以局限性还是挺大的。 最近正好也在看这方面的内容，陀螺仪的漂移量不是一个定值，所以LZ对于陀螺仪校正里面直接把漂移量减去的做法不太赞同。
参考：
出处：
看来是干货 黄脸猫，又见到你了，这帖子不错，实际操作起来要校正平台，这个平台有点贵 为楼主赞一个！ 本帖最后由 john800422 于 2016-2-10 13:09 编辑
zhongjiequan 发表于 2016-2-10 00:06
这个方法无法矫正安装误差，所以局限性还是挺大的。
剛想了一下，此方法主要以重力來校正，若裝置與重力有固定夾角的話，此方法確實無法校正安裝誤差，感謝提醒：）
因為之前用的是不同角度的重力值，所以可以校正安裝誤差，但缺點是需要外部的儀器精度才容易比較高，
不過現在換成三軸輸出平方和為重力平方後，就無法靜正安裝誤差，但不需要外部儀器可以有一定的校正精度了，
笑笑我笑了 发表于 2016-2-10 01:39
最近正好也在看这方面的内容，陀螺仪的漂移量不是一个定值，所以LZ对于陀螺仪校正里面直接把漂移量减去的做 ...
這邊假設為定值其實對積分的誤差累積就可以有很大的改善了，
若要校正更精確的話，可以建立更準確的噪音模型來校正，就像資料上說的，加入時間變數，
越準確的模型通常意味著越多的變數，越多的變數意味著使用更多的運算資源，
沒有絕對，看需求、應用而選擇 junzimengyou 发表于 2016-2-10 06:13
黄脸猫，又见到你了，这帖子不错，实际操作起来要校正平台，这个平台有点贵 ...
不需要校正平台，直接透過三軸平方和等於重力平方的條件來校正，
只要有足夠的採樣數據（單純的重力加速度）就可以校正了，這是這方法的好處。 taulabs的加速度计校正算法你可以看看 高斯牛顿取得是泰勒展开的2次近似多项式 计算量还是略大 您好，我看了您的帖子，有一個小問題，為什麼傳感器假定三軸正交就可以導出K矩陣是對稱陣，您能詳細的和我說說嗎？ 按照我的理解，如果按照楼主的假设（即传感器感受面方向正交）那真实的加速度和测量的加速的在不考虑零偏的情况下的转换仅仅是一个表示旋转的矩阵叠加一个对角阵吧 这一堆公司看了都头疼 猫哥，我问您的问题您能否抽时间回答一下呢？感激不尽 这个直接用奇异值分解就计算出来了，没必要高斯牛顿
页:
[1]