前言

我之前跟着B站Up主DR_CAN学习过卡尔曼滤波，B站的学习视频请参考此链接。尊重Up主的版权，特此申明，里面的内容是学习这个视频的笔记，大家可以看看这个视频，对照我的笔记更好的理解和学习。

本文记录了详细的笔记，里面包含公式的推导和个人的理解，涉及的数学理论比较多，比如说概率统计，矩阵的计算等。从浅显的实际问题出发，一步步推到公式，最后得出卡尔曼滤波的五个核心公式。最后给出平衡车项目中，滤除电机转速噪声的C语言代码。相信对大家有所启发。

一、笔记正文

由于第一个视频的内容我没有记录笔记，现在将其整理在这里，供大家学习。

1.0 简要记录：递归算法

1.0.1 实际问题：测量硬币的直径

现在有一把尺子，量硬币的直径，测量了很多次，将结果记录下来，由于尺子有误差，测量人员在测量过程中也存在误差，所以每次的结果不一定相同。最后的估计值就是多次测量的平均值。

公式推导到最后一步，已经有递归的影子了。这次的估计值是上一次的估计值加上这次的测量值减去上次的估计值再乘以k分之一。
随着k的增大，估计值趋于稳定，再测量下去就意义不大了。

1.0.2 卡尔曼增益

将上面公式里，后面的系数1/k写成K_k，就是得到了下面的公式。其中K_k就是卡尔曼增益。

之后就有估计误差、测量误差和卡尔曼增益的关系。当估计误差远大于测量误差，那么我们更相信测量值；当估计误差远小于测量误差，我们就更相信估计值。

1.0.3 卡尔曼滤波思想的应用——估计物体长度

1.0.3.1卡尔曼滤波思想实践的步骤

• 第一步是，计算卡尔曼增益K_k。公式上面已经写过了。
• 第二步是，计算这一次的估计值，等于上一次的估计值，加上卡尔曼增益乘以这次的测量值减去上一次的估计值。
• 第三步是，更新估计误差。

假定物体实际长度是50mm，尺子测量之前，我们人为估计的结果是40mm，随便定了个估计误差等于5mm。之后就用尺子测量，测量结果写为Z_k，比如第一次是Z₁等于51mm，测量误差是3mm（这个是尺子决定的）。
之后写表格：

• 第一行是估计值和估计误差。
• 第二行，写测量值51，测量误差是3。之后计算卡尔曼增益，得出0.625，写进去，再计算估计值得到46.875，写进去，最后更新估计误差，等于1.875，写在最后一列。
• 第三行重复第二行的步骤，一直写下去……

最后不断测量，更新估计值，最终的估计值会趋近真实值。

1.1 由实际问题引出数据融合

如果有两个天平对同一物体进行称重，天平1测得结果是30克，他的标准差是2克，也就是说，物体的质量在28-32克的概率是68.4%；另一个天平的测量结果是32克，他的标准差是4克，也就是说，物体的质量在28-36克的概率是68.4%。两个天平都没有那么准，如何根据这两个天平的测量结果估算出最可靠的物体质量？

1.2 感悟

这个问题让我们看出来，即便一个天平不准，但他的测量结果不代表不重要！加入他的测量结果，对实际值的估算有帮助的。这个世界，没谁能保证自己绝对正确，都有一定的置信区间，多分析别人的意见，会让我们离真实值更近一步。

1.3 协方差矩阵

协方差矩阵让我们清晰的看出来两个变量之间的线性相关性，也就是说，它们二者的变化会不会有一定的关联。世间万物，总是有千丝万缕的联系，我的一个举动，会影响到别人的很多方面。同样，我也在受别人的影响，那么影响的大小怎么确定？

1.4 由状态空间表达引出卡尔曼滤波器

二、C语言代码

下面的代码是对GB37电机（带有编码器）的电机转速测量的卡尔曼滤波处理，实际测量效果显示，除去了噪声，转速测量更平滑。

// 卡尔曼滤波参数结构体
typedef struct 
{
    float omega_hat; // 估计的角速度 (rad/s)
    float P;         // 状态协方差
    float Q;         // 过程噪声协方差
    float R;         // 测量噪声协方差
} KalmanFilter;

// 左右车轮的卡尔曼滤波实例
KalmanFilter left_wheel = {0.0, 1.0, 0.005, 1.0};  // 初始值：omega_hat=0, P=1, Q=0.005, R=1.0
KalmanFilter right_wheel = {0.0, 1.0, 0.005, 1.0}; // 初始值同上

// 卡尔曼滤波函数
void kalman_filter(KalmanFilter *kf, float z) {
    // 预测
    float omega_pred = kf->omega_hat;
    float P_pred = kf->P + kf->Q;

    // 更新
    float K = P_pred / (P_pred + kf->R); // 卡尔曼增益
    kf->omega_hat = omega_pred + K * (z - omega_pred);
    kf->P = (1 - K) * P_pred;
}

/*实际的函数调用*/
void GetMotorVelocity(void) 
{
	kalman_filter(&left_wheel, VelocityMotor.angular_velocity_left); // kalman滤波
	VelocityMotor.angular_velocity_left = left_wheel.omega_hat;
	
	kalman_filter(&right_wheel, VelocityMotor.angular_velocity_right); // kalman滤波
	VelocityMotor.angular_velocity_right = right_wheel.omega_hat;
}

结语

本文包含了卡尔曼滤波的推导过程，最后得出五个核心公式，最后给出平衡车项目中，测量电机转速的C语言代码。我这里还有一份详细的PDF笔记，也是同样的课程，大家点赞、收藏、关注，私信我免费获取。

---

本文结束，更多干货文章即将为大家奉上。