UKF算法滤波性能分析

1、UKF算法滤波性能分析高海南3110038011一、仿真问题描述考虑一个在二维平面x-y内运动的质点M,其在某一时刻k的位置、速度和加速度可用矢量x(k)风,yXk,%,即院T表示。假设M在水平方向(x)作近似匀加速直线运动，垂直方向(y)上亦作近似匀加速直线运动。两方向上运动具有加性系统噪声w(k),则在笛卡尔坐标系下该质点的运动状态方程为x(k 1) fk(x(k) w(k)FkX(k) w(k)其中10t0-02t2010t0-2Fk0010t0假设一坐标位置为(0,0)的雷达对M进行测距rk和测角k,实际测量中雷达具有加性测量噪声v(k),则在传感器极坐标系下，观测方程为z(k) hk

2、(x(k)rk Vr(k) v(k)k v (k),xk2tan2yk1 ykxkVr (k)v (k)显然在笛卡尔坐标系下,该模型运动观测方程为非线性的。我们根据雷达测量值使用UKF算法对目标进行跟踪，并与EKF算法结果进行比较。二、问题分析1.UKF滤波跟踪x(k1)fkx(k)w(k)对于非线Tt系统z(k)hk(x(k)v(k),设w(k)具有协方差阵Qk,v(k)具有协方差阵Rko ukf算法步骤如下:计算 点 幺1|k 1 ,依据xk 1|k 1和pk 1|k 1生成2n + 1个 点* 1|k 1 ,i 0,1,2,L 2n 0在UT变换时，取尺度参数0.01,0,2。计算点鼠，

3、即81k?k|k 1fk&1|k1),i0,1,2,L 2n2n m i i 4|ki 02nc i qip k|k 1ia|kXk|k 1泉|ki 0T?k|k 1 Qk 1(3)计算 点xU 1 pk|k 1通过量测方程对xk的传播，即Vfc 一事珠廿,功?工.+"】工球'=- Qs+匕)-二+l"+计算输出的一步提前预测，即(5)0"=4(号7)，=。, ,2"： 加坪T / - f 、工,七二£才点一-3)- 风jU>获得新的量测后，进行滤波更新：、邛=，半一1 +。(4工坤-1)； 公二心.正1；t = P. ,

4、 -P ,尸"(P . j2.扩展卡尔曼滤波算法分析x(k1)fkx(k)w(k)对于讨论的非线性系统z(k)hk(x(k)v(k),由于状态方程为线性的,定义fjfk(xk)xk=xk|k 1hXhk(Xk)xk=?k|k1XX由于系统状态方程为线性的，则fkFk而量测方程为非线性的，对其关于Xk求偏导，得到,Xhk(Xk)h k Xx|k2XkXk22ykyk2 ykyk, Xk2 yk2Xk2Xk2 ykEKF算法步骤如下：k时刻的一步提前预测0k|kfk1 Xk1|k 1状态预测误差协方差阵为X Xpk|k 1 fk pk 1|k 1 f k 卡尔曼滤波增益为Qk1 Fkpk

5、1|k 1 F kQkRkhk Xk|k 1p# IKkhXpk|k 1Kkpk|k1hkhkpk|k1hkI在k时刻得到新的量测后，状态滤波的更新公式为夕k|kXk|k1Kkzk状态滤波协方差矩阵为三、实验仿真与结果分析假设设系统噪声w(k)具有协方差阵Qk100000010000000.1200000020.10000000.0120000000.01,2，v(k)一 、.52具有协方差阵Rk 5000.012 ,二者不相关。观测次数N=50,采样时间为t=0,5o初始状态X(0)1000,5000,10,50,2,4T。则生成的运动轨迹如图1所示。图1M的轨迹图(1)t=0.5时UKF和

6、EKF滤波结果比较我们将UKF和EKF滤波算法进行比较，如图2所示。为了方便对比，我们将测量值得到的距离和角度换算到笛卡尔坐标系中得到x-y测量值，直观的可以看到UKF算法滤波结果优于EKF算法。图2滤波结果对比图下面定量分析滤波结果。首先计算UKF和EKF滤波值得到的位置(xukf,yukf)、(Xekf,yekf)与该时刻的实际位置(x,y)的距离dukf除x)2(yukfy)2、dekf；(xekfx)2(yekfy)2。对该模型做50次蒙特卡洛仿真，得到各个测量点(时刻)的距离均方根误差，如图3所示。在各个测量时刻UKF滤波结果优于EKE图3t=0.5时各个测量点的距离RMSE对比图(

7、2)采样间隔t对滤波结果的影响下面讨论不同的采样间隔t对滤波结果的影响。分别取t=0.1,1.0,1.5,得到滤波结果与RMSE如下图所示。E呻靠阳翻版时比Uh也富如能lODCi1SD0加髓JTO*0-KUO4&D0与伯博的答图5采样时间t=1.0时结果从上面的3张图可以看到，在采样间隔t不太大时（0.1,1.0）,EKF?口UKF算法均能跟踪目标，且UKF算法滤波精度优于EKF算法。而当t=1.5甚至更大时，EKF算法滤波不收敛，而UKF算法跟踪精度变化不大。对于EKF和UKF算法，在不同的t时，我们分别取其滤波协方差阵对角线的第二个元素（即y方向位置方差），作出位置方差变化图如下。

