版權聲明：本文爲博主原創博文，未經允許不得轉載，若要轉載，請說明出處並給出博文鏈接
本文詳細講解了ArduPilot中AP_TECS庫中的用於估計高度和垂直方向上的爬升速率的三階互補濾波的實現！
首先，需要強調的是，本文介紹的3階互補濾波（Complementary Filter）是在飛控無法獲取EKF（Extend Kalman Filter）提供的北東地三個方向上的速度的情況下才使用的，意思是首選擴展卡爾曼濾波，擴展卡爾曼濾波不OK，使用3階互補濾波。
其次，下面是閱讀代碼後的個人理解與心得，如有不對，還望指正!!!!!
3階互補濾波:(融合氣壓計的高度信息與加速度計的垂直方向數據）
omega2 = omega * omega，（omega-互補濾波的頻率 - 由全參數給定，默認爲3 radius / s）
error_h = baro_h - filter_h，（h-高度，baro-氣壓計，filter-互補濾波器）
Integ1 = error_h * omega2 * omega，（第一階段，可以理解爲高度方向上的加加速度 - 加速度的微分）
dd_h = Integ1 * dt + dd_h，（通過積分，獲得下一時刻的dd_h-濾波器估計出的高度的加速度）
Integ2 = dd_h + acc_h + error_h * omega2 * 3，（第二階，可以理解爲高度方向上校正後的加速度）
climb_rate = Integ2 * dt + climb_rate，（積分後得到下一時刻的爬升速率）
Integ3 = error_h * omega * 3 + climb_rate，（第三階段，可以理解爲高度方向上校正後的速度）
filter_h = Integ3 * dt + filter_h，（積分，估計出下一刻的高度）
其中，acc_h = - （acc_z + g）。acc_z =加速度計獲取的z方向（垂直向下）數據，g - 重力加速度
dt =採樣間隔
代碼如下：
/*
use a complimentary filter to calculate climb_rate. This is
designed to minimise lag
*/
float baro_alt = _ahrs.get_baro().get_altitude();
// Get height acceleration
float hgt_ddot_mea = -(_ahrs.get_accel_ef().z + GRAVITY_MSS);
// Perform filter calculation using backwards Euler integration
// Coefficients selected to place all three filter poles at omega
float omega2 = _hgtCompFiltOmega*_hgtCompFiltOmega;
float hgt_err = baro_alt - _height_filter.height;
float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega;
_height_filter.dd_height += integ1_input * DT;
float integ2_input = _height_filter.dd_height + hgt_ddot_mea + hgt_err * omega2 * 3.0f;
_climb_rate += integ2_input * DT;
float integ3_input = _climb_rate + hgt_err * _hgtCompFiltOmega * 3.0f;
// If more than 1 second has elapsed since last update then reset the integrator state
// to the measured height
if (DT > 1.0f) {
_height_filter.height = _height;
} else {
_height_filter.height += integ3_input*DT;
}
之所以不採用2階互補濾波，而採用3階互補濾波，是不想忽略加速度偏差或者重力計算偏差導致的誤差，進一步精確估計結果。
本文提出的3階互補濾波參考的是一篇來自AIAA的論文“優化巴洛慣性垂直通道的收益”。
想進一步學習的鏈接給出：