摘要：卡爾曼濾波（Kalman Filter）是傳感器數據融合領域的經典算法，在姿態解算、導航定位等嵌入式場景中廣泛應用。本文將從公式推導、代碼實現、參數調試三個維度深入解析卡爾曼濾波，并給出基于STM32硬件的完整工程案例。

---

一、卡爾曼濾波核心思想

1.1 什么是卡爾曼濾波？

卡爾曼濾波是一種最優遞歸估計算法，通過融合預測值（系統模型）與觀測值（傳感器數據），在噪聲干擾環境下實現對系統狀態的動態估計。其核心優勢在于實時性和自適應性。

1.2 適用場景

• 存在高斯白噪聲的線性系統

• 需要多傳感器數據融合的場景

• 實時性要求高的嵌入式系統（如無人機、平衡車）

---

二、卡爾曼濾波算法推導

2.1 五大核心公式

參數說明：

• QQ：過程噪聲協方差（系統不確定性）

• RR：觀測噪聲協方差（傳感器精度）

• PP：估計誤差協方差

---

三、STM32硬件實現方案

3.1 開發環境配置

• MCU: STM32F407ZGT6

• 傳感器: MPU6050（加速度計+陀螺儀）

• 開發工具: STM32CubeIDE + HAL庫

3.2 算法移植關鍵點

1. 矩陣運算庫選擇：使用ARM CMSIS-DSP庫加速矩陣運算

2. 浮點運算優化：啟用FPU硬件加速

3. 實時性保障：算法耗時需小于采樣周期

---

四、一維卡爾曼濾波代碼實現

// 卡爾曼結構體定義
typedef struct {float q;    // 過程噪聲方差float r;    // 測量噪聲方差float x;    // 狀態估計值float p;    // 估計誤差協方差float k;    // 卡爾曼增益
} KalmanFilter;// 初始化濾波器
void Kalman_Init(KalmanFilter *kf, float q, float r) {kf->q = q;kf->r = r;kf->p = 1.0f;kf->x = 0;
}// 卡爾曼迭代
float Kalman_Update(KalmanFilter *kf, float measurement) {// 預測階段kf->p += kf->q;// 更新階段kf->k = kf->p / (kf->p + kf->r);kf->x += kf->k * (measurement - kf->x);kf->p *= (1 - kf->k);return kf->x;
}

---

五、三維姿態解算應用實例

5.1 系統框圖

MPU6050 → I2C → STM32 → 卡爾曼濾波 → 串口輸出↑           ↓HAL庫      PID控制器

5.2 關鍵代碼片段

// 在main.c中實現
float Gyro[3], Accel[3];
KalmanFilter kf_x, kf_y, kf_z;int main(void) {// 初始化MPU6050_Init();Kalman_Init(&kf_x, 0.001, 0.5);// 類似初始化kf_y, kf_zwhile(1) {// 讀取原始數據MPU6050_ReadData(Gyro, Accel);// 執行濾波float roll = Kalman_Update(&kf_x, Accel[0]);// 同樣處理pitch/yaw// 通過串口輸出printf("Roll:%.2f\tPitch:%.2f\r\n", roll, pitch);HAL_Delay(10); // 10ms采樣周期}
}

---

六、參數調試經驗

1. Q值調整：增大Q會使濾波器更信任新測量值，響應更快但噪聲增大

2. R值調整：增大R會使濾波器更信任預測值，曲線平滑但滯后明顯

3. 典型參數范圍：

   • 加速度計：Q=0.001, R=0.5

   • 陀螺儀：Q=0.003, R=0.1

4. 調試工具：使用串口波形工具（如VOFA+）實時觀察數據曲線

---

七、性能優化技巧

1. 定點數優化：將float改為q15格式提升計算速度

2. 矩陣預計算：對固定參數矩陣提前計算

3. DMA傳輸：使用DMA加速傳感器數據讀取

4. 算法簡化：根據應用場景降維處理（如將三維轉為三個一維）

---

八、常見問題解答

Q1：如何處理非線性系統？
A：改用擴展卡爾曼濾波（EKF）或無跡卡爾曼濾波（UKF）

Q2：濾波器發散怎么辦？
A：檢查系統模型是否準確，適當增大Q值

Q3：如何驗證濾波效果？
A：通過靜態測試（方差分析）和動態測試（階躍響應）結合驗證

---

結語：卡爾曼濾波的實戰應用需要理論推導與工程經驗的結合。希望本文能為嵌入式開發者在傳感器數據處理方面提供有價值的參考。歡迎在評論區留言交流實際應用中的問題！