卡尔曼滤波
一个经典的例子：估算房间的温度
- 你有一个预测模型（不太准）：比如，你根据昨天的经验，预测现在房间温度是23℃。但这个预测是有不确定性的，可能误差在±2℃（即方差较大）。因为你不知道是否有人开了空调。
- 你有一个温度计（有噪声）：你用一个温度计测量，读数是25℃。但这个测量也是有不确定性的，因为温度计本身有误差，可能误差在±1℃（即方差较小）。
- 最佳估计是多少？
- 完全相信预测：23℃
- 完全相信测量：25℃
- 卡尔曼滤波的做法：它会像一个“聪明的裁判”，看谁更可靠。因为测量（25±1℃）的不确定性比预测（23±2℃）的小，所以裁判会更相信测量值。
- 最终的最佳估计值不会是简单的23或25，而是在25附近，比如24.8℃。这个值是通过数学上的加权平均得到的，权重与不确定性（方差）成反比。谁更确定（方差小），谁的权重就大。
卡尔曼滤波就是一个不断进行“预测-测量-加权融合”的循环过程，通过利用测量值来修正预测值，得到最优估计。
一、理论基础与假设
卡尔曼滤波建立在以下几个关键假设之上：
线性系统：系统的状态转移模型和观测模型必须是线性的。
高斯噪声：过程噪声（模型误差）和观测噪声均为加性高斯白噪声（Additive White Gaussian Noise, AWGN）。
马尔可夫性：当前状态仅依赖于上一时刻的状态，与更早的历史无关。
由于过程和观测噪声是高斯的，并且模型是线性的，因此状态估计值的概率分布也是高斯的，可以完全由其均值（mean） 和协方差（covariance） 来描述。卡尔曼滤波实质上就是在递归地更新这个高斯分布的均值和协方差。
二、系统模型
卡尔曼滤波针对的是离散时间线性动态系统，其模型由两个方程定义：
状态方程（过程模型）：
观测方程（测量模型）：
三、算法核心
卡尔曼滤波算法是一个递归过程，每次迭代分为两个步骤：预测步（Propagation Step）和更新步（Correction Step）。
1. 预测步（先验估计）
在获得 k 时刻的观测Zk 之前，我们基于 k-1 时刻的最优估计和系统模型来预测 k 时刻的状态。
先验状态估计：
这里 是在融合新观测之前的预测值，称为先验估计。这是一个确定性的预测，但是忽略了过程噪声
，因无法预测下一刻的噪声具体值
先验估计协方差：
表示了先验状态估计的不确定性。它由两部分组成：上一时刻不确定性
通过系统模型Fk的传播，当前过程噪声Qk引入不确定性；
2. 更新步（后验估计）
在获得 k 时刻的观测Zk 后，我们用它来修正预测值，得到更精确的估计。
计算卡尔曼增益：
卡尔曼增益Kk是算法的核心。它是一个权重矩阵，决定了我们应该在多大程度上相信预测值还是观测值。
如果观测噪声Rk→0（传感器非常精确），则，这意味着更新项
权重很大，更相信观测；
如果先验估计的不确定性（模型预测非常精确），则
，
更新项的权重很小，我们更相信预测。
后验状态估计（融合）：
这是一个修正公式，其中被称为新息（Innovation）或残差，即观测的实际值与预测的观测值之间的差异。算法将先验估计加上加权后的新息，得到后验估计
，这是在最小均方误差意义下的最优估计。
后验估计协方差：
这个公式更新了状态估计的不确定性。由于融入了新的观测信息，后验估计的不确定性总是小于先验估计的不确定性
3、相关代码（一阶）
.h头文件
#ifndef _KALMAN_FILTER_H_
#define _KALMAN_FILTER_H_typedef struct {float x; // 估计值float P; // 估计协方差float Q; // 过程噪声协方差 (系统不确定性)float R; // 测量噪声协方差 (传感器噪声)float K; // 卡尔曼增益
} KalmanFilter;void kalman_Init(KalmanFilter *kf, float init_x, float init_P, float Q, float R);
float kalman_Update(KalmanFilter *kf, float z);#endif .c文件
#include <stdio.h>typedef struct {float x; // 压力估计值float P; // 估计协方差float Q; // 过程噪声协方差 (系统动态不确定性)float R; // 测量噪声协方差 (传感器噪声)float K; // 卡尔曼增益
} KalmanFilter;// 初始化
void Kalman_Init(KalmanFilter *kf, float init_x, float init_P, float Q, float R)
{kf->x = init_x;kf->P = init_P;kf->Q = Q;kf->R = R;kf->K = 0.0f;
}// 更新函数 (输入：传感器测量值)
float Kalman_Update(KalmanFilter *kf, float z)
{// 预测kf->P = kf->P + kf->Q;// 计算卡尔曼增益kf->K = kf->P / (kf->P + kf->R);// 更新估计值kf->x = kf->x + kf->K * (z - kf->x);// 更新协方差kf->P = (1 - kf->K) * kf->P;return kf->x;
}主函数测试代码：
int main() {KalmanFilter kf;// init_x=0, init_P=1, Q=0.05, R=2.0// Q 越大响应越快，R 越大越平滑Kalman_Init(&kf, 0.0f, 1.0f, 0.05f, 2.0f);// 模拟气压传感器数据 (含噪声)float pressure_samples[20] = {5.1, 5.4, 6.0, 6.2, 6.1, 5.9, 7.0, 7.2, 7.1, 6.8, 6.5, 6.6, 8.0, 8.1, 8.2, 8.0, 7.9, 7.7,7.5, 7.3};for (int i = 0; i < 20; i++) {float filtered = Kalman_Update(&kf, pressure_samples[i]);printf("原始压力: %.2f, 滤波后: %.2f\n", pressure_samples[i], filtered);}return 0;
}
在测试数据中会出现滤波结果比原始值压力明显偏低，上升速度较慢等问题需要我们会调节相关参数，设定的初始值Q、R、P，一开始初始值为 5.1但估计值为1.76说明，系统模型一开始就不相信测量值，表明设定的P太小或R太大;
修改参数如下：
kalman_Init(&kf, 0.0f, 100.0f, 0.2f, 1.0f);
四、总结与特点
- 递归性：它不需要历史数据，只需要上一时刻的状态和当前的测量值，非常节省内存和计算资源，适合实时应用。
- 最优性：在假设系统模型和噪声均为高斯白噪声的前提下，卡尔曼滤波提供了统计意义下的最优估计（最小均方误差估计）。
- 适应性：通过调节 Q 和 R 这两个关键参数，可以动态调整对模型和传感器的信任度。
- 模型很准，传感器很烂：调小 Q，调大 R。滤波器会更依赖模型。
- 模型很烂，传感器很准：调大 Q，调小 R。滤波器会更依赖传感器。
- 应用领域：从阿波罗登月舱的导航到现在的无人机、自动驾驶汽车、机器人、股票预测、信号处理等，凡是需要从不确定信息中估计最优状态的领域，都有它的身影。