[深入淺出多旋翼飛控開發][姿態篇][四][非線性最小二乘與飛控傳感器校準]

作者：王偉韻
QQ : 352707983
Github

前言

搞好了傳感器，那意味著飛控已經完成了一半。

不用猜了，這句話正是鄙人說的:)。飛控的軟硬件相關工作，絕大部分是圍繞傳感器展開的，它們的重要性，可能比你想象中的更大。

硬件上，為了尋找性能優秀又符合產品實際需求的傳感器如陀螺儀、加速度計、磁力計等，我們經常需要對來自不同廠商的樣品進行實際性能測試與評估，即使實際上常用的型號就那么幾個。為了提升飛控的性能，許多傳感器問題我們還需要從硬件上進行解決，如降低電源噪聲、隔離機身震動甚至還要為傳感器提供恒定溫度的工作環境。

軟件上，傳感器校準、濾波與融合更是飛控程序中的重點，作為一名渣渣飛控工程師，渾渾噩噩工作數年，發現到頭來大部分時間都是花在了傳感器數據的處理上面，而且未來需要解決的問題，依然和傳感器有關。

本篇文章將結合實際飛控代碼，以循序漸進的方式向讀者講述，如何使用最優化算法，解決飛控中最基本的傳感器校準問題。

一.傳感器校準簡述

飛控上會使用各種各樣的傳感器，但對于多旋翼而言，最核心的是陀螺儀無疑。離開了陀螺儀，你甚至無法讓一架多旋翼飛行器正常離地。其次則是加速度計，提供姿態觀測，讓飛行器可以得到一個較為客觀的自身姿態觀測信息，使得自穩飛行成為可能（能夠自動保持自身姿態水平或一定角度）。除此之外，還有許許多多的的其它傳感器，共同為飛控提供穩定飛行所需要的數據。

其中，陀螺儀、加速度計和磁力計在飛行前（出廠前），均需要進行校準，才能達到正常工作所需要的性能。這便引出了一個問題：什么是傳感器校準（標定）。

答案很簡單，所有事物都不是非黑即白，而是時刻處于一個不可預知的混沌狀態。工廠按照特定的技術及工藝標準制造生產出一批傳感器，這些傳感器中所有個體的實際狀態處于一個隨機分布且均值為期望誤差的序列上。簡單來說，我們買來10個傳感器，這些傳感器均存在著特定誤差且這10個傳感器的誤差各不一致。這就要求我們在實際使用這些傳感器時，需要對每一個進行單獨的校準，得到特定的校準參數，從而正常發揮傳感器的性能。

二.傳感器誤差類型

接下來我們會面臨下面兩個問題：

傳感器有哪些誤差
如何測量傳感器的誤差

如果從我們目前所使用的MEMS傳感器的自身原理及制造工藝出發，分析傳感器的誤差來源，那可能需要長篇大論一番了，但那是超出作者能力的事情，也不是本篇文章所討論的重點。

根據實際經驗，我們可以把傳感器誤差分為兩大類：

零偏誤差（offset）
刻度誤差（scale）

這兩個是我們最容易理解，也是飛控中三大傳感器均需要解決的誤差類型，其中

零偏誤差： 指傳感器在零激勵下的輸出誤差。如陀螺儀在靜止狀態下，其角速度測量值理論為0，而實際上我們將飛控靜止放置，陀螺儀依然會輸出諸如1°/s之類的數據，這便是該陀螺儀傳感器的零偏誤差。

刻度誤差： 指傳感器在一定激勵輸入下，其輸出值與輸入值的比值。比如我們將飛控放在一個以500°/s恒定角速度運動的轉臺上，而陀螺儀實際測量得到的角速度輸出為495°/s，500 / 495 ≈ 1.01 便是該陀螺儀的刻度誤差。

從上述說明中我們可以看出，要正確測量傳感器的誤差，必須向傳感器施加特定的激勵信號，通過比較傳感器的實際輸出與激勵輸入量，計算得到傳感器的誤差。

