文章目录
课题简介
在非线性系统状态估计问题中，系统状态方程和观测方程往往并不满足严格的线性关系。传统卡尔曼滤波虽然具有计算效率高、结构清晰等优点，但其基本形式主要适用于线性高斯系统。当系统模型存在明显非线性、观测噪声发生变化或系统状态出现突变时，普通滤波方法容易出现估计误差增大、跟踪能力下降甚至滤波发散等问题。
无迹卡尔曼滤波（Unscented Kalman Filter, UKF）是一类常用的非线性滤波方法。相比扩展卡尔曼滤波（EKF），UKF 不需要对非线性函数进行雅可比矩阵线性化，而是通过 Sigma 点传播来近似非线性变换后的均值和协方差，因此在非线性系统状态估计中具有较好的适用性。
本课题在普通 UKF 的基础上，引入强跟踪滤波思想，构建强跟踪无迹卡尔曼滤波（Strong Tracking Unscented Kalman Filter, ST-UKF）算法。该算法能够根据观测残差动态调整滤波过程，提高系统在噪声突变、模型不确定或状态快速变化情况下的跟踪能力和估计稳定性。
研究背景
在目标跟踪、组合导航、无人机控制、机器人定位、车辆状态估计和多传感器融合等领域，系统状态通常具有明显的非线性特征。例如，目标运动模型可能包含非线性加速度项，传感器观测模型可能包含距离、角度、平方项或其他非线性映射关系。
对于这类系统，如果直接采用传统线性卡尔曼滤波，滤波模型与实际系统之间会存在较大偏差。EKF 虽然可以通过一阶泰勒展开处理非线性问题，但在非线性程度较强或初始误差较大时，线性化误差可能影响滤波精度。
UKF 通过构造一组确定性 Sigma 点来描述状态分布，并将这些点代入非线性函数进行传播，从而避免显式求导过程。该方法实现较为直观，适合用于 MATLAB 仿真教学和非线性滤波算法研究。
但是，普通 UKF 对模型误差和噪声异常的适应能力仍然有限。当系统在某一时间段内出现较大扰动时，普通 UKF 的估计结果可能会明显偏离真实状态。因此，在 UKF 中加入强跟踪机制，可以进一步增强滤波器对突变状态和异常噪声的适应能力。
主要研究内容
本课题主要围绕三维非线性状态量和三维观测量展开，设计普通 UKF 与强跟踪 UKF 两种滤波算法，并对二者的估计效果进行对比分析。
主要研究内容包括：
- 建立三维非线性状态转移模型；
- 建立三维非线性观测模型；
- 构造含过程噪声和观测噪声的仿真数据；
- 实现普通无迹卡尔曼滤波算法；
- 引入强跟踪机制，设计强跟踪 UKF 算法；
- 对比滤波前、普通 UKF 和强跟踪 UKF 的状态估计结果；
- 绘制状态曲线、误差曲线和误差累计分布曲线；
- 输出不同算法的误差统计结果，分析算法性能差异。
算法原理概述
普通 UKF 算法
UKF 的核心思想是利用 Sigma 点近似状态变量的概率分布。对于非线性系统：
x k = f ( x k − 1 ) + w k x_k = f(x_{k-1}) + w_k xk=f(xk−1)+wk
z k = h ( x k ) + v k z_k = h(x_k) + v_k zk=h(xk)+vk
其中， x k x_k xk 表示系统状态， z k z_k zk 表示观测量， w k w_k wk 为过程噪声， v k v_k vk 为观测噪声。
UKF 不直接对非线性函数 f ( ⋅ ) f(\cdot) f(⋅) 和 h ( ⋅ ) h(\cdot) h(⋅) 进行线性化，而是根据当前状态估计和协方差矩阵生成一组 Sigma 点。然后将 Sigma 点代入状态方程和观测方程，通过加权求和得到预测状态、预测观测以及相应协方差矩阵。
普通 UKF 的主要流程包括：
- 初始化状态估计和协方差矩阵；
- 生成 Sigma 点；
- 对 Sigma 点进行状态预测；
- 计算预测状态均值和预测协方差；
- 将预测 Sigma 点代入观测模型；
- 计算预测观测均值和观测协方差；
- 计算状态与观测之间的互协方差；
- 计算卡尔曼增益；
- 根据观测残差更新状态和协方差。
强跟踪 UKF 算法
强跟踪滤波的主要思想是利用观测残差信息来判断当前滤波模型是否能够准确描述系统状态变化。当观测残差增大时，说明系统可能出现了模型失配、状态突变或噪声异常，此时需要增强滤波器对新观测信息的响应能力。
强跟踪 UKF 在普通 UKF 的基础上引入自适应调节因子，通过残差序列动态修正滤波过程，使滤波器能够更快跟踪系统状态变化。
其核心优势包括：
- 提高对系统突变的响应速度；
- 增强对模型不确定性的适应能力；
- 降低异常噪声对估计结果的影响；
- 改善普通 UKF 在复杂动态场景下的稳定性。
运行结果
从仿真结果可以看出，未滤波状态受过程噪声影响较大，状态曲线与真实值之间存在明显偏差。普通 UKF 能够有效利用观测信息对系统状态进行修正，相比未滤波结果具有更高的估计精度。
在系统噪声变化或局部扰动增强的情况下，强跟踪 UKF 相比普通 UKF 具有更好的动态适应能力。由于其引入了基于观测残差的自适应调节机制，当系统状态变化较剧烈时，滤波器能够更快响应新的观测信息，从而降低估计误差。
误差曲线能够直观反映不同算法在各个时刻的估计偏差。CDF 曲线则可以进一步展示不同方法误差分布的整体情况。通常情况下，误差 CDF 曲线越靠左，说明该方法在大多数时间内的估计误差越小，整体稳定性越好。
部分代码
matlab
% 强跟踪无迹卡尔曼滤波（STUKF）与传统UKF效果对比
clear;clc;close all; %清空工作区、命令行，关闭小窗口
rng(0); %固定随机种子
%% 滤波模型初始化
t = 1:1:1000;% 定义时间序列
Q = 1*diag([1,1,1]);w=sqrt(Q)*randn(size(Q,1),length(t));% 设置过程噪声协方差矩阵和过程噪声
R = 1*diag([1,1,1]);v=sqrt(R)*randn(size(R,1),length(t));% 设置观测噪声协方差矩阵和观测噪声
P0 = 1*eye(3);% 初始状态估计协方差矩阵
X=zeros(3,length(t));% 初始化状态向量
Z=zeros(3,length(t)); %定义观测值形式
Z(:,1)=[X(1,1)^2/20;X(2,1);X(3,1)]+v(:,1); %观测量
residue_tag = 0; %自适应标签
...完整代码
完整代码链接：https://blog.csdn.net/callmeup/article/details/144435294?spm=1011.2415.3001.5331
可拓展研究方向
在现有代码基础上，可以进一步开展以下拓展研究：
-
非高斯噪声下的强跟踪 UKF 研究
将观测噪声设置为脉冲噪声、混合高斯噪声或厚尾噪声，分析强跟踪 UKF 在复杂噪声环境下的鲁棒性。
-
强跟踪 UKF 与其他滤波算法对比
可以加入 EKF、CKF、Sage-Husa UKF、自适应 UKF、最大相关熵 UKF 等方法，形成更加完整的算法对比。
-
目标跟踪场景扩展
将三维非线性状态模型替换为二维或三维目标运动模型，例如匀速模型、匀加速模型或转弯模型。
-
组合导航场景扩展
将算法应用到 GNSS/SINS 组合导航、UWB/SINS 融合定位或无人机协同导航中，提高课题的工程应用价值。
-
滤波参数自适应优化
进一步研究过程噪声协方差、观测噪声协方差和强跟踪因子的自适应调整方法，提高算法稳定性。
如有代码定制需求，可联系下方卡片