【MATLAB例程】维度自适应的UKF代码|从1维~n维，根据状态量和观测量维度自动调整滤波器结构|附代码下载链接

本文所述的代码实现了一个无迹卡尔曼滤波（UKF）算法，用于处理高维非线性系统的状态估计问题。代码通过动态设置状态量和观测量的维度（默认30维），展示了UKF在无需线性化雅可比矩阵的情况下处理多维非线性系统的能力。其核心是通过无迹变换（Unscented Transform）生成Sigma点，精确捕捉非线性系统的统计特性，从而提升状态估计的精度和稳定性。

代码介绍

初始化模块

• 高维动态设置：通过参数dim = 30定义状态和观测维度，支持任意维度的扩展（如机器人多自由度系统、金融多变量分析）。
• 噪声与协方差矩阵：定义过程噪声协方差Q和观测噪声协方差R，生成对应的高斯噪声w和v，确保系统动态的随机性。
• 状态初始化：真实状态X、未滤波状态X_和UKF估计状态X_ukf初始化为零矩阵，初始协方差P0为单位矩阵。

系统动态与观测模型

• 状态更新：假设系统为线性动态（X = eye(dim)*X_prev），但观测模型为非线性函数（Z = X.^2 + v），体现UKF处理非线性观测的优势。
• 未滤波状态生成：通过叠加过程噪声模拟实际系统中含噪声的状态量（X_），用于后续滤波效果对比。

UKF算法实现

• Sigma点生成：基于当前状态估计和协方差矩阵，通过Cholesky分解生成2n+1个Sigma点（n为状态维度），覆盖状态分布的均值和协方差。
• 权重分配：设置参数alpha（控制Sigma点分布范围）、lambda（缩放因子），计算均值权重Weights_m和协方差权重Weights_c。
• 预测步骤：
  • Sigma点通过状态转移模型传播（Sigma_pred = Sigma_Points * F + w_ukf）。
  • 加权平均计算预测状态Xpre和协方差P_pred。
• 观测更新：
  • 预测观测值Z_pred通过Sigma点非线性映射（Z_sigma = Sigma_pred.^2/20）。
  • 计算观测协方差S、互协方差TC及卡尔曼增益K，修正状态估计和协方差矩阵。

结果可视化与误差分析

• 时序对比图：绘制各维度真实值、UKF估计值和未滤波值的对比，直观展示滤波效果。
• 误差曲线：计算各维度滤波前后的绝对误差，突显UKF在噪声抑制上的优势。
• 多维支持：自动生成与维度匹配的子图，支持高维数据的并行分析。

代码特点

1. 高维非线性处理：无需雅可比矩阵线性化，通过Sigma点精确传递非线性系统的统计特性。
2. 参数灵活可调：通过alpha和lambda控制Sigma点分布，适应不同非线性强度场景。
3. 高效协方差传播：基于加权Sigma点更新协方差矩阵，避免EKF因线性化导致的误差累积。
4. 鲁棒性验证：默认30维状态空间测试，验证算法在高维场景下的稳定性。

改进方向

• 非线性动态扩展：当前状态转移为线性模型，可扩展为通用非线性函数（如X = f(X_prev)）。
• 自适应参数调整：根据残差动态优化alpha、lambda等参数，提升算法自适应性。
• 计算效率优化：利用矩阵运算替代循环（如Sigma点预测），加速高维计算。
• 模块化封装：将UKF核心算法封装为函数，支持外部调用和参数配置。

该代码为高维非线性系统的状态估计提供了通用框架，结合无迹卡尔曼滤波的理论优势与工程实现，适用于学术研究及工业应用。通过调整维度参数（如dim）和噪声协方差（Q、R），可快速适配机器人、自动驾驶、信号处理等领域的需求。

变维度演示

变维度使用的变量：

演示视频如下：

N维状态量的UKF例程

MATLAB源代码

部分代码：

% N维状态量的UKF例程
% 2025-01-16/Ver1
clear;clc;close all; %清空工作区、命令行，关闭小窗口
rng(0); %固定随机种子
%% 滤波模型初始化
% 定义时间序列
t = 1:1:1000;
dim = 10; %设置维度
dim_Q = dim;
dim_R = dim;
% 过程噪声协方差矩阵和过程噪声
Q = 1*diag(ones(1,dim_Q));
w = sqrt(Q)*randn(size(Q,1),length(t));
% 观测噪声协方差矩阵和观测噪声
R = 1*diag(ones(1,dim_R));
v = sqrt(R)*randn(size(R,1),length(t));
% 初始状态估计协方差矩阵
P0 = 1*eye(dim_Q);
% 初始化状态向量
X = zeros(dim_Q,length(t));
% 初始化扩展卡尔曼滤波状态向量
X_ukf = zeros(dim_Q,length(t));
X_ukf(:,1) = X(:,1); %给滤波后的状态量赋初值
Z = zeros(dim_R,length(t));% 给观测值分配空间
% Z(:,1) = [X(1,1)^2/20;X(2,1);X(3,1)] + v(:,1);% 定义初始观测值

%% 运动模型
% 初始化未滤波的状态向量
X_ = zeros(dim,length(t)); %给未滤波的值分配空间
X_(:,1) = X(:,1); %给未滤波的值赋初值
% 运动模型迭代

for i1 = 2:length(t)
    % 真实状态更新
    X(:, i1) = eye(dim_Q)*X(:, i1-1);
    % 未滤波状态更新
    X_(:, i1) = eye(dim_Q)*X_(:, i1-1) + w(:, i1-1);
    % 观测值更新
    Z(:, i1) = X(:, i1).^2 + v(:,i1);
end

完整代码下载链接：https://download.youkuaiyun.com/download/callmeup/90718157

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者