简介一套面向初学者的基于卡尔曼滤波的2维目标跟踪经典MATLAB程序源码聚焦于位置、速度等状态估计与轨迹平滑场景适合想通过实际代码理解卡尔曼滤波原理的入门者。程序源码完整保留卡尔曼滤波的核心环节状态转移矩阵与控制输入矩阵描述系统演化观测矩阵建立量测与状态的关系过程噪声与观测噪声协方差矩阵分别刻画模型和传感器的不确定性再通过卡尔曼增益的动态权衡完成预测与更新两个主要步骤。学习者可在MATLAB中直接运行和调试借助仿真移动目标上的滤波效果直观比较跟踪结果与含噪观测之间的差异从而将抽象公式转化为可重复的数值实验在此基础上还能学会设置与调整初始状态、噪声协方差等关键参数。资源以RAR压缩包提供体积仅33KB轻量易用。截至目前已有2205人学习下载是进一步学习扩展卡尔曼滤波EKF、无迹卡尔曼滤波UKF以及自动驾驶、无人机导航等应用的合适起点。1. 为什么卡尔曼滤波是二维目标跟踪的入门第一课你在雷达回波或者摄像头画面里拿到一堆离散的二维坐标点目标可能是行人、车辆或者无人机每个点都带着量测噪声。直接连线的话轨迹毛刺明显速度也没法稳定算出来。卡尔曼滤波解决的就是这个问题在状态空间模型下用“预测—修正”的递推结构把带噪位置还原成平滑的位置和速度估计。它的代码量在 MATLAB 里通常只有二三十行初学者只要具备矩阵乘法和for循环基础就能写出来。这个标题里的程序把场景限制在二维目标跟踪是最常被拿来做教学样例的类型状态向量取平面位置加速度观测向量取带噪声的位置。卡尔曼滤波本身是惯性导航、雷达跟踪、组合导航等领域的基础算法学会这套代码后续换坐标系、换运动模型、换传感器都是改矩阵的事情。很多初学者卡住的地方不是五条公式而是 Q、R 矩阵怎么设、初始协方差给多少、观测矩阵维度为什么老报错。下面从建模开始逐步把这段经典代码拆开讲清楚。2. 二维目标跟踪建模状态方程、观测矩阵与卡尔曼五公式的 MATLAB 映射2.1 先定运动模型为什么选 4 维状态向量的二维匀速模型目标跟踪的第一步不是写滤波代码而是先回答“目标怎么动”。二维匀速模型 Constant Velocity 是这类教程里最常见的假设目标在 x 方向和 y 方向各自做匀速直线运动两个方向互不耦合。状态向量取 4 维% 状态向量x [px; vx; py; vy] % px,py 是目标在平面上的位置vx,vy 是对应速度 F [1 dt 0 0; 0 1 0 0; 0 0 1 dt; 0 0 0 1];状态转移矩阵F是两个二维匀速子块的组合左上角负责px, vx右下角负责py, vy。每个子块的形式是[1 dt; 0 1]含义是下一个采样时刻的位置等于当前位置加上速度乘以采样间隔速度保持不变。dt 越小模型在时间上取得越密但同样的仿真时长内迭代次数越多。为什么不用三维或者一维三维会多出pz, vz两个状态矩阵规模变成6×6对入门者来说表格和轨迹画图都变复杂一维看不到二维平面上的方向变化。二维恰好能展示“位置速度”四个状态的完整递推过程又方便画图验证这也是标题里“2 维目标”的典型含义。观测矩阵H的选择直接反映传感器能测到什么。雷达或普通定位设备通常只给位置不给速度所以H [1 0 0 0; 0 0 1 0]; % 观测 [px; py] % 这意味着 z H * x_true v其中 v 是量测噪声H把 4 维状态映射到 2 维观测。如果传感器还能测径向速度H就会多一行但初始代码建议保持位置的 2 维观测否则调参难度明显上升。下面这张表汇总了这套模型里所有符号的物理含义代码里的变量名统一照此命名符号MATLAB 变量含义dtdt采样周期相邻两次观测的时间间隔FF状态转移矩阵描述目标运动规律HH观测矩阵描述状态到量测的映射QQ过程噪声协方差模型没考虑到的加速度扰动RR量测噪声协方差传感器本身的误差xx状态估计4×1矩阵PP状态协方差矩阵表示对估计的信任程度2.2 卡尔曼五公式拆解预测两步与更新三步在 MATLAB 里的对应卡尔曼滤波递推过程分两个阶段。预测阶段用运动模型把状态往前推一步x_pred F * x_prev; % 状态预测 P_pred F * P_prev * F Q; % 协方差预测x_pred是先验估计P_pred是先验协方差它必然大于上一拍的P_prev因为预测过程中加入了过程噪声Q不确定性变大了。更新阶段用新的量测去修正这个先验y z - H * x_pred; % 新息 S H * P_pred * H R; % 新息协方差 K P_pred * H / S; % 卡尔曼增益 x_est x_pred K * y; % 状态修正 P_est (eye(4) - K * H) * P_pred; % 协方差修正这里的核心是卡尔曼增益K。它决定“量测和模型各信多少”如果R远小于P_pred说明量测更可靠K偏大滤波结果更贴观测值反过来如果过程模型很可靠而量测噪声很大K变小滤波结果更贴预测值。初学者往往把注意力放在K的计算上实际上K只是P_pred、H、R的派生结果真正需要认真调的是Q和R。3. 用 MATLAB 实现二维目标跟踪的主循环完整源码与参数设置3.1 仿真参数表dt、Q、R 的推荐起始值初学者手头不一定有真实的雷达或视觉数据经典做法是先自己生成一条带噪声的轨迹滤波之后再对照真值验证效果。这样做的优势是“真值已知”可以直观看误差大小。先定一组能直接跑通的参数参数推荐值说明dt0.1采样周期 0.1 秒即 10Hz 量测频率仿真时长10共 100 个采样点画图足够密目标起始位置[0; 0]初始px, py目标速度[0.5; 0.3]初始vx, vy单位 m/s量测噪声标准差0.5位置观测误差约 0.5 米过程噪声强度0.01Q对角线量级后续按效果调整量测噪声标准差 0.5 意味着每个坐标的观测误差大致在正负 1 米内波动肉眼能明显看到毛刺滤波平滑效果容易体现。速度取 0.5 和 0.3 是为了让轨迹在图上呈斜向直线两个方向都有位移方便检查 x、y 两路滤波是否一致。3.2 主循环源码从初始化到卡尔曼增益的完整调用完整的 MATLAB 程序可以拆成三段。第一段生成仿真真值和带噪声观测dt 0.1; % 采样周期 T 0 : dt : 10; % 时间轴 n length(T); % 采样点数 F [1 dt 0 0; 0 1 0 0; 0 0 1 dt; 0 0 0 1]; H [1 0 0 0; 0 0 1 0]; % 只观测位置 R diag([0.5^2, 0.5^2]); % 量测噪声协方差 Q diag([0.01, 0.01, 0.01, 0.01]); % 过程噪声协方差 true_state zeros(4, n); meas zeros(2, n); true_state(:, 1) [0; 0.5; 0; 0.3]; for k 2 : n true_state(:, k) F * true_state(:, k - 1); meas(:, k) H * true_state(:, k) sqrtm(R) * randn(2, 1); endtrue_state每一列是一个时刻的[px; vx; py; vy]。量测由真值映射到位置后叠加高斯白噪声sqrtm(R) * randn(2,1)用来生成协方差为R的二维噪声向量。这里用sqrtm而不是sqrt因为sqrt是对矩阵逐元素开方只有对角线矩阵两者才一致写成sqrtm更通用。第二段是卡尔曼滤波主循环est_state zeros(4, n); % 保存每个时刻的滤波状态 P eye(4) * 100; % 初始协方差取大值表示初值不自信 for k 1 : n if k 1 % 第一帧只有位置量测速度先给 0 x [meas(1, 1); 0; meas(2, 1); 0]; else x est_state(:, k - 1); end % 预测 x_pred F * x; P_pred F * P * F Q; % 更新 z meas(:, k); y z - H * x_pred; % 新息 S H * P_pred * H R; % 新息协方差 K P_pred * H / S; % 卡尔曼增益 x_est x_pred K * y; % 后验状态 P (eye(4) - K * H) * P_pred; % 后验协方差 est_state(:, k) x_est; end这里有三处容易看漏的细节。第一K P_pred * H / S用的是右除等价于P_pred * H * inv(S)但数值稳定性更好不要自己写成inv(S)的形式。第二初始化时P eye(4)*100这是故意“过度自信”的反面操作把协方差设大代表“我完全不相信初值”滤波会在头几步快速收敛。第三Q在预测阶段加入协方差保证P不会因反复更新持续缩小到零否则滤波器会彻底信任历史估计后续量测再新也拉不回来。第三段画图验证plot(true_state(1, :), true_state(3, :), g-, LineWidth, 1.5); hold on; plot(meas(1, :), meas(2, :), r., MarkerSize, 6); plot(est_state(1, :), est_state(3, :), b-, LineWidth, 1.5); legend(真值, 量测, 卡尔曼滤波); xlabel(x 方向位置/m); ylabel(y 方向位置/m); grid on;绿色直线是目标真实运动路径红色点是带噪量测蓝色线是滤波输出。理想情况下蓝线比红色散点明显更贴近绿线且没有明显滞后。如果看到蓝线滞后优先查 Q 是不是给小了如果蓝线跟红点一样毛糙优先查 R 是不是给小了。这个判断规则贯穿所有Q/R调参过程。3.3 观测矩阵 H 与量测向量 z 的维度最常见的发散原因初学者跑这段代码最容易遇到的状态是“P 矩阵爆炸”或者“滤波结果直接飞到十万八千里”八成原因是维度对不上。z是2×1H是2×4y z - H*x_pred得到2×1S H*P_pred*H R得到2×2这些矩阵环环相扣。% 错误示范如果你把 z 写成 4×1但 H 还是 2×4 % 那么 y z - H*x_pred 维度冲突MATLAB 直接报错 % 即使强行用 reshape 改过去滤波结果也不会收敛很多人在网上找代码时会把别人工程里的H直接复制过来可是目标向量里多了一个维度H还是旧尺寸运行时矩阵维度不匹配。排查方式很简单在循环外检查size(H,1)是否等于size(meas,1)size(H,2)是否等于状态向量维度 4。代码写完后第一件事不是跑而是把这几个尺寸打印出来核对。4. MATLAB 调参排错Q、R 矩阵的设计与发散怎么处理4.1 Q 矩阵和 R 矩阵的物理含义以及先调 R 后调 Q 的顺序R矩阵物理意义明确它就是传感器噪声的协方差有量测硬件时直接填对方给出的精度指标。比如某定位设备标称标准差 0.5 米R diag([0.25, 0.25])这就是最合理的初始值。Q矩阵物理意义隐晦它代表“匀速模型没考虑到的所有扰动”比如目标轻微的转弯、加减速、阵风、路面颠簸。Q的取值有两种常见写法。最简单的写法是对角阵Q diag([1e-3, 1e-3, 1e-3, 1e-3]);这种写法把位置噪声和速度噪声当作独立同分布的白噪声方便但物理含义不强。更接近物理含义的做法是把加速度扰动当作连续白噪声经过采样周期离散化后得到q 0.01; % 加速度噪声功率谱密度单位 m^2/s^3 Qt [dt^3/3, dt^2/2; dt^2/2, dt] * q; Q blkdiag(Qt, Qt); % x、y 方向各自一套Qt矩阵中的dt^3/3对应位置过程噪声dt对应速度过程噪声非对角线元素表示位置和速度扰动的关联。这种写法在《估计理论》类教材里最常见也是从“卡尔曼滤波与惯性导航”这类工程场景向纯仿真场景过渡时需要理解的细节。对入门代码两种写法都能跑出类似曲线区别在于第二种对dt的依赖更真实改变采样周期时不需要手动重新整定四个对角线值。调参顺序建议固定为“先定 R再调 Q”。R有物理依据能测量、能查手册不应随意改动。Q是模型信任度的旋钮从小到大扫几轮看效果。下表是判断依据现象可能的参数原因调整方向滤波曲线平滑但明显滞后于真值Q 过小模型太“自信”增大 q 一个量级滤波曲线紧贴噪声毛刺抖动明显Q 过大或 R 过大减小 q或减小 R前几步大幅震荡后半段正常P 初值偏小或初速给错增大 P 初值到 100 以上新息均值长期偏离 0H 或状态模型不匹配重新核对观测模型4.2 状态初值 X0 与协方差 P0差分初始化与保守原则初值设置是另一个影响收敛速度的细节。最省事的做法是“第一帧位置 零速度”也就是第 3 章代码中的处理方式适合纯入门。稍微讲究一点的做法是用前两帧量测做差分估计初始速度if k 1 x [meas(1,1); 0; meas(2,1); 0]; P diag([1, 10, 1, 10]); elseif k 2 % 用第二帧位置和第一帧位置差分出速度 vx_init (meas(1,2) - meas(1,1)) / dt; vy_init (meas(2,2) - meas(2,1)) / dt; x [meas(1,2); vx_init; meas(2,2); vy_init]; P diag([1, 10, 1, 10]); end速度的初始协方差 10 对应约 3 m/s 的不确定度对常见地面目标来说足够宽松。这里有一个初学者常犯的错误P初值取得太小例如P eye(4)滤波器会认为初值很准确第一帧更新时K几乎为零后续量测对状态几乎没有修正作用曲线呈现“缓慢爬过去再逐渐纠偏”的形状。保守原则是宁可把P设大一个量级也不要设小。滤波收敛本质上是P从大到小收缩的过程起点设得略大会多花几个采样周期收敛设小了则要花很长时间才能“纠正错误认知”。4.3 用新息序列做发散检测:卡方检验的 MATLAB 做法滤波结果肉眼看着没问题不代表滤波器内部状态正常。工程上常用的验证手段是检查新息序列y z - H*x_pred的统计特性。理论上的新息服从零均值高斯分布协方差为S H*P_pred*H R。把每个时刻的新息除以对应标准差归一化新息应该落在正负 3 之间且没有明显的趋势性偏离。innov_norm zeros(2, n); for k 1 : n % 把第 k 步的 P_pred 和 S 存到数组里再计算 innov_norm(1, k) innov(1, k) / sqrt(S_hist(1, 1, k)); innov_norm(2, k) innov(2, k) / sqrt(S_hist(2, 2, k)); end plot(T, innov_norm(1, :), .-); yline(3, r--); % 注意 R2016b 之后才有 yline yline(-3, r--);如果超过 3σ 的点比例明显大于理论值说明滤波器过于乐观实际误差比自身估计的大。最常见的两个原因Q给小了导致P持续收缩、滤波器过度自信或者目标发生了模型之外的机动例如突然转弯。卡方检验的正式做法是计算新息马氏距离y * inv(S) * y与自由度为 2 的卡方分布分位数比较超过 9.21 时判定为异常适用雷达跟踪等实时场景。入门阶段先看innov_norm是否经常超 3σ 就够用了。5. 从 MATLAB 仿真到工程验证RMSE 评估与扩展卡尔曼滤波的衔接5.1 用误差曲线验证滤波效果直接量测误差对比滤波误差单次画图只能给人眼一个“看起来变平滑”的印象工程上要通过误差曲线量化。位置估计误差定义为滤波输出的px, py与真实轨迹的欧氏距离量测误差定义为带噪观测与真值的欧氏距离filter_pos sqrt((est_state(1,:) - true_state(1,:)).^2 ... (est_state(3,:) - true_state(3,:)).^2); measure_pos sqrt((meas(1,:) - true_state(1,:)).^2 ... (meas(2,:) - true_state(3,:)).^2); plot(T, measure_pos, r., MarkerSize, 5); hold on; plot(T, filter_pos, b-, LineWidth, 1.5); legend(量测误差, 滤波误差); xlabel(时间/s); ylabel(位置误差/m);量测误差围绕 0.5 米上下波动滤波误差在滤波收敛后应该显著低于量测误差通常能压到 0.3 米以下。如果头几步误差很大那是初始收敛阶段正常现象。如果滤波误差长期高于量测误差说明滤波器还不如直接用观测值这时优先检查Q是否太小或者运动模型和真值轨迹不一致。5.2 蒙特卡洛跑 20 次的 RMSE比单次更有说服力单次仿真的随机噪声会影响结论同样一组Q、R参数某一次刚好噪声小误差曲线很好看换个随机种子立刻变差。所以每次调完参数后把整个仿真循环包在for循环里重复 20 次计算累积 RMSE再对比参数好坏rmse_list zeros(20, 1); for mc 1 : 20 % 上面 3.2 节的完整滤波代码移进来 % 最后计算整条轨迹的 RMSE rmse_list(mc) sqrt(mean(filter_pos.^2)); end mean_rmse mean(rmse_list);固定随机种子的做法只在复现问题上有效调参评估请改用蒙特卡洛。用均值 RMSE 做横轴对比扫几组q [0.001, 0.01, 0.1, 1]能看到 RMSE 先降后升的 U 形曲线最低点对应的q就是当前场景下的近似最优过程噪声强度。这一步做完你对滤波器在当前场景下的上限心里彻底有数后续换传感器、改场景都沿着同样的验证流程走。5.3 下一步扩展卡尔曼滤波与 CTRV 模型怎么接上二维匀速直线是经典入门场景目标一转弯匀速模型立刻失配新息序列连续超 3σ。工程常见做法是换成 CTRV 恒定转弯率与速度模型状态向量变成[px; py; v; psi; omega]状态转移方程出现三角函数F不再固定而是每步实时求雅可比矩阵。这一步踩进去就进入扩展卡尔曼滤波的范畴预测和更新流程不变但F、H换成当前状态处求导得到的雅可比矩阵Q、R的设置逻辑仍然沿用第 4 章的方法。类似地雷达量测常在极坐标系下输出距离和方位角而目标状态是在直角坐标系里建模的观测方程h(x)是非线性的这也是H矩阵无法直接写死、必须换成雅可比矩阵的典型场景。你可以把第 3 章的H替换成极坐标量测方程然后求偏导得到每步动态更新的Hk这就把一维的入门程序扩展成了工程级的雷达目标跟踪方案。本文还有配套的精品资源点击获取