本案例已开源到MoHub，可点击链接获取：https://mohub.net/model/26332/summary
在雷达跟踪、GPS定位、移动机器人、无人车导航和传感器数据处理中，目标位置通常会受到测量噪声影响，表现为轨迹抖动、位置跳变或速度估计不稳定。
卡尔曼滤波的作用，就是在“运动模型预测”和“传感器测量结果”之间不断进行权衡，从带有噪声的数据中，估计出更加接近目标真实状态的位置和速度。
本文结合一个基于MWORKS.Syslab与Julia的二维目标跟踪案例，展示卡尔曼滤波如何实现：
二维运动轨迹平滑；
X、Y方向位置估计；
二维位置误差降低；
在只测量位置的情况下估计目标速度。
在本案例的仿真参数和固定随机数种子下，卡尔曼滤波将二维位置均方根误差RMSE由2.7877 m降低至1.6761 m，误差降低约39.88%。
蓝色曲线为真实轨迹，散点为带噪声的测量位置，虚线为卡尔曼滤波估计轨迹。
从图中可以看到，原始测量点在真实轨迹附近存在明显波动，而卡尔曼滤波估计轨迹更加连续、平滑，同时能够较好地跟随目标整体运动趋势。
一、二维目标轨迹为什么会出现抖动？
在实际系统中，目标位置通常由雷达、GPS、视觉传感器、编码器或其他测量设备获取。
受传感器精度、环境干扰、通信延迟和采样误差影响，测量结果很难与目标真实位置完全一致。
常见现象包括：
GPS轨迹在真实路径附近来回跳动；
雷达测得的目标位置存在随机偏差；
机器人定位点不连续；
无人车轨迹出现局部抖动；
直接对位置数据做差后，速度结果噪声很大。
简单的移动平均虽然可以让曲线变得平滑，但可能带来明显的响应滞后，也无法同时估计目标速度等无法直接测量的状态。
卡尔曼滤波不仅利用当前测量值，还会结合目标之前的状态和运动模型进行预测，因此更加适合连续动态系统中的状态估计问题。
二、卡尔曼滤波是怎么跟踪目标的？
卡尔曼滤波在每个采样时刻主要执行两个过程：预测和更新。
1. 预测
滤波器根据上一时刻的位置、速度和运动模型，预测目标当前可能处于什么位置。
例如，若目标上一时刻的位置和速度已知，就可以根据采样时间估计其下一时刻的位置。
2. 更新
当新的传感器测量结果到来后，滤波器会将测量结果与预测结果进行比较，再利用卡尔曼增益修正状态估计。
可以将这一过程简单理解为：
运动模型告诉滤波器“目标大概应该在哪里”，传感器告诉滤波器“这次测到了哪里”，卡尔曼滤波根据两者的不确定程度给出综合判断。
如果传感器噪声较大，滤波器会更加信任运动模型；如果目标运动状态变化较快，则需要提高滤波器对新测量信息的响应能力。
三、二维目标跟踪模型如何建立？
本案例假设目标在二维平面内做近似匀速运动，系统状态中同时包含位置和速度：
状态 = [X方向位置，Y方向位置，X方向速度，Y方向速度]传感器只提供二维位置测量：
测量 = [X方向位置，Y方向位置]也就是说，系统并没有直接测量目标速度，但卡尔曼滤波可以利用连续的位置变化和运动模型，间接估计X、Y方向的运动速度。
本案例的主要仿真参数如下：
| 参数 | 设置值 | 说明 |
|---|---|---|
| 采样时间 | 1.0 s | 相邻两次测量的时间间隔 |
| 仿真步数 | 100 | 生成100组运动与测量数据 |
| 初始位置 | [0, 0] | 目标从坐标原点附近出发 |
| 初始速度 | [1.2, 0.8] | X、Y方向初始速度 |
| 测量噪声标准差 | 约2 m | 模拟位置传感器误差 |
| 初始估计速度 | [0, 0] | 初始时不知道目标真实速度 |
程序固定了随机数种子，使运行结果能够重复验证。
需要说明的是，案例中的误差指标对应当前参数设置。改变目标运动模型、测量噪声或滤波参数后，最终结果也会发生变化。
四、X方向位置估计效果
测量值在真实位置附近波动，滤波估计在短暂收敛后稳定跟随真实轨迹。
目标的真实X方向位置整体保持增长趋势，但传感器测量点会在真实位置两侧随机波动。
卡尔曼滤波在初始阶段需要根据连续测量数据修正状态，因此会经历一个短暂的收敛过程。随着测量信息不断输入，估计结果逐渐接近真实位置，同时保留目标原有的运动趋势。
与直接使用原始测量值相比，滤波后的结果具有两个明显特点：
随机跳动明显减少；
没有将运动轨迹过度处理成一条平滑直线。
卡尔曼滤波不是简单地对数据进行平均，而是在抑制测量噪声的同时，尽量保留目标运动状态的变化。
五、Y方向位置估计效果
即使目标运动方向和速度出现变化，滤波结果仍能跟随整体运动趋势。
Y方向轨迹先上升，随后逐渐下降，其变化过程比X方向更加明显。
从结果可以看到，原始测量值仍然存在较大的随机偏差，而卡尔曼滤波估计结果能够比较稳定地跟随真实Y方向位置。
这说明卡尔曼滤波不仅适用于简单的直线轨迹，也能在运动模型允许一定过程扰动的情况下，跟踪带有方向变化的二维运动趋势。
当然，如果目标出现突然转弯、急加速或其他强机动行为，简单的匀速模型可能无法及时跟随。这时需要调整过程噪声参数，或者使用匀加速、转弯模型等更加合适的运动模型。
六、卡尔曼滤波能把误差降低多少？
为了量化滤波效果，本案例分别计算了X方向、Y方向和二维位置的均方根误差RMSE。
| 指标 | 原始测量 | 卡尔曼滤波 |
|---|---|---|
| X方向位置RMSE | 2.1001 m | 1.1016 m |
| Y方向位置RMSE | 1.8332 m | 1.2632 m |
| 二维位置RMSE | 2.7877 m | 1.6761 m |
在当前仿真条件下，二维位置RMSE由2.7877 m降低至1.6761 m，降低约39.88%。
滤波后的误差整体更低，较大的误差峰值也明显减少。
图中的位置误差采用估计位置与真实位置之间的二维欧氏距离，因此误差值始终大于或等于0。
从结果中可以看到：
原始测量误差波动较大；
滤波后的误差整体更低；
在滤波器初始收敛阶段，个别时刻仍可能出现较大误差；
随着状态估计逐渐稳定，卡尔曼滤波的优势更加明显。
需要注意的是，39.88%并不是卡尔曼滤波在所有场景下的固定提升比例。
实际滤波效果还会受到以下因素影响：
目标运动模型是否准确；
测量噪声大小；
过程噪声设置；
初始位置和速度；
采样频率；
Q、R、P等参数设置。
因此，判断滤波效果时，不能只观察轨迹是否“看起来更平滑”，还需要结合RMSE、最大误差和动态响应等指标进行综合评价。
七、只测量位置，为什么还能估计速度？
如果直接用相邻两个位置做差计算速度，测量噪声会被进一步放大，导致速度曲线出现明显抖动。
例如，某一时刻的位置测量值稍微偏大，而下一时刻的位置测量值稍微偏小，两者相减后就可能得到一个与真实速度偏差很大的结果。
卡尔曼滤波将速度作为系统状态的一部分，通过位置随时间的连续变化递推估计速度，因此不需要传感器直接提供速度测量。
滤波器在短暂收敛后，能够跟随真实速度的整体变化。
在当前案例中：
X方向速度估计能够围绕真实速度变化；
Y方向速度估计能够跟随目标由正向运动逐渐转为负向运动的过程；
估计速度仍会受到模型和噪声影响，但比直接对带噪声位置做差更加稳定。
这种“由位置推断速度”的能力，是卡尔曼滤波广泛应用于目标跟踪、导航定位和传感器融合的重要原因之一。
八、Q、R和P参数应该怎么调？
卡尔曼滤波的实际效果与三个参数密切相关：
过程噪声协方差Q；
测量噪声协方差R；
初始估计协方差P。
它们分别表示系统对运动模型、传感器测量和初始状态的不确定程度。
| 调整方式 | 主要影响 |
|---|---|
| 增大R | 降低对测量值的信任，结果更平滑，但可能响应变慢 |
| 减小R | 更加信任测量值，响应更快，但容易跟随测量噪声波动 |
| 增大Q | 允许目标状态发生较大变化，更适合机动目标 |
| 减小Q | 更加信任原有运动模型，适合运动状态较稳定的目标 |
| 增大初始P | 表示初始状态不确定性较高，滤波器初期修正幅度更大 |
1. 滤波后的轨迹仍然太抖怎么办？
可以适当增大R，降低滤波器对测量数据的信任程度。
滤波器会更加依赖运动模型进行预测，轨迹通常会变得更加平滑。但R不能设置得过大，否则可能导致轨迹响应滞后，无法及时跟随目标状态变化。
2. 目标转弯后，滤波结果跟不上怎么办？
可以适当增大Q，使滤波器允许目标状态发生更明显的变化。
如果目标经常出现急转弯、加速或减速，仅调整Q可能仍然不够，还需要将简单的匀速模型扩展成匀加速模型、转弯模型或其他更加符合目标运动特点的模型。
3. 初始阶段误差很大怎么办？
需要检查初始位置、初始速度和初始协方差P是否合理。
如果初始速度未知，可以增大对应状态的不确定性，使滤波器在初始阶段更加依赖测量数据，快速调整位置和速度估计。
4. Q和R是否有固定的推荐值？
没有一组参数可以适用于所有应用场景。
实际工程中，Q和R通常需要结合以下信息进行设置：
传感器精度；
测量数据的历史方差；
目标机动能力；
采样时间；
运动模型误差；
实际测试结果。
可以先根据传感器说明书和历史数据估计R，再根据目标运动变化程度调整Q，最后结合轨迹平滑程度和误差指标进行反复验证。
九、这个案例适合哪些应用场景？
卡尔曼滤波二维目标跟踪可以作为以下应用场景的基础。
1. 定位与导航
GPS轨迹平滑；
移动机器人定位；
无人车轨迹估计；
无人机位置与速度估计。
2. 雷达与目标跟踪
雷达目标位置滤波；
二维运动目标跟踪；
船舶或车辆轨迹估计；
多目标跟踪算法中的基础状态估计。
3. 传感器数据处理
带噪声位置数据平滑；
编码器与位置传感器数据融合；
GPS与IMU融合定位；
多源测量结果融合。
4. 教学与科研
自动控制原理；
现代控制理论；
随机过程；
数字信号处理；
导航与定位；
机器人与无人系统课程。
十、MWORKS案例实现了哪些内容？
本案例在MWORKS.Syslab中使用Julia语言完成，没有直接调用封装好的卡尔曼滤波函数，而是实现了完整的预测、更新和误差评价过程。
案例主要包括：
二维真实轨迹生成；
带噪声位置测量数据生成；
卡尔曼滤波预测与更新；
X、Y方向位置估计；
X、Y方向速度估计；
一维和二维RMSE计算；
轨迹、位置、速度和误差可视化。
工程中还使用了以下处理方式：
线性方程求解；
Joseph形式协方差更新；
协方差矩阵对称化；
数组预分配；
固定随机数种子；
多指标误差评价。
这些处理可以提高算法的数值稳定性、计算效率和结果可重复性。
对于学习者来说，可以直接运行案例观察结果，也可以修改以下参数：
测量噪声大小；
过程噪声大小；
目标初始位置；
目标初始速度；
采样时间；
目标运动轨迹；
Q、R和P参数。
通过比较修改前后的轨迹和误差变化，可以更加直观地理解卡尔曼滤波的工作机制。
十一、还可以继续扩展哪些方向？
当前案例采用的是二维近似匀速运动模型，在此基础上还可以继续扩展以下内容：
二维匀加速目标跟踪；
转弯和机动目标跟踪；
扩展卡尔曼滤波EKF；
无迹卡尔曼滤波UKF；
雷达极坐标观测；
GPS与IMU融合；
多传感器融合定位；
多目标跟踪；
交互式多模型算法；
Q、R参数自动优化。
如果目标运动具有明显的非线性，或者传感器观测模型不是线性的，就需要进一步考虑EKF、UKF或粒子滤波等方法。
例如，雷达传感器通常提供距离和角度信息，而目标状态可能采用笛卡尔坐标表示，此时测量模型具有非线性，就更适合使用扩展卡尔曼滤波或无迹卡尔曼滤波。
十二、完整案例获取
完整工程已在MoHub开放，包含：
Julia程序文件；
卡尔曼滤波核心算法；
轨迹与测量数据生成程序；
RMSE评价程序；
TyPlot绘图程序；
中文README；
参数说明和运行方法。
案例可以在MWORKS.Syslab中直接运行，并根据需要修改目标运动状态、测量噪声和滤波参数。
案例名称：卡尔曼滤波二维运动目标跟踪
MoHub案例地址：
https://mohub.net/model/26332/summary
总结
卡尔曼滤波通过融合运动模型和带噪声测量数据，可以实现二维目标的位置估计、轨迹平滑和速度估计。
在本案例的参数条件下，滤波后的二维位置RMSE由2.7877 m降低至1.6761 m，误差降低约39.88%。
从轨迹、位置和误差对比结果可以看到，卡尔曼滤波估计结果比原始测量数据更加连续、稳定，同时能够保持对目标运动趋势的跟踪。
对于正在学习卡尔曼滤波、目标跟踪、GPS轨迹处理、机器人定位或传感器数据融合的用户，可以通过该MWORKS.Syslab案例理解完整计算流程，并在此基础上进一步扩展EKF、UKF和多传感器融合等应用场景。
关键词：卡尔曼滤波、二维目标跟踪、轨迹平滑、GPS轨迹滤波、位置估计、速度估计、传感器数据融合、机器人定位、雷达目标跟踪、Julia、MWORKS、Syslab