本案例已开源到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方向的运动速度。

本案例的主要仿真参数如下：

程序固定了随机数种子，使运行结果能够重复验证。

需要说明的是，案例中的误差指标对应当前参数设置。改变目标运动模型、测量噪声或滤波参数后，最终结果也会发生变化。

四、X方向位置估计效果

测量值在真实位置附近波动，滤波估计在短暂收敛后稳定跟随真实轨迹。

目标的真实X方向位置整体保持增长趋势，但传感器测量点会在真实位置两侧随机波动。

卡尔曼滤波在初始阶段需要根据连续测量数据修正状态，因此会经历一个短暂的收敛过程。随着测量信息不断输入，估计结果逐渐接近真实位置，同时保留目标原有的运动趋势。

与直接使用原始测量值相比，滤波后的结果具有两个明显特点：

1. 随机跳动明显减少；

2. 没有将运动轨迹过度处理成一条平滑直线。

卡尔曼滤波不是简单地对数据进行平均，而是在抑制测量噪声的同时，尽量保留目标运动状态的变化。

五、Y方向位置估计效果

即使目标运动方向和速度出现变化，滤波结果仍能跟随整体运动趋势。

Y方向轨迹先上升，随后逐渐下降，其变化过程比X方向更加明显。

从结果可以看到，原始测量值仍然存在较大的随机偏差，而卡尔曼滤波估计结果能够比较稳定地跟随真实Y方向位置。

这说明卡尔曼滤波不仅适用于简单的直线轨迹，也能在运动模型允许一定过程扰动的情况下，跟踪带有方向变化的二维运动趋势。

当然，如果目标出现突然转弯、急加速或其他强机动行为，简单的匀速模型可能无法及时跟随。这时需要调整过程噪声参数，或者使用匀加速、转弯模型等更加合适的运动模型。

六、卡尔曼滤波能把误差降低多少？

为了量化滤波效果，本案例分别计算了X方向、Y方向和二维位置的均方根误差RMSE。

在当前仿真条件下，二维位置RMSE由2.7877 m降低至1.6761 m，降低约39.88%。

滤波后的误差整体更低，较大的误差峰值也明显减少。

图中的位置误差采用估计位置与真实位置之间的二维欧氏距离，因此误差值始终大于或等于0。

从结果中可以看到：

• 原始测量误差波动较大；

• 滤波后的误差整体更低；

• 在滤波器初始收敛阶段，个别时刻仍可能出现较大误差；

• 随着状态估计逐渐稳定，卡尔曼滤波的优势更加明显。

需要注意的是，39.88%并不是卡尔曼滤波在所有场景下的固定提升比例。

实际滤波效果还会受到以下因素影响：

• 目标运动模型是否准确；

• 测量噪声大小；

• 过程噪声设置；

• 初始位置和速度；

• 采样频率；

• Q、R、P等参数设置。

因此，判断滤波效果时，不能只观察轨迹是否“看起来更平滑”，还需要结合RMSE、最大误差和动态响应等指标进行综合评价。

七、只测量位置，为什么还能估计速度？

如果直接用相邻两个位置做差计算速度，测量噪声会被进一步放大，导致速度曲线出现明显抖动。

例如，某一时刻的位置测量值稍微偏大，而下一时刻的位置测量值稍微偏小，两者相减后就可能得到一个与真实速度偏差很大的结果。

卡尔曼滤波将速度作为系统状态的一部分，通过位置随时间的连续变化递推估计速度，因此不需要传感器直接提供速度测量。

滤波器在短暂收敛后，能够跟随真实速度的整体变化。

在当前案例中：

• X方向速度估计能够围绕真实速度变化；

• Y方向速度估计能够跟随目标由正向运动逐渐转为负向运动的过程；

• 估计速度仍会受到模型和噪声影响，但比直接对带噪声位置做差更加稳定。

这种“由位置推断速度”的能力，是卡尔曼滤波广泛应用于目标跟踪、导航定位和传感器融合的重要原因之一。

八、Q、R和P参数应该怎么调？

卡尔曼滤波的实际效果与三个参数密切相关：

• 过程噪声协方差Q；

• 测量噪声协方差R；

• 初始估计协方差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语言完成，没有直接调用封装好的卡尔曼滤波函数，而是实现了完整的预测、更新和误差评价过程。

案例主要包括：

1. 二维真实轨迹生成；

2. 带噪声位置测量数据生成；

3. 卡尔曼滤波预测与更新；

4. X、Y方向位置估计；

5. X、Y方向速度估计；

6. 一维和二维RMSE计算；

7. 轨迹、位置、速度和误差可视化。

工程中还使用了以下处理方式：

• 线性方程求解；

• 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