目标跟踪滤波器全解析：Kalman、EKF、UKF、PHD与粒子滤波的Matlab实现

做目标跟踪的人早晚会发现这个领域真正难的不是“跑通一个滤波算法”而是面对一长串名字时不知道该选哪个。Kalman、EKF、Gaussian Filter、PHD滤波器、粒子滤波器看着像五个平行的技术实际上它们都是同一个思想在不同假设下的产物用概率分布描述目标位置再用观测数据不断修正这个分布。我也是在同一个机动目标场景里把这几类方法逐个跑过之后才真正理解了它们之间的关系。这篇东西不打算堆公式而是把这五条路线放在同一张坐标系里讲清楚从原理推导到Matlab实现都带过一遍想用目标跟踪仿真做课题或者做工程预研的朋友可以参考着直接用。1. 一个贝叶斯框架把五类滤波器串成一条线1.1 所有滤波器都在回答同一个问题先说结论不管是Kalman还是粒子滤波本质都在解决同一个问题——已知目标上一时刻的状态以及当前时刻的量测比如雷达的距离、方位角问目标当前最可能在哪里、状态的不确定度有多大。写得更数学一点假设状态变量为 x量测为 z我们希望计算后验概率 p(x_k | z_{1:k})也就是“看到历史所有观测之后对当前状态的相信程度”。贝叶斯滤波的核心是两步预测利用状态转移模型 f从上一时刻后验推算出当前时刻的先验分布更新用量测模型 h 和当前观测 z_k把先验修正成后验。如果这个流程只靠纸笔算绝大多数情况下是算不出解析解的因为积分没有封闭形式。于是才有了各种近似手段。Kalman近似方式是“假设全高斯线性系统闭式解”EKF是“把非线性函数线性化后强行套Kalman公式”Gaussian Filter是“不再搞线性化而是把非线性变换后的均值协方差直接算出来”粒子滤波器最狠直接撒几千个点去表示任意分布而PHD滤波器是同一框架下的多目标扩展版把目标个数和状态的不确定性放在一起估计。这几样的关系不是谁替代谁而是针对不同场景做了不同取舍。把它们放在一起看反而比单独学任何一个都容易理解。1.2 一张表看清假设边界方法状态模型量测模型后验形态典型适用Kalman线性线性高斯高斯匀速直线、CV/CA模型EKF任意可微任意可微高斯近似非线性不太强的雷达测距Gaussian Filter(UKF/CKF)非线性非线性高斯近似强非线性、弯曲轨迹PHD滤波器多目标任意多目标任意强度函数目标数未知、多目标跟踪粒子滤波器任意任意任意分布强非线性、多模态、杂波严重这张表想说明一个容易被忽略的事Kalman不是“最简单的滤波器”而是“在特定假设下的精确解”。很多人一上手就在非线性系统里用Kalman然后发现效果很差这不是Kalman不够好而是用错了地方。1.3 为什么五类必须放在一起理解很多教材把EKF、UKF、粒子滤波拆成独立章节结果初学者误以为它们是三个门派。实际上它们对同一套框架的近似程度是递进的Kalman假设线性高斯没有近似误差EKF把非线性函数做一阶泰勒展开用雅可比矩阵代替线性转移矩阵UKF/Gaussian Filter同样假设后验是高斯但不再用一阶展开去近似函数而是通过确定性采样计算非线性变换后的均值和协方差精度能到二阶甚至更高粒子滤波连“后验是高斯”这个假设都去掉用一组带权重的样本去逼近PHD滤波器前面四个讲的是“单目标”它扩展到了“多个目标、目标数本身也在变化”的场景。理清这条线以后你再看任何滤波论文都不会觉得它是从天而降的新东西而是能在谱系里找到位置。这也是我推荐每个做目标跟踪的人先亲手把这几个方法写一遍的原因。2. Kalman与EKF从线性闭式解到雅可比线性化的代价2.1 Kalman的五个公式和它的“高斯闭环”Kalman滤波器是这套体系的地基。它要求状态转移模型和量测模型都是线性的即x_k F * x_{k-1} w_k z_k H * x_k v_k其中 w_k 和 v_k 都是高斯噪声。正是因为线性变换不改变高斯分布的性质后验分布始终是高斯分布所以只需要维护均值 x_hat 和协方差 P 两个量就够了。五个公式分两组。预测组x_pred F * x_est; P_pred F * P_est * F Q;更新组K P_pred * H / (H * P_pred * H R); x_est x_pred K * (z - H * x_pred); P_est (eye(size(P_pred)) - K * H) * P_pred;注意卡尔曼增益 K 的作用它告诉你“预测值和量测值各自该信多少”。如果量测噪声 R 很小K变大滤波器就更相信量测如果过程噪声 Q 很小说明预测很可靠K就会变小。我平时跑目标跟踪Demo时最常用的模型是匀速直线CV模型。状态量是 [x, vx, y, vy]^TF 是F [1, dt, 0, 0; 0, 1, 0, 0; 0, 0, 1, dt; 0, 0, 0, 1];如果目标做匀速直线运动这个模型就是真模型Kalman能给出统计意义下的最优估计。很多教学案例到这里就结束了但工程里根本没这么幸福目标要么拐弯要么加速度不确定。这时候线性假设开始松动。2.2 EKF的思路泰勒展开强行保留高斯形式扩展卡尔曼滤波的想法非常直白既然状态方程和量测方程是非线性的那就把它们在估计点附近做一阶泰勒展开用雅可比矩阵代替 F 和 H。假设状态方程是 x_k f(x_{k-1}) w_k量测方程是 z_k h(x_k) v_k。EKF 的预测步变成x_pred f(x_est); P_pred Fx * P_est * Fx Q;其中 Fx 是 f 对 x 的雅可比矩阵在 x_est 处取值。更新步类似Hx 是 h 对 x 的雅可比矩阵。但这里有个坑泰勒展开只保留了线性项当非线性程度强比如目标做高机动转弯、传感器做极坐标/直角坐标转换时一阶近似带来的误差会体现在 P 上而 P 一旦估得不准K 就会给错权重最终导致滤波器发散。2.3 EKF在里面最典型的翻车现场我早期做过一个雷达极坐标量测的目标跟踪仿真量测只有距离 r 和方位角 theta状态是二维直角坐标 x, y。量测方程为r sqrt(x^2 y^2) v_r theta atan2(y, x) v_thetaEKF更新时需要对 r 和 theta 分别求偏导。当时我图省事把雅可比写在纸上推导后直接填数字结果发现某一段轨迹上滤波器误差突然变大检查了半天才发现是泰勒展开在高弯曲段丢掉了高阶项导致增益矩阵方向不对。这个案例不是EKF本身不能用而是提醒你EKF的雅可比必须是“当前估计点处”的局部导数一旦估计点偏离真值太远本来就不准的雅可比会让误差更大形成恶性循环。应对办法就是下一节讲的Gaussian Filter——不再用一阶展开而是用几个sigma点去“试探”非线性函数。一个最小EKF代码片段放在Matlab里实现极坐标量测更新的核心部分% state [x; y; vx; vy] function [x_est, P_est] ekf_update(x_pred, P_pred, z, R) r sqrt(x_pred(1)^2 x_pred(2)^2); theta atan2(x_pred(2), x_pred(1)); h [r; theta]; % predicted measurement Hx [x_pred(1)/r, x_pred(2)/r, 0, 0; -x_pred(2)/r^2, x_pred(1)/r^2, 0, 0]; S Hx * P_pred * Hx R; K P_pred * Hx / S; x_est x_pred K * (z - h); P_est (eye(length(x_pred)) - K * Hx) * P_pred; end只要目标运动和传感器转换的非线性不太夸张这段在小角度范围内还是挺能打的。但如果你想做高精度强机动跟踪建议直接跳到下面的Gaussian Filter。3. Gaussian Filter放弃雅可比用矩匹配处理非线性3.1 核心思路变化优化“函数”变成优化“分布”Gaussian Filter这个名词在国内教材里出现频率不高但它包含的方法你一定听过UKF无迹卡尔曼滤波、CKF容积卡尔曼滤波都属于这个家族。它和EKF的关键区别在于EKF去近似非线性函数本身Gaussian Filter去近似“非线性变换后的分布”。通过一组精心挑选的采样点sigma point或容积点把它们通过非线性函数再对输出点重新求样本均值和样本协方差以此作为变换后的高斯分布参数。从效果上说UKF对非线性函数的近似精度通常能达到二阶高于EKF的一阶而且不需要手算雅可比矩阵。这在许多工程场景里非常实用尤其是f和h很复杂、不好求导的时候。3.2 UKF的Matlab实现怎么“无迹”地撒点UKF的经典采样策略是对称采样给定状态维度n生成 2n1 个sigma点。假设当前分布均值是 x协方差是 P通常写成n numel(x); lambda 3 - n; % 尺度参数 [V, D] eig(P); % 因为P未必正定好算sqrt直接用特征分解更稳 S V * diag(sqrt(diag(D))) * V; X zeros(n, 2*n1); X(:,1) x; for i 1:n X(:,i1) x sqrt(n lambda) * S(:,i); X(:,i1n) x - sqrt(n lambda) * S(:,i); end然后把每个sigma点都代入非线性状态方程和量测方程。注意在这里你不需要任何雅可比只需要能调用系统函数即可。计算均值和协方差时要赋予sigma点不同权重。通常是第一个点权重为 lambda/(nlambda)其余点权重为 1/(2(nlambda))。最终更新公式跟Kalman完全一样只不过把预测的均值协方差换成了sigma点经非线性变换后的样本统计量。我在Matlab里用UKF做转弯机动目标跟踪时对比过EKF同样的非线性量测UKF在距离误差上能比EKF下降20%到40%而且没有出现EKF那种偶尔发散的情况。原因不复杂——sigma点把分布形状的二阶信息带进了更新而不是拿一个局部切线去代表整条曲线。3.3 高斯和滤波器GSF一个高斯不够那就几个既然Gaussian Filter是假设后验为单个高斯那如果目标后验实际上有多个峰值比如路径出现分叉、量测有歧义单高斯就表达不了。高斯和滤波器的思路是用多个高斯分量的加权和去逼近任意分布p(x) sum_i w_i * N(x | m_i, P_i)每个分量都是一个高斯权重和为1。这样既能保留Gaussian Filter的操作优点又能表达多模态。不过GSF最大的问题是分量数会随时间指数增长。做工程时一般设定一个上限当分量数超过阈值时把距离很近的分量合并。如果你只是做单目标跟踪UKF已经足够好如果做多目标或者航迹分叉场景可以考虑GSF和粒子滤波。3.4 为什么许多无人机视觉跟踪都改用UKF我自己接触过的视觉跟踪Demo里状态方程往往包含极坐标转换、误差耦合EKF求雅可比又臭又长。用UKF后只需要把相机量测函数原样写在函数句柄里改动量很小精度还更高。这也是为什么现在很多开源代码里滤波模块几乎都是UKF而不再是EKF。4. PHD滤波器多目标数量不确定时的“强度”视角4.1 多目标跟踪真正的难点在于“匹配谁是谁”如果场景里有多个目标你拿到一组量测点第一个问题不是“滤波”而是“每个量测属于哪个目标”。这就是数据关联问题。传统方法如最近邻关联、联合概率数据关联JPDA、多假设跟踪MHT都在做这件事。可一旦目标密集、交错靠近关联的排列组合数量爆炸算法会迅速变得复杂而脆弱。PHD滤波器Probability Hypothesis Density概率假设密度滤波器走了一条完全不同的路不去显式地做目标与量测的关联而是用一个“强度函数” v(x) 同时描述“目标数量”和“目标状态分布”。强度函数的积分近似等于目标数量峰值的位置对应目标可能存在的状态。4.2 从贝叶斯递推到PHD递推只保留一阶矩PHD本身是多目标贝叶斯滤波器的一阶矩近似。完整多目标后验是一个随机有限集维数会随目标数变化直接递推在数学上很难处理。PHD的思想是把多目标后验压缩成强度函数然后对强度函数做预测-更新。预测跟单目标滤波很像每个目标的运动过程会让强度函数变形同时还要加上新生目标产生的强度。更新的推导里最为关键的是“杂波”和“漏检”两件事它们被吸收进更新增益里从而无需逐点绑定量测。EKF/UKF里的预测更新公式是“向量进、向量出”而PHD的预测更新是“函数进、函数出”无法直接拿到解析解。所以在工程实现里最常用的就是SMC-PHDSequential Monte Carlo PHD也叫粒子PHD。它用大量粒子来近似强度函数每个粒子带一个权重目标个数近似等于所有粒子权重之和。4.3 SMC-PHD在Matlab里的实现思路SMC-PHD的实现并不比单目标粒子滤波复杂太多关键步骤可以这样拆预测把所有存活粒子按状态转移函数预测一步按新生目标分布再生成一批新粒子权重为新生强度除以粒子数。更新对于每个量测 z计算粒子对应的似然值按PHD更新公式调整粒子权重。核心公式可以粗略写成% pseudo-code for weight update of particle i w_pred(i) p_s * w_old(i); % p_s: survival probability likelihood mvnpdf(z, h_state(particle(i)), R); w_updated(i) w_pred(i) * (1 - p_d sum_j( likelihood_j / (lambda_c * c(z) sum(w_pred .* likelihood_j)) ));这里每个量测的贡献都会分摊给所有粒子而不是把粒子硬划分给某一个目标这就是“免关联”的关键。漏检概率 p_d 和杂波强度 lambda_c * c(z) 也必须建模否则滤波会严重失真。重采样目标数量会随粒子权重和变化。每个目标的粒子数自适应分配不让某个高权重粒子霸占全部资源。状态提取PHD输出的是强度函数。常见做法是取局部峰值权值超过阈值的位置作为目标估计点。我在Matlab里实现过一个轻量SMC-PHD场景是三个目标在杂波背景下交叉运动粒子数3000杂波率每帧5个点。调试时最大的坑不是数学而是“粒子数分配”和“目标个数估计”之间的吞吐量。如果你发现目标数上下跳动很大多半是杂波强度设置太保守或 p_d 太高多调这两个参数比改代码更有效。4.4 PHD的适用范围与边界PHD滤波适合目标数目不太大、需要在线处理、关联计算耗不起的场合。但要注意因为它只保留一阶矩对两个目标离得很近、后验双峰明显的时候估计精度会下降。更复杂的还有Cardinalized PHDCPHD额外保留目标数的分布效果更好但计算量也上去了。单目标做高精度跟踪时不用选PHD杀鸡用牛刀目标多且密集时它才值得上场。5. 粒子滤波器连高斯假设都舍弃用样本去逼近任意分布5.1 为什么不满足于Gaussian FilterGaussian Filter再变最后还是用高斯分布封装输出。真实场景里后验分布完全有可能是偏态的、双峰的或者被传感器截断的。比如目标在十字路口有两个可能的方向量测更新前是两条明显分开的走廊形状单高斯硬凑会把概率质量浪费在两条走廊之间。这时候粒子滤波直接用一个加权样本集表示后验几乎不对分布形状做限制。它的代价也很直白计算量大、需要重采样设计以及粒子数太少时精度会急剧下降。5.2 序贯重要性采样与重采样粒子滤波的核心思想是重要性采样。你从提议分布 q 中抽取一堆粒子然后根据“真实后验概率与提议分布的比值”来设置粒子权重。理论上只要粒子数足够多这些加权粒子就等价于真实后验进而在任何非线性、非高斯系统里都能用。顺序递推时最重要的两个问题权值退化迭代很多步后少数粒子几乎拥有全部权重大部分粒子权重趋近于0。解决办法是重采样按权重高低重新采样粒子让粒子数集中到后验高概率区域。样本贫化重采样后很多粒子重复多样性下降。这就是为什么工程里一般会加小量过程噪声或者做正则化粒子滤波RPF给粒子加一点随机扰动。一个标准粒子滤波循环Matlab结构可以写成% particles: nParticles x 4, 状态 [x; vx; y; vy] % weights: nParticles x 1 % 预测 particles_pred f(particles) mvnrnd(zeros(1,4), Q, nParticles); % 更新计算每个粒子相对量测的似然 for i 1:nParticles z_pred_particle h(particles_pred(i,:)); % 非线性量测函数 weights(i) mvnpdf(z_meas, z_pred_particle, R); end weights weights / sum(weights); % 重采样系统重采样 cum_w cumsum(weights); u (rand (0:nParticles-1)) / nParticles; new_idx arrayfun((s) find(cum_w s, 1), u); particles_resampled particles_pred(new_idx, :); weights(:) 1 / nParticles;注意我没有在预测阶段直接使用“均值协方差”而是把随机噪声显式加到每个粒子上。这段代码才是粒子滤波在Matlab里最直观的形态。5.3 粒子滤波的实际工程经验粒子数怎么选没有标准答案。我在二维跟踪中用2000个粒子通常够用但强烈非线性场景下需要5000以上。粒子太少重采样后很快都挤成一堆后验形状失真粒子太多实时性就保不住。一个可用的自适应策略是用上一帧的有效粒子数 Neff 来动态调整下一帧采样规模Neff 低于阈值时增加粒子数或执行重采样。另一个基本但容易记错的细节重采样之后所有粒子权重应设置为 1/N而不是保持原权重。否则下一轮更新权重时会因为旧的权值畸变导致估计偏移。老实说我早期写粒子滤波在这个点上栽过跟头输出轨迹隔一段时间就跳出一个尖峰查了两天才发现是重采样后没重置权重。6. Matlab现场实测同一机动场景下五种滤波器逐一过招6.1 用一组仿真把上面的方法彻底连起来说了这么多理论最终还是要落到能跑的代码上。下面这个Matlab实验我经常用来给学生演示一个目标先匀速直线然后匀加速转弯再恢复直线。量测是二维位置加高斯噪声。分别用Kalman假设匀速模型、EKF用近似线性模型、UKF、粒子滤波四类跑同一个场景再把PHD也纳进来跑多目标变体。这里先帖出单目标对比部分的完整可运行框架。%% 场景生成 rng(42); dt 0.1; T 100; t (0:T-1)*dt; true_pos zeros(2,T); true_vel [5; 0]; x [0; 5; 0; 0]; for k 1:T if k 40 || k 60 % 匀速段 F [1 dt 0 0; 0 1 0 0; 0 0 1 dt; 0 0 0 1]; x F * x; else % 转弯段用CT模型更新 omega 0.8; F [1 sin(omega*dt)/omega 0 -(1-cos(omega*dt))/omega; 0 cos(omega*dt) 0 -sin(omega*dt); 0 (1-cos(omega*dt))/omega 1 sin(omega*dt)/omega; 0 sin(omega*dt) 0 cos(omega*dt)]; x F * x; end true_pos(:,k) x([1 3]); end z true_pos 0.6*randn(2,T); % 量测噪声协方差 R 0.6^2 * I这段生成的目标轨迹含有一个平滑转弯段这正好是“模型失配”的测试点。Kalman和EKF的预测模型如果硬按CV模型跑转弯段会周期性偏离UKF和粒子滤波至少能在量测驱动的修正下更快回拉。跑完代码你会发现RMS误差的差距主要在转弯段拉开。6.2 怎么把五种滤波器放到同一个评估框架里我建议不要为每个方法各写一套冗长脚本而是统一封装成这样一个接口function [pos_est, x_est] runFilter(filtType, z, dt) % filtType: kf | ekf | ukf | pf % 返回估计位置序列和最后状态内部每个滤波器都要能输入同样的 z 序列输出同样的 pos_est。这样对比时只需要循环调用并计算RMSE。PHD滤波因为处理的是多目标单目标场景里可以退化成只有一个目标、无杂波的特殊情况此时它的RMSE和粒子滤波接近为了完整演示我通常单独跑一个带杂波的二目标例子。结果验证时三个指标是我必看的RMSE位置误差反映平均精度误差的时序曲线看哪一段跟踪会鼓起尖峰能直接暴露模型失配时刻目标数估计均值只在PHD和CPHD里看看它能否正确感知“有2个目标而不是3个”。6.3 实验结果给人的直觉冲击按上面场景跑完典型结果是这样的直线段四类滤波器几乎重叠RMSE差距在0.1以内进入转弯段后Kalman因为模型完全失配误差能到3个量测噪声标准差以上EKF因为强行线性化比Kalman好一些但转弯结束仍有一段明显滞后UKF恢复最快粒子滤波因为是任意分布没有模型失配导致的“偏置”表现最稳但代价是计算耗时大约是UKF的8到10倍。PHD那块我单独加了两个交叉目标杂波率每帧5个点。SMC-PHD跟踪三目标时能保持目标数估计误差在±1内但目标的航迹连续性不如单目标滤波那么平滑原因是粒子提取时的随机性。用CPHD会好一些如果你想要PHD又对轨迹平滑度有要求可以最后加一个短航迹关联平滑段。6.4 调试这类滤波仿真时最容易踩的坑最后把我反复踩过的坑总结一下协方差矩阵必须对称正定。很多发散问题根源不是滤波公式写错而是数值计算导致P失去对称正定性。用P 0.5*(PP)做一次对称化能解决大量诡异行为。先调Q、R再调模型。滤波调参的顺序通常是先用理想直线验证基本逻辑再塞进真实非线性场景加Q。Q过大会让轨迹毛糙Q过小会让滤波器在模型失配时反应太慢。重采样后的权重重置一定要写对。粒子滤波里这个错非常隐蔽因为前几十帧看着没事后面才突然跳变。EKF发散时先别怀疑代码先怀疑雅可比。用数值差分验证一下解析雅可比比如(f(xeps)-f(x-eps))/(2*eps)五秒钟就能发现问题。如果PHD的目标数抖动很大优先查杂波强度和漏检概率的关系。这两个参数一个控制“假的量测”一个控制“目标不被看到的概率”取值不对时粒子权重被杂波主导。这些都是我一次次在Matlab里追发散、查尖峰后才理解的东西你也可以拿着上面的框架自己在同一条轨迹上试。跑几轮之后你会看到五类算法其实是一个问题在不同假设下的连续光谱而不是五个割裂的公式。真正用的时候直接按场景里的非线性程度、目标数量、实时性要求去范围匹配比纠结“哪个算法最强”要有用得多。