UKF的算法介绍.docxVIP

UKF的算法介绍——作者，niewei120，nuaa参数说明：其中sigama点的获得方法如下：例外的一种描述方式其滤波算法表示如下：UKF 中各参数的影响：α 确定了采样点与均值的远近程度，通常取 0 到 1 之间的正值，所取值越大则距离平均值越远，当其值为零时，UKF 的滤波效果相当于 EKF；UT中的 k 为一比例因子，通常取为 0，当状态分布可以认为是高斯分布时，可取 k = n - 3(n为状态向量 x 的维数)；β 在正态分布时取 2。当 Q 和 R 过大时，UKF 的状态非常不稳定，有时甚至还会发散。整个的matlab的程序如下：（1）sigama点的计算，输出为siagma点和权重值，以及sigama点的数量function [xPts, wPts, nPts] = scaledSymmetricSigmaPoints(x,P,alpha,beta,kappa)% % This function returns the scaled symmetric sigma point distribution.%% [xPts, wPts, nPts] = scaledSymmetricSigmaPoints(x,P,alpha,beta,kappa) %% Inputs:% x mean状态量均值% P covariance 协方差% alpha scaling parameter 1% a决定万周围Sigma点的分布状态，调节其值能够使得高阶项影响最小% beta extra weight on zeroth point%调节beta的值可以提高方差的精度，对于高斯分布，beta为2是最优的% kappa scaling parameter 2 (usually set to default 0)% 是一个比例因子kappa=alpha^2*(n+kappa)-n%% Outputs:% xPts The sigma points sigma点% wPts The weights on the points 所在点的权重% nPts The number of points sigma点的数目%%%% (C) 2000 Rudolph van der Merwe % (C) 1998-2000 S. J. Julier.% Number of sigma points and scaling terms sigma点的个数n = size(x(:),1);nPts = 2*n+1; % were using the symmetric SUT采用的是对称sui，此时的sigma点为2n+1% Recalculate kappa according to scaling parameters 根据标定参数重新计算kappa值kappa = alpha^2*(n+kappa)-n;% Allocate space 分配空间wPts=zeros(1,nPts);xPts=zeros(n,nPts);% Calculate matrix square root of weighted covariance matrix 计算根号下的协方差矩阵Psqrtm=(chol((n+kappa)*P)); % Array of the sigma points sigma点的阵列xPts=[zeros(size(P,1),1) -Psqrtm Psqrtm];% Add mean back inxPts = xPts + repmat(x,1,nPts); % Array of the weights for each sigma point sigma点的权重阵列wPts=[kappa 0.5*ones(1,nPts-1) 0]/(n+kappa);% Now calculate the zeroth covariance term weight 计算0处的协方差权重wPts(nPts+1) = wPts(1) + (1-alpha^2) + beta;（2）ukf滤波算法function [xEst,PEst,xPred,PPred,zPred,inovation,S,K]=ukf(xEst,PEst,U,Q,ffun,z,R,hfun,dt,alpha,beta,kappa)% TITLE : UNSCENTED KALMAN FILTER%% PURPOSE : This function performs one complet