4、感知融合基础：卡尔曼滤波(KF)原理、扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)、粒子滤波(PF)简介
聊到定位漂移，就绕不开感知融合。说白了，传感器都有各自的脾气——GPS会飘，IMU会累积误差，视觉有时候会丢帧。怎么把这些“各有缺陷”的数据揉在一起，输出一个靠谱的结果？这就是滤波算法干的事。
我个人习惯把滤波算法分成两派：一派是“参数化”的，比如卡尔曼滤波及其变种；另一派是“非参数化”的，比如粒子滤波。今天咱们就把这四兄弟捋一遍。
核心思想：任何单一传感器都有局限性，融合的本质是用不同传感器的优势互相弥补，用数学方法把不确定性降到最低。
4.1 卡尔曼滤波（KF）—— 线性系统下的最优估计
卡尔曼滤波，我愿称之为“传感器融合的Hello World”。它假设系统是线性的，噪声是高斯分布的。在这个前提下，它是最优的。
它的核心逻辑就两步：预测和更新。
- 预测：根据上一时刻的状态，推测当前时刻的状态。比如，我知道上一秒车的位置和速度，就能猜出这一秒大概在哪。
- 更新：用当前传感器的观测值，去修正这个预测。比如GPS说你在A点，但预测说你在B点，那就取个加权平均。
这个“加权”的权重，就是卡尔曼增益。增益越大，越相信观测；增益越小，越相信预测。
// 伪代码示例：卡尔曼滤波核心步骤
// 预测
x_pred = F * x_prev + B * u // 状态预测
P_pred = F * P_prev * F^T + Q // 协方差预测
// 更新
K = P_pred * H^T * (H * P_pred * H^T + R)^-1 // 卡尔曼增益
x_est = x_pred + K * (z - H * x_pred) // 状态更新
P_est = (I - K * H) * P_pred // 协方差更新我的经验：调KF的时候，Q和R矩阵是最头疼的。Q是过程噪声，R是观测噪声。我刚开始做项目时，总想把Q设得很小，觉得模型很准，结果一遇到急转弯就崩了。后来学乖了——Q稍微给大一点，让滤波器“灵活”一些，反而更鲁棒。
4.2 扩展卡尔曼滤波（EKF）—— 对付非线性系统
现实世界哪有那么多线性系统？车的运动模型是非线性的，IMU的姿态更新也是非线性的。这时候KF就不好使了。
EKF的思路很简单：把非线性函数做一阶泰勒展开，近似成线性。说白了，就是求个雅可比矩阵，然后套用KF的框架。
嗯，这里要注意：EKF的近似精度取决于非线性程度。如果系统高度非线性，一阶近似误差会很大，滤波器可能直接发散。
// EKF 核心区别：计算雅可比矩阵
// 假设状态转移函数 f(x) 是非线性的
F = df/dx // 在预测点求雅可比矩阵
// 观测函数 h(x) 也是非线性的
H = dh/dx // 在预测点求雅可比矩阵
// 其余步骤与KF一致避坑指南：我曾经在一个项目中，用EKF做视觉SLAM的位姿估计。模型写得很复杂，雅可比矩阵推导了好几页纸。结果跑起来发现，只要转角稍微大一点，滤波器就震荡。后来排查发现，是雅可比矩阵在奇异点附近数值不稳定。所以——能用解析解就别用数值差分，能用UKF就别硬上EKF。
4.3 无迹卡尔曼滤波（UKF）—— 用采样点代替线性化
EKF的线性化近似，说白了有点“粗糙”。UKF换了个思路：我不去近似那个非线性函数，而是用一堆采样点（Sigma点）去逼近概率分布。
你想想看，一个高斯分布，我取几个关键点（比如均值附近和标准差附近），把这些点通过非线性函数传过去，再重新拟合一个高斯分布。这样既保留了非线性信息，又不用算雅可比矩阵。
我个人觉得，UKF是“性价比”最高的滤波算法。它比EKF更准，比PF更快，而且实现起来也不复杂。
// UKF 核心步骤：生成Sigma点
// n: 状态维度, lambda: 缩放参数
// 生成 2n+1 个Sigma点
X0 = x_mean
Xi = x_mean + sqrt((n + lambda) * P) // i = 1..n
Xi = x_mean - sqrt((n + lambda) * P) // i = n+1..2n
// 每个Sigma点通过非线性函数
Yi = f(Xi)
// 加权计算新的均值和协方差
y_mean = sum(Wi * Yi)
P_y = sum(Wi * (Yi - y_mean) * (Yi - y_mean)^T)我的习惯：在车载定位项目中，只要模型不是特别复杂，我优先用UKF。它不需要推导雅可比矩阵，代码维护成本低。而且对于中度非线性的系统（比如车辆运动模型+IMU预积分），UKF的精度和EKF差不多，但稳定性好很多。
4.4 粒子滤波（PF）—— 不假设任何分布
前面三种滤波，都假设状态服从高斯分布。但现实中有很多情况不满足这个假设——比如多峰分布、重尾分布。这时候，粒子滤波就派上用场了。
粒子滤波的核心思想：用一堆随机粒子（样本）来近似任意分布。每个粒子代表一个可能的状态，粒子的权重代表这个状态的可信度。
流程大概是：
- 初始化：撒一堆粒子，均匀分布或按先验分布
- 预测：每个粒子根据运动模型往前走一步，加一点随机噪声
- 更新：用观测值更新每个粒子的权重——越符合观测的粒子权重越大
- 重采样：权重大的粒子多复制几份，权重小的粒子淘汰掉
说白了，就是“优胜劣汰，适者生存”。
// 粒子滤波核心：重采样（Resampling）
// 输入：粒子集 {x_i, w_i}，其中 w_i 是归一化权重
// 输出：新的粒子集
// 系统重采样算法
cumsum = 0
for i = 1 to N:
cumsum += w_i
r = random(0, 1/N) // 均匀随机数
for j = 1 to N:
if r < cumsum:
new_particles[j] = x_i
break
r += 1/N避坑指南：粒子滤波的计算量跟粒子数成正比。我曾经在一个项目中用了10000个粒子，结果在嵌入式平台上跑不动，一帧要算200毫秒。后来压缩到2000个粒子，配合自适应重采样策略，才勉强跑到30帧。所以——粒子数不是越多越好，够用就行。
4.5 四种滤波算法的对比
| 算法 | 适用场景 | 优点 | 缺点 | 计算量 |
|---|---|---|---|---|
| KF | 线性高斯系统 | 最优解，实现简单 | 只能处理线性 | 低 |
| EKF | 弱非线性系统 | 成熟，应用广泛 | 需算雅可比，可能发散 | 中 |
| UKF | 中高度非线性系统 | 无需雅可比，精度高 | 参数调优略复杂 | 中 |
| PF | 非高斯、多峰分布 | 能处理任意分布 | 计算量大，粒子退化 | 高 |
4.6 知识体系图：滤波算法选型逻辑
这张图是我自己总结的选型逻辑。你想想看，拿到一个融合问题，先问自己三个问题：系统线性吗？噪声高斯吗？计算资源够吗？答案就出来了。
最后说一句：没有银弹。KF简单但局限，PF强大但昂贵。真正的高手，是能在不同场景下灵活切换，甚至组合使用。比如我在一个项目中，就用UKF做状态估计，用PF做重定位——各取所长。