输入误差不稳定时的UKF探索（基于MATLAB编程，有源代码）

于 2024-05-05 15:10:47 发布
阅读量205 收藏
点赞数 3
分类专栏： MATLAB技巧 卡尔曼专题免费专栏 文章标签： matlab 算法 开发语言
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/callmeup/article/details/138467011
版权

当输入的状态（加速度、角速度、速度等值）误差不稳定时，通过自适应UKF的思想，设计动态变化Q的无迹卡尔曼滤波

设计思路

定义500~700这段时间输入状态误差翻倍，以至于输入的噪声变大，带来Q应该也要变大。

程序源码

% 状态误差不稳定时，调节Q的UKF与传统UKF效果对比
% 2024©Evand
% 作者联系方式：evandjiang@qq.com（除前期达成一致外，付费咨询）
% 2024-5-5/Ver1
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);
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);
    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);
for k = 2 : length(t)
    Xpre = X_aukf(:,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;
    % 判断是否需要自适应调节 %%%
    residue_num(k) = residue(1);
    if k>400 && k<700
        residue_tag = 1;
    else
        residue_tag = 0;
    end
    %
        % 自适应调节Q %%%
    if residue_tag == 1
        Q_adaptive = 100*Q;
    else
        Q_adaptive = Q;
    end
    %
    P_pred = P_pred+Q_adaptive;
    X_aukf(:,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_aukf(:,k) = Xpre;
end
%% 绘图
figure;
subplot(3,1,1);
plot(t,X(1,:),t,X_ukf(1,:),t,X_aukf(1,:),t,X_(1,:));
title('value');
legend('real','UKF','AUKF','未滤波');
subplot(3,1,2);
plot(t,X(2,:),t,X_ukf(2,:),t,X_aukf(2,:),t,X_(2,:));
subplot(3,1,3);
plot(t,X(3,:),t,X_ukf(3,:),t,X_aukf(3,:),t,X_(3,:));

figure;
subplot(3,1,1); %1st
plot(t,X_(1,:)-X(1,:),t,X_ukf(1,:)-X(1,:),t,X_aukf(1,:)-X(1,:));
title('误差');
legend('未滤波','UKF','AUKF');
subplot(3,1,2); %2nd
plot(t,X_(2,:)-X(2,:),t,X_ukf(2,:)-X(2,:),t,X_aukf(2,:)-X(2,:));
subplot(3,1,3); %3rd
plot(t,X_(3,:)-X(3,:),t,X_ukf(3,:)-X(3,:),t,X_aukf(3,:)-X(3,:));

figure;
subplot(3,1,1); %1st
hold on
cdfplot(abs(X_(1,:)-X(1,:)));
cdfplot(abs(X_ukf(1,:)-X(1,:)));
cdfplot(abs(X_aukf(1,:)-X(1,:)));
legend('未滤波','UKF','AUKF');
subplot(3,1,2); %2nd
hold on
cdfplot(abs(X_(2,:)-X(2,:)));
cdfplot(abs(X_ukf(2,:)-X(2,:)));
cdfplot(abs(X_aukf(2,:)-X(2,:)));
subplot(3,1,3); %3rd
hold on
cdfplot(abs(X_(3,:)-X(3,:)));
cdfplot(abs(X_ukf(3,:)-X(3,:)));
cdfplot(abs(X_aukf(3,:)-X(3,:)));
%% 总误差输出示例
% fprintf('未滤波时 三轴误差最大值:%f\n',max(diag((X_-X)'*(X_-X)))^0.5);
% fprintf('UKF 三轴误差最大值:%f\n',max(diag((X_ukf-X)'*(X_ukf-X)))^0.5);
% fprintf('自适应UKF 三轴误差最大值:%f\n',max(diag((X_aukf-X)'*(X_aukf-X)))^0.5);

fprintf('未滤波时 三轴误差平均值:%f\n',mean(diag((X_-X)'*(X_-X)))^0.5);
fprintf('UKF 三轴误差平均值:%f\n',mean(diag((X_ukf-X)'*(X_ukf-X)))^0.5);
fprintf('自适应UKF 三轴误差平均值:%f\n',mean(diag((X_aukf-X)'*(X_aukf-X)))^0.5);

结果输出

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

MATLAB卡尔曼
关注
专栏目录
UKF matlab程序
09-27
UKF无味卡尔曼滤波器的matlab程序，
自适应调节Q和R的自适应UKF（AUKF_QR）的MATLAB程序
MATLAB卡尔曼的博客
05-05 528
基于三维模型的UKF，设计一段时间的输入状态误差较大，此时通过对比预测的状态值与观测值的残差，在相应的情况下自适应调节系统协方差Q和观测协方差R，构成自适应无迹卡尔曼滤波（AUKF），与传统的UKF相比，三轴误差的平均值得到了降低，带经典UKF的误差对比、无滤波情况下的UKF对比。带中文注释。
【滤波跟踪】基于自适应UKF和UKF算法实现运动刚体的位姿估计附matlab代码
matlab_dingdang的博客
08-20 2534
本文研究了基于单目视觉的运动刚体位姿估计问题,提出了基于自适应无迹卡尔曼滤波算法(Adaptive Unscented Kalman Filter,AUKF)的位姿估计方法.考虑到运动刚体位姿估计系统的量测方程为非线性且过程噪声统计特征未知,通过递推噪声估计器在线估计过程噪声的均值和方差阵,解决了位姿估计系统中过程噪声统计特性未知时估计精度下降的问题.实验结果表明,AUKF算法提高了位姿估计的精度,并实现了过程噪声统计特性的在线估计.部分理论引用网络文献，若有侵权联系博主删除。
ukf 在matlab 下的实现,ukf在matlab下的实现
weixin_29883867的博客
03-24 835
一个应用实例详解卡尔曼滤波及其算法实现 标签: 算法 filtermatlabalgorithm 优化工作 2012-05-14 10:48 75511 人阅读 评论(25) 收藏 举报 分类: 数据结构及其......整个跟踪的 MATLAB 仿真运算时间是:EKF 共用了 6.126 秒,而 UKF 共用了 11.996 秒。从运算消耗来看两者基本在一个数量级, UKF 消耗不到 EKF 时间...
基于扩展(EKF)和无迹卡尔曼滤波(UKF)的电力系统动态状态估计(Matlab代码实现）
2201_75535220的博客
07-29 80
文献来源：摘要：准确估计电力系统动态对于提高电力系统的可靠性、韧性、安全性和稳定性非常重要。随着逆变器型分布式能源的不断集成，对电力系统动态的了解比以往任何时候都更为必要和关键，以实现电力系统的正确控制和运行。尽管最近测量设备和传输技术的进展极大地减小了测量和传输误差，但这些测量仍然不完全摆脱测量噪声的影响。因此，需要对嘈杂的测量进行滤波，以获得准确的电力系统运行动态。本文使用扩展卡尔曼滤波器（EKF）和无迹卡尔曼滤波器（UKF）来估计电力系统的动态状态。
ukf.rar_UKF_UKF matlab_UKF matlab 代码_不敏_不敏卡尔曼
09-14
在本压缩包文件中，我们主要探讨UKF的理论基础、实现过程以及提供的MATLAB代码示例。 **1. UKF的基本原理：** 卡尔曼滤波基于贝叶斯定理和最小均方误差准则，但在处理非线性系统时会失准。不敏卡尔曼滤波利用“不敏...
UKF.rar_Beidou satellite_UKF matlab 代码_UKF代码_matlab北斗_北斗卫星
07-15
标签进一步细化了这个压缩包的内容：“beidou_satellite”表示这是与北斗卫星系统相关的项目，“ukf_matlab_代码”表明是用MATLAB编程语言实现的UKF算法，“ukf代码”重申了这一点，“matlab北斗”和“北斗卫星”则...
基于matlab编程实现的扩展(EKF)和无迹卡尔曼滤波(UKF)的电力系统动态状态估计.rar
最新发布
05-19
在MATLAB编程环境下实现这两种滤波器，开发者可以利用MATLAB强大的数学库和矩阵运算功能，简化算法实现过程。MATLAB代码通常包括定义系统模型、设定滤波参数、实施滤波循环等部分。通过对实际测量数据进行处理，可以...
基于matlab的无人机航迹预测对比仿真，对比ekf,ukf,pf,改进pf算法+含代码操作演示视频
05-22
基于matlab的无人机航迹预测对比仿真，对比ekf,ukf,pf,改进pf算法+含代码操作演示视频 运行注意事项：使用matlab2021a或者更高版本测试，运行里面的Runme.m文件，不要直接运行子函数文件。运行时注意matlab左侧的...
基于matlab对比KF,EKF,UKF三种滤波跟踪算法的性能仿真+matlab操作视频
06-07
2.内容：基于matlab对比KF,EKF,UKF三种滤波跟踪算法的性能仿真+matlab操作视频 3.用处：用于KF,EKF,UKF三种滤波跟踪算法编程学习 4.指向人群：本硕博等教研学习使用 5.运行注意事项： 使用matlab2021a或者更高...
AUKF自适应无迹卡尔曼滤波
06-14
在无迹卡尔曼滤波的基础上进行了改进，设计的自适应无迹卡尔曼滤波算法，适合有无迹卡尔曼滤波基础的人进行学习。
本人自己写的UKF代码
06-08
无损卡尔曼滤波器
滤波算法 | 无迹卡尔曼滤波（UKF）算法及其MATLAB实现
lovetaozibaby的博客
02-21 9606
本文接着分享位姿跟踪和滤波算法中用到的一些常用程序，希望为后来者减少一些基础性内容的工作时间。位姿跟踪 | 相关内容目录和链接总结（不断更新中~~~）本文分享无迹卡尔曼滤波（UKF）算法的一些基本公式和MATLAB程序。
利用matlab实现扩展卡尔曼滤波（EKF）与无迹卡尔曼滤波（UKF）
qq_43131852的博客
05-31 7696
例题 将下例进行EKF和UKF代码实现，进行仿真复现。 EKF算法模型 clc; clear; %测量值模拟 T=0.05;%滤波周期 T_go=10;%滤波时间 N=T_go/T;%观测次数 t=0:T:T_go-T;%假定输出序列（供画图用） x=zeros(2,N); z=zeros(2,N); x(:,1)=[1;0];%真值初始值 mu=[0;0]; Q=[0.01,0;0,0.0001]; R=[0.1,0;0,0.1]; rng(1); w=mvnrnd(mu,Q,N)'; v=mvnr
【状态估计】【卡尔曼滤波kalman】UKF、EKF、粒子滤波PF学习与研究（Matlab代码实现）
weixin_67304359的博客
04-25 822
卡尔曼滤波（Kalman Filter）是一种用于估计系统状态的强大工具，适用于线性动态系统和观测的高斯噪声的情况。但是，对于非线性系统和非高斯噪声的情况，卡尔曼滤波可能表现不佳。为了处理这些情况，人们开发了一些扩展的滤波方法，其中最常见的包括扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）和粒子滤波（PF）。1. **扩展卡尔曼滤波（EKF）**：EKF 是对卡尔曼滤波的一种扩展，用于处理非线性系统。它通过在线性化状态转移和观测方程来逼近非线性系统的动态。
基于UKF无迹卡尔曼滤波的电池Soc估计matlab仿真
熟悉MATLAB/fpga/RK3588/LABVIEW/protus/单片机/dsp/Multisim
01-22 1317
通过UKF无迹卡尔曼滤波对电池的SOC进行估计，并对比电池真实的SOC，输出两者的对比仿真结果。电池状态估计，特别是电池的荷电状态（State of Charge, SoC）估计，在电动汽车、混合动力汽车以及其他电池应用领域中至关重要。无迹卡尔曼滤波（Unscented Kalman Filter, UKF）是一种非线性滤波方法，特别适用于处理非线性系统的状态估计问题。
一文详解自适应无迹卡尔曼滤波（AUKF）
Luminosity_azur的博客
06-04 6626
本文是对自适应无迹卡尔曼滤波（AUKF）的相关阐释，包括算法背景、算法概述及核心运算式、示例及MATLAB代码、算法优缺点分析四个部分。
自适应无迹卡尔曼滤波AUKF算法
HSXfnmDjjKAb的博客
03-04 731
通过利用提供的MATLAB代码，读者可以自行运行AUKF算法，将其应用于自己的实际问题中，进一步加深对该算法的理解和应用能力。同时，本文还提供了丰富的学习资料，包括相关论文、书籍和教程，供读者参考学习。总之，通过本文的介绍，读者将能够全面了解AUKF算法的原理和应用，并获得配套的学习资料和代码，以便深入研究和实践。通过结合车辆动力学、路面附着系数识别、轮胎模型等方面的知识，AUKF算法能够更准确地估计车辆的状态，提供精准的车辆姿态、位置、速度等信息，为自动驾驶系统的决策与控制提供必要的数据支持。
ukf卡尔曼滤波 matlab编程
11-28
UKF是一种无迹卡尔曼滤波（Unscented Kalman Filter）算法，它可以用于非线性系统的状态估计。在Matlab中进行UKF卡尔曼滤波的编程可以按照以下步骤进行。 首先，需要定义非线性系统的状态方程和观测方程。状态方程描述系统的状态变化规律，而观测方程描述系统状态和测量值之间的关系。 其次，需要初始化系统的状态和协方差矩阵。这些初始值可以根据实际情况来确定。 接下来，可以编写一个函数来实现UKF算法。这个函数包括UKF的预测步骤、更新步骤以及计算卡尔曼增益的步骤。 然后，可以利用Matlab中的相关函数来实现UKF算法中的数学计算，比如sigma points的选取、计算预测状态和观测状态的均值和协方差、计算卡尔曼增益等。 最后，可以利用编写的UKF函数对具体的数据进行状态估计。可以通过实际的观测值和系统模型进行数据模拟，然后使用编写的UKF函数对模拟数据进行状态估计，观察估计状态与真实状态之间的误差。 总而言之，在Matlab中进行UKF卡尔曼滤波的编程需要对UKF算法本身有一定的了解，同时需要熟悉Matlab的相关函数和工具。通过编程实践，可以更深入地理解UKF算法的原理和应用，为实际工程问题的解决提供技术支持。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

MATLAB卡尔曼

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值