滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现

这篇具有很好参考价值的文章主要介绍了滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

简介

本文接着分享位姿跟踪和滤波算法中用到的一些常用程序,希望为后来者减少一些基础性内容的工作时间。以往分享总结见文章:位姿跟踪 | 相关内容目录和链接总结(不断更新中~~~)

本文分享无迹卡尔曼滤波(UKF)算法的一些基本公式和MATLAB程序。

UKF滤波

首先简单介绍一下UKF滤波的优点和公式。本文偏工程性介绍,因此UKF的原理就不再过多介绍了。

滤波流程和公式

首先需要明确滤波模型中的状态变量、状态方程和测量方程。

状态变量x=[ ]
状态方程x(k+1) = f(x(k))
测量方程z(k+1) = h(x(k+1))

很简单对吧?

接着求解介绍求解流程 :

  1. 相比于一般的卡尔曼滤波,UKF算法增加了两次无迹变换,公式为:
    滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现
    权重和方差计算公式为:
    滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现

  2. Sigma点传播:

滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现

  1. 计算x的预测值和协方差矩阵:

滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现
4. 得到一组新的Sigma点:

滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现
5. 代入观测方程中,得到测量量的预估值:
滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现

  1. 获得观测量的预测值和协方差矩阵:

滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现

  1. 更新状态变量和协方差矩阵:
    滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现
    滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现
    滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现

另外,每次写论文画卡尔曼流程图中,都找不到参考的模板。我自己画了个滤波流程图,不一定符合每个人的审美,以备参考:

滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现

MATLAB程序

MATLAB程序如下,包括main.m函数和ukf.m函数:

main.m


P=[]
Q=[]
R=[]


f=@(x)[]
h=@(x)[]

z_msm = []
x0=[]
ux = x0;   
                                                            % 滤波初始值
for k=2:n_before_colli
    [xukf(:,k), P] = ukf(f_before,ux,P,h_before,z_pre(:,k),Q,R);
    ux=xukf(:,k);                                                          % k时刻的状态变量估计值
end


ukf.m

function [X,P]=ukf(ffun,X,P,hfun,Z,Q,R)
x_temp = X;

L=numel(X);
m=numel(Z);
% alpha=1e-2;

alpha = 2;
ki=0;
beta=2;
% alpha = 2;
% ki=3-L;
% beta=2;

lambda=alpha^2*(L+ki)-L;
c=L+lambda;
Wm=[lambda/c 0.5/c+zeros(1,2*L)];
Wc=Wm;
Wc(1)=Wc(1)+(1-alpha^2+beta);
c=sqrt(c);
Xsigmaset=sigmas(X,P,c); 
[X1means,X1,P1,X2]=ut(ffun,Xsigmaset,Wm,Wc,L,Q);   
[Zpre,Z1,Pzz,Z2]=ut(hfun,X1,Wm,Wc,m,R);
Pxz=X2*diag(Wc)*Z2';
K=Pxz*inv(Pzz);
X=X1means+K*(Z-Zpre);
P=P1-K*Pxz';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%d%%%%%
% UT
function [Xmeans,Xsigma_pre,P,Xdiv]=ut(fun,Xsigma,Wm,Wc,n,COV)
LL=size(Xsigma,2);
Xmeans=zeros(n,1);
Xsigma_pre=zeros(n,LL);
for k=1:LL                
    Xsigma_pre(:,k)=fun(Xsigma(:,k));
    Xmeans=Xmeans+Wm(k)*Xsigma_pre(:,k);
end
Xdiv=Xsigma_pre-Xmeans(:,ones(1,LL));
P=Xdiv*diag(Wc)*Xdiv'+COV;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% sigma
function Xset=sigmas(X,P,c)
A = c*chol(P)';
Y = X(:,ones(1,numel(X)));
Xset = [X Y+A Y-A];

结论

PS:为了避免论文查重,以下内容从正文中剪切。

UKF的优点:

滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现文章来源地址https://www.toymoban.com/news/detail-413851.html

