目标跟踪算法全解析：从卡尔曼滤波到粒子滤波的MATLAB工程实践

1. 从贝叶斯滤波说起：所有跟踪算法的共同底座

在目标跟踪这个领域摸爬滚打久了，你会发现一个很有意思的现象：不管你是做无人机视角下的车辆跟踪，还是雷达数据里的空中目标监测，甚至是在做机器人SLAM时的位姿估计，最后都会落到同一个数学框架里——贝叶斯滤波。卡尔曼滤波器、扩展卡尔曼滤波器（EKF）、高斯滤波器、PHD滤波器、粒子滤波器，它们本质上都是贝叶斯滤波在不同假设条件下的具体形态。这句话我建议你反复读三遍，因为搞懂了这个底层逻辑，后面所有算法对你而言就不是孤立的知识点，而是一棵树的枝叶。

目标跟踪的核心问题用一句大白话概括就是：我有一堆带噪声的观测值（比如雷达测到的距离、角度，或者摄像头检测出的像素框），我要通过这些观测值去估计目标真实的状态（位置、速度、加速度），并且要尽量跟得上、跟得准。这里面的麻烦在于两个不确定性叠加：目标自己怎么运动是未知的（运动模型误差），传感器测出来的是不准的（观测噪声）。贝叶斯滤波的思路就是不跟不确定性硬刚，而是用一个概率分布来描述“目标在某个状态的可能性有多大”，每一次新的观测来了，就用它修正一下这个分布，让它越来越靠近真实状态。

这个框架有个递推公式，核心就两步：预测和更新。预测，就是用运动模型把上一时刻的状态分布往前提一步，得到先验分布；更新，就是拿到当前观测之后，用似然度去修正预测结果，得到后验分布。理解了这个“预测-更新”循环，你就抓住了所有跟踪算法的命门。卡尔曼滤波无非是假设状态分布是高斯分布、运动模型是线性的，于是每一步的预测和更新都能用闭式解算出来；EKF是卡尔曼滤波在非线性模型下的妥协方案，对非线性函数做一阶泰勒展开，硬是把问题拉回线性框架里求解；高斯滤波器则是一个更宽泛的家族，它的假设是状态分布始终是高斯分布，只是在如何处理非线性、如何处理目标维度上有不同的取舍。

PHD滤波器（概率假设密度滤波器）是个特殊的存在，它是为了解决多目标跟踪中的“数据关联”难题而生的。想象一下有5个目标、每帧回来8个量测，你要弄清楚哪个量测对应哪个目标，这个组合爆炸的关联问题在密集场景下几乎是无解的。PHD滤波器的聪明之处在于：它不关心具体哪个目标在哪个位置，它只关心“目标空间的概率密度”长什么样。通过对这个密度做一阶矩近似，用“密度积分之和等于目标数量期望”这一性质，直接把多目标跟踪变成了一个单目标空间上的密度递推问题，躲开了数据关联的泥潭。

粒子滤波则是另一条路——既然高维非线性系统里解析解拿不到，那我就用一堆带权重的离散样本点（粒子）去近似后验分布。粒子多的地方代表目标可能在哪里，权重大的粒子代表它有多可信。它最大的价值是突破了“高斯分布”这个假设束缚，在强非线性、非高斯噪声条件下依然能工作。代价就是计算量大，而且在高维状态空间里粒子数会指数增长，这就是所谓的维度灾难。

把这五种滤波器放在一起看，它们其实构成了一个完整的工具箱：在线性高斯问题上直接用卡尔曼，非线性不严重就用EKF，非线性强且分布非高斯就用粒子滤波，多目标场景上PHD滤波是一条新路。这篇文章我会用MATLAB代码把每个算法的核心逻辑串起来讲，不会照本宣科地贴大段源码，而是讲清楚每段代码背后对应的数学假设和工程取舍。适合刚入门目标跟踪的读者，也适合那些用工具箱调包调了很久、但遇到问题不知道怎么调参的工程师。读完以后，你能获得的最核心能力不是“我会用某个函数”，而是“这个场景下我该相信哪个算法、为什么”。

2. 线性高斯世界的黄金标准：Kalman滤波器的原理与MATLAB实现

2.1 卡尔曼滤波的五条黄金公式

卡尔曼滤波之所以在工程界统治了几十年，就是因为它把贝叶斯递推的“预测-更新”两步，落成了一套可以机械执行的矩阵运算。它的前提假设说起来其实挺苛刻的：系统是线性的、过程噪声和观测噪声都是零均值高斯白噪声、初始状态也是高斯分布的。在这些前提下，神奇的事情发生了——一个高斯分布经过线性变换之后仍然是高斯分布，而且高斯分布的均值和协方差就足以完整描述整个分布。所以卡尔曼滤波器里根本不需要维护一整个概率密度函数，只需要维护两个量：均值向量和协方差矩阵。

