CW32L012对MPU6050的数据进行卡尔曼滤波

卡尔曼滤波（最优估计）

一、什么是卡尔曼滤波

通俗理解卡尔曼滤波：一个「会自动调权重的裁判」

卡尔曼滤波解决的核心问题是：你有两个途径（测量和预估得到）知道同一个东西的状态，但这两个途径都不准，卡尔曼帮你算出最准的结果

最优估计公式：

二、为什么这里要用卡尔曼

下面是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 模型会同步倾斜、转向，曲线会实时跟踪欧拉角数值变化，你能快速判断卡尔曼融合后的姿态解算是否准确（如零漂是否被抑制、动态晃动时欧拉角是否无明显跳变），无需逐行核对数字。

二、通信帧格式

初始化L012串口，并根据通信格式编写匿名上位机的通信驱动：

程序实际现象

【CW32L012解算MPU6050的姿态数据并进行卡尔曼滤波的方法-哔哩哔哩】

https://b23.tv/2GZqcIg

一、开发板现象：

OLED屏幕实时显示解算的三轴角度

二、匿名上位机现象

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