8、图7不同采样间隔的y方向位置滤波方差变化图出现上述现象的原因为当采样间隔t增大时，非线性函数Taylor展开式的高阶项无法忽略，EKF算法线性化（一阶展开）使得系统产生较大的误差，导致了滤波的不稳定。由于UKF算法可以精确到二阶或者三阶Taylor展开项，所以这种现象不明显，但是当t进一步增大，尤其是跟踪目标的状态变化剧烈时，更高阶项误差影响不可忽略，进而UKF算法也会发散导致无法跟踪目标。（3）测量误差对滤波结果的影响取采样间隔不变，如t=0.5s,对于不同白测量误差Rk,分析其对EKF?口UKF10050002 ,结果00.0012算法滤波结果的影响。分别取 Rik0 0.0012 R2k

9、误差较大时，UKF和EKF滤波精度相差不大。综合以上分析可以看到，UKF算法对于解决非线性模型滤波问题时， 相对于EKF算法，它不需要计算雅克比矩阵，具有较好的跟踪精度，而且在非线性严重或者高阶误差引入时，会推迟或延缓滤波发散，因此在实际中得到了广泛的应用附：m代码注：UT变换及UKF函数均来自于YiCaoatCranfieldUniversity,04/01/2008Unscented Kalman Filter for nonlinear dynamic systems%numer of states%numer of measurements%default, tunable%defau

