本MATLAB 代码实现了平方根无迹卡尔曼滤波（SR-UKF）算法，用于处理三维非线性状态估计问题
文章目录
运行结果
三轴状态曲线对比：
三轴误差曲线对比：
误差统计特性输出（命令行截图）：
代码概述
-
初始化：
- 清空工作区和命令窗口，关闭所有图形窗口。
- 定义时间序列
t和设置过程噪声、观测噪声的协方差矩阵 (Q和R)。 - 初始化状态估计协方差矩阵
P0和状态向量X，同时定义观测值Z。
-
运动模型：
- 使用循环生成真实状态
X和未滤波状态X_，根据给定的运动模型更新状态。 - 生成观测值
Z，包含加入的观测噪声。
- 使用循环生成真实状态
-
平方根UKF实现：
- 初始化滤波器的状态和协方差矩阵。
- 在每个时间步
k中：- 生成 sigma 点并计算它们的权重。
- 对 sigma 点进行预测，计算新的状态和观测值。
- 更新状态估计和协方差矩阵，使用卡尔曼增益结合观测更新状态。
-
结果绘图：
- 绘制真实值、未滤波值和滤波值的对比图。
- 计算并绘制滤波前后的误差。
-
误差输出：
- 输出滤波前和 SR-UKF 后的三轴误差平均值。
代码
部分代码：
matlab
% 平方根无迹卡尔曼滤波（SR-UKF），三维非线性
% 2024-12-13/Ver1
clear;clc;close all; %清空工作区、命令行，关闭小窗口
% rng(0); %固定随机种子
%% 滤波模型初始化
t = 1:1:100;% 定义时间序列
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; %自适应标签
%% 运动模型
% 初始化未滤波的状态向量
X_ = zeros(3,length(t)); %给未滤波的值分配空间
X_(:,1) = X(:,1); %给未滤波的值赋初值
for i1 = 2:length(t)
X(:,i1) = [X(1,i1-1) + (2.5 * X(1,i1-1) / (1 + X(1,i1-1).^2)) + 8 * cos(1.2*(i1-1));
X(2,i1-1)+1;
X(3,i1-1)]; %真实值
X_(:,i1) = [X_(1,i1-1) + (2.5 * X_(1,i1-1) / (1 + X_(1,i1-1).^2)) + 8 * cos(1.2*(i1-1));
X_(2,i1-1)+1;
X_(3,i1-1)] + w(:,i1-1);%未滤波的值
Z(:,i1) = [X(1,i1).^2 / 20;X(2,i1);X(3,i1)] + v(i1); %观测值
end
%% 平方根UKF
P = P0;
X_ukf=zeros(3,length(t));
X_ukf(:,1)=X(:,1);
alpha = 0.95; % 自适应增益因子，用于更新观测噪声协方差
for k = 2 : length(t)
Xpre = X_ukf(:,k-1);% 预测下一状态代码定制、讲解等需求，可交流