不同類型的傳感器，所需要的激勵信號源也不一樣。如陀螺儀，要提供精確的激勵信號，必須依靠專業的轉臺設備，這是個人及小公司均不具備的條件。好在對于大多數應用而言，陀螺儀的刻度誤差在一個可以忍受的范圍內，因此可被我們忽略。但對于精益求精的產品而言，要提升飛控性能到極致，刻度誤差校準還是必須的，如某行業龍頭，其飛控產品及整機產品，出廠前均需使用轉臺進行校準。缺少這些條件的個人愛好者，就只能暫時忽略這一項，只校準陀螺儀的零偏誤差了，而大多數情況下，陀螺儀的零偏校準方式很簡單，使用均值校準即可。因此，陀螺儀的校準不在本篇文章的討論范圍內。

而本文中討論的重點：加速度計與磁力計，其信號激勵源均來自于自然界，前者為地球的重力場，后者為地球的地磁場。

三.傳感器校準的簡單方式

1.加速度計

假設有下面的情形，我們將飛控水平放置于水平面，測得加速度計z軸輸出值 $g_1= 0.97g$ ，然后將飛控翻轉并同樣水平置于水平面，再測得加速度計z軸輸出值 $g_2 = -0.99g$ 。

設z軸零偏誤差為 $scale_z$ ，刻度誤差為 $offset_z$ ，已知實際重力加速度為g，可以得到下列兩條等式：

$(g_1 - offset_z) * scale_z = g$
$(g_2 - offset_z) * scale_z = -g$

從而可以計算得到：

$scale_z ≈ 1.02$
$offset_z = -0.01g$

將這個方法推廣到其余兩軸，便能得到其校準參數 $scale_x, scale_y,offset_x,offset_y$ 。

缺陷：

這個校準方法較為簡單，但是其可靠性建立于一個理想條件下：加速度計在采集每一個方向的數據時，必須嚴格貼合水平面。否則只能得到一個不太準確的校準數值。實際應用中，我們很難保證校準時讓飛控的每一個面保持水平，因為飛行器本身就是一個不規則物體，更多時候我們采集到的加速度數據為 $[0.05g,0.08g,0,95g]$ ，而非 $[0,0,0.98g]$ 。或者說，這種方式的實現成本過高，難以實際應用。

2.磁力計

磁力計的簡單校準方法原理等同于加速度計，即采集得到磁力計的三個軸的測量極值（最大與最小值），然后單獨計算出每個軸的零偏誤差與刻度誤差。實現區別在于由于傳感器激勵信號源不一樣，我們需要將磁力計在三維空間中進行各個方向的360°旋轉，才能盡可能準確采集到極值。

缺陷：

實際應用中，磁力計校準對于飛行器使用者來說可能頗為頻繁，而飛行器大小又各不一致，在很多情況下，我們是很難將飛行器翻來覆去地在各個方向上進行旋轉，其可操作性非常低，且不方便普通用戶的使用。如果旋轉不全面，便無法采集到磁力計的真實極值，校準效果會大打折扣，甚至校準值嚴重偏離真實值。

四.建立傳感器誤差模型

對于程序員而言，解決實際問題的標準姿勢如下：

提出實際問題--->建立數學模型--->轉換為程序--->求解

而上一節中，我們描述了一個較為簡單的用于加速度計與磁力計的校準方法，但是其前提條件較為苛刻，且校準效果不佳。于是，在這里我們提出了一個待解決的問題：找到一種使用便捷并且效果更好的校準算法。使用便捷，意味著約束條件越少越好，即在校準算法對傳感器的采樣點的數量和位置要求不能過于嚴格的情況下依然可以取得良好的校準效果。

為了解決這個問題，得到傳感器的誤差參數，首先我們需要建立傳感器的誤差模型，并將其轉換為數學方程，從而進行求解。

以加速度計為例建立傳感器誤差模型

設三軸加速度計的零偏誤差與刻度誤差分別為
$o_x ,o_y,o_z,s_x,s_y,s_z$
某一時刻該三軸加速度計的測量值分別為
$x,y,z$
設當前時刻真實加速度向量為
$a=(a_x,a_y,a_z)$
有以下關系：
$\begin{cases} a_x=(x-o_x)s_x\\ a_y=(y-o_y)s_y\\ a_z=(z-o_z)s_z\\ \end{cases}$

