一.濾波介紹

模擬信號濾波：
最常用的濾波方法可以在信號和地之間并聯一個電容，因為電容通交隔直，信號突變會給電容充電，電容兩端電壓不會突變，電容越大越明顯。
電容濾電壓的毛刺，電感濾電流的毛刺。電感串聯進電路中，電流出現毛刺，因為通過電感兩端的電流不能突變，實現對電流的濾波。
數字信號濾波：
使用算法進行濾波。
1.均值濾波（滑動窗口濾波）：每來一個值使用前四個值進行平均使用平均值。
2.中值濾波：將數據排序，取數據奇數部分的中值代替取到的數值。
3.一階低通濾波：結果 = 系數 * 上次的值 + （1 - 系數）X 這次的值
4.卡爾曼濾波：核心5個公式。
5.互補濾波：
加速度對時間積分 速度：響應迅速，結果容易受到外界影響。
加速度對時間微分 速度：結果不容受影響，響應不及時。

二.對角速度進行一階低通濾波

編寫一階低通濾波函數：

#define ALPHA 0.8
/* 一階低通率波 */
int16_t Com_Filter_LowPass(int16_t newData, int16_t lastData)
{return ALPHA * lastData + (1 - ALPHA) * newData;
}

使用一階低通濾波：
記住讀取數據時使用臨界區。

    /* 1. 讀取原始數據 */taskENTER_CRITICAL();Inf_MPU6050_ReadGyroAccelCalibrated(gyroAccel);taskEXIT_CRITICAL();/* 2. 對角速度做一階低通低通濾波 */static int16_t lastDatas[3] = {0};gyroAccel->gyro.gyroX = Com_Filter_LowPass(gyroAccel->gyro.gyroX, lastDatas[0]);gyroAccel->gyro.gyroY = Com_Filter_LowPass(gyroAccel->gyro.gyroY, lastDatas[1]);gyroAccel->gyro.gyroZ = Com_Filter_LowPass(gyroAccel->gyro.gyroZ, lastDatas[2]);lastDatas[0] = gyroAccel->gyro.gyroX;lastDatas[1] = gyroAccel->gyro.gyroY;lastDatas[2] = gyroAccel->gyro.gyroZ;

一階低通濾波效果演示：
使用虛擬數字示波器觀察濾波前后數據波形，濾波效果明顯。
CH1（紅）：濾波前
CH2（黃）：濾波后

三.對加速度進行卡爾曼濾波

直接移植下面的卡爾曼濾波代碼：
.c:
三個結構體分別是XYZ三軸的參數。
卡爾曼濾波函數參數分別為結構體，對誰做濾波。
返回值為濾波后結果。

/* 卡爾曼濾波參數 */
KalmanFilter_Struct kfs[3] = {{0.02, 0, 0, 0, 0.001, 0.543},{0.02, 0, 0, 0, 0.001, 0.543},{0.02, 0, 0, 0, 0.001, 0.543}};
double Common_Filter_KalmanFilter(KalmanFilter_Struct *kf, double input)
{kf->Now_P = kf->LastP + kf->Q;kf->Kg    = kf->Now_P / (kf->Now_P + kf->R);kf->out   = kf->out + kf->Kg * (input - kf->out);kf->LastP = (1 - kf->Kg) * kf->Now_P;return kf->out;
}

.h:

/* 卡爾曼濾波器結構體 */
typedef struct
{float LastP;   // 上一時刻的狀態方差（或協方差）float Now_P;   // 當前時刻的狀態方差（或協方差）float out;     // 濾波器的輸出值，即估計的狀態float Kg;      // 卡爾曼增益，用于調節預測值和測量值之間的權重float Q;       // 過程噪聲的方差，反映系統模型的不確定性float R;       // 測量噪聲的方差，反映測量過程的不確定性
} KalmanFilter_Struct;extern KalmanFilter_Struct kfs[3];

使用卡爾曼濾波：
傳入結構體和濾波的對象。

    /* 3. 對加速度做卡爾曼濾波 */// OutData[0] = gyroAccel->accel.accelX ;gyroAccel->accel.accelX = Common_Filter_KalmanFilter(&kfs[0], gyroAccel->accel.accelX);gyroAccel->accel.accelY = Common_Filter_KalmanFilter(&kfs[1], gyroAccel->accel.accelY);gyroAccel->accel.accelZ = Common_Filter_KalmanFilter(&kfs[2], gyroAccel->accel.accelZ);// OutData[1] = gyroAccel->accel.accelX ;// OutPut_Data();

觀察濾波前后效果：
效果良好