到了这里,关于滤波算法 | 无迹卡尔曼滤波(UKF)算法及其MATLAB实现的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 卡尔曼滤波器原理讲解及其matlab实现

    目录 一:卡尔曼滤波器的信号模型[1-2] 二:其他方程及变量介绍 三:卡尔曼滤波器递推公式 四:matlab仿真[3] 参考文献: 引言:在进行一些信号处理的过程中,我们通常会采集到一些数据,但是实际测量到的数据是受到噪声干扰了之后的,故与真实的数据有一些偏差。因此

    2023年04月08日
    浏览(43)
  • 了解卡尔曼滤波器4--非线性状态估算器(EKF,UKF,PF)

            一般来说,我们希望我们的生活是线性的,就像这条线,这可能表示成功、收入或者幸福。但实际上,生活并不是线性的,它充满了起伏,有时甚至更复杂。         如果您是工程师,您经常会需要处理非线性系统,为了帮助您,我们将讨论非线性状态估算器

    2023年04月20日
    浏览(52)
  • 线性卡尔曼跟踪融合滤波算法(Matlab仿真)

            卡尔曼滤波的原理和理论在CSDN已有很多文章,这里不再赘述,仅分享个人的理解和Matlab仿真代码。         假设目标的状态为X =  [x, y, vx, vy],符合匀速直线运动目标,也即                  其中F为状态转移矩阵,                  在匀速直线(const velo

    2024年01月17日
    浏览(41)
  • 【状态估计】卡尔曼滤波器、扩展卡尔曼滤波器、双卡尔曼滤波器和平方根卡尔曼滤波器研究(Matlab代码实现)

    💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果 🎉3 参考文献 🌈4 Matlab代码实现 本文包括

    2024年02月08日
    浏览(43)
  • 【MATLAB源码-第106期】基于matlab的SAR雷达系统仿真,实现雷达目标跟踪功能,使用卡尔曼滤波算法。

    1. 雷达系统参数设定:    - 工作频率:选择一个适合的工作频率,例如X波段(8-12 GHz)。    - 脉冲重复频率(PRF):设定一个适当的PRF,确保雷达覆盖所需的范围。    - 天线增益和波束宽度:根据目标探测的需求确定天线的增益和波束宽度。 2. 目标和环境模拟:

    2024年01月16日
    浏览(57)
  • 1. 简明误差卡尔曼滤波器(ESKF)及其推导过程

    本文主要介绍一种特殊正交群 SO(3) text{SO(3)} SO(3) 上的 ESKF(Error State Kalman Filter, 误差卡尔曼滤波器) (有时也叫做 流形上的ESKF )推导过程。 在现代的大多数 IMU 系统中,人们往往使用 误差状态卡尔曼滤波器(Error State Kalman Filter, ESKF) ,而非 原始状态的卡尔曼滤波器 。大部

    2024年02月06日
    浏览(71)
  • 卡尔曼滤波模型及Matlab模型建立

    目录 一、卡尔曼滤波 1.概念解析: 2.卡尔曼滤波的最优估计模型 3.实例——小车匀加速直线运动 4.Matlab建模  二、扩展卡尔曼滤波(EKF:Extended KAlman Filter) 1.非线性系统的局部线性化 2.扩展卡尔曼滤波模型         卡尔曼滤波的目的:由于人的主观认识(数学模型的建立而产

    2023年04月10日
    浏览(33)
  • 卡尔曼滤波器-概述及用递归思想解读卡尔曼滤波器 | 卡尔曼滤波器应用举例(附Matlab程序)| 数学基础-数据融合、协方差矩阵、状态空间方程

      卡尔曼滤波器是最优化的(Optimal)、递归的(Recursive)、数字处理的(Data Processing)算法(Algorithm)。卡尔曼滤波器更像是观测器,而不是一般意义上的滤波器,应用广泛,尤其是在导航中,它的广泛应用是因为生活中存在大量的不确定性。   当描述一个系统的不确

    2024年02月06日
    浏览(51)
  • 【算法系列】卡尔曼滤波算法

    ·【算法系列】卡尔曼滤波算法 ·【算法系列】非线性最小二乘求解-直接求解法 ·【算法系列】非线性最小二乘求解-梯度下降法 ·【算法系列】非线性最小二乘-高斯牛顿法  ·【算法系列】非线性最小二乘-列文伯格马夸尔和狗腿算法  文章目录 系列文章 文章目录 前言 一、

    2024年02月12日
    浏览(38)
  • 40行MATLAB代码实现卡尔曼滤波-简单易懂

    最近学习了卡尔曼滤波,体会到了数据融合下进行最优估计的思想。如果你也是小白,可以通过这个例子自己动手感受数据融合。 学习资料参考B站大神DR_CAN博士,连接: 【卡尔曼滤波器】直观理解与二维实例 基于上述视频中Excel的例子,使用MATLAB编写了一个简单的卡尔曼滤

    2024年02月04日
    浏览(48)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包