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

一、什么是卡爾曼濾波

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

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

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

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

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

總的來說：

陀螺儀動(dòng)態(tài)的時(shí)候輸出的值精準(zhǔn)，但是靜止的時(shí)候會(huì)有漂移

而加速度計(jì)就反過來了，加速度計(jì)靜態(tài)的時(shí)候輸出的值準(zhǔn)，但是動(dòng)態(tài)特性差

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

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

一、思路

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

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

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

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

二、具體實(shí)現(xiàn)

2.1預(yù)測階段（先驗(yàn)估計(jì)）

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

對(duì)應(yīng)代碼：

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

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

先計(jì)算：

再加上Q得到：

對(duì)應(yīng)代碼：

 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更新階段（后驗(yàn)修正）

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

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

對(duì)應(yīng)代碼：

  // 計(jì)算卡爾曼增益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ù)測值，得到最優(yōu)估計(jì)

對(duì)應(yīng)代碼：

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

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

對(duì)應(yīng)代碼：

  // 更新協(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;

匿名上位機(jī)通信

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

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

二、通信幀格式

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

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

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

https://b23.tv/2GZqcIg

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

OLED屏幕實(shí)時(shí)顯示解算的三軸角度

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

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