卡爾曼濾波（最優(yōu)估計）

一、什么是卡爾曼濾波

通俗理解卡爾曼濾波：一個「會自動調(diào)權(quán)重的裁判」

卡爾曼濾波解決的核心問題是：你有兩個途徑（測量和預估得到）知道同一個東西的狀態(tài)，但這兩個途徑都不準，卡爾曼幫你算出最準的結(jié)果

最優(yōu)估計公式：

二、為什么這里要用卡爾曼

下面是MPU6050加速度計和陀螺儀的優(yōu)缺點：

總的來說：

陀螺儀動態(tài)的時候輸出的值精準，但是靜止的時候會有漂移

而加速度計就反過來了，加速度計靜態(tài)的時候輸出的值準，但是動態(tài)特性差

MPU6050 的陀螺儀和加速度計是 “天生的互補搭檔”，但單靠其中一個或簡單濾波，要么漂移、要么動態(tài)失真；而卡爾曼濾波的核心是 “動態(tài)加權(quán)的最優(yōu)估計” —— 它能根據(jù) MPU6050 的工作狀態(tài)（靜態(tài) / 動態(tài)），自動判斷兩個傳感器的可靠性，把陀螺儀的 “動態(tài)精準” 和加速度計的 “靜態(tài)穩(wěn)定” 結(jié)合起來，最終輸出既無漂移、又能快速響應的 roll/pitch 角度，這是其他濾波方式無法實現(xiàn)的。

三、卡爾曼濾波實現(xiàn)

一、思路

根據(jù)之前的內(nèi)容，我們已經(jīng)得到了加速度計解算的roll和pitch，以及角速度計解算的roll、pitch、yaw，因為加速度計無法解算yaw，所以這里只對roll和pitch做數(shù)據(jù)融合

卡爾曼最優(yōu)估計的核心目標是融合 “加速度計解算的角度” 和 “陀螺儀角速度積分的角度”，最終輸出最優(yōu)的角度估計。整體邏輯分為兩大階段：

預測階段：基于陀螺儀角速度（扣除偏置后）積分，預測當前角度；同時更新狀態(tài)誤差協(xié)方差矩陣 P（反映估計的不確定性）。

更新階段：用加速度計的角度作為觀測值修正預測值，得到最優(yōu)角度；同時修正陀螺儀偏置（補償零漂），并更新協(xié)方差矩陣 P。

二、具體實現(xiàn)

2.1預測階段（先驗估計）

狀態(tài)預測：用陀螺儀積分預測角度

對應代碼：

// 預測階段：更新角度預測
    float rate = newRate - *kalmanBias;  // 真實角速度 = 測量角速度 - 偏置
    *kalmanAngle += dt * rate;           //角度預測：θ_k = θ_{k-1}+ dt*(ω - b)

協(xié)方差預測：更新估計的不確定性

先計算：

再加上Q得到：

對應代碼：

 kalmanP[0][0] += dt * (dt * kalmanP[1][1] - kalmanP[0][1] - kalmanP[1][0] + Q_ANGLE);
    kalmanP[0][1] -= dt * kalmanP[1][1];
    kalmanP[1][0] -= dt * kalmanP[1][1];
    kalmanP[1][1] += Q_BIAS * dt;

2.2更新階段（后驗修正）

殘差（創(chuàng)新項）：觀測值與預測值的偏差

卡爾曼增益：權(quán)衡 “預測” 和 “觀測” 的可信度

對應代碼：

  // 計算卡爾曼增益K
    float S = kalmanP[0][0] + R_MEASURE;  // 角度方差+測量噪聲方差（R）
    float K[2];
    K[0] = kalmanP[0][0] / S;
    K[1] = kalmanP[1][0] / S;

狀態(tài)更新：修正預測值，得到最優(yōu)估計

對應代碼：

  // 更新階段：利用加速度計測量值修正預測角度
    float y = newAngle - *kalmanAngle;  // 改用傳入的角度
    *kalmanAngle += K[0] * y;           // 修正角度
    *kalmanBias += K[1] * y;            // 修正偏置

協(xié)方差更新：修正估計的不確定性

對應代碼：

  // 更新協(xié)方差矩陣P
    float P00_temp = kalmanP[0][0];
    float P01_temp = kalmanP[0][1];
    kalmanP[0][0] -= K[0] * P00_temp;
    kalmanP[0][1] -= K[0] * P01_temp;
    kalmanP[1][0] -= K[1] * P00_temp;
    kalmanP[1][1] -= K[1] * P01_temp;

匿名上位機通信

一、為何用匿名上位機調(diào)試

匿名上位機是一款由國內(nèi)開發(fā)者打造、面向嵌入式開發(fā)場景的開源 / 低成本 PC 端調(diào)試工具，相比普通串口助手只能顯示一串冰冷的歐拉角數(shù)字，匿名上位機可將 Roll/Pitch/Yaw 以實時曲線、數(shù)值儀表盤、3D 模型姿態(tài)仿真等形式呈現(xiàn) —— 當我們晃動搭載 MPU6050 的 CW32L012 開發(fā)板時，上位機的 3D 模型會同步傾斜、轉(zhuǎn)向，曲線會實時跟蹤歐拉角數(shù)值變化，你能快速判斷卡爾曼融合后的姿態(tài)解算是否準確（如零漂是否被抑制、動態(tài)晃動時歐拉角是否無明顯跳變），無需逐行核對數(shù)字。

二、通信幀格式

初始化L012串口，并根據(jù)通信格式編寫匿名上位機的通信驅(qū)動：

程序?qū)嶋H現(xiàn)象

【CW32L012解算MPU6050的姿態(tài)數(shù)據(jù)并進行卡爾曼濾波的方法-嗶哩嗶哩】

https://b23.tv/2GZqcIg

一、開發(fā)板現(xiàn)象：

OLED屏幕實時顯示解算的三軸角度

二、匿名上位機現(xiàn)象

【CW32L012使用MPU6050介紹-嗶哩嗶哩】 https://b23.tv/fi7hCnw