基于当前统计模型与粒子滤波的机动目标跟踪系统

资 源 简 介

本项目是一个基于MATLAB开发的综合性仿真平台，专注于解决机动目标的精确跟踪问题。系统核心基于“当前”统计模型（Current Statistical Model, CSM），该模型利用非零均值的时间相关瑞利分布来描述机动加速度，能够自适应地调整过程噪声协方差，从而显著提高对高机动目标（如战斗机、导弹等）的跟踪精度。项目主要功能包括：1. 实现了基于当前统计模型的自适应卡尔曼滤波算法，包含完整的状态方程和观测方程构建，能够对目标的位置、速度和加速度进行实时估计；2. 集成了粒子滤波（Particle Filter）算法，用于处理非线性、非高斯条件下的复杂目标跟踪任务，并可作为基准算法进行性能对比；3. 包含了生成模拟机动轨迹（如匀加速、转弯、机动突防等）的仿真器，以及添加高斯白噪声的观测数据生成模块；4. 提供了丰富的可视化工具，能够绘制真实轨迹与估计轨迹的对比图、三维误差分析图、位置RMSE（均方根误差）曲线、速度及加速度跟踪性能曲线，帮助用户深入理解算法在不同机动强度下的表现。随项目附带相关子程序和参考文献，便于深入研究算法原理。

详 情 说 明

项目：基于当前统计模型与粒子滤波的机动目标跟踪系统

1. 项目介绍

本项目是一个基于MATLAB开发的机动目标跟踪仿真平台。系统旨在解决高机动目标的精确状态估计问题，核心采用了“当前”统计模型（Current Statistical Model, CSM）结合卡尔曼滤波（KF），并引入粒子滤波（Particle Filter, PF）作为对比算法。

该程序通过模拟包含匀速、匀加速和变加速（转弯/机动突防）等多种运动模式的复杂轨迹，验证了CSM算法在自适应调整过程噪声协方差方面的优势，同时也展示了粒子滤波在处理非线性/非高斯问题上的通用性。

2. 功能特性

• 多阶段机动轨迹仿真：能够生成包含平稳飞行、机动加速和机动转弯/减速的组合轨迹，真实模拟目标的复杂运动特性。
• 自适应卡尔曼滤波（CSM-KF）：实现了基于当前统计模型的自适应滤波，能够根据当前的加速度估计值动态调整过程噪声协方差矩阵（Q阵），从而在目标发生机动时快速响应。
• 粒子滤波（PF）基准对比：集成了基于序列重要性重采样（SIR）的粒子滤波算法，使用恒定加速度（CA）模型进行粒子传播，作为系统性能的对比基准。
• 量测数据生成：模拟了仅包含位置信息（X, Y）的观测数据，并添加了高斯白噪声。
• 全面的性能评估：计算并显示位置、速度、加速度的均方根误差（RMSE），并统计全局平均位置误差。
• 可视化分析：提供二维轨迹对比图、三维误差分析图以及单轴状态跟踪详情图。

3. 系统要求

• MATLAB R2016b 或更高版本
• 不需要额外的工具箱（代码基于基础矩阵运算实现）

4. 使用方法

直接运行 MATLAB 中的 main 函数即可启动仿真。程序将依次执行以下步骤：

1. 初始化参数并生成真实机动轨迹。
2. 生成带噪声的模拟观测数据。
3. 运行CSM-KF算法进行状态估计。
4. 运行PF算法进行状态估计。
5. 计算误差指标并在控制台输出平均位置误差。
6. 自动弹出三组分析图表。

5. 代码详细功能与实现逻辑

5.1 参数初始化与环境设置

程序首先固定随机种子 rng(1) 以确保仿真结果的可复现性。定义了总时长100秒，采样周期1秒。关键物理参数包括机动频率 alpha (1/20) 和最大加速度 a_max (60 m/s²)。

5.2 真实轨迹生成 (True Trajectory Generation)

系统构建了一个 6维状态向量 [x, vx, ax, y, vy, ay]，通过分段逻辑模拟复杂的机动过程：

