4、经典算法详解：卡尔曼滤波、扩展卡尔曼滤波、粒子滤波、交互多模型
好，咱们进入正题。这一章我打算聊聊导弹跟踪里最核心的四个算法。说实话，我在这个领域摸爬滚打十几年，这四个算法几乎贯穿了我所有的项目。你想想看，从雷达捕获目标到最终命中，中间全靠这些算法在撑着。
我个人习惯把这类算法分成两派：一派是“解析派”，像卡尔曼滤波这种，有明确的数学公式；另一派是“模拟派”，像粒子滤波，靠一堆粒子去逼近真实状态。各有各的脾气，咱们一个一个说。
4.1 卡尔曼滤波（KF）—— 线性系统的黄金标准
卡尔曼滤波，说白了就是一套“预测+修正”的递归流程。我当年第一次接触它时，觉得这玩意儿简直像魔术——明明只有一堆带噪声的测量值，它却能给你一个相当靠谱的状态估计。
它的核心思想其实很简单：
- 预测阶段：根据上一时刻的状态，预测当前时刻的状态和误差协方差
- 更新阶段：用当前的测量值去修正预测值，得到最优估计
数学上就五个公式，我写给你看：
// 预测
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嗯，这里要注意：卡尔曼滤波只适用于线性系统。什么意思？就是你的状态转移和测量关系都必须是线性的。我在项目中遇到过有人拿它去处理非线性问题，结果发散得一塌糊涂。所以，用之前先问问自己：你的系统是线性的吗？
适用场景：匀速直线运动、匀加速运动等线性模型。
嵌入式实现要点：矩阵运算尽量用定点数，避免浮点开销。我一般会把协方差矩阵P初始化为单位矩阵乘以一个较大的数，比如100，这样收敛会快一些。
我的小技巧：在嵌入式平台上，矩阵求逆是个坑。对于2x2或3x3的矩阵，直接手写公式求逆，比调用库函数快得多。我曾经在STM32上做过测试，手写求逆比库函数快了将近3倍。
4.2 扩展卡尔曼滤波（EKF）—— 非线性系统的妥协方案
现实世界哪有那么多线性系统？导弹跟踪目标时，目标可能做机动转弯，雷达测量也有非线性关系。这时候，扩展卡尔曼滤波就派上用场了。
EKF的思路很直接：把非线性函数线性化。怎么线性化？用泰勒展开，取一阶近似。说白了，就是在当前估计点附近，用切线代替曲线。
我举个例子。假设你的状态转移是非线性的：
x_k = f(x_{k-1}) + w_k那么EKF的做法是：
- 计算雅可比矩阵 F_k = ∂f/∂x，在 x_{k-1} 处求值
- 然后用这个 F_k 代替标准KF中的 F
- 测量更新同理，计算 H_k = ∂h/∂x
听起来简单吧？但坑就在这里。我曾经在一个项目中，因为雅可比矩阵算错了符号，导致滤波器直接发散。排查了整整两天才找到问题。所以我的建议是：雅可比矩阵一定要手算一遍，然后用数值微分验证。
注意：EKF的线性化误差在强非线性系统中会被放大。如果系统非线性太强，EKF可能还不如不滤波。我见过最夸张的情况，EKF的估计误差比原始测量噪声还大。
EKF的代码实现和KF很像，只是多了雅可比矩阵的计算。这里我贴一段伪代码：
// 预测
x_pred = f(x_prev) // 非线性函数
F = jacobian_f(x_prev) // 计算雅可比
P_pred = F * P_prev * F^T + Q
// 更新
H = jacobian_h(x_pred)
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_pred4.3 粒子滤波（PF）—— 暴力美学
说到粒子滤波，我脑子里第一个蹦出来的词就是“暴力”。它不跟你讲什么线性化、高斯假设，直接扔一堆粒子去模拟状态分布。你想想看，当系统是非线性、非高斯的时候，卡尔曼家族基本歇菜，但粒子滤波照样能打。
粒子滤波的核心流程：
- 初始化：生成N个粒子，每个粒子代表一个可能的状态
- 预测：每个粒子按照状态转移方程向前推一步，加上过程噪声
- 更新：根据测量值，计算每个粒子的权重（似然）
- 重采样：根据权重重新采样，权重高的粒子多复制几份，权重低的淘汰掉
- 估计：所有粒子的加权平均就是最终的状态估计
我当年第一次实现粒子滤波时，选了1000个粒子，结果在DSP上跑得跟蜗牛一样。后来优化到200个粒子，效果也还行。所以这里有个经验：粒子数不是越多越好，够用就行。一般50-200个粒子在嵌入式平台上比较合适。
粒子滤波的优势：可以处理任意分布，包括多峰分布。这在目标跟踪中特别有用——比如目标可能出现在两个不同的位置，粒子滤波能同时跟踪这两种可能性。
劣势：计算量大，粒子退化问题（大部分粒子权重趋近于0）。
避坑指南：我曾经在重采样环节踩过坑。如果直接用轮盘赌法，会导致粒子多样性丧失。后来改用系统重采样（Systematic Resampling），效果好了很多。另外，建议加入一个“有效粒子数”的监控，当有效粒子数低于阈值时再触发重采样，而不是每步都重采样。
4.4 交互多模型（IMM）—— 对付机动目标的利器
最后一个，也是我个人最喜欢的——交互多模型。为什么喜欢？因为它解决了卡尔曼滤波的一个致命弱点：模型单一。
你想想看，目标可能一会儿匀速直线，一会儿急转弯，一会儿加速逃逸。你用单一模型去跟踪，肯定跟不上。IMM的思路是：同时跑多个模型，每个模型对应一种运动模式，然后根据测量值动态调整每个模型的权重。
IMM的流程大致如下：
- 输入交互：根据模型转移概率，混合各模型的估计值
- 模型滤波：每个模型独立运行自己的滤波器（可以是KF、EKF或PF）
- 模型概率更新：根据每个模型的似然，更新模型权重
- 输出融合：所有模型的加权平均作为最终输出
我一般用三个模型：匀速模型（CV）、匀加速模型（CA）、协同转弯模型（CT）。这三个基本能覆盖大部分机动情况。
| 模型 | 状态向量 | 适用场景 |
|---|---|---|
| CV（匀速） | [x, y, vx, vy] | 巡航阶段 |
| CA（匀加速） | [x, y, vx, vy, ax, ay] | 加速/减速 |
| CT（协同转弯） | [x, y, v, θ, ω] | 转弯机动 |
注意：IMM的计算量是单个模型的N倍（N是模型数量）。在嵌入式平台上，我建议最多用3-4个模型。模型转移概率矩阵的设定也很关键，我一般设对角线元素为0.8-0.9，非对角线元素均分剩下的概率。
最后说一句，这四个算法没有绝对的优劣。我个人的选择标准是：
- 线性系统、计算资源紧张 → 卡尔曼滤波
- 弱非线性、资源适中 → 扩展卡尔曼滤波
- 强非线性、非高斯、资源充裕 → 粒子滤波
- 目标机动性强、需要多模式跟踪 → 交互多模型
嗯，这一章就到这里。下一章我会讲讲这些算法在嵌入式平台上的具体移植技巧，包括定点化、内存优化、实时性保障这些实战内容。到时候见。