【逐行注释】自适应Q和R的AUKF(自适应无迹卡尔曼滤波),附下载链接

发布于:2024-11-29 ⋅ 阅读:(32) ⋅ 点赞:(0)

在这里插入图片描述

自适应Q的KF

自适应无迹卡尔曼滤波(Adaptive Unscented Kalman Filter,AUKF)是一种用于状态估计的滤波算法。它是基于无迹卡尔曼滤波(Unscented Kalman Filter,UKF)的改进算法。

自适应 Q 和 R Q和R QR的无迹卡尔曼滤波在无迹卡尔曼滤波的基础上,引入了自适应的思想。通过动态地、同时调整状态转移协方差、观测噪声协方差的大小,从而提高系统的估计精度。

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

逐行注释的说明

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

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

运行结果

  • 三轴的估计值、真值、滤波前的值对比:
    在这里插入图片描述

  • 三轴的误差累积密度函数绘图:
    在这里插入图片描述

上图放大可得到如下细节:
在这里插入图片描述

【注】CDF曲线越靠左上角,说明整体误差越小

部分代码

% 自适应调节Q和R的UKF与传统UKF效果对比
% 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; %自适应标签
%% 运动模型
% 初始化未滤波的状态向量
fprintf('完整代码下载链接:https://gf.bilibili.com/item/detail/1105566012');
X_ = zeros(3,length(t)); %给未滤波的值分配空间

各模块解释

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

其中包括如下部分:

  1. 初始化:初始化参数
  2. 运动模型:通过迭代的方式,计算系统在所有时刻下的真值和未滤波的值
  3. UKF:经典的无迹卡尔曼滤波
  4. AUKF:经过自适应的UKF
  5. 绘图:绘制曲线图和CDF图像
  6. 总误差输出:fprintf的输出示例
    【现在输出的是平均值,如下:】
    在这里插入图片描述
    将代码里面的mean换成max即为最大值,换成std即为计算标准差

如有需要,可通过文末卡片联系作者