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