广东建设信息公开网站,网站页面统计代码,做网站的找哪个,微网站医院策划案目录简介UKF滤波滤波流程和公式MATLAB程序结论简介
本文接着分享位姿跟踪和滤波算法中用到的一些常用程序#xff0c;希望为后来者减少一些基础性内容的工作时间。以往分享总结见文章#xff1a;位姿跟踪 | 相关内容目录和链接总结#xff08;不断更新中~~~#xff09;
本…
目录简介UKF滤波滤波流程和公式MATLAB程序结论简介
本文接着分享位姿跟踪和滤波算法中用到的一些常用程序希望为后来者减少一些基础性内容的工作时间。以往分享总结见文章位姿跟踪 | 相关内容目录和链接总结不断更新中~~~
本文分享无迹卡尔曼滤波UKF算法的一些基本公式和MATLAB程序。
UKF滤波
首先简单介绍一下UKF滤波的优点和公式。本文偏工程性介绍因此UKF的原理就不再过多介绍了。
滤波流程和公式
首先需要明确滤波模型中的状态变量、状态方程和测量方程。
状态变量x[ ] 状态方程x(k1) f(x(k)) 测量方程z(k1) h(x(k1))
很简单对吧
接着求解介绍求解流程 相比于一般的卡尔曼滤波UKF算法增加了两次无迹变换公式为 权重和方差计算公式为 Sigma点传播 计算x的预测值和协方差矩阵 4. 得到一组新的Sigma点 5. 代入观测方程中得到测量量的预估值
获得观测量的预测值和协方差矩阵 更新状态变量和协方差矩阵
另外每次写论文画卡尔曼流程图中都找不到参考的模板。我自己画了个滤波流程图不一定符合每个人的审美以备参考 MATLAB程序
MATLAB程序如下包括main.m函数和ukf.m函数
main.m P[]
Q[]
R[]f(x)[]
h(x)[]z_msm []
x0[]
ux x0; % 滤波初始值
for k2:n_before_colli[xukf(:,k), P] ukf(f_before,ux,P,h_before,z_pre(:,k),Q,R);uxxukf(:,k); % k时刻的状态变量估计值
end
ukf.m
function [X,P]ukf(ffun,X,P,hfun,Z,Q,R)
x_temp X;Lnumel(X);
mnumel(Z);
% alpha1e-2;alpha 2;
ki0;
beta2;
% alpha 2;
% ki3-L;
% beta2;lambdaalpha^2*(Lki)-L;
cLlambda;
Wm[lambda/c 0.5/czeros(1,2*L)];
WcWm;
Wc(1)Wc(1)(1-alpha^2beta);
csqrt(c);
Xsigmasetsigmas(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);
PxzX2*diag(Wc)*Z2;
KPxz*inv(Pzz);
XX1meansK*(Z-Zpre);
PP1-K*Pxz;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%d%%%%%
% UT
function [Xmeans,Xsigma_pre,P,Xdiv]ut(fun,Xsigma,Wm,Wc,n,COV)
LLsize(Xsigma,2);
Xmeanszeros(n,1);
Xsigma_prezeros(n,LL);
for k1:LL Xsigma_pre(:,k)fun(Xsigma(:,k));XmeansXmeansWm(k)*Xsigma_pre(:,k);
end
XdivXsigma_pre-Xmeans(:,ones(1,LL));
PXdiv*diag(Wc)*XdivCOV;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% sigma
function Xsetsigmas(X,P,c)
A c*chol(P);
Y X(:,ones(1,numel(X)));
Xset [X YA Y-A];
结论
PS为了避免论文查重以下内容从正文中剪切。
UKF的优点
