平方根无迹卡尔曼滤波（SR-UKF）算法，用于处理三维非线性状态估计问题

本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);% 预测下一状态

代码定制、讲解等需求，可交流