前文講到APM的三階互補方案，之前附的圖是從學長博客裏面摳的，感覺還不是很詳細，於是自己就畫了下，順便重新理一下思路。
上圖中下標爲O的表示原始量（Origion）,C表示矯正後的量（Correction）,a,v,s這些一目瞭然，表示加速度、速度、位置，其中帶下標c的表示融合後的狀態量，即在控制中用作實際反饋的量，讀者可以結合上圖與上文博客後面所貼的相關代碼、註釋閱讀。
鏈接如下：
上文最後提到由於觀測傳感器滯後性（主要原因：1、支持的最大采樣頻率小；2、原始數據輸出噪聲大、大多數時候需要數字低通濾波造成時延），造成的直接把當前慣導估計值與觀測傳感器做差比較得到狀態誤差的方式不可取。
在提出解決辦法前，首先我們來談下傳感器採樣週期造成的觀測傳感器滯後的問題：
首先以大家熟悉的超聲波傳感器爲例，其中辨識度最高的爲HC-SR04
直接給出採集時序圖：
超聲波傳感器工作需要模塊發射頭向外發射波長約爲6mm、頻率爲40khz的超聲波信號，觸發模塊開始工作需要在模塊觸發引腳產生一個不少於10us的高電平，取觸發時刻爲T0,當前方有物體時，聲波會反射回來，反射回來的信號被模塊上的接收頭所接收，並且在模塊的輸出引腳上產生一個迴響應電平信號,取響應時刻回T1,根據聲波一來一往的總時間與聲音在空氣中傳播的速度，即可計算距前方物體距離。
此模塊爲了防止發射信號對迴響信號的影響，需要兩次發射間隔至少爲60ms，模塊測量距離範圍爲2cm~400cm，實際使用過程中，也就發現2.5m以內數據還好，超過2.5m後，稍微有點角度誤差就很大，我們就按照最大檢測距離爲2.5m計算，最大采集時間爲60ms+(2.5*2/340)*1000ms=74.7ms
至此我們總結下，常見的HC-SR04模塊最小採樣週期爲60ms，最
大接近75ms。
75ms似乎完全可以接受，下面再以常用的M8N GPS爲例：
直接截取ublox軟件設置截圖：
這裏首先可以看到GPS時鐘源，一般就選GPS time即可，接着是
測量週期,一般模塊初始化爲1Hz,對於一般的應用來講，1Hz刷新
速率完全夠了，這也是模塊默認的刷新速率。更快的刷新速率意
味着需要佔用芯片的更大通訊資源、處理壓力，同時模塊的功耗
也就越大。
（尚不瞭解刷新速率加大是否會對數據輸出精度有影響。。。）
本文當前採用的不是串口解析Nema標準字符數據幀形式，本文只
對UBX裏面PVT語句進行解析，PVT數據幀信息如下圖所示：
PVT語句基本上包含了GPS定點所需的所有常用信息，本文直接
GPS測量頻率設置成M8N所能允許的最大采樣頻率10Hz後，此時
的GPS採樣時間即爲100ms。
下面接着討論常用高度觀測傳感器———氣壓計MS5611，數據
手冊截圖如下：
其中氣壓計採集過程爲氣壓、溫度交替採集，採樣頻率可設置，
採集時，需要先開啓ADC（氣壓、溫度）轉換，然後採集，這裏
的Responce Time表示的爲開啓到採樣的所需間隔時間。一般爲
保證壓力數據、溫度數據的實時同步性，在採集溫度前、開啓氣
壓ADC數據轉換，同理，採集氣壓前，亦開啓溫度ADC轉換。這
樣可以實現採樣週期的最小化。
從MS5611數據手冊可以很容易的知道溫度+氣壓採樣週期最小爲
0.5*2=1ms，最大爲8.22*2=16.44ms,這裏需要注意的是不同採
樣頻率下，傳感器的輸出精度是不一樣的，從
0.012mbar到0.065mbar，其中當採樣週期爲16.44ms時，氣壓
誤差爲0.012mbar，採樣週期爲1.0ms時，氣壓誤差爲
0.065mbar,這裏出現的mbar表示氣壓單位毫帕。單位轉換關係如
下圖：
故0.0012mbar=1.20Pa，有常識可知1Pa誤差約等於10cm,將氣
壓誤差轉換爲距離後誤差在12cm~65cm。更小採樣的採樣週期
意味着更大的採樣誤差，不同的噪聲誤差對應着卡爾曼濾波時觀
測噪聲的選取大小。
這裏我們參考Autoquad飛控裏面的採樣設置，直接摳圖如下：
這裏可以看到，傳感器ADC轉換時，溫度與氣壓均設置成4096，
即此時最小採樣週期爲16.44ms,在AQ飛控CoOS任務調度週期爲
2.5ms，意味着只能實現最小20ms的氣壓計採樣週期。（其中緣
由大家自己算下加法即可）。
最後在介紹一種位置觀測傳感器：光流PX4FLOW
下面一段話爲官網PX4FLOW中文介紹截取部分：
PX4Flow 是一款智能光學流動傳感器。傳感器擁有原生752×480 像素分辨率，計算光
學流的過程中採用了4倍分級和剪裁算法，計算速度達到250Hz（白天，室外），具備
非常高的感光度。與其他滑鼠傳感器不同，它可以以 120Hz（黑暗，室內）的計算速度
在室內或者室外暗光環境下工作，而無需照明LED。你也可以對它重新編程，用於執行
其他基礎的，高效率的低等級機器視 覺任務。
PX4FLOW支持USB、串口、I2C兩種方式對數據進行解析，爲了減小芯片開銷
作者採用的方式爲I2C形式，其中I2C相關數據爲如下圖：
其中包含：光流點數、光流積分量、結合高度轉換後的光流速度、超聲波距離、圖像
質量、三軸角速度、超聲波測量時間、陀螺儀內部溫度等數據。
其中：光流速度、超聲波距離、圖像質量爲定點、定高所需的有用數據，作者在這裏只
對這三組數據進行了獲取。
PX4FLOW主控採用的是STM32F405,芯片主要資源開銷爲DCMI獲取攝像頭數據與處理
+融合高度、陀螺儀光流算法（其餘超聲波數據採集、板載陀螺儀SPI數據採集、
Mavlink、USB等基本上可以忽略）。
這裏PX4FLOW給出了最大處理速度時耗時4ms，尚不清楚這部分指的是處理完一場圖
像數據加融合的總開銷，還是隻是針對融合算法。
作者主控採用的是STM32F103RCT6，採用模擬I2C採集PX4FLOW數據，爲保證陀螺儀
、加計採樣以及控制週期最小化，在採集I2C時，對PX4FLOW採用的是隊列採集模式，
即第一個2ms採集X軸流速、第二個2ms採集Y軸流速、第三個2ms採集超聲波距離、第
四個2ms採集圖像質量，光流數據更新一次爲8ms。
至此我們列舉幾種常見傳感器採樣週期如
下：
超聲波HC-SR04：75ms
GPS M8N:100ms
氣壓計MS5611：20ms
PX4FLOW:8ms
上述我們只是討論了數字傳感器採集過程中的傳感器採樣週期造成的時延。
這裏我們討論的時延是相對於慣性導航主導傳感器——加速度計而言的。
MPU6500加速度計當不設置內部數字低通時，陀螺儀最大輸出頻率爲8Khz，加速度計爲
4Khz，對於四旋翼而言，最大采樣頻率爲1Khz就完全夠用了，足夠高的採樣週期能減小
數據融合時的積分誤差（頻率混疊可以忽略），同時保證傳感器數據數字低通時的羣時
延也更小。
作者由於STM32F103RCT6芯片運算速度限制，MEMS傳感器組合爲MPU6500、HMC5983、
MS5611，這三組傳感器數據通過SPI採集進來分別耗時：73us、56us、82us。
姿態解算最大耗時約：320us
GPS串口解析最大耗時約：200us
PX4FLOW單個數據採集最大耗時約：0.5ms
三軸慣導卡爾曼融合最大耗時約：270us
控制器、數字濾波等最大耗時約：150us
系統總時間開銷約爲1.65ms
作者選取總定時調度週期爲2.0ms
上面我們考慮的是一類原始數據採集過程中的滯後，接下來介紹一類因數字低
通濾波器造成的傳感器時延問題。
傳感器數字低通的滯後性：
首先以氣壓計傳感器爲例子，上文講到當MS5611氣壓傳感器設置成精度最高時，即開啓
ADC採集時，溫度、氣壓轉換都設置成4096，此時官方給出的傳感器誤差爲12cm，Okay，
現在我們先來一組原始氣壓通過壓差法獲取的相對高度值波形。
上圖中，中間藍色線表示速度波形、灰色表示加速度波形、上面總共有三條線，放大後
如下：噪聲比較大的爲原始氣壓通過壓差換算高度值、平滑點的紅色線表示慣導估計高
度，較爲滯後的綠色線表示，巴特沃斯2Hz截止頻率後的濾波滯後的高度值。
這裏注意到，氣壓計原始高度波動比較大，高度噪聲基本上在50cm以內，數字低通後的
氣壓高度即波動比較小，不考慮傳感器靜止漂移情況下，短時間內波動在15cm以內，但
是運動起來後，發現滯後性很明顯。（曲線從一定高度至另一高度的跟蹤出現明顯
先後）
如果覺得還不夠明顯，當速度快一點後，就立馬一目瞭然了。
注意速度峯值大的地方，慣導位置估計高度與低通後的氣壓觀測高度，明顯都是紅色線
先起來、觀測傳感器後起來，中間來回上下拖動飛機一段更爲明顯。後面最後一個速度
峯值較小時，兩曲線基本重合，看不出明顯差異。
下面給出利用MATLAB設計巴特沃斯低通濾波器過程(Tip:作者信號與系統很渣，只會依葫
蘆畫瓢):
首先本文巴特沃斯濾波器爲2階，階數越高，雖然保證了阻帶更快衰減，但是系統相
延亦增長。
1、調用MATLAB濾波器設計工具箱，命令行輸入fdatool即可，界面如下。
2、左下方分別勾選Lowpass、IIR、濾波器階數、採樣頻率、截止頻率即可。
3、點擊Design Filter濾波器設計完成，得到系統幅頻響應如下。
4、點擊Analysis,選Phase Delay即可看到系統相延特性。
上面兩個圖的差異在於設置的濾波器階次不一樣，對比可知，階次越高系統相延越
大。
二階巴特沃斯數字低通濾波器參數如上圖所示。
通過fdatool濾波器工具箱導出的濾波器參數，由巴特沃斯數字濾波器了離散方程：
數字控制器設計時，根據實時採樣的加速度計數據，遞推更新即可。
/*************************************************
函數名: LPButterworth(float curr_input,Butter_BufferData *Buffer,Butter_Parameter *Parameter)
說明: 二階巴特沃斯數字低通濾波器
入口: float curr_input 當前輸入
出口: 濾波器輸出值
備註: 2階Butterworth低通濾波器
*************************************************/
float LPButterworth(float curr_input,Butter_BufferData *Buffer,Butter_Parameter *Parameter)
{
static int LPB_Cnt=0;
/* 加速度計Butterworth濾波 */
/* 獲取最新x(n) */
Buffer->Input_Butter[2]=curr_input;
if(LPB_Cnt>=500)
{
/* Butterworth濾波 */
Buffer->Output_Butter[2]=
Parameter->b[0] * Buffer->Input_Butter[2]
+Parameter->b[1] * Buffer->Input_Butter[1]
+Parameter->b[2] * Buffer->Input_Butter[0]
-Parameter->a[1] * Buffer->Output_Butter[1]
-Parameter->a[2] * Buffer->Output_Butter[0];
}
else
{
Buffer->Output_Butter[2]=Buffer->Input_Butter[2];
LPB_Cnt++;
}
/* x(n) 序列保存 */
Buffer->Input_Butter[0]=Buffer->Input_Butter[1];
Buffer->Input_Butter[1]=Buffer->Input_Butter[2];
/* y(n) 序列保存 */
Buffer->Output_Butter[0]=Buffer->Output_Butter[1];
Buffer->Output_Butter[1]=Buffer->Output_Butter[2];
return (Buffer->Output_Butter[2]);
}
相關結構體定義如下。
typedef struct
{
//volatile
float Input_Butter[3];
//volatile
float Output_Butter[3];
}Butter_BufferData;
typedef struct
{
float a[3];
float b[3];
}Butter_Parameter;
自研飛控視頻，鏈接如下：
此篇博客廢話太多，疏於整理，難免有不正之處，歡迎交流指正！！！
下節討論傳感器延時修正的簡單處理方法與慣性導航數據融合的細節部分。。。