10、lt, tunable%default, tunable%scaling factor%scaling factor%weights for meansfunctiony,Y,P,Y1=ut(f,X,Wm,Wc,n,R)%UnscentedTransformationL=size(X,2);y=zeros(n,1);Y=zeros(n,L);fork=1:LY(:,k尸f凶:,k);y=y+Wm(k)*Y(:,k);endY1=Y-y(:,ones(1,L);P=Y1*diag(Wc)*Y1'+R;functionx,P=ukf(fstate,x,P,hmeas,z,Q,R)%UKFL

11、=numel(x);m=numel(z);alpha=1e-2;ki=0;beta=2;lambda=alphaA2*(L+ki)-L;c=L+lambda;Wm=lambda/c0.5/c+zeros(1,2*L);Wc=Wm;Wc(1)=Wc(1)+(1-alphaA2+beta);c=sqrt(c);X=sigmas(x,P,c);x1,X1,P1,X2=ut(fstate,X,Wm,Wc,L,Q);z1,Z1,P2,Z2=ut(hmeas,X1,Wm,Wc,m,R);measurmentsP12=X2*diag(Wc)*Z2'K=P12*inv(P2);x=x1+K*(z-z1

12、);P=P1-K*P12'；functionX=sigmas(x,P,c)%SigmapointsaroundreferencepointA=c*chol(P)'%Cholesky分解Y=x(:,ones(1,numel(x);X=xY+AY-A%weightsforcovariance%sigmapointsaroundx%unscentedtransformationofprocess%unscentedtransformationof%transformedcross-covariance%stateupdate%covarianceupdatefunctionP_k,X

13、_k尸ekf(f,h,Q,R,Z,x,P)t=1;fx=10t0tA2/20;010-t0-tA2/2;0010t0;00010t;000010;000001;%一步提前预测值和预测误差的协方差阵分别是：x_1=f(x);%k-1时刻对k时亥Ux值的预测P_k_k_1=仅*P*fx'+Q;%k-1时刻对k时刻p值的预测hx=x_1(1)/sqrt(x_1(1)A2+x_1(242)x_1(2)/sqrt(x_1(1)A2+x_1(2)A2)0000;-x_1(2)/(x_1(1)A2+x_1(242)-x_1(1)/(x_1(1)A2+x_1(2)A2)0000;%获取k时刻测量值z后

14、，滤波更新值和相应的滤波误差的协方差矩阵K_k=P_k_k_1*hx'*inv(hx*P_k_k_1*hx'+R);%k时刻kalman滤波增益X_k=x_1+K_k*(Z-h(x_1);P_k=P_k_k_1-K_k*hx*P_k_k_1;ukf_test.mclear;clcn=6;t=0.5;MC=50;Q=100000;010000;000.01000;0000.0100;00000.00010;000000.0001;%过程噪声协方差阵R=1000;00.001A2;%量测噪声协方差阵f=(x)x(1)+t*x(3)+0.5*tA2*x(5);x(2)+t*x(4)+

15、0.5*tA2*x(6);x(3)+t*x(5);x(4)+t*x(6);x(5);x(6);%x1为X轴位置，x2为丫轴位置，x3、x4分别X,%Y轴的速度，x5、x6为；两方向的加速度h=(x)sqrt(x(1)A2+x(2)A2);atan(x(2)/x(1);%measurementequations=1000;5000;10;50;2;-4;x0=s+sqrtm(Q)*randn(n,1);%initialstatewithnoiseP0=10000000;01000000;001000;000100;00000.10;000000.1;%initialstatecovraiance

16、N=50;%totaldynamicstepsukV=zeros(n,N);%ukfestmateekV=zeros(n,N);sV=zeros(n,N);%actualzV=zeros(2,N);%测量值ekx=zeros(MC,N);eky=zeros(MC,N);eux=zeros(MC,N);euy=zeros(MC,N);fori=1:NsV(:,i)=f(s);s=sV(：,i)；endplot(sV(1,:),sV(2,:),'k-')title('M的弹道图')formc=1:MCuP=P0;eP=P0;ux=x0;ek_x=x0;fork=1:

17、Nz=h(sV(:,k)+sqrtm(R)*randn(2,1);%测量值measurmentszV(:,k)=z;%savemeasurmentux,uP=ukf(f,ux,uP,h,z,Q,R);%ukfPukf(k尸uP(2,2);ukV(:,k)=ux;P_k,ek_x=ekf(f,h,Q,R,z,ek_x,eP);ekV(:,k)=ek_x;Pekf(k)=P_k(2,2);endekx(mc,:)=ekV(1,:)-sV(1,:);eky(mc,:)=ekV(2,:)-sV(2,:);eux(mc,:)=ukV(1,:)-sV(1,:);euy(mc,:)=ukV(2,:)-sV(

18、2,:);endaux=mean(eux,1);auy=mean(euy,1);akx=mean(ekx,1);aky=mean(eky,1);a=ekx.A2+eky.A2;b=eux.A2+euy.A2;fori=1:MCforj=1:Ndsekf(i,j尸sqrt(a(i,j);dsukf(i,j尸sqrt(b(i,j);endendrmse_ekf=std(dsekf,0,1);rmse_ukf=std(dsukf,0,1);figuret=1:N;plot(sV(1,t),sV(2,t),'k-',sV(1,t)+aux(t),sV(2,t)+auy(t),'b-',sV(1,t)+akx(t),sV(2,t)+aky(t),'r-',zV(1,t).*cos(zV(2,t),zV(1,t).*sin(zV(2,t),'g')legend('实际值','