自适应调节Q和R的自适应UKF（AUKF

自适应调节Q和R的自适应UKF（AUKF_QR）的MATLAB程序

news2024/11/17 0:05:19

简述

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

运行截图

在这里插入图片描述
误差图：

平均误差输出的结果图：

部分源代码

% 自适应调节Q和R的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);
        Z(:,i1) = [X(1,i1).^2 / 20;X(2,i1);X(3,i1)] + 10*v(i1); %观测值
    else
        w(:,i1) = w(:,i1);
    end
    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);%未滤波的值
    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);
for k = 2 : length(t)
    Xpre = X_ukf(:,k-1);
    % sigma点和权重
    apha = 0.1; %【自己可以设置，取值：0.001~1】
    % calculateSigPntsandWeights
    n = size(X,1);
    State_aug = Xpre;
    lambda = 3;
    % sigma点
    Sigma_Points = zeros(n, 2*n+1);
    Sigma_Points(:,1) = State_aug;
    A = chol(P,'lower');
    for i = 1:n
        Sigma_Points(:,i+1) = State_aug + sqrt(lambda+n)*A(:,i);
        Sigma_Points(:,i+1+n) = State_aug -sqrt(lambda+n)*A(:,i);
    end
    Weights_m = zeros(2*n+1,1);
    for i = 1:2*n+1
        if i==1
            Weights_m(i,1) = lambda / (lambda+n);
            Weights_c(i,1) = lambda / (lambda+n)+1-apha^2+2;
        else
            Weights_m(i,1) = 0.5 / (lambda+n);
            Weights_c(i,1) = 0.5 / (lambda+n);
        end
    end
    % 预测
    
    for i = 1:size(Weights_m)
        Sigma_pred(:,i) = [
            Sigma_Points(1,i)+2.5*Sigma_Points(1,i)/(1+Sigma_Points(1,i)^2)+8*cos(1.2*(k-1));
            Sigma_Points(2,i)+1;
            Sigma_Points(3,i)]+w(:,k);
    end
    % State_pred
    Xpre = Sigma_pred*Weights_m;
    n = size(Xpre,1);
    P_pred = zeros(n, n);

    for i = 1:size(Weights_m)
        x_diff = Sigma_pred(:,i) - Xpre;
        P_pred = P_pred + Weights_c(i,1)*x_diff*transpose(x_diff);
    end
    % 由各个状态量的点来求观测量
    for i = 1:size(Weights_m)
        Z_sigma(:,i) = [Sigma_pred(1,i)^2/20;Sigma_pred(2,i);Sigma_pred(3,i)];
    end
    Z_pred = Z_sigma*Weights_m;
    P_pred = P_pred+Q;
    X_ukf(:,k) = Xpre;
    % 观测更新
    nx = size(Xpre,1);
    nz = size(Z_pred,1);
    S = zeros(nz, nz);
    for i = 1:size(Weights_m)
        z_diff = Z_sigma(:,i) - Z_pred;
        S = S + Weights_c(i,1)*z_diff*transpose(z_diff);
    end
    S = S+R;
    TC = zeros(nx, nz);
    for i = 1:size(Weights_m)
        z_diff = Z_sigma(:,i) - Z_pred;
        x_diff = Sigma_pred(:,i) - Xpre;
        TC = TC + Weights_c(i,1)*x_diff*transpose(z_diff);
    end
    K = TC/S;
    % 更新P和滤波的状态量
    residue = Z(:,k) - Z_pred;
    Xpre = Xpre + K*residue;
    P = P_pred - K*S*transpose(K);
    X_ukf(:,k) = Xpre;

end
%% AUKF
P = P0;
X_aukf=zeros(3,length(t));
X_aukf(:,1)=X(:,1);