去年做安防项目的原型验证时我遇到一个特别朴素的需求画面里有行人来回走动客户要求跟踪时不丢人、ID不换、最好还能预判下一帧位置。听起来不难但当真把视频丢进去跑一遍检测器漏检、抖框、遮挡、误检全来了一个比一个头疼。折腾到最后真正稳住的方案还是那套经典组合——卡尔曼滤波加检测器再加匈牙利关联。这篇文章就是我把整个流程在MATLAB里从零写完之后的一次完整复盘原理、代码、误差量化、调参排查一次讲清楚适合正在做目标跟踪课题、想快速验证算法效果的同学也适合需要在不依赖深度学习框架的前提下做跟踪原型的工程师。1. 项目整体设计与思路拆解1.1 为什么行人跟踪要用“检测器卡尔曼滤波”这套组合先说清楚一个误区行人跟踪并不是让算法自己去“找”行人而是让算法把每一帧检测到的目标“连”起来。所以主流的框架是两步走第一步是检测器给出逐帧的检测框第二步是跟踪器负责跨帧关联、状态平滑和轨迹管理。卡尔曼滤波在这套框架里干的活简单说就是三件事预测目标下一帧在哪里平滑掉检测框的随机抖动以及在目标被短暂遮挡时用运动模型把轨迹“续”下去。为什么不是光流法也不是纯模板匹配光流法在行人密集场景里计算量很大而且一旦有遮挡光流场很容易被打乱模板匹配更麻烦行人稍微转个身视角一变模板就漂了。卡尔曼滤波的优势在于它只用了一个轻量级的运动模型通常就是匀速或者匀加速加上对噪声的统计假设就能在实时场景里给出一个相当稳定的位置估计。更重要的是它不需要训练数据不需要GPU在MATLAB里从零实现也就是几十行矩阵运算的事情。相比之下端到端的深度学习跟踪方法虽然上限高但要准备标注数据、要训练、要标定超参在原型验证阶段根本不划算。还有一个容易忽略的点卡尔曼滤波把“不确定性”这个信息也保留下来了。每次预测都会算出协方差矩阵P它告诉我们当前位置估计有多可信。这个信息在做数据关联时特别有用——离预测位置越远的检测框匹配代价就越高等于给错误关联加了一道天然门槛。这是启发式跟踪方法很难做到的事。1.2 MATLAB做这个项目的优势以及它的边界在哪选MATLAB做这个项目不是因为它性能强而是因为它的调试体验和矩阵表达太适合这类算法了。卡尔曼滤波的核心迭代就是矩阵乘法和加法的循环MATLAB写起来几乎和教科书公式一一对应变量名可以直接用A、H、Q、R不需要像C那样操心内存释放和数据布局。另外Computer Vision Toolbox里现成的函数很多peopleDetectorACF可以直接跑行人检测bboxOverlapRatio计算IoUassignmunkres直接做匈牙利匹配一个循环写下来基本就是拼积木。但性能是它的边界。我实测过1080p、30fps的视频用ACF检测器全画面逐帧检测MATLAB里能跑到每秒2到5帧就算不错了。所以这个方案适合的事情是验证算法逻辑、分析误差来源、快速产出演示Demo。真要上嵌入式平台或者做实时视频分析最后还是要转到C或Python重写。不过这不影响它在原型阶段的巨大价值我后面会专门讲从原型到部署迁移时要注意哪些坑。另外提醒一句MATLAB版本对工具箱API影响很大R2021a和R2023b里很多函数用法不一样代码抄来抄去经常报错。我建议直接用R2021a以上的版本下面的代码也都基于这个版本写的。2. 核心模块拆分从运动模型到数据关联2.1 状态向量怎么选4维还是8维行人跟踪里最常用的状态向量有两种一种是4维的[x, y, vx, vy]也就是中心坐标加两个方向上的速度另一种是8维的[x, y, vx, vy, ax, ay, w, h]在位置速度基础上又加了加速度和框尺寸。初学的时候容易觉得状态维度越丰富越好但实际调下来4维状态在大多数行人场景里反而更稳。为什么因为8维状态里加速度分量在普通行人的运动模式下几乎都是零均值的噪声你把它加进去估计器需要花更多帧去收敛这部分的误差效果就是轨迹在启动阶段抖动得更厉害。特别是检测器本身的量测噪声有3、5个像素时高维状态会把一部分噪声“吸收”成虚假的加速度估计反而让预测框飘起来。我的经验是除非你要跟踪的目标有明显的强机动特征比如车辆急加速、无人机变向否则先把4维状态跑通再看误差分析的结果决定要不要加维度。4维状态对应的状态转移矩阵很好写假设帧间间隔为dtA [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1];这里的物理含义很清楚新一帧的位置等于上一帧的位置加上速度乘时间速度则保持不变。真实行人在帧间会有加减速这部分“模型说不准”的差异就交给了过程噪声Q来兜底。换个角度理解匀速模型叠加Q之后实际上等价于一个“带有随机加速度扰动”的运动模型只是简化了表达而已。这个设计思路非常重要它决定了卡尔曼滤波在模型失配时不会直接发散。2.2 量测方程与Q、R参数设定量测方程负责把状态向量映射到我们能观测到的量。在这个项目里检测器给我们的是检测框的中心坐标所以量测矩阵H就是把状态里的位置分量挑出来H [1 0 0 0; 0 1 0 0];接下来是卡尔曼滤波里最玄学的两个参数Q和R。很多新手上来就直接抄别人代码里的数值然后发现换成自己的视频就全乱套。Q的物理意义是“运动模型每帧积累的不确定度”它反映的是目标加速度有多“野”。行人正常行走时Q的对角元取0.1到1量级就够了如果场景里有人突然奔跑、急转就需要调大。R对应的是检测器的量测噪声协方差简单说就是检测框中心位置的抖动程度。ACF检测器在光照稳定的情况下中心抖动一般在1到3个像素换算成平方像素就是1到9遇到遮挡、模糊可能到10以上。我调参时有个习惯先把R定下来因为检测器的噪声特性相对可测你可以手动标注几十帧GT统计检测框中心与真实中心的偏差方差这个值就是R。然后再扫Q从很小的值开始逐步增大离线跑一段验证视频看跟踪误差的变化曲线取RMSE最低的那组Q。这比凭感觉拍脑袋要可靠得多。另外初始协方差P0不用太纠结直接设成对角元100的矩阵表示初始状态很不确定滤波器自己会快速收敛。有一点特别提醒Q和R的量纲不一致。Q里面位置分量的单位是像素平方速度分量的单位是像素/帧平方如果dt不同同一组参数表现会差很多。最好先把位置和速度做归一化处理或者统一到标准帧率下再调参否则换个视频帧率就要重新标定一遍。2.3 数据关联与轨迹管理真正的工程难点卡尔曼滤波本身解决的是“单条轨迹如何平滑预测”但多目标跟踪面临的核心问题其实是谁是“谁”——数据关联。没有正确的关联滤波再完美也白搭两个行人交叉走过去ID一换整条轨迹就废了。最经典的做法是IoU匹配加匈牙利算法。先计算每个检测框和每条预测轨迹框之间的IoU用1减去IoU作为代价矩阵然后用匈牙利算法二分图匹配让总代价最小。MATLAB里这步很方便costMatrix 1 - bboxOverlapRatio(detBoxes, trackBoxes); costMatrix(isnan(costMatrix)) 1; matches assignmunkres(costMatrix);要注意一点assignmunkres处理的是最小化问题而且不接受NaN。bboxOverlapRatio在检测框和轨迹框完全不相交时返回01减去它就是1这没问题但如果出现空矩形之类的异常要提前清洗否则代价矩阵里混进NaN会导致匹配结果整个崩掉。代价矩阵再优化一下还能加gating。对每条轨迹计算检测框与预测位置的马氏距离超过阈值比如3个标准差的直接把代价设为无穷大意思是不允许匹配。这个限制能大幅减少密集人群里的错误关联。更高级的做法是引入颜色直方图或ReID特征向量作为第二层代价但在原型阶段IoU加距离gating已经能覆盖80%以上的场景。轨迹管理也是必须做的。我记得最开始的实现里没有生命周期管理结果就是每帧漏检都产生一条新轨迹200帧视频跑完凭空多了400条“幽灵轨迹”。正确的做法是给每条轨迹标记状态新检测框连续三帧都能关联上才升级为确认轨迹并对外输出确认轨迹如果连续超过maxAge帧没有匹配到检测框才判定死亡并删除。tentative状态下的轨迹不输出这样能过滤掉大量误检引起的碎片轨迹。3. MATLAB手写卡尔曼跟踪器的实操过程3.1 手写还是用工具箱我的选择MATLAB里实现卡尔曼滤波有三种路径一是直接用工具箱的trackerGNN配置好运动模型就能跑二是用configureKalmanFilter这个点跟踪函数三是从零手写预测更新函数。我的建议是如果你是为了交差或者快速出效果用trackerGNN但如果你想搞懂原理、调好参数、做好误差分析一定要手写一遍。理由很直接跟踪器一旦工作不正常工具箱的封装会把你和问题隔离开你连中间状态都看不到排查无从下手。手写版虽然代码多一点但每一步都能打印出P矩阵、状态向量、协方差变化哪里发散一眼就能看出来。我的工程代码里维护了三个核心函数。predict函数做预测步update函数做更新步另外还有一个函数负责初始化新轨迹。预测步的本质是把状态向量和协方差矩阵都按照A矩阵往后推一步同时加上过程噪声Qfunction [x_pred, P_pred] predict(x, P, A, Q) x_pred A * x; P_pred A * P * A Q; end更新步则把预测结果和检测量测做加权融合权重由卡尔曼增益K决定function [x_upd, P_upd] update(x_pred, P_pred, z, H, R) S H * P_pred * H R; K P_pred * H / S; x_upd x_pred K * (z - H * x_pred); P_upd (eye(size(x_pred, 1)) - K * H) * P_pred; % 维持对称性 P_upd (P_upd P_upd) / 2; end这里K的表达式我特意用了右除而不是inv(S)这是一个很容易被忽略的数值稳定性问题。矩阵求逆在S病态时会产生极大的数值误差而用右除运算MATLAB会走数值稳定的线性方程求解路径。对于4维状态的问题区别可能还不明显一旦状态升到8维而且R设置得很小S矩阵接近奇异这时候inv直接就可能把协方差矩阵弄成非对称的后面滤波自然就崩了。3.2 视频读入与检测器接入主程序的第一步是读取视频并获取检测框。VideoReader逐帧读取检测器我用的是ACFAggregate Channel Features检测器为例因为它不需要训练就能跑对直立行人效果稳定很适合做算法验证。接入代码大概是这样vReader VideoReader(test.mp4); detector peopleDetectorACF(); while hasFrame(vReader) frame readFrame(vReader); [boxes, scores] detect(detector, frame); % 过滤低置信度检测框 validIdx scores 0.5; boxes boxes(validIdx, :); ... end这个置信度阈值0.5不是随手写的。ACF输出的分数在不同视频、不同季节的衣着特征下分布差异很大。我在一个室内视频里跑阈值0.3就能滤掉90%的误检换到室外的场景0.3又会让很多误检混进来。所以更稳妥的做法是先跑几帧画出分数直方图看一下分布区间再定阈值。这一步属于预处理做不好后面跟踪器的压力全部转嫁到数据关联上。检测框拿到之后要转成卡尔曼滤波需要的状态向量形式。我这里取的是检测框中心坐标作为量测值状态向量里也是中心坐标加速度。至于框的宽度和高度在这个入门级实现里先不放进状态直接用原始检测框的宽高去画轨迹框等后面做更复杂的多目标再扩展。量测值z记得要转成double类型MATLAB里视频帧是uint8直接拿去做矩阵运算会造成类型不匹配的错误这是新手特别容易踩的坑。3.3 主循环预测、关联、更新、管理整个跟踪主循环是这样的流程对每条轨迹调用predict函数得到本帧的预测位置和预测框用预测框和当前帧检测框计算IoU代价矩阵调用assignmunkres求解最优匹配被匹配上的轨迹用检测框坐标做update更新未匹配的检测框初始化新轨迹先标记为tentative未匹配的轨迹计数加一超过maxAge就删除对确认状态且本帧有更新的轨迹输出并绘制。端正了顺序之后你会发现这个循环就是一个个很清晰的功能模块。写的时候要注意一点同一帧内一组检测框只能匹配一条轨迹。assignmunkres返回的匹配矩阵已经保证了这一点但有些人在自己写贪心匹配时容易忽略这个约束出现一个检测框同时更新两条轨迹的bug后果就是两个目标越靠越近ID就乱了。轨迹管理在代码里其实就是几个数组的状态流转。我给每条轨迹维护一个trackAge计数和一个totalVisibleCount计数连续三帧都被关联到的轨迹状态从tentative转confirmed立即对外输出。一旦某条轨迹的trackAge超过maxAge我常用50帧直接删除存档。如果场景里有较长时间的严重遮挡比如目标在柱子后面躲了半分钟50帧还不够那就要结合外观特征来恢复轨迹卡尔曼滤波本身救不回来。3.4 可视化画框、画轨迹、画ID调试跟踪算法最忌讳只看数字画出来才看得清楚。我用insertShape和insertMarker把检测框、预测框、历史轨迹画在视频帧上frame insertShape(frame, Rectangle, trackBox, Color, yellow, LineWidth, 2); frame insertShape(frame, FilledRectangle, predBox, Color, cyan); frame insertObjectAnnotation(frame, Rectangle, trackBox, sprintf(ID:%d, trackId));这里有个调试心得把检测框画成绿色预测框画成红色历史轨迹画成一条彩色线段序列。这样一眼就能看出卡尔曼滤波预测和检测之间的偏移量。如果绿框动不动盖不住人说明R设小了如果红框一直带头跑在绿框前边说明Q设大了或者模型失配严重。这种“视觉化反馈”比看RMSE数字更直接能帮你迅速定位问题在哪一步。轨迹历史我保存在一个数组里每次更新后把最新的位置追加进去画的时候取最近20帧坐标连线。这样既能看清楚运动趋势又不会让画面被密密麻麻的轨迹线糊满。还有一个小技巧ID的颜色用hsv色相映射这样同一个ID的轨迹和标签框颜色一致多目标场景下辨识度会高很多。4. 误差分析仿真实验与量化指标4.1 误差来源拆解误差分析不能只盯着最终RMSE要把误差分成几个来源分别看。第一是建模误差匀速运动模型无法准确描述突然加速、急转弯这类机动这一部分误差会直接体现在卡尔曼预测的滞后上。第二是量测误差检测器给出的边框本身有位置偏差和尺度偏差即使滤波完全正确也不可能把误差压到检测器本身精度以下。第三是关联误差数据关联匹配错了滤波器会把量测用错目标上这是最隐蔽也最致命的误差它在RMSE上可能表现为一个突然的巨大跳变。第四是数值误差协方差矩阵在反复迭代中可能失去对称性甚至非正定导致滤波器收敛到错误值。理清这些来源之后你再去看误差指标就能判断某个异常是建模的问题还是关联的问题。比如误差曲线在第40帧出现一个巨大尖峰大概率不是Q和R的锅而是关联错误或者遮挡触发了预测失灵。4.2 仿真实验的设计为了把误差分析做公平我用MATLAB构造了一个仿真场景。设定一条带轻微加速度的行人轨迹t 0:1/30:100/30; x_true 10 2 * t 0.05 * t.^2; y_true 50 3 * t;然后给真实位置叠加均值为0、标准差为3像素的高斯噪声模拟检测器输出这组带噪数据就是卡尔曼滤波的输入量测。实验对比三组直接用带噪检测坐标作为轨迹无滤波、经典4维卡尔曼滤波、Q调得偏大的卡尔曼滤波。每组的性能用两个指标衡量位置RMSE和最大单帧误差。跑完的结果很能说明问题。无滤波的RMSE基本等于噪声标准差大约在3.0像素左右经典卡尔曼滤波能把RMSE压到1.2像素上下直观原因就是预测过程把高频随机抖动滤掉了但Q调得过大时RMSE反而回升到2.5像素因为滤波器太信任跟随量测把噪声又放回来了。这三组放一起对比就能直观看出卡尔曼滤波的收益边界在哪里。4.3 参数敏感性Q和R怎么影响结果Q和R的比值决定了滤波器是更相信预测还是更相信量测。Q/R越大卡尔曼增益K越接近1滤波器越跟随检测器噪声滤除效果差Q/R越小K越接近0滤波器越依赖模型预测轨迹平滑但一旦目标真的加速转向误差滞后会非常明显。在仿真中我在固定R的情况下扫描Q记录不同Q对应的位置RMSE得到的曲线是典型的U形Q太小模型不确定度不够遇到加速度变化时跟不上Q太大噪声又滤不干净。最优点一般落在中段某个区域而且附近往往有一段平坦地带说明Q的调参有一定容忍度。有人问为什么不直接把Q往小了设这样轨迹多平滑。答案是不行因为真实视频里检测器不是稳定输出的目标平时匀速、偶尔机动你指望模型完全握住运动规律一旦遇到拐弯就会整个飞掉。适当调大Q相当于给模型留了一条“不确定度缓冲带”这个缓冲带是滤波器稳定性的来源。4.4 真实视频上的误差估计仿真实验能说明滤波器理论上限但真实视频才是检验标准。这里有个关键问题真实视频没有真值RMSE怎么算我的做法是采样标注。从视频里均匀抽出20到30帧手动框出目标的真实位置自己写个简单的鼠标选点脚本就行然后在这些帧上比较跟踪器输出的轨迹坐标与标注坐标计算误差。标注帧数不需要多能覆盖直行、转弯、遮挡几个关键阶段就够了目的是让误差曲线有代表性而不是追求统计上的高精度。在真实视频上跑下来我观察到的误差规律和仿真基本一致但绝对数值会大不少。主要原因是真实检测器不仅噪声大还会偶发漏检和框偏而这恰恰是仿真里最难建模的部分。所以我的建议是仿真用来调参和理解原理真实视频用来验证算法的鲁棒性两者缺一不可。有一点要特别提醒分析误差时不要只看平均RMSE。多目标跟踪场景里ID Switch次数和轨迹断连次数往往比位置误差更影响用户体验。一个目标位置偏3像素没人注意但两个目标ID互换一眼就看出来了。所以误差分析至少要看三个维度一是位置误差RMSE和最大误差二是关联质量ID Switch和碎片轨迹数三是时间维度轨迹在遮挡/漏检场景下能存活多久。5. 常见问题与排查技巧实录5.1 目标丢失、ID互换、轨迹抖动这三个问题几乎每个做跟踪的人都会遇到而且经常会同时出现。目标丢失的典型原因是检测器漏检次数超过maxAge轨迹被误删解决方向不是去调卡尔曼滤波器而是优化检测阈值和轨迹管理参数。ID互换则几乎都是数据关联的锅最直接的诱因是IoU代价在目标交叉或者密集排列时区分度不够办法是加入马氏距离gating限制匹配的最大距离同时给代价矩阵加一行“如果不确定性高就拒绝匹配”的约束。轨迹抖动的根源通常是Q和R比值不对滤波器的平滑能力没有发挥出来重新标定R然后扫描Q通常能消除大部分抖动。这里我提供一个排查顺序能少走很多弯路先看检测器输出稳不稳再看轨迹管理状态对不对然后才动卡尔曼参数。很多人一上来就拧Q和R结果发现检测器本身就在乱跳调滤波参数事倍功半。先把源头数据理顺滤波器的参数空间就干净多了。5.2 卡尔曼发散与协方差病态如果你发现轨迹突然以极快的速度飞出画面或者位置估计在两条完全不连续轨迹之间跳跃大概率是协方差矩阵P出问题了。P矩阵在数学上必须是半正定的但数值计算会破坏这个性质尤其是当Q或R设置不合适或者S矩阵接近奇异时P的对称性就保不住了导致增益计算错误。我的经验是三步排查第一步打印每次更新后P矩阵的本征值出现负值基本就是病态了第二步在update函数里强制重新对称化P (P P) / 2这一步能挽回大部分数值问题第三步如果还发散就把P矩阵的初始值和Q调大一点增加不确定性可以让滤波器对异常量测的抵制能力更强。此外还有一个特别容易忽略的点视频帧率变化会影响dt如果程序中途改了帧率A矩阵里的时间步长没有同步更新模型的时间连续性被打破滤波就会莫名其妙漂移。处理办法是显式把dt作为参数传入预测函数而不是作为固定常量写死在代码里。5.3 MATLAB运行速度优化的几个实用技巧MATLAB跑视频跟踪最大的痛就是慢。我实测同一个逻辑在Python用numpy写一遍大约能快3到5倍C能快一两个数量级。但在原型阶段有几个方法能让MATLAB版本的使用体验好很多优先降分辨率。1080p的视频降到720p甚至640pACF检测速度大幅提升跟踪精度损失有限。别在循环里画图。VideoPlayer或者figure绘制每帧画面非常耗时我先跑完跟踪把轨迹和框都存成结构体最后再统一可视化。实际开发中这种“先盲跑再回放”的模式还能帮你快速跑参数扫描效率提升特别明显。用parfor并行跑参数扫描。Q和R调参时会对同一段视频跑几十组参数group的回放其实是天然并行的parfor一次性跑完省下的时间很可观。只处理ROI区域。如果行人都集中在画面某一区域先用背景差分子检测只在包含目标的ROI里跑检测器速度能快一倍以上。这些优化做完之后即便是MATLAB也能在720p视频上跑到接近实时的速度。当然真要部署到嵌入式平台还得重写成C或者用C代码生成器转一版出来不过这已经属于另一个话题了。5.4 版本差异与工具箱函数坑写这篇文章时我用的是MATLAB R2023b和R2024a两个版本分别验证过trackerGNN、assignmunkres这些函数行为基本一致但ACF检测器的模型文件路径在旧版本里不一样新手经常遇到找不到模型或者报未定义函数的问题。解决办法也说得很俗要么升级到R2020以上版本要么在代码里用which peopleDetectorACF确认函数路径。另一个坑是bboxOverlapRatio的输出是单精度或double取决于输入类型如果检测框是int32类型除法计算出来会有量化误差在代价矩阵里凑出来的微小差异可能影响匈牙利匹配的优先级。统一转成double再做IoU计算是最稳妥的习惯。工具函数有时候也会坑人。assignmunkres要求代价矩阵必须是有限数NaN和Inf都不能有。gating时我把不匹配的代价设为Inf结果调用assignmunkres直接报错一开始还以为是算法实现的问题后来查文档才发现是这个限制。解决起来也不难把Inf换成一个很大的常数比如1e6效果完全一样。写在最后的几句体会这套卡尔曼行人跟踪器的原型我从头到尾写完最大的感触是卡尔曼滤波本身并不难难的是让它在一个不完美的检测系统上稳定工作。每次你把Q和R调得更“合理”总会发现真实视频里有新的意外——突然的低头、剧烈的转身、两三个人挨在一起走路。所以我现在做这类项目一定会预留误差分析的时间和工具把检测、关联、滤波三个模块的误差拆开来看哪一环不行就修哪一环而不是笼统地把所有问题都归咎于某一个参数。最后再分享一个小技巧也是我踩过很多次坑之后才养成的习惯任何时候动手改代码之前先跑一遍当前版本的基线保存好误差指标和可视化的结果。这不只是做对比实验的需求而是你改完参数之后很可能需要回退到上一版。如果没有基线你永远无法回答“这个改动到底是变好了还是变坏了”这个问题。所有调参高手都不是直觉强而是对比做得好。这套方法放到卡尔曼滤波的项目里值得你从一开始就认真对待。 MATLAB实现卡尔曼滤波行人跟踪器：原理、代码与误差分析
去年做安防项目的原型验证时我遇到一个特别朴素的需求画面里有行人来回走动客户要求跟踪时不丢人、ID不换、最好还能预判下一帧位置。听起来不难但当真把视频丢进去跑一遍检测器漏检、抖框、遮挡、误检全来了一个比一个头疼。折腾到最后真正稳住的方案还是那套经典组合——卡尔曼滤波加检测器再加匈牙利关联。这篇文章就是我把整个流程在MATLAB里从零写完之后的一次完整复盘原理、代码、误差量化、调参排查一次讲清楚适合正在做目标跟踪课题、想快速验证算法效果的同学也适合需要在不依赖深度学习框架的前提下做跟踪原型的工程师。1. 项目整体设计与思路拆解1.1 为什么行人跟踪要用“检测器卡尔曼滤波”这套组合先说清楚一个误区行人跟踪并不是让算法自己去“找”行人而是让算法把每一帧检测到的目标“连”起来。所以主流的框架是两步走第一步是检测器给出逐帧的检测框第二步是跟踪器负责跨帧关联、状态平滑和轨迹管理。卡尔曼滤波在这套框架里干的活简单说就是三件事预测目标下一帧在哪里平滑掉检测框的随机抖动以及在目标被短暂遮挡时用运动模型把轨迹“续”下去。为什么不是光流法也不是纯模板匹配光流法在行人密集场景里计算量很大而且一旦有遮挡光流场很容易被打乱模板匹配更麻烦行人稍微转个身视角一变模板就漂了。卡尔曼滤波的优势在于它只用了一个轻量级的运动模型通常就是匀速或者匀加速加上对噪声的统计假设就能在实时场景里给出一个相当稳定的位置估计。更重要的是它不需要训练数据不需要GPU在MATLAB里从零实现也就是几十行矩阵运算的事情。相比之下端到端的深度学习跟踪方法虽然上限高但要准备标注数据、要训练、要标定超参在原型验证阶段根本不划算。还有一个容易忽略的点卡尔曼滤波把“不确定性”这个信息也保留下来了。每次预测都会算出协方差矩阵P它告诉我们当前位置估计有多可信。这个信息在做数据关联时特别有用——离预测位置越远的检测框匹配代价就越高等于给错误关联加了一道天然门槛。这是启发式跟踪方法很难做到的事。1.2 MATLAB做这个项目的优势以及它的边界在哪选MATLAB做这个项目不是因为它性能强而是因为它的调试体验和矩阵表达太适合这类算法了。卡尔曼滤波的核心迭代就是矩阵乘法和加法的循环MATLAB写起来几乎和教科书公式一一对应变量名可以直接用A、H、Q、R不需要像C那样操心内存释放和数据布局。另外Computer Vision Toolbox里现成的函数很多peopleDetectorACF可以直接跑行人检测bboxOverlapRatio计算IoUassignmunkres直接做匈牙利匹配一个循环写下来基本就是拼积木。但性能是它的边界。我实测过1080p、30fps的视频用ACF检测器全画面逐帧检测MATLAB里能跑到每秒2到5帧就算不错了。所以这个方案适合的事情是验证算法逻辑、分析误差来源、快速产出演示Demo。真要上嵌入式平台或者做实时视频分析最后还是要转到C或Python重写。不过这不影响它在原型阶段的巨大价值我后面会专门讲从原型到部署迁移时要注意哪些坑。另外提醒一句MATLAB版本对工具箱API影响很大R2021a和R2023b里很多函数用法不一样代码抄来抄去经常报错。我建议直接用R2021a以上的版本下面的代码也都基于这个版本写的。2. 核心模块拆分从运动模型到数据关联2.1 状态向量怎么选4维还是8维行人跟踪里最常用的状态向量有两种一种是4维的[x, y, vx, vy]也就是中心坐标加两个方向上的速度另一种是8维的[x, y, vx, vy, ax, ay, w, h]在位置速度基础上又加了加速度和框尺寸。初学的时候容易觉得状态维度越丰富越好但实际调下来4维状态在大多数行人场景里反而更稳。为什么因为8维状态里加速度分量在普通行人的运动模式下几乎都是零均值的噪声你把它加进去估计器需要花更多帧去收敛这部分的误差效果就是轨迹在启动阶段抖动得更厉害。特别是检测器本身的量测噪声有3、5个像素时高维状态会把一部分噪声“吸收”成虚假的加速度估计反而让预测框飘起来。我的经验是除非你要跟踪的目标有明显的强机动特征比如车辆急加速、无人机变向否则先把4维状态跑通再看误差分析的结果决定要不要加维度。4维状态对应的状态转移矩阵很好写假设帧间间隔为dtA [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1];这里的物理含义很清楚新一帧的位置等于上一帧的位置加上速度乘时间速度则保持不变。真实行人在帧间会有加减速这部分“模型说不准”的差异就交给了过程噪声Q来兜底。换个角度理解匀速模型叠加Q之后实际上等价于一个“带有随机加速度扰动”的运动模型只是简化了表达而已。这个设计思路非常重要它决定了卡尔曼滤波在模型失配时不会直接发散。2.2 量测方程与Q、R参数设定量测方程负责把状态向量映射到我们能观测到的量。在这个项目里检测器给我们的是检测框的中心坐标所以量测矩阵H就是把状态里的位置分量挑出来H [1 0 0 0; 0 1 0 0];接下来是卡尔曼滤波里最玄学的两个参数Q和R。很多新手上来就直接抄别人代码里的数值然后发现换成自己的视频就全乱套。Q的物理意义是“运动模型每帧积累的不确定度”它反映的是目标加速度有多“野”。行人正常行走时Q的对角元取0.1到1量级就够了如果场景里有人突然奔跑、急转就需要调大。R对应的是检测器的量测噪声协方差简单说就是检测框中心位置的抖动程度。ACF检测器在光照稳定的情况下中心抖动一般在1到3个像素换算成平方像素就是1到9遇到遮挡、模糊可能到10以上。我调参时有个习惯先把R定下来因为检测器的噪声特性相对可测你可以手动标注几十帧GT统计检测框中心与真实中心的偏差方差这个值就是R。然后再扫Q从很小的值开始逐步增大离线跑一段验证视频看跟踪误差的变化曲线取RMSE最低的那组Q。这比凭感觉拍脑袋要可靠得多。另外初始协方差P0不用太纠结直接设成对角元100的矩阵表示初始状态很不确定滤波器自己会快速收敛。有一点特别提醒Q和R的量纲不一致。Q里面位置分量的单位是像素平方速度分量的单位是像素/帧平方如果dt不同同一组参数表现会差很多。最好先把位置和速度做归一化处理或者统一到标准帧率下再调参否则换个视频帧率就要重新标定一遍。2.3 数据关联与轨迹管理真正的工程难点卡尔曼滤波本身解决的是“单条轨迹如何平滑预测”但多目标跟踪面临的核心问题其实是谁是“谁”——数据关联。没有正确的关联滤波再完美也白搭两个行人交叉走过去ID一换整条轨迹就废了。最经典的做法是IoU匹配加匈牙利算法。先计算每个检测框和每条预测轨迹框之间的IoU用1减去IoU作为代价矩阵然后用匈牙利算法二分图匹配让总代价最小。MATLAB里这步很方便costMatrix 1 - bboxOverlapRatio(detBoxes, trackBoxes); costMatrix(isnan(costMatrix)) 1; matches assignmunkres(costMatrix);要注意一点assignmunkres处理的是最小化问题而且不接受NaN。bboxOverlapRatio在检测框和轨迹框完全不相交时返回01减去它就是1这没问题但如果出现空矩形之类的异常要提前清洗否则代价矩阵里混进NaN会导致匹配结果整个崩掉。代价矩阵再优化一下还能加gating。对每条轨迹计算检测框与预测位置的马氏距离超过阈值比如3个标准差的直接把代价设为无穷大意思是不允许匹配。这个限制能大幅减少密集人群里的错误关联。更高级的做法是引入颜色直方图或ReID特征向量作为第二层代价但在原型阶段IoU加距离gating已经能覆盖80%以上的场景。轨迹管理也是必须做的。我记得最开始的实现里没有生命周期管理结果就是每帧漏检都产生一条新轨迹200帧视频跑完凭空多了400条“幽灵轨迹”。正确的做法是给每条轨迹标记状态新检测框连续三帧都能关联上才升级为确认轨迹并对外输出确认轨迹如果连续超过maxAge帧没有匹配到检测框才判定死亡并删除。tentative状态下的轨迹不输出这样能过滤掉大量误检引起的碎片轨迹。3. MATLAB手写卡尔曼跟踪器的实操过程3.1 手写还是用工具箱我的选择MATLAB里实现卡尔曼滤波有三种路径一是直接用工具箱的trackerGNN配置好运动模型就能跑二是用configureKalmanFilter这个点跟踪函数三是从零手写预测更新函数。我的建议是如果你是为了交差或者快速出效果用trackerGNN但如果你想搞懂原理、调好参数、做好误差分析一定要手写一遍。理由很直接跟踪器一旦工作不正常工具箱的封装会把你和问题隔离开你连中间状态都看不到排查无从下手。手写版虽然代码多一点但每一步都能打印出P矩阵、状态向量、协方差变化哪里发散一眼就能看出来。我的工程代码里维护了三个核心函数。predict函数做预测步update函数做更新步另外还有一个函数负责初始化新轨迹。预测步的本质是把状态向量和协方差矩阵都按照A矩阵往后推一步同时加上过程噪声Qfunction [x_pred, P_pred] predict(x, P, A, Q) x_pred A * x; P_pred A * P * A Q; end更新步则把预测结果和检测量测做加权融合权重由卡尔曼增益K决定function [x_upd, P_upd] update(x_pred, P_pred, z, H, R) S H * P_pred * H R; K P_pred * H / S; x_upd x_pred K * (z - H * x_pred); P_upd (eye(size(x_pred, 1)) - K * H) * P_pred; % 维持对称性 P_upd (P_upd P_upd) / 2; end这里K的表达式我特意用了右除而不是inv(S)这是一个很容易被忽略的数值稳定性问题。矩阵求逆在S病态时会产生极大的数值误差而用右除运算MATLAB会走数值稳定的线性方程求解路径。对于4维状态的问题区别可能还不明显一旦状态升到8维而且R设置得很小S矩阵接近奇异这时候inv直接就可能把协方差矩阵弄成非对称的后面滤波自然就崩了。3.2 视频读入与检测器接入主程序的第一步是读取视频并获取检测框。VideoReader逐帧读取检测器我用的是ACFAggregate Channel Features检测器为例因为它不需要训练就能跑对直立行人效果稳定很适合做算法验证。接入代码大概是这样vReader VideoReader(test.mp4); detector peopleDetectorACF(); while hasFrame(vReader) frame readFrame(vReader); [boxes, scores] detect(detector, frame); % 过滤低置信度检测框 validIdx scores 0.5; boxes boxes(validIdx, :); ... end这个置信度阈值0.5不是随手写的。ACF输出的分数在不同视频、不同季节的衣着特征下分布差异很大。我在一个室内视频里跑阈值0.3就能滤掉90%的误检换到室外的场景0.3又会让很多误检混进来。所以更稳妥的做法是先跑几帧画出分数直方图看一下分布区间再定阈值。这一步属于预处理做不好后面跟踪器的压力全部转嫁到数据关联上。检测框拿到之后要转成卡尔曼滤波需要的状态向量形式。我这里取的是检测框中心坐标作为量测值状态向量里也是中心坐标加速度。至于框的宽度和高度在这个入门级实现里先不放进状态直接用原始检测框的宽高去画轨迹框等后面做更复杂的多目标再扩展。量测值z记得要转成double类型MATLAB里视频帧是uint8直接拿去做矩阵运算会造成类型不匹配的错误这是新手特别容易踩的坑。3.3 主循环预测、关联、更新、管理整个跟踪主循环是这样的流程对每条轨迹调用predict函数得到本帧的预测位置和预测框用预测框和当前帧检测框计算IoU代价矩阵调用assignmunkres求解最优匹配被匹配上的轨迹用检测框坐标做update更新未匹配的检测框初始化新轨迹先标记为tentative未匹配的轨迹计数加一超过maxAge就删除对确认状态且本帧有更新的轨迹输出并绘制。端正了顺序之后你会发现这个循环就是一个个很清晰的功能模块。写的时候要注意一点同一帧内一组检测框只能匹配一条轨迹。assignmunkres返回的匹配矩阵已经保证了这一点但有些人在自己写贪心匹配时容易忽略这个约束出现一个检测框同时更新两条轨迹的bug后果就是两个目标越靠越近ID就乱了。轨迹管理在代码里其实就是几个数组的状态流转。我给每条轨迹维护一个trackAge计数和一个totalVisibleCount计数连续三帧都被关联到的轨迹状态从tentative转confirmed立即对外输出。一旦某条轨迹的trackAge超过maxAge我常用50帧直接删除存档。如果场景里有较长时间的严重遮挡比如目标在柱子后面躲了半分钟50帧还不够那就要结合外观特征来恢复轨迹卡尔曼滤波本身救不回来。3.4 可视化画框、画轨迹、画ID调试跟踪算法最忌讳只看数字画出来才看得清楚。我用insertShape和insertMarker把检测框、预测框、历史轨迹画在视频帧上frame insertShape(frame, Rectangle, trackBox, Color, yellow, LineWidth, 2); frame insertShape(frame, FilledRectangle, predBox, Color, cyan); frame insertObjectAnnotation(frame, Rectangle, trackBox, sprintf(ID:%d, trackId));这里有个调试心得把检测框画成绿色预测框画成红色历史轨迹画成一条彩色线段序列。这样一眼就能看出卡尔曼滤波预测和检测之间的偏移量。如果绿框动不动盖不住人说明R设小了如果红框一直带头跑在绿框前边说明Q设大了或者模型失配严重。这种“视觉化反馈”比看RMSE数字更直接能帮你迅速定位问题在哪一步。轨迹历史我保存在一个数组里每次更新后把最新的位置追加进去画的时候取最近20帧坐标连线。这样既能看清楚运动趋势又不会让画面被密密麻麻的轨迹线糊满。还有一个小技巧ID的颜色用hsv色相映射这样同一个ID的轨迹和标签框颜色一致多目标场景下辨识度会高很多。4. 误差分析仿真实验与量化指标4.1 误差来源拆解误差分析不能只盯着最终RMSE要把误差分成几个来源分别看。第一是建模误差匀速运动模型无法准确描述突然加速、急转弯这类机动这一部分误差会直接体现在卡尔曼预测的滞后上。第二是量测误差检测器给出的边框本身有位置偏差和尺度偏差即使滤波完全正确也不可能把误差压到检测器本身精度以下。第三是关联误差数据关联匹配错了滤波器会把量测用错目标上这是最隐蔽也最致命的误差它在RMSE上可能表现为一个突然的巨大跳变。第四是数值误差协方差矩阵在反复迭代中可能失去对称性甚至非正定导致滤波器收敛到错误值。理清这些来源之后你再去看误差指标就能判断某个异常是建模的问题还是关联的问题。比如误差曲线在第40帧出现一个巨大尖峰大概率不是Q和R的锅而是关联错误或者遮挡触发了预测失灵。4.2 仿真实验的设计为了把误差分析做公平我用MATLAB构造了一个仿真场景。设定一条带轻微加速度的行人轨迹t 0:1/30:100/30; x_true 10 2 * t 0.05 * t.^2; y_true 50 3 * t;然后给真实位置叠加均值为0、标准差为3像素的高斯噪声模拟检测器输出这组带噪数据就是卡尔曼滤波的输入量测。实验对比三组直接用带噪检测坐标作为轨迹无滤波、经典4维卡尔曼滤波、Q调得偏大的卡尔曼滤波。每组的性能用两个指标衡量位置RMSE和最大单帧误差。跑完的结果很能说明问题。无滤波的RMSE基本等于噪声标准差大约在3.0像素左右经典卡尔曼滤波能把RMSE压到1.2像素上下直观原因就是预测过程把高频随机抖动滤掉了但Q调得过大时RMSE反而回升到2.5像素因为滤波器太信任跟随量测把噪声又放回来了。这三组放一起对比就能直观看出卡尔曼滤波的收益边界在哪里。4.3 参数敏感性Q和R怎么影响结果Q和R的比值决定了滤波器是更相信预测还是更相信量测。Q/R越大卡尔曼增益K越接近1滤波器越跟随检测器噪声滤除效果差Q/R越小K越接近0滤波器越依赖模型预测轨迹平滑但一旦目标真的加速转向误差滞后会非常明显。在仿真中我在固定R的情况下扫描Q记录不同Q对应的位置RMSE得到的曲线是典型的U形Q太小模型不确定度不够遇到加速度变化时跟不上Q太大噪声又滤不干净。最优点一般落在中段某个区域而且附近往往有一段平坦地带说明Q的调参有一定容忍度。有人问为什么不直接把Q往小了设这样轨迹多平滑。答案是不行因为真实视频里检测器不是稳定输出的目标平时匀速、偶尔机动你指望模型完全握住运动规律一旦遇到拐弯就会整个飞掉。适当调大Q相当于给模型留了一条“不确定度缓冲带”这个缓冲带是滤波器稳定性的来源。4.4 真实视频上的误差估计仿真实验能说明滤波器理论上限但真实视频才是检验标准。这里有个关键问题真实视频没有真值RMSE怎么算我的做法是采样标注。从视频里均匀抽出20到30帧手动框出目标的真实位置自己写个简单的鼠标选点脚本就行然后在这些帧上比较跟踪器输出的轨迹坐标与标注坐标计算误差。标注帧数不需要多能覆盖直行、转弯、遮挡几个关键阶段就够了目的是让误差曲线有代表性而不是追求统计上的高精度。在真实视频上跑下来我观察到的误差规律和仿真基本一致但绝对数值会大不少。主要原因是真实检测器不仅噪声大还会偶发漏检和框偏而这恰恰是仿真里最难建模的部分。所以我的建议是仿真用来调参和理解原理真实视频用来验证算法的鲁棒性两者缺一不可。有一点要特别提醒分析误差时不要只看平均RMSE。多目标跟踪场景里ID Switch次数和轨迹断连次数往往比位置误差更影响用户体验。一个目标位置偏3像素没人注意但两个目标ID互换一眼就看出来了。所以误差分析至少要看三个维度一是位置误差RMSE和最大误差二是关联质量ID Switch和碎片轨迹数三是时间维度轨迹在遮挡/漏检场景下能存活多久。5. 常见问题与排查技巧实录5.1 目标丢失、ID互换、轨迹抖动这三个问题几乎每个做跟踪的人都会遇到而且经常会同时出现。目标丢失的典型原因是检测器漏检次数超过maxAge轨迹被误删解决方向不是去调卡尔曼滤波器而是优化检测阈值和轨迹管理参数。ID互换则几乎都是数据关联的锅最直接的诱因是IoU代价在目标交叉或者密集排列时区分度不够办法是加入马氏距离gating限制匹配的最大距离同时给代价矩阵加一行“如果不确定性高就拒绝匹配”的约束。轨迹抖动的根源通常是Q和R比值不对滤波器的平滑能力没有发挥出来重新标定R然后扫描Q通常能消除大部分抖动。这里我提供一个排查顺序能少走很多弯路先看检测器输出稳不稳再看轨迹管理状态对不对然后才动卡尔曼参数。很多人一上来就拧Q和R结果发现检测器本身就在乱跳调滤波参数事倍功半。先把源头数据理顺滤波器的参数空间就干净多了。5.2 卡尔曼发散与协方差病态如果你发现轨迹突然以极快的速度飞出画面或者位置估计在两条完全不连续轨迹之间跳跃大概率是协方差矩阵P出问题了。P矩阵在数学上必须是半正定的但数值计算会破坏这个性质尤其是当Q或R设置不合适或者S矩阵接近奇异时P的对称性就保不住了导致增益计算错误。我的经验是三步排查第一步打印每次更新后P矩阵的本征值出现负值基本就是病态了第二步在update函数里强制重新对称化P (P P) / 2这一步能挽回大部分数值问题第三步如果还发散就把P矩阵的初始值和Q调大一点增加不确定性可以让滤波器对异常量测的抵制能力更强。此外还有一个特别容易忽略的点视频帧率变化会影响dt如果程序中途改了帧率A矩阵里的时间步长没有同步更新模型的时间连续性被打破滤波就会莫名其妙漂移。处理办法是显式把dt作为参数传入预测函数而不是作为固定常量写死在代码里。5.3 MATLAB运行速度优化的几个实用技巧MATLAB跑视频跟踪最大的痛就是慢。我实测同一个逻辑在Python用numpy写一遍大约能快3到5倍C能快一两个数量级。但在原型阶段有几个方法能让MATLAB版本的使用体验好很多优先降分辨率。1080p的视频降到720p甚至640pACF检测速度大幅提升跟踪精度损失有限。别在循环里画图。VideoPlayer或者figure绘制每帧画面非常耗时我先跑完跟踪把轨迹和框都存成结构体最后再统一可视化。实际开发中这种“先盲跑再回放”的模式还能帮你快速跑参数扫描效率提升特别明显。用parfor并行跑参数扫描。Q和R调参时会对同一段视频跑几十组参数group的回放其实是天然并行的parfor一次性跑完省下的时间很可观。只处理ROI区域。如果行人都集中在画面某一区域先用背景差分子检测只在包含目标的ROI里跑检测器速度能快一倍以上。这些优化做完之后即便是MATLAB也能在720p视频上跑到接近实时的速度。当然真要部署到嵌入式平台还得重写成C或者用C代码生成器转一版出来不过这已经属于另一个话题了。5.4 版本差异与工具箱函数坑写这篇文章时我用的是MATLAB R2023b和R2024a两个版本分别验证过trackerGNN、assignmunkres这些函数行为基本一致但ACF检测器的模型文件路径在旧版本里不一样新手经常遇到找不到模型或者报未定义函数的问题。解决办法也说得很俗要么升级到R2020以上版本要么在代码里用which peopleDetectorACF确认函数路径。另一个坑是bboxOverlapRatio的输出是单精度或double取决于输入类型如果检测框是int32类型除法计算出来会有量化误差在代价矩阵里凑出来的微小差异可能影响匈牙利匹配的优先级。统一转成double再做IoU计算是最稳妥的习惯。工具函数有时候也会坑人。assignmunkres要求代价矩阵必须是有限数NaN和Inf都不能有。gating时我把不匹配的代价设为Inf结果调用assignmunkres直接报错一开始还以为是算法实现的问题后来查文档才发现是这个限制。解决起来也不难把Inf换成一个很大的常数比如1e6效果完全一样。写在最后的几句体会这套卡尔曼行人跟踪器的原型我从头到尾写完最大的感触是卡尔曼滤波本身并不难难的是让它在一个不完美的检测系统上稳定工作。每次你把Q和R调得更“合理”总会发现真实视频里有新的意外——突然的低头、剧烈的转身、两三个人挨在一起走路。所以我现在做这类项目一定会预留误差分析的时间和工具把检测、关联、滤波三个模块的误差拆开来看哪一环不行就修哪一环而不是笼统地把所有问题都归咎于某一个参数。最后再分享一个小技巧也是我踩过很多次坑之后才养成的习惯任何时候动手改代码之前先跑一遍当前版本的基线保存好误差指标和可视化的结果。这不只是做对比实验的需求而是你改完参数之后很可能需要回退到上一版。如果没有基线你永远无法回答“这个改动到底是变好了还是变坏了”这个问题。所有调参高手都不是直觉强而是对比做得好。这套方法放到卡尔曼滤波的项目里值得你从一开始就认真对待。