UKF算法滤波性能分析

1、UKF算法滤波性能分析高海南 3110038011一、仿真问题描述考虑一个在二维平面x-y内运动的质点M，其在某一时刻k的位置、速度和加速度可用矢量表示。假设M在水平方向（x）作近似匀加速直线运动，垂直方向（y）上亦作近似匀加速直线运动。两方向上运动具有加性系统噪声，则在笛卡尔坐标系下该质点的运动状态方程为其中假设一坐标位置为（0,0）的雷达对M进行测距和测角，实际测量中雷达具有加性测量噪声，则在传感器极坐标系下，观测方程为显然在笛卡尔坐标系下，该模型运动观测方程为非线性的。我们根据雷达测量值使用UKF算法对目标进行跟踪，并与EKF算法结果进行比较。二、问题分析1. UKF滤波跟踪对于非线性系

2、统，设具有协方差阵，具有协方差阵。ukf算法步骤如下：(1) 计算点，依据和生成2n+1个点，。在UT变换时，取尺度参数，。(2) 计算点，即(3) 计算点通过量测方程对的传播，即(4) 计算输出的一步提前预测，即(5) 获得新的量测后，进行滤波更新：2. 扩展卡尔曼滤波算法分析对于讨论的非线性系统，由于状态方程为线性的，定义由于系统状态方程为线性的，则，而量测方程为非线性的，对其关于求偏导，得到EKF算法步骤如下：k时刻的一步提前预测状态预测误差协方差阵为卡尔曼滤波增益为在k时刻得到新的量测后，状态滤波的更新公式为状态滤波协方差矩阵为三、实验仿真与结果分析假设设系统噪声具有协方差阵，具有协方

3、差阵，二者不相关。观测次数N=50，采样时间为t=0.5。初始状态。则生成的运动轨迹如图1所示。图1 M的轨迹图(1) t=0.5时UKF和EKF滤波结果比较我们将UKF和EKF滤波算法进行比较，如图2所示。为了方便对比，我们将测量值得到的距离和角度换算到笛卡尔坐标系中得到x-y测量值，直观的可以看到UKF算法滤波结果优于EKF算法。图2 滤波结果对比图下面定量分析滤波结果。首先计算UKF和EKF滤波值得到的位置、与该时刻的实际位置的距离、。对该模型做50次蒙特卡洛仿真，得到各个测量点（时刻）的距离均方根误差，如图3所示。在各个测量时刻UKF滤波结果优于EKF。图3 t=0.5时各个测量点的距

4、离RMSE对比图(2) 采样间隔t对滤波结果的影响下面讨论不同的采样间隔t对滤波结果的影响。分别取t=0.1,1.0,1.5，得到滤波结果与RMSE。如下图所示。图4 采样时间t=0.1时结果图5 采样时间t=1.0时结果图 6 采样时间t=1.5时结果从上面的3张图可以看到，在采样间隔t不太大时（0.1,1.0），EKF和UKF算法均能跟踪目标，且UKF算法滤波精度优于EKF算法。而当t=1.5甚至更大时，EKF算法滤波不收敛，而UKF算法跟踪精度变化不大。对于EKF和UKF算法，在不同的t时，我们分别取其滤波协方差阵对角线的第二个元素（即y方向位置方差），作出位置方差变化图如下。图7 不同

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

6、由上面两图对比可知，当测量误差较小时，UKF滤波精度优于EKF；当测量误差较大时，UKF和EKF滤波精度相差不大。综合以上分析可以看到，UKF算法对于解决非线性模型滤波问题时，相对于EKF算法，它不需要计算雅克比矩阵，具有较好的跟踪精度，而且在非线性严重或者高阶误差引入时，会推迟或延缓滤波发散，因此在实际中得到了广泛的应用。附：m代码注：UT变换及UKF函数均来自于Yi Cao at Cranfield University, 04/01/2008function y,Y,P,Y1=ut(f,X,Wm,Wc,n,R)%Unscented TransformationL=size(X,2);y=