• 1-30秒：匀速直线运动。
• 31-60秒：X轴进行匀加速机动，Y轴保持匀速。
• 61-100秒：X轴和Y轴同时进行变加速/减速机动，模拟协同转弯或规避动作。
• 实现方式：利用基础牛顿运动学公式积分，并在特定时间段强制修改加速度输入 u_k，同时加入微量的过程噪声 sigma_w 增加随机性。

5.3 观测数据模拟

基于真实轨迹的位置信息（X, Y），叠加标准差为 sigma_v (10m) 的高斯白噪声，生成观测向量 Z。

5.4 算法实现细节

#### 算法一：当前统计模型自适应卡尔曼滤波 (CSM-KF)

• 状态预测：使用 get_CSM_Phi 函数构建离散化的 Singer 模型状态转移矩阵 F_csm。
• 过程噪声自适应：这是算法的核心。在每一步迭代中，利用上一时刻的加速度估计值，通过 get_adaptive_sigma 函数计算针对当前机动强度的加速度方差 sigma_a。
• Q阵构建：结合机动频率 alpha 和计算出的 sigma_a，利用 get_CSM_Q_structure 构建动态更新的过程噪声协方差矩阵 Q_csm。
• 均值修正：利用当前统计模型的特性，将上一时刻的加速度作为当前时刻加速度均值的非零先验。
• 滤波更新：执行标准的卡尔曼滤波更新步骤（计算卡尔曼增益 K、更新状态估计 X 和 协方差 P）。

#### 算法二：粒子滤波 (Particle Filter)

• 粒子初始化：在初始真实状态附近生成 1000 个粒子。
• 粒子传播：采用 恒定加速度模型 (CA) 作为粒子的演化模型，并施加较大的固定过程噪声 Q_pf_diag 以覆盖潜在的机动范围。
• 权重计算：基于观测值 Z 与粒子预测位置的欧氏距离，计算高斯似然函数来更新粒子权重。
• 状态估计：采用加权求和的方式计算当前时刻的状态估计值。
• 重采样：调用 resample_systematic 函数执行系统重采样，解决粒子退化问题。

5.5 结果分析与可视化

代码最后部分计算了 CSM-KF 和 PF 在位置、速度、加速度三个维度的 RMSE，并通过图形展示：

• 图1 (Target Trajectory Tracking)：在 X-Y 平面上绘制真实轨迹、量测点、CSM-KF 估计轨迹和 PF 估计轨迹的对比。
• 图2 (Estimation Errors analysis)：分三个子图展示位置、速度和加速度随时间变化的 RMSE 曲线，直观反映算法在机动发生时的收敛速度。
• 图3 (X-axis State Estimations)：专门针对 X 轴（机动剧烈轴），分别展示位置、速度、加速度的真实值与估计值对比，重点观察对加速度突变的跟踪能力。

6. 关键函数说明

get_CSM_Phi(alpha, T)

• 功能：计算基于 Singer 模型的离散状态转移矩阵。
• 原理：基于机动频率 alpha 和采样时间 T，构建描述二阶时间相关过程的转移矩阵，包含了位置、速度、加速度之间的耦合关系。

get_CSM_Q_structure(alpha, T)

• 功能：计算 CSM 过程噪声协方差矩阵的结构部分。
• 原理：实现了“当前”统计模型理论中复杂的 Q 阵结构元素计算，该矩阵结构是固定的，但最终幅值受加速度方差调节。

get_adaptive_sigma(a_hat, a_max)

• 功能：实现 CSM 的自适应方差调节逻辑。
• 逻辑：根据上一时刻的加速度估计值 a_hat 与最大加速度 a_max 的关系，利用修正的瑞利分布（Modified Rayleigh）概率密度函数特性，动态计算当前时刻加速度的方差 sigma_a。当目标机动强烈（a_hat 大）时，方差会自动减小；当机动较弱时，方差增大以保持对突发的敏感度（注：具体实现根据代码逻辑主要反映了机动余量的关系）。

resample_systematic(particles, weights, N)

• 功能：执行系统重采样（Systematic Resampling）。
• 逻辑：根据粒子权重生成的累积分布函数（CDF），通过生成均匀分布的随机数来选择新的粒子索引，保留高权重粒子，淘汰低权重粒子。