用户手册
Gengjian/Gengjian20260710143424
仓库范围 基于卡尔曼滤波二维目标跟踪
简介：本案例使用 Julia 语言，在 MWORKS.Syslab 中自行实现标准线性卡尔曼滤波，对二维平面内匀速运动目标的位置和速度进行递推估计，并通过轨迹曲线、误差曲线和均方根误差评价滤波效果。
教学案例信息与通讯工程机器人
介绍 文件

基于 Julia 与 MWORKS.Syslab 的卡尔曼滤波二维运动目标跟踪

案例简介

本案例使用 Julia 语言在 MWORKS.Syslab 环境中实现基于标准卡尔曼滤波的二维运动目标跟踪仿真。模拟目标在二维平面内做近似匀速运动，为其位置加入高斯随机测量噪声，利用自行编写的卡尔曼滤波算法对目标的二维位置和速度进行实时估计，并通过 Syslab 的 TyPlot 科学绘图工具对滤波效果进行可视化分析与评价。

本案例使用 Julia 语言开发，运行于 MWORKS.Syslab 环境，并调用 Syslab 提供的矩阵计算、线性代数、随机数、统计分析和科学绘图等工具完成二维目标跟踪仿真。

适用课程

  • 自动控制原理
  • 现代控制理论
  • 随机过程
  • 数字信号处理
  • 导航与定位
  • 机器人技术
  • 无人系统
  • 传感器数据融合
  • Julia 科学计算

适用场景

  • 雷达/激光雷达目标跟踪教学演示
  • 计算机视觉目标运动估计
  • 传感器数据滤波与降噪
  • 导航系统位置估计
  • 无人机/机器人运动状态估计

开发环境

项目 说明
编程语言 Julia 1.10
运行平台 MWORKS.Syslab 2026a
操作系统 Windows / Linux（Syslab 支持平台）

Julia 语言说明

本案例全部代码使用 Julia 语言编写，遵循 Syslab 支持的 Julia 1.10 语法规范。

使用的 Syslab 工具箱

工具箱/标准库 用途
LinearAlgebra 矩阵运算、Cholesky 分解
Statistics 统计计算（均值等）
Random 随机数生成、种子固定
TyPlot 科学绘图与可视化
Printf 格式化控制台输出

卡尔曼滤波基本原理

卡尔曼滤波是一种递归的最优线性状态估计器，适用于线性高斯系统的状态估计。其核心思想是：

  1. 预测步骤：基于系统模型和上一时刻的状态估计，预测当前时刻的状态和协方差；
  2. 更新步骤：利用当前时刻的测量值对预测进行修正，得到最优状态估计。

每个滤波周期依次执行：状态预测 → 协方差预测 → 新息计算 → 卡尔曼增益计算 → 状态更新 → 协方差更新。

状态变量与观测变量

状态变量（4维）

变量 含义 单位
px X 方向位置 m
py Y 方向位置 m
vx X 方向速度 m/s
vy Y 方向速度 m/s

观测变量（2维）

变量 含义 单位
px X 方向位置测量 m
py Y 方向位置测量 m

状态空间模型

运动模型

目标假设在二维平面内做近似匀速运动，过程噪声用于模拟速度的微小随机变化。

x(k) = F · x(k-1) + w(k)

状态转移矩阵 F（4×4）：

F = [1  0  dt  0;
     0  1  0   dt;
     0  0  1   0;
     0  0  0   1]

观测模型

仅观测二维位置：

z(k) = H · x(k) + v(k)

观测矩阵 H（2×4）：

H = [1  0  0  0;
     0  1  0  0]

其中 dt = 1.0s，w(k) 和 v(k) 均为零均值高斯白噪声。

文件结构

KalmanFilter2DTracking/
├── main.jl                    # 主程序入口
├── generate_target_data.jl    # 目标运动数据生成
├── kalman_filter_2d.jl        # 卡尔曼滤波核心算法
├── evaluate_tracking.jl       # 跟踪效果评价指标
├── plot_tracking_results.jl   # 结果可视化绘图
└── README.md                  # 本说明文件

运行方法

在 MWORKS.Syslab 中打开工程目录，运行主程序：

include("main.jl")

程序将自动完成：

  1. 设置随机数种子，保证结果可复现
  2. 生成真实运动轨迹与含噪声测量数据
  3. 执行卡尔曼滤波估计
  4. 计算并输出 RMSE 评价结果
  5. 生成五组结果图

参数说明

