UKF的算法介绍

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)%%Thisfunctionreturnsthescaledsymmetricsigmapointdistribution.%%[xPts,wPts,nPts]=scaledSymmetricSigmaPoints(x,P,alpha,beta,kappa)%%Inputs:%xmean状态量均值%Pcovariance协方差%alphascalingparameter1%a决定万周围Sigma点的分布状态，调节其值能够使得高阶项影响最小%betaextraweightonzero'thpoint%调节beta的值可以提高方差的精度，对于高斯分布，beta为2是最优的%kappascalingparameter2(usuallysettodefault0)%是一个比例因子kappa=alpha^2*(n+kappa)-n%%Outputs:%xPtsThesigmapointssigma点%wPtsTheweightsonthepoints所在点的权重%nPtsThenumberofpointssigma点的数目%%%%(C)2000RudolphvanderMerwe%(C)1998-2000S.J.Julier.%Numberofsigmapointsandscalingtermssigma点的个数n=size(x(:),1);nPts=2*n+1;%we'reusingthesymmetricSUT采用的是对称sui，此时的sigma点为2n+1%Recalculatekappaaccordingtoscalingparameters根据标定参数重新计算kappa值kappa=alpha^2*(n+kappa)-n;%Allocatespace分配空间wPts=zeros(1,nPts);xPts=zeros(n,nPts);%Calculatematrixsquarerootofweightedcovariancematrix计算根号下的协方差矩阵Psqrtm=(chol((n+kappa)*P))';%Arrayofthesigmapointssigma点的阵列xPts=[zeros(size(P,1),1)-PsqrtmPsqrtm];%AddmeanbackinxPts=xPts+repmat(x,1,nPts);%Arrayoftheweightsforeachsigmapointsigma点的权重阵列wPts=[kappa0.5*ones(1,nPts-1)0]/(n+kappa);%Nowcalculatethezero'thcovariancetermweight计算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:UNSCENTEDKALMANFILTER%%PURPOSE:ThisfunctionperformsonecompletestepoftheunscentedKalmanfilter.%%SYNTAX:[xEst,PEst,xPred,PPred,zPred,inovation]=ukf(xEst,PEst,U,Q,ffun,z,R,hfun,dt,alpha,beta,kappa)%%INPUTS:-xEst:statemeanestimateattimekk时刻卡尔曼状态量均值的预测值%-PEst:statecovarianceattimekk时刻协方差的预测值%-U:vectorofcontrolinputs输入的控制量%-Q:processnoisecovarianceattimekk时刻的状态噪声%%-z:observationatk+1k+1时刻的观测量%-R:measurementnoisecovarianceatk+1k+1的测量噪声的协方差%%-ffun:processmodelfunction状态方程%-hfun:observationmodelfunction观测方程%-dt:timestep(passedtoffun/hfun)时间步长%-alpha(optional):sigmapointscalingparameter.Defaultsto%1.影响sigma向量围绕分布的扩展因子%-beta(optional):higherordererrorscalingparameter.%Defaultto0.另一个规模因子%-kappa(optional):scalartuningparameter1.Defaultsto%0.先验分布的因子%%OUTPUTS:-xEst:updatedestimateofstatemeanattime%k+1k+1时刻的状态量的更新值%-PEst:updatedstatecovarianceattimek+1%k+1时刻的协方差的更新值%-xPred:predictionofstatemeanattimek+1%k+1时刻的状态量的预测值%-PPred:predictionofstatecovarianceattime%k+1k+1时刻的协方差预测值%-inovation:innovationvector更新矢量%NOTES:Theprocessmodelisoftheform,x(k+1)=ffun[x(k),v(k),dt,u(k)]%wherev(k)istheprocessnoisevector.Theobservationmodelis%oftheform,z(k)=hfun[x(k),w(k),dt,u(k)],wherew(k)isthe%observationnoisevector.%%Thiscodewaswrittentobereadable.Thereissignificant%scopeforoptimisationeveninMatlab.%%Processdefaultsif(nargin10)alpha=1;end;if(nargin11)beta=0;end;if(nargin12)kappa=0;end;%Calculatethedimensionsoftheproblemandafewuseful%scalarsstates=size(xEst(:),1);observations=size(z(:),1);vNoise=size(Q,2);%过程噪声wNoise=size(R,2);%观测噪声noises=vNoise+wNoise;%Augmentthestatevectorwiththenoisevectors.%Note:Forsimple,additivenoisemodelsthispart%canbedonedifferentlytosaveoncomputationalcost.%Fordetails,contactRudolphv.d.Merweif(noises)N=[Qzeros(vNoise,wNoise);zeros(wNoise,vNoise)R];PQ=[PEstzeros(states,noises);zeros(noises,states)N];xQ=[xEst;zeros(noises,1)];elsePQ=PEst;xQ=xEst;end;%CalculatethesigmapointsandtherecorrespondingweightsusingtheScaledUnscented%Transformation[xSigmaPts,wSigmaPts,nsp]=scaledSymmetricSigmaPoints(xQ,PQ,alpha,beta,kappa);%计算sigma点的均值、权重和sigma点的数目%DuplicatewSigmaPtsintomatrixforcodespeedup重复wSigmaPts使得矩阵运算速度提升wSigmaPts_xmat=repmat(wSigmaPts(:,2:nsp),states,1);wSigmaPts_zmat=repmat(wSigmaPts(:,2:nsp),observations,1);%Workouttheprojectedsigmapointsandtheirmeans%Thisroutineisfairlygeneric.Theonlythingtowatchoutforare%angulardiscontinuities.Thereisastandardtrickfordoingthis-%contactme(Julier)fordetails!xPredSigmaPts=feval(ffun,xSigmaPts(1:states,:),repmat(U(:),1,nsp),xSigmaPts(states+1:states+vNoise,:),dt);%状态更新zPredSigmaPts=feval(hfun,xPredSigmaPts,repmat(U(:),1,nsp),xSigmaPts(states+vNoise+1:states+noises,:),dt);%观测更新%Calculatethemean.BasedondiscussionswithC.Schaefer,form%ischosentomaximisenumericalrobustness.%-Ivectorizedthispartofthecodeforaspeedincrease:RvdM2000xPred=sum(wSigmaPts_xmat.*(xPredSigmaPts(:,2:nsp)-repmat(xPredSigmaPts(:,1),1,nsp-1)),2);%状态量的一步预测值zPred=sum(wSigmaPts_zmat.*(zPredSigmaPts(:,2:nsp)-repmat(zPredSigmaPts(:,1),1,nsp-1)),2);%观测量的一步预测值xPred=xPred+xPredSigmaPts(:,1);zPred=zPred+zPredSigmaPts(:,1);%Workoutthecovariancesandthecrosscorrelations.Notethat%theweightonthe0thpointisdifferentfromthemean%calculationduetothescaledunscentedalgorithm.exSigmaPt=xPredSigmaPts(:,1)-xPred;%sigma点状态值的误差ezSigmaPt=zPredSigmaPts(:,1)-zPred;%sigma点量测值的误差PPred=wSigmaPts(nsp+1)*exSigmaPt*exSigmaPt';%状态量的协方差PxzPred