【逐行注释】自适应Q的AUKF|MATLAB代码（附下载链接）

news2024/11/22 23:20:02

请添加图片描述

文章目录

逐行注释的说明
运行结果
自适应UKF
- 介绍
- 实现过程
部分代码
各模块解释

逐行注释的说明

每一行都标有中文注释：
在这里插入图片描述

是我自己一个字一个字打的，如果有错别字等问题，欢迎指正。

运行结果

三轴的估计值、真值、滤波前的值对比：

在这里插入图片描述

三轴的误差值对比：
三轴的误差累积密度函数绘图：

自适应UKF

介绍

自适应无迹卡尔曼滤波 $（ A d a pt i v e U n sce n t e d K a l man F i lt er ， A U K F ）$ 是一种用于状态估计的滤波算法。它是基于无迹卡尔曼滤波 $（ U n sce n t e d K a l man F i lt er ， U K F ）$ 的改进算法。

自适应无迹卡尔曼滤波在无迹卡尔曼滤波的基础上，引入了自适应的思想。它通过动态地调整无迹点的数量和分布，以适应系统的动态变化。具体来说，它使用一种自适应的方法来根据系统的动态特性进行无迹点的选择和更新，从而提高系统的估计精度。

实现过程

自适应无迹卡尔曼滤波适用于非线性和非高斯的系统状态估计问题，可以广泛应用于机器人导航、目标跟踪、航天器导航等领域。它通过动态地调整无迹点的数量和分布，能够更好地适应系统的动态特性，提高估计精度，同时具有较低的计算复杂度。

部分代码

% 自适应调节Q的UKF与传统UKF效果对比
% author:Evand
% 作者联系方式：微信matlabfilter（除前期达成一致外，付费咨询）
% 2024-5-5/Ver1
% 2024-9-9/Ver2/添加逐行注释
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; %自适应标签
%% 运动模型
% 初始化未滤波的状态向量
X_ = zeros(3,length(t)); %给未滤波的值分配空间
X_(:,1) = X(:,1); %给未滤波的值赋初值
fprintf('完整代码的下载链接见：https://gf.bilibili.com/item/detail/1106217012')
%% UKF
P = P0;
X_ukf=zeros(3,length(t));
X_ukf(:,1)=X(:,1);

各模块解释

程序共有下列几个模块：
在这里插入图片描述
其中：

UKF为经典的无迹卡尔曼滤波
AUKF为经过自适应的UKF
绘图部分：用于绘制和比较不同滤波方法（未滤波、UKF和AUKF）在三维状态下的表现。具体来说，它分为三个部分：
- 绘制状态值：
  创建一个新的图形窗口，并在其中绘制三个子图，每个子图对应状态的一个维度。
  在每个子图中，绘制真实状态值（X）、UKF滤波后的状态值（X_ukf）、AUKF滤波后的状态值（X_aukf）和未滤波的状态值（X_）。
  每个子图都有相应的y轴标签和图例，第一幅图还包括标题。
- 绘制误差对比：
  创建另一个图形窗口，并在其中绘制三个子图，每个子图对应状态的一个维度。
  在每个子图中，绘制未滤波、UKF和AUKF滤波前后的误差对比。
  每个子图都有相应的y轴标签和图例，第一幅图还包括标题。
- 绘制误差累计概率密度图：
  创建第三个图形窗口，并在其中绘制三个子图，每个子图对应状态的一个维度。
  在每个子图中，绘制未滤波、UKF和AUKF滤波误差的累计概率密度图（CDF）。
  每个子图都有相应的y轴标签和图例，第三幅图还包括x轴标签。
  最后，代码计算并输出三种方法的三轴误差平均值。
总误差输出示例部分给出了未滤波时、UKF、AUKF的误差最大值对比，可换成min、mean、std等，用于计算其他统计特性