7、zeros(n,1);Y=zeros(n,L);for k=1:L Y(:,k)=f(X(:,k); y=y+Wm(k)*Y(:,k); endY1=Y-y(:,ones(1,L);P=Y1*diag(Wc)*Y1'+R;function X=sigmas(x,P,c)%Sigma points around reference pointA = c*chol(P)'%Cholesky分解Y = x(:,ones(1,numel(x);X = x Y+A Y-Afunction x,P=ukf(fstate,x,P,hmeas,z,Q,R)% UKF Unscented Kal

8、man Filter for nonlinear dynamic systemsL=numel(x); %numer of statesm=numel(z); %numer of measurementsalpha=1e-2; %default, tunableki=0; %default, tunablebeta=2; %default, tunablelambda=alpha2*(L+ki)-L; %scaling factorc=L+lambda; %scaling factorWm=lambda/c 0.5/c+zeros(1,2*L); %weights for meansWc=Wm

9、;Wc(1)=Wc(1)+(1-alpha2+beta); %weights for covariancec=sqrt(c);X=sigmas(x,P,c); %sigma points around xx1,X1,P1,X2=ut(fstate,X,Wm,Wc,L,Q); %unscented transformation of processz1,Z1,P2,Z2=ut(hmeas,X1,Wm,Wc,m,R); %unscented transformation of measurmentsP12=X2*diag(Wc)*Z2' %transformed cross-covaria

10、nceK=P12*inv(P2);x=x1+K*(z-z1); %state updateP=P1-K*P12' %covariance updatefunction P_k,X_k=ekf(f,h,Q,R,Z,x,P)t=1;fx=1 0 t 0 t2/2 0; 0 1 0 -t 0 -t2/2; 0 0 1 0 t 0; 0 0 0 1 0 t; 0 0 0 0 1 0; 0 0 0 0 0 1;%一步提前预测值和预测误差的协方差阵分别是：x_1 =f( x ); %k-1时刻对k时刻x值的预测P_k_k_1 = fx*P*fx' + Q; %k-1时刻对k时刻p值的预测h

11、x= x_1(1)/sqrt(x_1(1)2+x_1(2)2) x_1(2)/sqrt(x_1(1)2+x_1(2)2) 0 0 0 0; -x_1(2)/(x_1(1)2+x_1(2)2) -x_1(1)/(x_1(1)2+x_1(2)2) 0 0 0 0;%获取k时刻测量值z后，滤波更新值和相应的滤波误差的协方差矩阵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.mclea

12、r;clcn=6;t=0.5;MC=50;Q=1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 0.01 0 0 0; 0 0 0 0.01 0 0; 0 0 0 0 0.0001 0; 0 0 0 0 0 0.0001;%过程噪声协方差阵R = 100 0; 0 0.0012;%量测噪声协方差阵f=(x)x(1)+t*x(3)+0.5*t2*x(5);x(2)+t*x(4)+0.5*t2*x(6);x(3)+t*x(5);x(4)+t*x(6);x(5);x(6);%x1为X轴位置，x2为Y轴位置，x3、x4分别X，%Y轴的速度，x5、x6为；两方向的加速度h=(x)sqrt(x(

13、1)2+x(2)2);atan(x(2)/x(1);% measurement equations=1000;5000;10;50;2;-4;x0=s+sqrtm(Q)*randn(n,1); % initial state with noiseP0 =100 0 0 0 0 0; 0 100 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 0.1 0; 0 0 0 0 0 0.1; % initial state covraianceN=50; % total dynamic stepsukV = zeros(n,N); %ukf estmateekV

14、= 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);for i=1:N sV(:,i)= f(s); s = sV(:,i);endplot( sV(1,:),sV(2,:), 'k-')title('M的弹道图')for mc=1:MC uP=P0;eP=P0; ux=x0;ek_x=x0; for k=1:N z = h(sV(:,k) + sqrtm(R)*randn(

15、2,1); % 测量值measurments zV(:,k) = z; % save measurment ux, uP = ukf(f,ux,uP,h,z,Q,R); % ukf Pukf(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); end ekx(mc,:)=ekV(1,:)-sV(1,:); eky(mc,:)=ekV(2,:)-sV(2,:); eux(mc,:)=ukV(1,:)-sV(1,:); euy(mc,:)=ukV(2,:)-

16、sV(2,:);endaux=mean(eux,1);auy=mean(euy,1);akx=mean(ekx,1);aky=mean(eky,1);a=ekx.2+eky.2;b=eux.2+euy.2;for i=1:MC for j=1:N dsekf(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('实际值','ukf估计值','ekf估计值','测量值'