自适应Q的KF
自适应无迹卡尔曼滤波(Adaptive Unscented Kalman Filter,AUKF)是一种用于状态估计的滤波算法。它是基于无迹卡尔曼滤波(Unscented Kalman Filter,UKF)的改进算法。
自适应 Q 和 R Q和R Q和R的无迹卡尔曼滤波在无迹卡尔曼滤波的基础上,引入了自适应的思想。通过动态地、同时调整状态转移协方差、观测噪声协方差的大小,从而提高系统的估计精度。
自适应无迹卡尔曼滤波适用于非线性和非高斯的系统状态估计问题,可以广泛应用于机器人导航、目标跟踪、航天器导航等领域。它通过动态地调整无迹点的数量和分布,能够更好地适应系统的动态特性,提高估计精度,同时具有较低的计算复杂度。
逐行注释的说明
每一行都标有中文注释:
是我自己一个字一个字打的,如果有错别字等问题,欢迎指正。
运行结果
三轴的估计值、真值、滤波前的值对比:
三轴的误差累积密度函数绘图:
上图放大可得到如下细节:
【注】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)); %给未滤波的值分配空间
各模块解释
程序共有下列几个模块:
其中包括如下部分:
- 初始化:初始化参数
- 运动模型:通过迭代的方式,计算系统在所有时刻下的真值和未滤波的值
- UKF:经典的无迹卡尔曼滤波
- AUKF:经过自适应的UKF
- 绘图:绘制曲线图和CDF图像
- 总误差输出:fprintf的输出示例
【现在输出的是平均值,如下:】
将代码里面的mean换成max即为最大值,换成std即为计算标准差
如有需要,可通过文末卡片联系作者