4、滤波器选择：卡尔曼滤波 vs 粒子滤波在变换中的应用
说到坐标变换的精度提升，滤波器这块是绕不开的坎。我这些年做机器人定位，跟卡尔曼滤波和粒子滤波打了无数次交道。说白了，这两个滤波器就像工具箱里的两把扳手——各有各的用处，选错了，拧螺丝都能拧出问题来。
4.1 卡尔曼滤波：线性系统的老朋友
卡尔曼滤波，我习惯叫它KF。这玩意儿在坐标变换里用得最多。为什么？因为它快，而且在线性高斯系统里，它就是最优解。
我记得刚入行那会儿，做一个AGV小车的定位。轮式里程计加上IMU，用卡尔曼滤波做融合，效果出奇的好。你想想看，小车在平地上跑，运动模型基本是线性的，噪声也接近高斯分布——这不就是KF的舒适区吗？
核心思想：卡尔曼滤波通过预测-更新两个步骤，不断修正状态估计。在坐标变换中，状态通常就是位置(x, y, θ)和速度(vx, vy, ω)。
来个简单的代码示例，展示KF在2D坐标变换中的预测步骤：
// 卡尔曼滤波预测步骤 - 2D坐标变换
void predict(Eigen::Vector3d& state, Eigen::Matrix3d& cov,
double dt, double v, double omega) {
// 状态：x, y, theta
// 控制输入：线速度v, 角速度omega
double theta = state(2);
state(0) += v * cos(theta) * dt; // x预测
state(1) += v * sin(theta) * dt; // y预测
state(2) += omega * dt; // 角度预测
// 雅可比矩阵 - 线性化
Eigen::Matrix3d F;
F << 1, 0, -v * sin(theta) * dt,
0, 1, v * cos(theta) * dt,
0, 0, 1;
// 协方差更新
cov = F * cov * F.transpose() + Q; // Q是过程噪声
}嗯，这里要注意一点。上面的代码里我用了雅可比矩阵做线性化，这其实是扩展卡尔曼滤波(EKF)的做法。纯线性KF不需要这一步，但实际中坐标变换很少是完全线性的，所以EKF更常见。
4.2 粒子滤波：非线性非高斯的救星
粒子滤波，PF，这玩意儿我第一次用是在一个室内定位项目里。当时用UWB做测距，信号经常被人体遮挡，噪声分布根本不是高斯——卡尔曼滤波直接崩了。换成粒子滤波，问题迎刃而解。
为什么会这样？因为粒子滤波不假设噪声分布，它用一堆粒子来近似后验概率。说白了，就是暴力枚举所有可能的位置，然后根据观测值给每个粒子打分。
我的经验：粒子数一般取500-2000个。太少，精度不够；太多，实时性扛不住。我曾经试过10000个粒子，结果CPU直接拉满，帧率掉到个位数。
粒子滤波的核心步骤，我整理成了一张流程图：
4.3 怎么选？一张表说清楚
我经常被问到：到底用KF还是PF？其实没有标准答案，得看你的场景。下面这张表是我多年总结的经验：
| 对比维度 | 卡尔曼滤波(KF/EKF) | 粒子滤波(PF) |
|---|---|---|
| 系统模型 | 线性或弱非线性 | 任意非线性 |
| 噪声分布 | 必须高斯 | 任意分布 |
| 计算复杂度 | O(n³)，n为状态维度 | O(N·n)，N为粒子数 |
| 实时性 | 高，适合嵌入式 | 低，粒子数多时卡顿 |
| 多模态支持 | 不支持 | 天然支持 |
| 典型应用 | 轮式里程计+IMU融合 | UWB定位、全局定位 |
避坑指南：我曾经在一个项目中，非要用卡尔曼滤波处理多模态分布的场景。结果滤波器一直在两个峰值之间震荡，定位精度惨不忍睹。后来换成粒子滤波，问题才解决。所以记住——如果你的后验分布可能是多峰的，别犹豫，上粒子滤波。
4.4 实战中的选择策略
说了这么多，到底怎么选？我一般按这个思路来：
- 先看系统模型：如果运动模型和观测模型都是线性的，噪声也接近高斯——直接上KF，别折腾。
- 再看计算资源：嵌入式平台、算力有限，优先考虑EKF。PC端或者有GPU加速，PF可以上。
- 最后看精度需求：如果要求厘米级精度，且环境复杂（多路径、遮挡多），PF更靠谱。
嗯，还有一个技巧。我有时候会用混合方案——先用粒子滤波做全局定位，收敛到单峰后切换成卡尔曼滤波做跟踪。这样既保证了全局收敛性，又保证了实时性。不过这个切换逻辑要设计好，不然容易出问题。
小提示：如果你用的是ROS，可以试试robot_localization包。它提供了EKF和UKF的实现，配置起来很方便。但粒子滤波的话，我建议自己写，因为场景差异太大，通用包很难满足需求。
最后说一句，滤波器没有银弹。我见过有人把粒子滤波吹上天，也见过有人死磕卡尔曼滤波。但说白了，工具是死的，人是活的。理解每个滤波器的假设和局限，才能在实际项目中做出正确的选择。
可可精品资料库