靜止狀態下，不管加速度計方向如何，其測量得到的加速度為重力加速度在各軸上的投影。而在地球上，重力加速度向量模值總是等于 $1g$ 。假設 $a_x,a_y,a_z$ 單位為 $g$ ，便有以下等式成立：
$a_x^2+a_y^2+a_z^2=1$
即
$(x-o_x)^2s_x^2+(y-o_y)^2s_y^2+(z-o_z)^2s_z^2=1$

至此，我們將加速度計的誤差求解轉化成為了一個數學問題：已知 $x,y,z$ ，由上一條等式，求解出 $o_x ,o_y,o_z,s_x,s_y,s_z$

五.求解誤差方程

上一節中，我們得到了傳感器的誤差方程： $(x-o_x)^2s_x^2+(y-o_y)^2s_y^2+(z-o_z)^2s_z^2=1$
求解方程，對于經歷過九年義務教育的我們來說是一件非常熟悉的事情，但這里存在著幾個棘手的問題：

該方程為六元二次方程，存在著6個未知數，如果只存在 $x,y,z$ 一組觀測值，那將會有無窮多解。因此我們需要獲得多組觀測值 $x_i,y_i,z_i$ ，以構建方程組，從而求出唯一解，滿足方程組里的每一個方程
對于六元二次方程而言，使用六組獨立的觀測值，構建出包含6個方程的方程組，理論上可以求出方程組的唯一解。可是事實上，我們從傳感器上獲取到的觀測值，都是帶有一定隨機噪聲的，即觀測值并非完全準確，從而導致求得的方程解未必精確
使用計算機，我們可以輕松求出線性方程組的唯一精確解（如高斯消元法），而非線性方程問題，無論是從理論上還是計算方法上，都要復雜得多，大部分時候，對于無特定形式的非線性方程 $f(x)=0$ ，是沒有辦法直接求得精確解的。

為了解決上述問題，這里引進了一種數學方法：最小二乘法

最小二乘法（又稱最小平方法）是一種數學優化技術。它通過最小化誤差的平方和尋找數據的最佳函數匹配。利用最小二乘法可以簡便地求得未知的數據，并使得這些求得的數據與實際數據之間誤差的平方和為最小。

對于現實中的絕大部分問題，我們都是無法求得精確解的，于是退而求其次，使用最小二乘法，我們可以得到其近似解。

以傳感器誤差方程為例，設 $r$ 為某特定解下方程的誤差，有
$r=1 - (x-o_x)^2s_x^2+(y-o_y)^2s_y^2+(z-o_z)^2s_z^2$
對于方程組而言，其誤差的平方和函數為
$S=\sum_{i=0}^{n} r_i^2$
所謂最小二乘法，便是求得一組特定解，使得 $S$ 最小(求函數極值)，該條件下的解，便是方程組的最優近似解。

對于線性方程組，可將誤差函數 $S=0$ 轉換為矩陣方程并進行求解即可，可惜現實問題大部分均為非線性問題，如本文中的傳感器誤差方程便是一個非線性方程，是無法變換為矩陣方程形式的，因此我們必須換一個思路來解決問題。

假定該函數存在著局部最小值，那給定一個初始解，通過不斷地迭代計算，從而找到誤差平方和函數 $S$ 的局部最優解，這便是非線性最小二乘的基本思想。

接下來，我們將討論求解非線性最小二乘的具體算法。

六.高斯牛頓法

非線性最小二乘的求解算法有許多種，比較常見的有高斯牛頓法，Levenberg-Marquardt法（LM），DogLeg法等。其中高斯牛頓法是最為經典的一種算法，APM飛控中便是使用了該算法，而LM法可以看成是高斯牛頓法的改良版，魯棒性更好，在實際工程中應用更廣泛，天穹飛控中則使用了LM法。而LM法的基礎部分和高斯牛頓法完全一致，因此我們從高斯牛頓法開始講起。

由于算法原理細節部分較為復雜，需要用到大量公式，為了方便理解，將結合天穹飛控的實際代碼進行講解，高斯牛頓法和LM法的具體實現大部分是一樣的，所以可以結合著來看。對于初學者來說徹底弄懂這一部分的原理對自己能力的提升幫助較大，因為此類最優化算法在飛控之外的非常多工程領域都是有著大量應用的，如計算機視覺和機器學習等。

天穹飛控中的LM法完整代碼

要搞明白高斯牛頓法，我們要先來了解一下牛頓法

