此處感謝華南虎和互聯網的眾多大佬的無償分享。

入門常識

先簡單了解以下概念：疊加性，齊次性。

用大白話講，疊加性：多個輸入對輸出有影響。齊次性：輸入放大多少倍，輸出也跟著放大多少倍

卡爾曼濾波符合這兩個特性。

卡爾曼濾波 ：修正值 = 估計值 + 觀測值。符合疊加性和齊次性。

協方差的基礎知識

把Xt代入，然后將式子進行化簡。

公式推導

基礎公式理解

對于卡爾曼濾波，我們可以先從狀態方程和觀測方程開始理解，重點搞懂重點是狀態方程和觀測方程的實際含義。
?

狀態方程公式的參數解析。Xk：當前狀態值，Uk：輸入值， Wk：過程噪聲。A，B為系數。

觀測方程公式的參數解析。Yk：觀測值。Xk：當前狀態值。Vk：觀測噪聲

簡單理解：Vk是不可避免的儀器系統誤差，Wk是由外界因素引起的誤差。

Q：過程噪聲的方差。R：觀測噪聲的方差。

最優估計值，先驗估計值，（觀測值）后驗估計值。

卡爾曼濾波其實就是取估計值和測量值之間重合的部分。兩個概率之間重合的部分。

卡爾曼的實現過程

使用上一次的最優結果預測當前值，同時使用觀測值修正之前預測的當前值，得到最終結果（修正后的結果叫做最優估計）。

不需要嚴格推導，了解是什么意思，怎么用即可。

狀態更新方程

卡爾曼增益化簡后

過程噪聲少，Q可以取小一點。反之，過程噪聲大，Q取大一點。

傳感器誤差小，R可以取小一點。反之，傳感器誤差大，R取大一點。

執行流程

卡爾曼濾波算法介紹

基本思想：通過不精確的測量來估測真實值。測出來的是不精確的測量值，再利用數學模型和不精確的測量來估測真實值。

一句話概括：根據系統模型和現有狀態預測下一刻的狀態，然后根據實際的測量結果進行校正。

使用場景：卡爾曼主要用來多傳感器的數據融合，單一傳感器建議用其他濾波算法。

如何使用？

以下面的代碼為例：先定義好卡爾曼濾波結構體，并為其賦值。之后把結構體參數傳入，把要過濾的數據傳入。卡爾曼濾波算法就會把過濾好的數據，放在其結構體的輸出成員中存儲好。我們再用變量把卡爾曼濾波結構體中的輸出成員的值接收到就行了。

代碼

kalman.c

/*** 一維卡爾曼濾波器實現* @param ekf 卡爾曼濾波器結構體指針，包含濾波所需參數* @param input 當前時刻的測量值* * 算法流程說明：* 1. 預測階段：根據上一時刻的狀態估計當前狀態* 2. 計算卡爾曼增益：確定測量值和預測值的權重* 3. 更新估計：結合預測值和測量值得到最優估計* 4. 更新協方差：為下一時刻的預測做準備*/
void kalman_1(struct _1_ekf_filter *ekf, float input)
{// 1. 預測協方差矩陣（時間更新）// Now_P = LastP + Q// 預測誤差 = 上一時刻誤差 + 過程噪聲// Q值越大，表明系統模型越不可靠，濾波器對新測量值更敏感ekf->Now_P = ekf->LastP + ekf->Q;// 2. 計算卡爾曼增益// Kg = Now_P / (Now_P + R)// 卡爾曼增益權衡預測值和測量值的權重// R值越大（測量噪聲大），增益越小，更信任預測值ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);// 3. 狀態更新方程（測量更新）// out = out + Kg * (input - out)// 本質是預測值與測量值的加權融合// 當Kg=1時完全信任測量值，當Kg=0時完全信任預測值ekf->out = ekf->out + ekf->Kg * (input - ekf->out);// 4. 更新誤差協方差矩陣// LastP = (1-Kg) * Now_P// 更新當前估計誤差，用于下一時刻的預測// 隨著迭代進行，P值會收斂到穩態ekf->LastP = (1 - ekf->Kg) * ekf->Now_P;
}

kalman.h

#ifndef _KALMAN_H    // 防止頭文件被重復包含
#define _KALMAN_H// 定義一維擴展卡爾曼濾波器結構體
struct _1_ekf_filter
{float LastP;    // 上一時刻的協方差（預測誤差）float Now_P;    // 當前時刻的協方差（估計誤差）float out;      // 濾波器輸出值（最優估計值）float Kg;       // 卡爾曼增益，權衡預測值和測量值的權重float Q;        // 過程噪聲協方差，反映系統模型的不確定性float R;        // 測量噪聲協方差，反映測量設備的不確定性
};// 一維卡爾曼濾波器函數
// 參數：ekf-卡爾曼濾波器結構體指針，input-當前時刻的測量值
extern void kalman_1(struct _1_ekf_filter *ekf, float input);#endif // _KALMAN_H

調用示例

最后我們要使用的數據在結構體的.out中

for (i = 0; i < 6; i++) // 處理讀取的數據{pMpu[i] = (((int16_t)buffer[i << 1] << 8) | buffer[(i << 1) + 1]) - MpuOffset[i]; // 整合為16bit，并減去水平靜止校準值if (i < 3)																		  // 以下對加速度做卡爾曼濾波{{static struct _1_ekf_filter ekf[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}};kalman_1(&ekf[i], (float)pMpu[i]); // 一維卡爾曼pMpu[i] = (int16_t)ekf[i].out;}}// 如果是陀螺儀數據，進行低通濾波if (i > 2) // 以下對角速度做一階低通濾波{uint8_t k = i - 3;const float factor = 0.15f; // 濾波系數static float tBuff[3];		// 濾波緩沖區pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;}}