这五条公式请你务必背下来。第一条是状态一步预测：x_pred = F * x_prev，F是状态转移矩阵；第二条是协方差一步预测：P_pred = F * P_prev * F' + Q，Q是过程噪声协方差。这两条属于预测阶段。更新阶段首先算卡尔曼增益：K = P_pred * H' / (H * P_pred * H' + R)，这个增益本质上是“预测的不确定性”和“观测的不确定性”之间的一个权重折中。然后是状态更新：x_est = x_pred + K * (z - H * x_pred)，括号里那项叫新息，是实际观测和预测观测之间的误差。最后是协方差更新：P_est = (I - K * H) * P_pred。

用生活化的方式理解这套操作，你就把它想象成拼车导航。预测阶段就是你按既定路线往前推断，但你知道路况可能变，所以不确定性的圈越画越大（P_pred变大）。到了下一个有摄像头的位置（观测），导航会拿实际拍到的情况跟它预测的情况做比较，如果差得大就说明之前猜错了，导航会大幅修正路线认知（增益K偏大）；如果预测和观测差不多，说明路况和预期吻合，就小幅修正一下（K偏小）。更新完以后，不确定性圈缩小了（P_est变小），因为新信息进来了，你更确定自己在哪了。这个“预测让不确定性变大、更新让不确定性变小”的节奏就是Kalman滤波器的呼吸节律。

2.2 MATLAB代码逐行拆解：一维匀速运动跟踪

我直接写一段最经典的一维匀速运动目标跟踪代码，场景设定是：目标在一条直线上匀速运动，传感器每个周期测一次位置（带噪声）。这段代码虽然简单，但你把它吃透了，后面扩展成二维、三维甚至带加速度模型都是水到渠成的事。

% 系统参数设置 dt = 0.1; % 采样周期100ms F = [1 dt; 0 1]; % 状态转移矩阵：位置+速度 H = [1 0]; % 观测矩阵：只能测到位置 Q = [0.01 0; 0 0.01]; % 过程噪声协方差 R = 0.25; % 观测噪声协方差（测距标准差0.5m，方差为0.25） x = [0; 1]; % 初始状态：位置0m，速度1m/s P = eye(2); % 初始协方差矩阵，表示初始状态的不确定性 % 生成真实轨迹和观测数据 T = 10; % 仿真时长10秒 N = T/dt; % 总步数 truePos = zeros(1, N); meas = zeros(1, N); for k = 1:N x_true(1,k) = x(1) + x(2) * k * dt; % 真实位置 meas(k) = x_true(1,k) + sqrt(R) * randn; % 加噪声观测 end % 卡尔曼滤波主循环 x_est = zeros(2, N); for k = 1:N % 预测 x_pred = F * x(:, k); % 状态预测 P_pred = F * P * F' + Q; % 协方差预测 % 更新 K = P_pred * H' / (H * P_pred * H' + R); % 计算滤波增益 x(:, k+1) = x_pred + K * (meas(k) - H * x_pred); % 状态更新 P = (eye(2) - K * H) * P_pred; % 协方差更新 x_est(:, k+1) = x(:, k+1); end

我解释几个初学者特别容易懵的参数。Q的物理含义是“你有多相信运动模型”。Q越大，说明你认为模型本身的误差越大（比如目标会突然加速刹车），滤波器就会更信任观测值；Q取太小，滤波器就会顽固地坚持自己预测的轨迹，观测来了也不怎么改，结果就是响应迟钝、出现明显的滞后。R的物理含义则相反，是“你有多相信传感器”。R太小说明你认为传感器特准，那么滤波器会疯狂跟随观测，噪声会被大量代进估计结果里，轨迹变得毛糙。实践中Q和R的比值比它们的绝对值更重要，这个比值直接决定了卡尔曼增益的稳态水平。我见过很多人调不出来滤波效果，十有八九是Q和R的量级跟实际系统对不上。一个务实的做法是用传感器标定数据估算R，把观测噪声方差统计出来填入代码，再去调整Q，而不是两个参数一起瞎试。

这段代码还有一个关键点你必须注意：初始协方差P的设定。P越大，滤波前期就越相信观测；P取得很小，滤波器可能要跟真值跟好久才能收敛。实战中可以把P矩阵对角元设成比预期误差平方大一些的量级，比如位置协方差设为9，代表初始位置不确定度约3米，这个我实测下来收敛速度是比较理想的。

2.3 卡尔曼滤波的工程适用范围和隐藏限制

卡尔曼滤波器好归好，但它的适用范围其实很窄。很多人不注意就踩坑的点在于“线性”这个假设。实际工程里的传感器模型大多是非线性的：雷达测的是距离和方位角，想要得到目标在直角坐标系下的位置，这个过程本身就是个非线性变换；视觉SLAM里观测到的像素坐标跟3D世界坐标之间的关系也是非线性的。这种情况下硬套标准Kalman就有问题了，滤波器会给出看起来平滑、但实际偏差很大的轨迹。判断你的系统能不能用线性Kalman，你就看观测方程里会不会出现状态变量的平方、除法、三角函数这类操作，只要有，就说明这系统是非线性的，该考虑扩展卡尔曼了。

另一个隐藏限制是高斯假设。卡尔曼滤波要求所有噪声都是高斯分布的，如果传感器有粗大误差（比如雷达偶发产生巨大的野值，或者视频检测框突然匹配错误），这些异常值不服从高斯分布，卡尔曼滤波会被严重带偏。我遇到过的摄像头跟踪目标碰撞后被遮挡的情况，检测框一下跳到背景区域，就是典型的非高斯大偏差量测，标准Kalman几乎没有抵抗能力，轨迹硬生生被拉飞。后面我们会讨论怎么在工程上做野值剔除，但现在你要记住：Kalman不是万能的，它是一把精确但脆弱的螺丝刀，而你手里还需要锤子、改锥来应对不同的场景。

3. 非线性系统的妥协与进阶：EKF与Gaussian Filter家族

3.1 EKF到底做了什么妥协

扩展卡尔曼滤波器（EKF）的出现，本质上就是一句大白话：既然系统是非线性的，那我把非线性函数在当前估计点做一阶泰勒展开，只保留线性项，然后按照卡尔曼的框架来算。说得好听点叫“局部线性化”，说白了就是拿一条直线去近似一条曲线，在这个点附近误差很小，离远了误差就上来了。EKF的工作过程跟Kalman几乎一样，只是两个地方发生了变化：状态转移矩阵F不再直接给定了，而是要用f(x)对x求雅可比矩阵之后在当前估计点取值；观测矩阵H也要换成h(x)的雅可比矩阵。预测时用的是原始的非线性函数f和h，不是线性近似版，只是在算协方差传递的时候用了近似。

我举个典型的例子：用雷达跟踪目标，状态向量是三维位置和三维速度，而观测是距离和方位角（一个二维向量）。观测方程h(x) = [sqrt(px^2 + py^2 + pz^2); atan2(py, px)]，这里就有平方根和反正切，明显是非线性。计算雅可比矩阵H的时候，你会得到类似H = [px/r, py/r, pz/r, 0, 0, 0; -py/px^2/(1+py^2/px^2), 1/px/(1+py^2/px^2), 0, 0, 0, 0]这样一堆式子。很多教程直接把这个矩阵砸给你，导致初学者看到一堆导数就劝退了。其实你不需要手动推这些导数，MATLAB里可以用symbolic工具箱自动求导，或者用数值求导做近似。真正要理解的是：这个矩阵的含义是“当前状态估计点的观测函数斜率”，斜率越陡，说明观测对状态越敏感，同样的观测误差对状态修正的影响就越小。

还有一个实战经验我踩过坑：EKF要求你选一个好的线性化展开点。展开点选得准，近似就贴得近；选得偏，近似就严重失真，甚至导致滤波发散。什么意思呢？就是你先得有个靠谱的初始估计。初始位置要是距离真值好几公里远，这时候做线性化展开，切线可能跟原曲线偏差极大，第一次更新就会把估计拉向错误的方向，后面越滚越远。做EKF的工程初始化，宁可保守地扩大初始协方差，也不要随便拍脑袋设一个和真值差很远的初值。

3.2 Gaussian Filter家族：不止EKF一条路

说完EKF，我顺便把Gaussian Filter（高斯滤波器）这个大类概念理清楚。这个家族的共同核心信念是：不管系统怎么折腾，只要我每一次递推结束时强制把状态分布近似成一个高斯分布，那整套递推就能跑下去。EKF是其中最朴素的一支，它用线性化来保证“高斯经过非线性变换后依然是高斯”这个假象。但是线性化带来的误差使得EKF在非线性强的时候精度变得很差。

同一家族里另外两个成员你必须知道：无迹卡尔曼滤波（UKF）和容积卡尔曼滤波（CKF）。UKF的思想是：我不去线性化那个非线性函数，而是选择一组确定的采样点（Sigma点），让这些点的均值和协方差跟当前状态分布匹配，然后直接把每个点都丢进非线性函数里走一遍，再根据变换后点的分布来重构高斯分布的均值和协方差。这个方法不需要求导数，而且在二阶精度上优于EKF的一阶精度，实现起来也好用。CKF则是用球面径向规则选择采样点，在高维问题上的数值稳定性比UKF更好。

我个人的经验建议是：当非线性强度中等时，UKF往往比EKF更省心。因为EKF需要你推雅可比矩阵的解析表达式，稍不留意推错了，滤波效果就是一团乱麻。而UKF只需要你能把f(x)和h(x)写出来，哪怕里面嵌套了查表、插值等“数学上不光滑”的操作也能对付。当然UKF也有代价，比如计算量大了不少，对协方差矩阵的正定性要求高，数值上还容易出现负方差导致Cholesky分解失败。但如果你的系统维度不太高，UKF基本是比EKF更稳健的选择。

3.3 一个二维EKF目标跟踪例子

假设目标在二维平面上运动，状态包括位置和速度共四个量：x = [px; py; vx; vy]，观测是传感器测到的距离和角度。传感器在原点。我们写出观测方程：

% EKF观测函数：距离+方位角 function z = hfunc(x) px = x(1); py = x(2); z = [sqrt(px^2 + py^2); atan2(py, px)]; end % 计算雅可比矩阵（数值法或解析法） H = [px/sqrt(px^2+py^2), py/sqrt(px^2+py^2), 0, 0; -py/(px^2+py^2), px/(px^2+py^2), 0, 0];

实际跑EKF主循环时，预测部分用线性运动模型F，更新部分用上面计算出的H。这里我要提一个非常关键的细节：距离和角度两个观测值的量纲不一样，距离以米为单位可能是一百多，角度以弧度为单位只有零点几。在计算新息和增益的过程中，如果R矩阵里距离方差和角度方差的量级设置不当，距离主导更新，角度信息就被淹没了。这个在雷达目标跟踪里叫“距离/角度匹配问题”，常见解法是在R矩阵里做归一化处理，或者把状态方程改写成用距离、角度作为状态量，避免量纲失衡。

EKF在工程中暴露出来的最大毛病就是：当非线性严重、且初始误差大时，滤波容易发散。比如你做无人机降落时的地面目标跟踪，机载相机到目标的距离变化很快，视线角变化也剧烈，EKF的局部线性化会频繁失效。应对策略包括把滤波频率调到比目标动态快很多，让每帧之间的状态变化足够小，线性化误差自然就小了。另一个实用技巧是加一个简单的野值检测：如果新息的马氏距离大于某个阈值（例如3倍标准差），就认为这次观测是野值，跳过更新步骤只做预测。这个技巧虽小，但能挡住传感器偶尔的跳变和误检带来的灾难，我强烈建议你在所有滤波实现里都加上这么一层保护。

4. 多目标场景的利器：PHD滤波器原理与SMC-PHD实现

4.1 先搞明白多目标跟踪为什么难

单目标跟踪的困难就是噪声和不确定性，但是多目标跟踪要面对的核心难题，是数据结构里多了一层“关联”的维度。假设现在画面里有3个目标，雷达每帧打回来5个点，你怎么知道哪一个点属于哪一个目标？也许某个点其实是目标1的雷达回波，另外两个点是目标2旁边的杂物，还有一个点完全就是噪声。传统的方法，比如最近邻关联（NN）、联合概率数据关联（JPDA）、多假设跟踪（MHT），本质上都在做同一件事：想办法给每个量测分配一个“管理标签”，跟哪个目标最匹配就归给谁。但问题来了，目标数量一多、杂波密度一大，关联组合呈指数级增长，算法跑不动、误配率高，系统变得极不稳定。

PHD滤波器从根上换了一种哲学。它的名字叫“概率假设密度”，你可以把每个目标想象成空间里的一团“质量”，PHD滤波估计的对象不是目标本身，而是目标在全空间上的平均强度函数。这个强度函数的物理意义非常直观：如果你在某个区域对PHD做积分，得到的数值就代表这个区域内期望的目标数量。所有目标联合起来的PHD在空间上的积分，就是当前帧期望的目标总数。所以PHD滤波做跟踪，根本不需要维护“哪个目标ID对应哪个量测”这样的硬关联，它只老老实实地维护一个密度场，每一帧新量测来了，就在量测附近“加质量”，没有量测覆盖的区域“质量衰减”，目标的出现、消失、分裂都自动体现在密度的增减里。然后你用峰值提取算法从密度场中挖出目标和目标数来。这就是PHD滤波躲开组合爆炸的妙处。

4.2 PHD滤波的预测与更新公式到底在算什么

PHD滤波器也是贝叶斯框架里的一个递推，只不过递推对象从目标的概率密度函数换成了强度函数。标准PHD预测方程里有四个部分：存活目标的强度预测、新生目标的强度叠加、衍生目标的强度叠加、以及目标消亡导致的强度衰减。如果你第一次看到那个长长的公式很晕，我可以给你拆一下。存活目标强度预测，就是让旧目标按照运动模型往前推，同时乘一个存活概率（目标不消失的概率），再加上过程噪声带来的扩散。新生目标强度，就是“新目标会在哪些位置冒出来”的先验分布，比如战场监控里新目标往往从图像边缘进入，你就可以把新生强度预设成靠近边缘的高斯混合。

PHD更新公式的核心长得像卡尔曼增益的“强度版”。它计算的是每个量测对各个网格点或粒子的更新权重，权重的大小取决于该点的预测强度与其到量测的似然函数的乘积，还要除以一个归一化因子，这个因子等于杂波密度与所有候选点的预测强度贡献之和。工程实现里最常见的是SMC-PHD（序贯蒙特卡洛PHD），就是用粒子来承载强度函数，粒子的权重之和就是目标数的期望。每帧做完预测和更新后，根据总权重估计目标数，再用聚类或峰值搜索把目标位置提出来。

4.3 SMC-PHD的MATLAB代码骨架

以下是一个极简的SMC-PHD目标跟踪框架，处理一维空间里的多个目标，包含目标的新生和消亡。我故意简化到只剩核心逻辑，代码里每个环节做什么我会逐行讲清楚。

% 参数初始化 N_max = 1000; % 粒子数 rho = 0.99; % 存活概率 birth_rate = 10; % 每帧新生粒子的数量 clutter_rate = 10; % 每帧杂波量测的平均数（泊松分布） X = zeros(N_max, 1); % 粒子状态 W = zeros(N_max, 1); % 粒子权重 % 状态转移：x_k = x_{k-1} + v*dt + noise % 观测：z_k = x_k + noise for k = 1:T % 1. 预测：存活粒子转移并降低权重 X = X + 0.5 * randn(N_max, 1); % 随机游走模型 W = rho * W; % 乘存活概率 % 2. 补充新生粒子 X(1:birth_rate) = rand(birth_rate, 1) * 100; % 均匀空间新生 W(1:birth_rate) = 0.1 / birth_rate; % 3. 更新：每个量测对所有粒子更新权重 for z_idx = 1:length(Z) q = exp(-0.5 * ((Z(z_idx) - X)./sqrt(R)).^2); % 高斯似然 % 分母 = 杂波项 + 所有粒子的似然加权积分 pq = W .* q; denom = clutter_rate / V + sum(pq); W = W + pq / denom; % 按量测贡献累积权重 end % 4. 估计目标数和位置 N_est = sum(W); % 聚类或峰值提取目标状态 % 5. 重采样：按权重比例复制或删除粒子 [X, W] = resample(X, W); end

这段骨架代码里最容易出问题的是更新阶段分母里的杂波密度项。杂波密度的物理含义是“单位空间内平均有多少个杂波点”，如果这个值取得太小，滤波器会把很多杂波点当作目标新生的证据，导致目标数虚高；取得太大，又会导致真实目标的信息被压制，目标数被低估。工程上一般用经验公式来估计：先测量一个没有目标的空场景，统计每帧回波数除以监测空间体积，得到一个初始的杂波密度底数，再在线微调。还有一个规律我实测下来很管用：杂波密度对滤波的“目标个数估计”影响远大于对“目标位置估计”的影响。如果你只关心位置而忽略个数，可以把杂波密度设得略高一些来保证位置的纯净性；如果想同时得到可靠的目标数，那就得花心思离线标定这个参数。

SMC-PHD的另一个大坑是重采样。标准粒子滤波的重采样逻辑是按权重抽粒子，权重低的粒子很容易被丢弃，这在PHD里有个隐患：新生粒子的权重设置不当的情况下，低权重粒子正好对应那些刚出现、还没被量测确认的目标，一次性被重采样丢掉，新目标就永远“生”不出来了。解决办法有两个：一是设置一个“新生粒子保护比例”，在重采样时强制保留一部分新生粒子不参与淘汰；二是把新生粒子分布在上一帧观测值附近，而不是均匀撒在整个空间里。第二个方法效率高得多，适合目标从观测中“闪现”出来的场景，比如声呐或雷达目标。

5. 非线性非高斯的万能备胎：粒子滤波的原理与MATLAB实战

5.1 粒子滤波的“笨功夫”和核心三要素

粒子滤波的原理一句话就能说明白：既然后验分布解析算不出来，那我就生成一堆带权重的样本，让这些样本尽量均匀地覆盖状态空间，用它们的加权和去近似真实分布。每个粒子就是一个“假设的目标状态”，权重就是“这个假设在当前观测下的可信程度”。算法循环分四步：采样、预测、加权、重采样。最开始粒子从先验分布里抽样，均匀撒开；预测阶段每个粒子按运动模型往前“走一步”，再加上过程噪声；加权阶段，每个粒子根据当前量测计算似然度并更新权重；重采样阶段，按权重比例重新抽出一批新粒子，权重高的粒子会有更多后代，权重低的粒子逐渐淘汰。这样一轮下来，粒子群就会在目标真正所在的位置附近聚集。

但是这里有个数学上必须拧清的细节：标准的粒子滤波退化问题。不做重采样的粒子滤波，经过几十帧递推后，往往出现一个粒子的权重趋近于1、其他所有粒子权重趋近于0的尴尬局面。这叫粒子退化。为什么会出现？因为权重更新的过程其实是一个连乘的过程，每一帧每个粒子的权重都要乘一个似然度，而似然度往往在真值附近才有峰值，离真值远的粒子乘几次小值之后权重指数级衰减。一旦所有粒子退化成一个点，你对后验分布的描述就跟一个点估计没区别了，滤波失去了所有“表达不确定性”的意义。重采样正是用来解决退化问题的：每次更新完权重之后，按权重把粒子重新抽一遍，把退化的粒子处理掉，把资源集中到高权重区域。但这也带来了新问题——粒子多样性丧失。

我在做粒子滤波时就踩过一个典型的坑：重采样频率太高，导致所有粒子被复制到某几个峰值点附近，结果目标一旦出现大幅机动，粒子群来不及散开去捕捉新位置，滤波直接跟不上目标。解决这个问题的标准做法是引入一个“有效粒子数”指标：Neff = 1 / sum(W.^2)，当Neff降到粒子总数的一半以下时才触发重采样。这个策略叫做“自适应重采样”，它能在退化风险和多样性风险之间做一个平衡。现在几乎所有工业级粒子滤波实现都默认带这个机制，你的代码里也建议加上。

5.2 粒子滤波MATLAB完整示例：强非线性系统目标跟踪

我写一个勉强能用但足以说明原理的粒子滤波示例。场景是一个目标在二维平面做非常规的曲线运动，传感器观测的是目标的距离函数，这个函数设计成高度非线性（比如z = x1 * exp(...) 这种形式），就是为了体现出粒子的价值——你根本不需要去求雅可比矩阵，只需要能计算每个粒子的似然值就行。

N = 5000; % 粒子数量，太少会抖，太多会卡 X = 50*randn(2, N); % 初始粒子群，均值为真值附近的猜测 W = ones(1, N)/N; % 所有粒子权重相同 for k = 1:T % 预测：目标状态按非线性函数变化加噪声 X(1,:) = X(1,:) .* X(2,:) .* ... % 这里写你的非线性运动模型 X(2,:) = X(2,:) + randn(1,N) * sqrt(Q); X = X + sqrt(Q) * randn(2,N); % 更新：计算每个粒子对当前量测的似然 for i = 1:N z_pred = nonlinearmeas(X(:,i)); % 预测观测 q(i) = exp(-0.5 * (z - z_pred)' / R * (z - z_pred)'); end W = W .* q; W = W / sum(W); % 归一化权重 % 自适应重采样 Neff = 1 / sum(W.^2); if Neff < N/2 [X, W] = resample(X, W); end % 状态估计：加权平均或最大权重粒子 x_est = sum(repmat(W, 2, 1) .* X, 2); end

这段代码里有两个地方需要特别强调。一个是预测阶段务必加上过程噪声。粒子滤波里如果过程噪声设得太小，粒子群会逐渐挤成一团，所有粒子用的是同一个运动模型，没有噪声的扩散就没有多样性，时间长了整个粒子群会收敛到错误位置，这种情况叫“样本贫化”。过程噪声的大小控制了粒子在预测阶段的“探索半径”，相当于告诉滤波器“目标可能偏离模型预测多远”。另一个重要的实操点是：状态估计不要总是用加权平均。在多模态分布下，加权平均会给你一个介于两个峰值之间的无意义平均值。比如目标可能在A点也可能在B点，你取平均得到一个C点，这个C点没有任何真实含义。遇到这种情况，我建议你先用聚类算法（比如K-means或者MeanShift）把粒子分群，每个群当作一个可能的“目标或模态”，然后单独计算每一群的加权中心和权重和。这个思路在机动目标跟踪里尤其重要——目标突然大转弯时，粒子分布本来就会出现多个尖峰，必须分组处理。

5.3 粒子滤波的“维度灾难”和工程化建议

粒子滤波不是什么场景都适用的，它最怕的就是状态维度太高。维度升高时，要覆盖整个状态空间所需的粒子数呈指数增长，这叫维度灾难。一个简单的经验数据：一维问题500个粒子往往就够用，二维问题可能需要2000到5000个，三维需要上万，六维以上粒子滤波基本跑不动。实际做工程时，你可以用两个办法绕开这个问题：一是把系统拆解，只对高度非线性的部分用粒子滤波，其他部分用Kalman（这就是Rao-Blackwellized粒子滤波的思路，在SLAM里非常常用）；二是用更好的提议分布。标准粒子滤波直接用先验分布做提议，效率低；如果当前量测信息质量高，可以用无迹卡尔曼或者扩展卡尔曼来生成提议分布，让粒子直接撒在更接近后验的区域。这项技术叫“无迹粒子滤波”或“EKF提议粒子滤波”，实现复杂度上了一个台阶，但粒子数可以降一个数量级。

另外在MATLAB里跑粒子滤波，性能优化有两个小技巧：一个是把内层循环尽量改成向量化运算，全过程用repmat和bsxfun或者MATLAB的自动广播机制，避免写沉重的for循环；另一个是用rng控制随机数种子，保证每次实验的随机过程可复现。你如果跑一段粒子滤波，发现结果每次都不一样，大概率是随机数种子没固定，这在调参和对比实验时非常折磨人。

6. 五种滤波器横向对比：怎么选才是最优解

6.1 算法特性与适用场景对照

这一节我直接用表格把五种滤波器的核心特性摆出来，方便你做选型时快速浏览。所谓“选型”，本质就是回答四个问题：系统线性吗？噪声是高斯吗？目标多吗？算力预算够吗？

看这张表你会发现一个规律：假设越弱，算法越通用，但代价是计算量越来越大、调参难度越来越高。这背后是数学里的“没有免费午餐定理”——你没有任何办法在不付出代价的情况下同时拥有线性Kalman的简洁、粒子滤波的通用和PHD的多目标能力。所以工程选型的第一原则是“够用就好”，能用Kalman解决的问题不要为了炫技上粒子滤波。

6.2 实战选型的三条经验法则

我总结三条经历过实战检验的选择思路，你在做方案评审时可以直接拿来用。

第一，先评估非线性的“深度”。如果一个系统观测方程的三角函数项在小角度近似下可以看成一阶（比如视线角变化很小的雷达跟踪），EKF足够了；但如果角度变化跨度很大，非线性才是主要矛盾，直接用UKF或者粒子滤波。判断标准粗浅粗暴：你可以离线用蒙特卡洛模拟比较EKF和UKF在典型轨迹下的误差，如果两者差距小于10%，用EKF；差距超过30%，果断升级。

第二，警惕目标数量是“真多”还是“假多”。真实的多目标跟踪里，如果目标数量不超过3个，而且杂波密度不高，用JPDA或者简单的关联+Kalman是最省事的；目标上了5个、杂波又密，数据关联组合会迅速爆炸，这时候才轮到PHD滤波器上场。所以不要一听“多目标”就无脑上PHD，先算算关联矩阵的规模。如果关联组合空间超过一万种，传统方法基本瘫痪，PHD的价值才能充分体现。PHD滤波器处理的目标数和杂波量也是有上限的，目标超过几十个以后PHD里的聚类和后处理同样吃力，那时要考虑带标签的多伯努利滤波器（LMB，即带标签的多伯努利滤波器）这一更强大的变体了。

第三，永远优先考虑鲁棒性。目标跟踪系统是跑在真实世界里的，传感器掉线、目标遮挡、杂波突变、数据丢帧，各种脏场景都会出现。我在实际项目里经历过图像检测中断了30帧、恢复后目标已经漂移出画面的大事件，那种情况下无论你选哪个滤波器，都有可能跟不上。所以工程架构上要把滤波器当成“估计器”而不是“救世主”，给它配上前端的量测预处理（去野值、卡范围、轨迹门控），配好后端的失效保护（连续多帧无量测时初始化新轨迹、删除旧轨迹），滤波算法本身只是这个流水线里的一环。

6.3 性能评价指标不能只看误差

写到这里，我要给你一个特别重要的提醒：评价滤波器好坏，不要只盯着一两个指标。很多入门者把RMSE（均方根误差）当作唯一标准，这是个很大误区。举一个场景：目标匀速走直线，跟踪滤波器的RMSE可能很小，但目标突然做一个急转弯，滤波器经过20帧才反应过来，这段弯道期间的误差就会超过正常值的数倍。RMSE作为全程平均，会把快速恢复阶段的低误差和机动期的巨大误差混合在一起，掩盖真实的问题。我建议你在做算法对比时同时看三样东西：稳态精度、瞬态响应和收敛时间。稳态精度就是直线运动时的RMSE；瞬态响应是目标突然机动时滤波器误差的峰值有多高、多久压回去；收敛时间则是滤波器从初始化到稳定跟踪的帧数。这三个指标分别反映了算法的精度、鲁棒性和启动性能。

另外还要看估计的一致性，学术上叫“归一化估计误差平方”（NEES）或者“归一化新息平方”（NIS）。简单说就是你估计的协方差和实际误差之间是否自圆其说：如果你觉得位置误差是1米（协方差矩阵对角线开根号），而实际误差总是5米，说明滤波器过度自信，协方差估计太小，这是警告信号。做多目标跟踪的话，除了位置误差，还要看目标数的估计偏差、漏检率和虚警率。一个好的跟踪算法，应当能自动发现新目标、记住旧目标、合理删除消失目标。这些指标比单个位置误差重要得多。

7. 常见问题与排查技巧实录：那些年我踩过的坑

7.1 滤波发散：最让人头疼的头号问题

十个工程问题里九个是发散问题。滤波发散在现象上特别好认：估计值像脱缰野马一样飘走，或者剧烈振荡，误差越来越大。原因通常是以下几类。

第一，参数给错了。Q、R量级差得太离谱，或者R设得比真实噪声方差小太多，滤波器过度信任观测，被噪声牵着鼻子走。排查方法是先把R改成传感器标定的精确值（用一段静止或已知运动的真实数据统计误差方差），然后调Q保证滤波器的平滑度。第二，模型给错了。目标运动模型跟实际运动模式差太多，比如目标是匀加速运动，你硬用匀速模型，那么预测偏差每帧都在累积，滤波器再怎么更新也追不上。这种问题在状态方程里表现为新息序列（观测减预测观测）呈现明显的非零均值或相关性。第三，数值问题。协方差矩阵在长时间运行后失去正定性，出现负数方差，EKF和UKF里尤其常见。对策是每个循环里强制把P矩阵对称化，比如P = (P + P')/2，再加一个小的单位阵保证正定。第四，非线性展开点离真值太远，EKF直接歪楼。对策是加大初始协方差，让滤波器先用几帧把基础状态校准到接近真值，再进行完整更新。

实战中排查发散问题，我建议你先画三张图：状态估计曲线、协方差对角元曲线、新息序列曲线。新息序列如果总是偏向同一个方向（均值不为零且持续），就是模型或参数的系统性偏差；如果新息方差远大于R，说明你的预测和真实情况脱节了。这两个特征能帮你把问题缩小到“参数类”还是“模型类”。

7.2 粒子滤波的样本贫化与耗时爆炸

粒子滤波常见问题里，样本贫化（粒子多样性丧失）和计算耗时长是两大主诉。贫化的典型表现是：粒子群全部扎堆在一个点附近，目标稍微机动就完全跟不上。我用过的一个土办法比较有效：在粒子更新后，随机抽取一小部分比例的粒子，给它们加一个稍大的过程噪声，人为制造“探索粒子”。这相当于在粒子群里养了一批“侦察兵”，专门负责在目标可能机动的位置撒网。缺点是会让稳态精度有所下降，所以探索粒子的比例要控制好，我一般设成5%到10%。

计算耗时方面，粒子滤波的瓶颈几乎都出现在“每个粒子都对每个量测计算似然”这一层。多目标场景下，粒子数乘量测数的双重循环非常可怕。优化思路要么是减少粒子数（用更好的提议分布），要么是减少量测数（门控：只在粒子附近区域找量测，距离远的直接排除）。还有一个实用技巧是先用卡尔曼滤波做一次粗跟踪，再用粒子滤波在卡尔曼预测位置周围的小范围做细跟踪，这样可以大幅降低粒子搜索空间，粒子数可以降一半以上。

7.3 PHD滤波的目标数忽高忽低怎么破

PHD滤波器在实际应用中反馈最多的就是目标数抖动问题。明明场景里就一个目标在慢慢走，PHD给出的目标数期望却一会儿0.8一会儿1.5，提取完峰值后还会出现伪目标。这个问题一般出在三个地方。

第一个是新生强度设置太重。新生强度代表你对“每帧会出现新目标”的先验置信度，设置太高，量测一抖就会在噪声附近“生”出强度来，被当成新目标。解决办法：先在一个只有杂波、没有目标的场景里跑一遍PHD，统计它给出的目标数期望的均值，把这个值归零或调低，再回到真实场景调整。第二个是聚类参数不合理。SMC-PHD输出的是粒子群，要用聚类提取目标位置，如果聚类距离阈值设得太小，同一个目标的粒子被拆成两群，就出现两个目标；阈值太大，两个接近的目标被合并成一个。这个阈值跟前端传感器分辨率、目标间距分布有关，只能靠离线数据调。第三个是存活概率设置偏低。目标暂时被遮挡、连续几帧没量测时，存活概率会不断衰减粒子权重，一旦权重降到阈值以下，重采样时这个目标直接消失，之后目标又出现了，就会作为一个“新生目标”被重新建立。表现就是目标数先掉后涨，计数跳动。对策是适当调高存活概率，或者引入目标轨迹ID管理，通过轨迹连续性判定目标不是新生的而是旧的。

7.4 野值、丢帧、时钟抖动：工程上的脏场景

最后讲几个不是滤波器本身、但严重影响滤波器效果的工程问题。野值数据是传感器偶发的巨大误差，直接喂给卡尔曼会把状态估计拉飞。我前面提到的马氏距离门控必须做：计算新息的马氏距离，超过3倍标准差就直接丢弃本次量测。丢帧问题常见于视觉处理链路导致的周期性丢帧，你的运动模型假设采样周期是固定dt，一旦实际间隔不是dt，预测误差会加大。做法是让滤波器知道每一帧的真实间隔才对，即把状态转移矩阵中的dt作为动态参数传入，而不是写死。时钟抖动问题在分布式传感器系统里特别常见，多个传感器各自有采样时钟，融合时时间戳对不齐，量测信息和状态时间不匹配，滤波器精度大打折扣。处理手段是做一个时间同步层，先把所有数据对齐到统一时间戳再进滤波器，或者使用带时间漂移补偿的多传感器融合架构。

这些都是我从实际项目中一点一点摔出来的经验，每一个坑都对应过凌晨两点还在看曲线的经历。目标跟踪不是一套理论就能通吃的领域，真正的功夫在细节里——参数的标定、预处理的稳健性、脏数据的处理、数值稳定的保护，这些才是滤波算法能跑得又稳又准的关键所在。

最后再分享一个小技巧：无论你用哪种滤波器，一定记得把每一帧的中间变量（预测状态、新息、增益、协方差）全部存下来，跑完一次仿真后画出来看。比任何调试工具都管用的是肉眼直接看曲线，你很快就能发现自己参数的是非曲直。这套习惯让我从一个“人工调参师”逐渐变成了“看曲线猜原因”的半个老手，你也可以试试。