1.牛頓法

為求得函數最優解（極值點），牛頓法將問題轉換為了求解 $f^{'} (x)=0$

首先給定一個初始解 $x_0$ ，然后在 $x_0$ 處進行一階泰勒展開，得到
$f^{'} (x) \approx f^{'} (x_0) + (x-x_0)f^{''} (x_0)$
令
$f^{'} (x_0) + (x-x_0)f^{''} (x_0) = 0$
求解得到 $x$ ，相比于 $x_0$ ，有 $f^{'} (x) < f^{'} (x_0)$ ，即新解 $x$ 比初始解更接近最優解。當然該解和最優解之間還有一定未知的距離，于是我們將新解作為初始解，重復這一步驟，經過不斷地迭代計算，最終會收斂于 $f^{'} (x)=0$ 。

牛頓法尋找極值點的動畫示意圖：

牛頓法尋找極值點的動畫示意圖

牛頓法的優點：

收斂速度快，且可用于求解任意連續函數的最優化問題

牛頓法的缺點：

可以看出，牛頓法實際上等同于尋找函數的局部極值點，因此必須選取一個相對接近的初始點，否則會導致無法正確收斂
需要求解目標函數的Hessian矩陣（二階導）的逆矩陣，計算比較復雜

2.高斯牛頓法

高斯牛頓法是一種非線性最小二乘最優化方法。其利用了目標函數的泰勒展開式把非線性函數的最小二乘化問題化為每次迭代的線性函數的最小二乘化問題

高斯牛頓法實際上是牛頓法的簡化形式，或者說改進版。多維情況下，當維數較大時，牛頓法中的Hessian矩陣求逆是往往行不通的，而高斯牛頓法對牛頓法中的Hessian矩陣進行了化簡，使得問題可解。

下面講解高斯牛頓法的具體實現

首先定義目標函數
$S(\beta)= \sum_{i=0}^{5}r _i^2( \beta)$
其中
$\beta=[\beta_0,\beta_1,\beta_2,\beta_3,\beta_4,\beta_5]$
對應了加速度計誤差方程中的六個變量，即
$[o_x ,o_y,o_z,s_x,s_y,s_z]$
而 $r _i^2( \beta)$ 為傳感器誤差方程的誤差平方和函數，即
$r _i^2( \beta)=(1 - (x_i-o_x)^2s_x^2+(y_i-o_y)^2s_y^2+(z_i-o_z)^2s_z^2)^2$

設定一個初解 $\beta_\alpha$ ，將目標函數 $S(\beta)$ 在 $\beta_\alpha$ 附近泰勒展開，并舍棄高階無窮項，近似有：
$S(\beta)= S(\beta_\alpha)+[\bigtriangledown S(\beta_\alpha)]^T(\beta-\beta_\alpha)+\frac{1}{2}(\beta-\beta_\alpha)^TH_S(\beta_\alpha)(\beta-\beta_\alpha)$
我們的目標是求出特定解，使 $S(\beta)$ 有最小值。當函數有最小值（屬于極小值）時，其一階導為0，即
$\bigtriangledown S(\beta)=\bigtriangledown S(\beta_\alpha)+H_S(\beta_\alpha)(\beta-\beta_\alpha)=0$
從而得到了高斯牛頓法的迭代公式：
$\beta=\beta_\alpha - [H_S(\beta_\alpha)]^{-1}\bigtriangledown S(\beta_\alpha)$
設
$\bigtriangleup = [H_S(\beta_\alpha)]^{-1}\bigtriangledown S(\beta_\alpha)$
則有
$\beta=\beta_\alpha - \bigtriangleup$
這里便是迭代的關鍵了，給定初解后，每一次計算出 $\bigtriangleup$ ，便得到了一個新解，然后使用新解不斷進行迭代計算，使其逐漸逼近最優解。

可能看到這里，你依然是一臉懵逼，但沒有關系，你只需要知道，為了求解出函數的最優解，也就是傳感器的最優校準參數，我們只需要不斷去計算 $\bigtriangleup$ 就行了。

接下來便講解如何計算 $\bigtriangleup$ (這里是整個算法實現的重點）

$\bigtriangleup$ 中的 $H_S$ 為 $S$ 函數的Hessian矩陣（實際上是S函數的二階導）
$H_S=\left[ \begin{matrix} \frac{\partial^2 S}{\partial \beta_0^2}&\cdots&\frac{\partial^2 S}{\partial \beta_0 \beta_5}&\\ \vdots&\ddots&\vdots&\\ \frac{\partial^2 S}{\partial \beta_5 \beta_0}&\cdots&\frac{\partial^2 S}{\partial \beta_5^2}&\\ \end{matrix} \right]$
前文提到，高斯牛頓法對牛頓法所做的改進，便是對Hessian矩陣進行了簡化，其中有
$\frac{\partial^2 S}{\partial \beta_i \beta_j}=2\sum_{k}(\frac{\partial r_k}{\partial \beta_i} \frac{\partial r_k}{\partial \beta_j} + r_k\frac{\partial^2 r_k}{\partial \beta_i \beta_j} )$
忽略二階微分項，近似有
$\frac{\partial^2 S}{\partial \beta_i \beta_j}=2\sum_{k}\frac{\partial r_k}{\partial \beta_i} \frac{\partial r_k}{\partial \beta_j}$
于是有
$H_S=2 \left[ \begin{matrix} \frac{\partial r_0}{\partial \beta_0}&\cdots&\frac{\partial r_0}{\partial \beta_5}&\\ \vdots&\ddots&\vdots&\\ \frac{\partial r_5}{\partial \beta_0}&\cdots&\frac{\partial r_5}{\partial \beta_5}&\\ \end{matrix} \right]^T \left[ \begin{matrix} \frac{\partial r_0}{\partial \beta_0}&\cdots&\frac{\partial r_0}{\partial \beta_5}&\\ \vdots&\ddots&\vdots&\\ \frac{\partial r_5}{\partial \beta_0}&\cdots&\frac{\partial r_5}{\partial \beta_5}&\\ \end{matrix} \right] =2J_r^TJ_r$
這里的 $J_r$ 實際上就是函數 $r$ （傳感器誤差方程的誤差函數）的雅可比矩陣

$\bigtriangleup$ 中的另外一個成員 $\bigtriangledown S$ 為 $S$ 函數的一階導的列向量，即
$\bigtriangledown S = \left[ \begin{matrix} \frac{\partial S}{\partial \beta_0}\\ \vdots\\ \frac{\partial S}{\partial \beta_5}\\ \end{matrix} \right]$
又有
$\frac{\partial S}{\partial \beta_i}=\sum_{k}\frac{\partial (r_k^2)}{\partial \beta_i} =2 \sum_{k}\frac{\partial r_k}{\partial \beta_i} r_k$
故 $\bigtriangledown S$ 可改寫為
$\bigtriangledown S =\left[ \begin{matrix} \frac{\partial S}{\partial \beta_0}\\ \vdots\\ \frac{\partial S}{\partial \beta_5}\\ \end{matrix} \right] =2 \left[ \begin{matrix} \frac{\partial r_0}{\partial \beta_0}&\cdots&\frac{\partial r_0}{\partial \beta_5}&\\ \vdots&\ddots&\vdots&\\ \frac{\partial r_5}{\partial \beta_0}&\cdots&\frac{\partial r_5}{\partial \beta_5}&\\ \end{matrix} \right]^T \left[ \begin{matrix} r_0\\ \vdots\\ r_5\\ \end{matrix} \right] =2J_r^Tr$

于是我們終于得到了 $\bigtriangleup$ 的最終形式
$\bigtriangleup = [J_r(\beta_\alpha)^T J_r(\beta_\alpha)]^{-1} J_r(\beta_\alpha)^T r(\beta_\alpha)$
或者換一個形式
$J_r(\beta_\alpha)^T J_r(\beta_\alpha) \bigtriangleup =J_r(\beta_\alpha)^T r(\beta_\alpha)$
這就成了一個線性方程組，未知量是 $\bigtriangleup$ ，使用高斯消元法，便能求解出 $\bigtriangleup$ 的唯一解。

至此，高斯牛頓法的所有計算步驟均結束，我們只需要不斷進行迭代計算：求解出 $\bigtriangleup$ ，從而得到新解 $\beta$ ，然后進行下一次迭代，不斷逼近函數最優解。

隨著迭代次數的增加，最終會得到一個無限接近最優的解，但是考慮到時間成本和實際使用需求，在校準程序中我們設置一個閾值 $eps$ ，當方程解達到一定精度時（定義為迭代步長的平方 $\bigtriangleup ^2 < eps$ ），停止迭代，結束此次校準。

看完了，還是有點懵，怎么辦？

下面貼上高斯牛頓法的完整實現代碼，結合代碼進行理解和分析，事半功倍，如果還沒看懂，那就多看幾遍。

主程序

/**********************************************************************************************************
*函 數 名: GaussNewton
*功能說明: 高斯牛頓法求解傳感器誤差方程，得到校準參數
*形    參: 傳感器采樣數據（6組） 零偏誤差指針 比例誤差指針 數據向量長度
*返 回 值: 無
**********************************************************************************************************/
void GaussNewton(Vector3f_t inputData[6], Vector3f_t* offset, Vector3f_t* scale, float length)
{
    uint32_t cnt    = 0;
    double   eps    = 0.000000001;
    double   change = 100.0;
    float    data[3];  
    float    beta[6];      //方程解
    float    delta[6];     //迭代步長
    float    JtR[6];       //梯度矩陣
    float    JtJ[6][6];    //Hessian矩陣
   
    //設定方程解初值
    beta[0] = beta[1] = beta[2] = 0;
    beta[3] = beta[4] = beta[5] = 1 / length;

    //開始迭代，當迭代步長小于eps時結束計算，得到方程近似最優解
    while(change > eps) 
    {
        //矩陣初始化
        ResetMatrices(JtR, JtJ);

        //計算誤差方程函數的梯度JtR和Hessian矩陣JtJ
        for(uint8_t i=0; i<6; i++) 
        {
            data[0] = inputData[i].x;
            data[1] = inputData[i].y;
            data[2] = inputData[i].z;
            UpdateMatrices(JtR, JtJ, beta, data);
        }

        //高斯消元法求解方程：JtJ * delta = JtR，得到delta
        GaussEliminateSolveDelta(JtR, JtJ, delta);

        //計算迭代步長
        change = delta[0]*delta[0] +
                 delta[0]*delta[0] +
                 delta[1]*delta[1] +
                 delta[2]*delta[2] +
                 delta[3]*delta[3] / (beta[3]*beta[3]) +
                 delta[4]*delta[4] / (beta[4]*beta[4]) +
                 delta[5]*delta[5] / (beta[5]*beta[5]);

        //更新方程解
        for(uint8_t i=0; i<6; i++) 
        {
            beta[i] -= delta[i];
        }
            
        //限制迭代次數
        if(cnt++ > 100)
            break;
    }

    //更新校準參數
    scale->x  = beta[3] * length;
    scale->y  = beta[4] * length;
    scale->z  = beta[5] * length;
    offset->x = beta[0];
    offset->y = beta[1];
    offset->z = beta[2];
}

主程序中調用了三個子函數，其中：

ResetMatrices，用于初始化矩陣變量

static void ResetMatrices(float JtR[6], float JtJ[6][6])
{
    int16_t j,k;
    for(j=0; j<6; j++) 
    {
        JtR[j] = 0.0f;
        for(k=0; k<6; k++) 
        {
            JtJ[j][k] = 0.0f;
        }
    }
}

UpdateMatrices，用于計算求解 $\bigtriangleup$ 所用到的 $J_r(\beta_\alpha)^T J_r(\beta_\alpha)$ (Hessian矩陣）和 $J_r(\beta_\alpha)^T r(\beta_\alpha)$ 這兩個矩陣

static void UpdateMatrices(float JtR[6], float JtJ[6][6], float beta[6], float data[3])
{
    int16_t j, k;
    float dx, b;
    float residual = 1.0;
    float jacobian[6];
    
    for(j=0; j<3; j++) 
    { 
        b = beta[3+j];
        dx = (float)data[j] - beta[j];
        //計算殘差 (傳感器誤差方程的誤差)
        residual -= b*b*dx*dx;
        
        //計算雅可比矩陣
        jacobian[j] = 2.0f*b*b*dx;
        jacobian[3+j] = -2.0f*b*dx*dx;
    }
    
    for(j=0; j<6; j++) 
    {
        //計算函數梯度
        JtR[j] += jacobian[j]*residual;
        
        for(k=0; k<6; k++) 
        {
            //計算Hessian矩陣（簡化形式，省略二階偏導），即雅可比矩陣與其轉置的乘積
            JtJ[j][k] += jacobian[j]*jacobian[k];
        }
    }
}

GaussEliminateSolveDelta，使用高斯消元法求解 $\bigtriangleup$

static void GaussEliminateSolveDelta(float JtR[6], float JtJ[6][6], float delta[6])
{
    int16_t i,j,k;
    float mu;
   
    //逐次消元，將線性方程組轉換為上三角方程組
    for(i=0; i<6; i++)
    {
        //若JtJ[i][i]不為0，將該列在JtJ[i][i]以下的元素消為0
        for(j=i+1; j<6; j++)
        {
            mu = JtJ[i][j] / JtJ[i][i];
            if(mu != 0.0f) 
            {
                JtR[j] -= mu * JtR[i];
                for(k=j; k<6; k++)
                {
                    JtJ[k][j] -= mu * JtJ[k][i];
                }
            }
        }
    }

    //回代得到方程組的解
    for(i=5; i>=0; i--)
    {
        JtR[i] /= JtJ[i][i];
        JtJ[i][i] = 1.0f;
        
        for(j=0; j<i; j++)
        {
            mu = JtJ[i][j];
            JtR[j] -= mu * JtR[i];
            JtJ[i][j] = 0.0f;
        }
    }

    for(i=0; i<6; i++)
    {
        delta[i] = JtR[i];
    }
}

事實上，在實際應用中使用高斯牛頓法，會出現許許多多的問題，因此下面介紹一種基于高斯牛頓改良優化而來的算法。

七.Levenberg-Marquardt法

實際應用中，高斯牛頓法有著如下缺點：

初始點選取較為嚴苛，若初始點距離極小值點過遠，迭代步長過大會導致迭代下一代的函數值不一定小于上一代的函數值
高斯牛頓法中的Hessian矩陣在某些情況下可能是非正定的（不可逆）
殘差 $r$ 過大時可能導致高斯牛頓法無法收斂

而Levenberg-Marquardt法（LM法，又稱阻尼最小二乘法）改善了高斯牛頓法的不足之處，并結合了高斯牛頓法和梯度下降法的優點，而實際上所做的改動非常小。

在高斯牛頓法中，迭代步長 $\bigtriangleup$ 被定義為
$\bigtriangleup = [H_S(\beta_\alpha)]^{-1}\bigtriangledown S(\beta_\alpha)$
而在LM法中，加入了阻尼因子 $\mu$ ，有
$\bigtriangleup = [H_S(\beta_\alpha) + \mu I]^{-1}\bigtriangledown S(\beta_\alpha)$
從而消除了Hessian矩陣非正定的情況，更重要的是迭代過程中阻尼因子 $\mu$ 是可變的，用于調整下降方向，當：

$\mu$ 趨近于0時，算法退化為高斯牛頓法
$\mu$ 很大時，效果相當于梯度下降法

使用LM法時，需要加入一些 $\mu$ 的調整策略，使得距離解較遠的時候，使用梯度下降法，距離極小值較近的時候，使 $\mu$ 減小，這樣就近似高斯牛頓法，能得到比梯度下降法更快的收斂速度。

算法的具體實現這里就不貼出來了，可以到這里查看：天穹飛控中的LM法

八.傳感器數據采集

前幾節介紹了如何使用非線性最小二乘法校準傳感器，下面簡單說明校準時采集所需傳感器數據的具體步驟。

1.加速度計

在天穹飛控中，加速度校準使用標準的六面采集法，需要將飛控按照上、下、前、后、左、右六個方向靜止放置一段時間（2s左右），順序隨意且飛控方向大致準確即可，飛控自動檢測當前加速度計方向，并采集多組數據取平均值，最后得到6組方向各異的數據，導入LM校準函數，便能求出加速度計的6個校準參數。

加速度校準采集代碼

天穹飛控兼容mavlink，可以使用QGroundControl地面站完成加速度計的校準步驟，如下圖：

使用QGC校準加速度計.jpg

點擊左側的Accelerometer按鈕，確認OK，飛控便會進入加速度校準流程，同時QGC會顯示出當前校準進度以及圖片提示，當對應方向采集數據完畢時，對應圖片外框會變綠，紅色的則是還沒有采集數據的方向。

2.磁力計

磁力計的數據采集方式采用比較通用的兩圈旋轉方式，即：將飛控水平旋轉一圈，然后再豎直旋轉一圈。在這個過程中，記錄下各軸的最大和最小值數據。

磁力計校準采集代碼

同樣可以使用QGC進行磁力計校準，點擊Compass并確認OK。開始按照上述步驟旋轉飛控，完成校準。

然而相對加速度計校準而言，磁力計校準對算法的要求更高。由于加速度計校準環境通常比較單一，且靜止狀態下加速度計數據基本不受外界干擾，所以采集到的數據都是比較準確的，使得算法很容易收斂，實測大概只需4-5次的迭代運算，就可以達到所需精度。

而磁力計數據采集在實際應用過程中，可能存在以下問題：

校準環境比較復雜，而且校準頻率非常頻繁。用戶可能是在室內校準，也可能是在城市的建筑間校準，或者是在充滿了未知磁場干擾的環境下校準。這便導致了在校準期間，可能由于各種未知的外部干擾，使得磁力計數據采集出現“野值”
為了簡化校準步驟，我們采用了較為簡單的兩圈旋轉方式，但由于地磁場的分布方向原因，實際上這樣我們采集到的磁力計數據并不是在整個球面上分布均勻的，如下圖所示（圖片來自正在開發中的天穹地面站）：

磁力計數據分布示意圖

為了解決問題1，我們對采集到的數據做了一定的濾波平滑處理，同時實時計算數據的一個平均模值，當新值的大小超過平均模值一定比例時，拋棄這個值，避免在構建誤差方程組時，引入包含較大干擾量的觀測值，導致算法無法收斂或最終計算出來的傳感器校準參數存在誤差。

問題2考驗了算法的性能，即在樣本分布不均勻的情況下是否依然可以收斂，經過測試和驗證，天穹飛控中使用的LM法是能夠經受得住這個考驗的。

九.總結

本篇文章介紹了飛控中最基本的傳感器誤差問題，并提出了有效的解決方法。其中所使用到的非線性最小二乘算法，在現代計算機工程領域有著非常廣泛的應用，所以即使對飛控技術興趣不大的童鞋，本文也是值得一看的。

而對于我們的飛控而言，完成了傳感器基本誤差的校準，只是相當于邁出了第一步，后面還有無數難題待解決，再接下來的教程中，會陸續針對不同的問題尋求最優的解決方案，當然由于本人水平有限，只能起到拋磚引玉的作用，希望有更多的有識之士，加入這個開源飛控項目，一起完善，共同進步。

最后再留一下傳送門：

天穹飛控
 飛控交流論壇
飛控交流Q群：472648354

三个男躁一个女,国精产品一区一手机的秘密,麦子交换系列最经典十句话,欧美国产综合欧美视频

姿態篇：四.非線性最小二乘與飛控傳感器校準

姿態篇：四.非線性最小二乘與飛控傳感器校準

前言

一.傳感器校準簡述

二.傳感器誤差類型

三.傳感器校準的簡單方式

1.加速度計

2.磁力計

四.建立傳感器誤差模型

五.求解誤差方程

六.高斯牛頓法

1.牛頓法

2.高斯牛頓法

七.Levenberg-Marquardt法

八.傳感器數據采集

1.加速度計

2.磁力計

九.總結

推薦閱讀更多精彩內容

三个男躁一个女,国精产品一区一手机的秘密,麦子交换系列最经典十句话,欧美 国产 综合 欧美 视频

姿態篇：四.非線性最小二乘與飛控傳感器校準

前言

一.傳感器校準簡述

二.傳感器誤差類型

三.傳感器校準的簡單方式

1.加速度計

2.磁力計

四.建立傳感器誤差模型

五.求解誤差方程

六.高斯牛頓法

1.牛頓法

2.高斯牛頓法

七.Levenberg-Marquardt法

八.傳感器數據采集

1.加速度計

2.磁力計

九.總結

推薦閱讀更多精彩內容

三个男躁一个女,国精产品一区一手机的秘密,麦子交换系列最经典十句话,欧美国产综合欧美视频