一区二区视频在线播放,熟女乱伦一区,国产精品一区二区高清在线

卡爾曼濾波（最優估計）

一、什么是卡爾曼濾波

通俗理解卡爾曼濾波：一個「會自動調權重的裁判」

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

最優估計公式：

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

下面是MPU6050加速度計和陀螺儀的優缺點：

總的來說：

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

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

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

三、卡爾曼濾波實現

一、思路

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

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

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

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

二、具體實現

2.1預測階段（先驗估計）

狀態預測：用陀螺儀積分預測角度

對應代碼：

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

協方差預測：更新估計的不確定性

先計算：

再加上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更新階段（后驗修正）

殘差（創新項）：觀測值與預測值的偏差

卡爾曼增益：權衡 “預測” 和 “觀測” 的可信度

對應代碼：

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

狀態更新：修正預測值，得到最優估計

對應代碼：

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

協方差更新：修正估計的不確定性

對應代碼：

  // 更新協方差矩陣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;

匿名上位機通信

一、為何用匿名上位機調試

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

二、通信幀格式