本文全面拆解卡尔曼滤波算法，从核心思想到落地应用，为工程开发与理论学习提供完整参考。首先明确卡尔曼滤波“融合预测与观测”的递归本质，详细分析其“高效实时处理、高斯假设下最优估计、多源信息融合”等优点，同时指出“线性高斯依赖、模型参数敏感、高维计算复杂”等局限性，帮读者精准判断算法适用场景。
卡尔曼滤波算法的核心思想是融合预测和观测，通过递归的方式，在存在不确定性的情况下，计算最优的估计。
1. 卡尔曼滤波的优缺点
优点：
- 高效递归：计算量小，只需当前时刻的估计和测量，无需保存大量历史数据，适合实时处理。
- 最优估计：在系统线性且噪声为高斯的假设下，能提供统计意义下的最优估计（最小方差估计）。
- 处理噪声：能有效处理系统和测量中的随机噪声。融合多源信息：天然适合多传感器数据融合，能综合不同传感器的优点，提供更鲁棒的估计。
局限性：
- 线性与高斯假设：标准的卡尔曼滤波要求系统动态模型和观测模型是线性的，且过程噪声和观测噪声均为高斯白噪声。实际系统常存在非线性。
- 模型依赖性：滤波效果严重依赖于系统模型（F, H）和噪声统计特性（Q, R）的准确性。这些参数若设置不当，会导致估计偏差甚至发散。
- 计算复杂度随状态维数增加：对于高维状态空间，矩阵运算的计算量会显著增加
2. 卡尔曼应用方向
由于卡尔曼滤波的计算量小，所以在实时性的场景下应用较为广泛。
| 应用领域 | 具体应用场景 | 卡尔曼滤波的作用 |
| 目标追踪 | 雷达、视频监控、无人驾驶中的车辆 / 行人跟踪 | 预测目标下一时刻位置，融合多传感器（如雷达、摄像头）数据，平滑轨迹，估计更准确的位置、速度、加速度。 |
| 电机控制与滤波 | 伺服电机、机器人关节电机、无人机旋翼控制（如 STM32 测电机速度） | 滤除编码器反馈信号中的噪声，更准确地估计电机转速和位置；结合电流传感器等，实现更精密的状态观测和控制。 |
| 导航与定位 | 无人机、自动驾驶汽车、机器人、手机导航 | 多传感器融合的核心算法。例如，融合 GPS（绝对位置但更新慢、有噪声）和 IMU（惯性测量单元，高频但误差累积）数据，提供连续、高精度的位置、速度和姿态估计。 |
| 信号处理与经济预测 | 去除信号中的噪声、股票价格预测、经济指标分析 | 从含噪声的时间序列数据中提取真实趋势，或基于历史数据进行短期预测。 |
| 航空航天 | 卫星轨道确定、导弹制导、飞行器姿态控制 | 估计飞行器的精确位置、速度和姿态，是导航和控制系统的重要组成部分。 |
3. 卡尔曼滤波实例展示
卡尔曼滤波计算量小、效果明显和无需保存历史数据，适合实时信号处理。下图为某次电机运转后，电机速度的原始图像和卡尔曼滤波后的曲线图（橘红色为滤波后曲线，蓝色为原始曲线）。
4. 卡尔曼滤波原理，由以下五个核心公式组成：
1. 状态方程预测
2. 误差协方差预测
3. 增益计算
4. 状态方程更新
5. 误差协方差更新
根据上述公式编写 C 语言代码如下：
float Kalman_Filter(Kalman* p,float dat)
{if(!p) return 0;
p->X =p->A*p->X_last;
p->P = p->A*p->P_last+p->Q;
p->kg = p->P/(p->P+p->R);
p->X_now = p->X+p->kg*(dat-p->X);
p->P_now = (1-p->kg)*p->P;
p->P_last = p->P_now;
p->X_last = p->X_now;
return p->X_now;
}Kalman 结构体中变量解释：
A: 状态转移矩阵（或系数），表示状态如何随时间变化。
Q: 过程噪声协方差，表示系统模型的不确定性。
R: 观测噪声协方差，表示传感器测量的不确定性。
X_last: 上一时刻的后验状态估计 (k-1|k-1)。
P_last: 上一时刻的估计误差协方差 (k-1|k-1)。
X: 当前时刻的先验状态预测 (k|k-1)。
P: 当前时刻的预测误差协方差 (k|k-1)。
kg: 卡尔曼增益 (Kk)，决定了我们更相信预测还是测量。
X_now: 当前时刻的后验状态估计 (k|k)，即最终的滤波输出。
P_now: 当前时刻的估计误差协方差 (k|k)。
5. Kalman 滤波和 C 代码对应解释
（1）预测阶段 (Prediction)
p->X =p->A*p->X_last;解释：这行代码是状态预测，基于上一状态和系统模型预测当前状态。它利用系统的状态转移模型（矩阵 A）和上一时刻的最优估计（X_last）来预测当前时刻系统的状态（X）。这里没有控制输入（BU(k)），所以公式简化为 A * X_last。
p->P = p->A*p->P_last+p->Q; 解释：这行代码是误差协方差预测方程。它预测了当前状态估计（X）的不确定性（P）。A * P_last 表示不确定性随着状态预测而传递，Q 则增加了系统过程噪声（模型不精确性）带来的不确定性。在代码的一维形式中，A’ (A 的转置) 就是 A。
（2）更新阶段 (Update)
p->kg = p->P/(p->P+p->R); 解释：这行代码计算卡尔曼增益（kg）。卡尔曼增益是一个权衡因子，决定了我们应该更相信预测值还是测量值。
- 如果观测噪声 R 很大（传感器不可靠），分母变大，kg 会变小，意味着滤波器会更信任预测值。
- 如果预测不确定性 P 很大（模型不准确），分子变大，kg 会接近 1，意味着滤波器会更信任测量值。
- 在一维情况下，观测矩阵 H 通常为 1，所以公式简化为 P /(P + R)。
p->X_now = p->X+p->kg*(dat-p->X); 解释：状态更新方程，也是卡尔曼滤波的核心，使用观测值修正预测值，得到当前时刻的最优状态估计。它通过将预测值（X）和测量值（dat）进行加权融合，得到当前时刻的最优状态估计（X_now）。(dat-p->X) 是测量残差（或新息），即实际测量值与预测值之间的差异。卡尔曼增益 kg 作为权重，决定了这个差异中有多少信息应该被吸收到最终的估计中。在一维情况下，H 为 1。
p->P_now = (1-p->kg)*p->P; 解释：这行代码是误差协方差更新方程。在融合了测量信息后，我们对系统状态的估计变得更加精确，因此不确定性会减小。(1-p->kg) 正反映了这种不确定性的降低。在一维情况下，单位矩阵 I 就是 1。
（3）为下一次迭代做准备
p->P_last = p->P_now;p->X_last = p->X_now;
解释：将当前时刻的后验估计（X_now）和后验误差协方差（P_now）传递下去，作为下一时刻迭代的“上一时刻”的先验值。这样，滤波器就可以递归地运行下去。
6. 总结
一维卡尔曼滤波器通过预测和更新两个步骤，递归地融合预测（模型）和测量（传感器）信息，从而在存在不确定性的情况下得到系统状态的最优估计。
- 调整参数 Q 和 R 是使滤波器性能最优的关键：
- 过程噪声协方差 Q：表示你对系统模型的信任程度。如果模型越不精确，Q 应设置得越大，这会使滤波器更依赖于测量值。
- 观测噪声协方差 R：表示你对传感器测量的信任程度。如果传感器噪声越大，R 应设置得越大，这会使滤波器更依赖于预测值。
- 通常需要通过实验和经验来调整 Q 和 R，以达到最佳的滤波效果。