参数 含义 默认值
dt 采样时间间隔 1.0 s
N 仿真步数 100
xTrue0 目标初始状态 [0, 0, 1.2, 0.8]
Q 过程噪声协方差矩阵 diag([0.01, 0.01, 0.05, 0.05])
R 测量噪声协方差矩阵 diag([4.0, 4.0])
P0 初始协方差矩阵 diag([10, 10, 5, 5])
xEst0 滤波器初始状态 [0, 0, 0, 0]
seed 随机数种子 2026

仿真结果

图号 图名 说明
图1 二维运动目标卡尔曼滤波跟踪结果 真实轨迹、测量点、滤波估计轨迹对比
图2 X方向位置随时间变化 X位置真实值、测量值、估计值对比
图3 Y方向位置随时间变化 Y位置真实值、测量值、估计值对比
图4 二维位置误差对比 原始测量误差与卡尔曼滤波误差对比
图5 目标速度估计 X/Y方向真实速度与估计速度对比

RMSE 评价结果

运行后控制台输出示例：

============================================================
   卡尔曼滤波二维运动目标跟踪 —— RMSE 评价结果
============================================================

测量位置 X 方向 RMSE：2.1001 m
测量位置 Y 方向 RMSE：1.8332 m
测量位置二维 RMSE：    2.7877 m

滤波位置 X 方向 RMSE：1.1016 m
滤波位置 Y 方向 RMSE：1.2632 m
滤波位置二维 RMSE：    1.6761 m

卡尔曼滤波误差降低比例：39.88 %
============================================================

结论：

  1. 测量点在真实轨迹周围存在明显随机波动
  2. 卡尔曼滤波估计轨迹明显更加平滑，能有效跟随真实目标运动
  3. 滤波后的二维位置 RMSE 显著小于测量 RMSE（降低约 39.88%）
  4. 初始阶段允许存在短暂滤波收敛过程

参数调整方法

  1. 增大 Q 矩阵：增大过程噪声协方差，滤波器对模型的信任度降低，将更依赖测量值，响应更快但平滑效果减弱
  2. 增大 R 矩阵：增大测量噪声协方差，滤波器将更信任运动模型，输出更平滑但对真实变化的响应变慢
  3. 调整 P0：增大初始协方差，滤波器初期收敛更快，但初期估计波动更大
  4. 改变速度：修改 xTrue0 中的 vx、vy 值可改变目标运动速度

卡尔曼增益分析

卡尔曼增益 K 决定了预测值与测量值在状态更新中的权重分配。增益较大时滤波器更信任测量值（响应快但噪声大），增益较小时更信任模型预测（平滑但响应慢）。本案例中增益通过求解线性方程组 S \ (H * PPred) 计算，避免显式矩阵求逆，提高数值稳定性。

协方差矩阵使用 Joseph 形式

状态协方差更新采用 Joseph 形式：

P = (I - K·H) · P_pred · (I - K·H)^T + K · R · K^T

该形式在数值上比简化形式更稳定，能更好地保持协方差矩阵的对称正定性。每次更新后还进行对称化处理：

P = (P + P^T) / 2

常见问题

Q1：为什么选择 Joseph 形式而非简化形式更新协方差？

Joseph 形式对舍入误差更具鲁棒性，即使在计算精度有限的情况下也能保证协方差矩阵的对称正定性，避免滤波发散。

Q2：卡尔曼增益不直接求逆吗？

本案例使用 S \ (H · PPred) 即线性方程组求解来计算增益，等效于 inv(S) * H * PPred，但数值更稳定。

Q3：如何调整滤波的平滑程度？

调大 R 使滤波器更信任模型（输出更平滑）；调小 R 使滤波器更信任测量（响应更快但噪声较大）。

Q4：初始估计不准确有什么影响？

本案例将初始估计 xEst0 设为零向量以体现滤波器从无信息状态开始收敛的过程，滤波初期会有短暂的收敛阶段。

可扩展方向

  • 二维匀加速目标跟踪（CA模型）
  • 转弯目标跟踪（CT模型）
  • 扩展卡尔曼滤波（EKF）处理非线性观测
  • 无迹卡尔曼滤波（UKF）
  • 交互式多模型（IMM）算法
  • 雷达目标跟踪（极坐标观测）
  • GPS 与惯性导航融合
  • 多传感器数据融合
  • 多目标跟踪（数据关联）
  • 卡尔曼滤波参数自动优化
  • 不同噪声条件下的算法性能对比分析
  • 自适应卡尔曼滤波（噪声协方差在线估计）