简述

基于三维模型的UKF，设计一段时间的输入状态误差较大，此时通过对比预测的状态值与观测值的残差，在相应的情况下自适应扩大系统方差Q，构成自适应无迹卡尔曼滤波（AUKF），与传统的UKF相比，三轴误差的平均值得到了降低，带经典UKF的误差对比、无滤波情况下的UKF对比。带中文注释。

部分代码

% 自适应调节Q的UKF与传统UKF效果对比
% author:Evand
% 作者联系方式：evandjiang@qq.com（除前期达成一致外，付费咨询）
% 2024-5-5/Ver1
clear;clc;close all;
%% 滤波模型初始化
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);
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)]; %真实值if i1>500 && i1<700 %设定IMU误差较大的时间段w(:,i1) = 10*w(:,i1);elsew(:,i1) = w(:,i1);endX_(:,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);%未滤波的值Z(:,i1)=[X(1,i1)^2/20;X(2,i1);X(3,i1)]+v(:,i1); %观测量
end

运行结果

真值、滤波前后的曲线对比：

误差对比：

误差的平均值对比：

下载地址

https://download.csdn.net/download/callmeup/89267114