UKF法滤波性能分析

1、UKF算法滤波性能分析高海南 3110038011一、仿真问题描述考虑一个在二维平面x-y内运动的质点M，其在某一时刻k的位置、速度和 加速度可用矢量x(k)=Xk,yk，xk,yk，xk,ykf表示。假设M在水平方向(x)作近似 匀加速直线运动，垂直方向(y)上亦作近似匀加速直线运动。两方向上运动具 有加性系统噪声|w(k)，则在笛卡尔坐标系下该质点的运动状态方程为x(k 1) = f k( x (k) w (k) = Fk x(k) w(k)其中-110t002苴010t02Fk =0010t000010t0000101 1000001 一假设一坐标位置为(0,0)的雷达对M进行测距凶和测

2、角，实际测量中 雷达具有加性测量噪声v(k)，则在传感器极坐标系下，观测方程为z(k) = h k( x (k) v(k)” +v(k) I"k+v 试k)_-Qx十+ vr (k)I tan'也+v(p(k) Xk显然在笛卡尔坐标系下，该模型运动观测方程为非线性的。我们根据雷达测 量值使用UKF算法对目标进行跟踪，并与 EKF算法结果进行比较。问题分析具/、1. UKF滤波跟踪 x(k +1) = f k x(k) + w (k)对于非线性系统I z(k) =hk(x(k)十v(k)，设Iw(k)|具有协方差阵有协方差阵RJ。ukf算法步骤如下： 计算=I点I,依据|Xk1

3、N_|l和 |pkg 生成2n +1个可点|,i =0,12,|刘。在UT变换时，取尺度参数叵三001，叵三0，匸三2。计算点创，即麻=fk （ &）,i =0,1,2|）2n2n 父甲丄=送i|ki -0.2nciiTP k|k4 =送 COi （冷k x<|k 丄 JCO $|k 父/丄）+QkJ_i =0 计算門点陽寸P k2通过量测方程对 区的传播，即於旳=*比-】：於J 和t+Q(R")吒4】)厂212阳=和t -Q(h +久)P屮一J - i = JS 十 2* 一 S;(4)计算输出的一步提前预测，即VS），1 = 0,1,-,2m；2fiJf-oPg二&

4、#163;（谓珈-】XV -和_】）；ufD(5)获得新的量测后，进行滤波更新:耳啡二左年j + 耳二族_心阡); 鞋=P*2.扩展卡尔曼滤波算法分析| x(k+1)= fkx(k) + w(k)对于讨论的非线性系统'z(k) = hk(x(k) + v(k)，由于状态方程为线性的,定义fx _ 歼k（Xk）f kpXk= xk|k）OX1h -")cXx k =?k|k 1由于系统状态方程为线性的，则f k =FkEKF算法步骤如下：k时刻的一步提前预测而量测方程为非线性的，对其关于 显求偏导，得到状态预测误差协方差阵为pk|kJ ：" fk p k J|k J

5、f kQTkJ Fkp k J|k dF k' Q kJ卡尔曼滤波增益为Kk = pk|k k hkP k|k 4h k ' R k在k时刻得到新的量测后，状态滤波的更新公式为父k|k = Xk|k Kk zk - hk父k|k状态滤波协方差矩阵为p k|k = I - Kkh k p k|k JN=50,采样三、实验仿真与结果分析100000T010000Qk =000.12000w (k)具有协方差阵00020.10000000.01201000000.012一假设设系统噪声；520 1R k =0.012 一0v(k)具有协方差阵二者不相关。观测次数时间为t=0.5。初始

6、状态|x(°)二口000,5000,10,50,2, -4丁。则生成的运动轨迹如图1所示图1 M的轨迹图t=0.5时UKF和EKF滤波结果比较我们将UKF和 EKF滤波算法进行比较，如图2所示。为了方便对比，我们将 测量值得到的距离和角度换算到笛卡尔坐标系中得到 x-y测量值，直观的可以看 到UKF算法滤波结果优于EKF算法。图2滤波结果对比图F面定量分析滤波结果。首先计算UKF和 EKF滤波值得到的位置（XukfVukf）（Xekfjekf）与该时刻的实际位置(x,y)| 的距离 dukf =J(Xukf x)2+(yukf y)2、I22dekf 二.'(Xekf -X)

7、(yekf - 丫)对该模型做50次蒙特卡洛仿真，得到各个测量点（时刻）的距离均方根误差，如图 3所示。在各个测量时刻 UKF滤波结果优于EKF图3 t=0.5时各个测量点的距离 RMSE对比图(2)采样间隔t对滤波结果的影响下面讨论不同的采样间隔t对滤波结果的影响。分别取t=0.1,1.0,1.5，得到 滤波结果与RMSE如下图所示。=0 1 Kb"® RMSE图4采样时间t=0.1时结果图5 采样时间t=1.0时结果ekfAMSEukfRMSE图6采样时间t=1.5时结果从上面的3张图可以看到，在采样间隔t不太大时（0.1,1.0） , EKF和UKF算 法均能跟踪目标

8、，且UKF算法滤波精度优于EKF算法。而当t=1.5甚至更大时， EKF算法滤波不收敛，而UKF算法跟踪精度变化不大。对于 EKF和 UKF算法，在 不同的t时，我们分别取其滤波协方差阵对角线的第二个元素（即 y方向位置方 差），作出位置方差变化图如下。图7不同采样间隔的y方向位置滤波方差变化图出现上述现象的原因为当采样间隔t增大时，非线性函数Taylor展开式的高 阶项无法忽略，EKF算法线性化（一阶展开）使得系统产生较大的误差，导致了 滤波的不稳定。由于UKF算法可以精确到二阶或者三阶 Taylor展开项，所以这种 现象不明显，但是当t进一步增大，尤其是跟踪目标的状态变化剧烈时，更高阶 项

9、误差影响不可忽略，进而 UKF算法也会发散导致无法跟踪目标。（3）测量误差对滤波结果的影响取采样间隔不变，如t=0.5s,对于不同的测量误差r，分析其对EKF和UKF算法滤波结果的影响分别取Rik10 0 1 “0 0.0012 一0 I0.0012，结果图9测量误差阵为R2k时滤波结果由上面两图对比可知，当测量误差较小时，UKF滤波精度优于EKF；当测量误差较大时，UKF和 EKF滤波精度相差不大。综合以上分析可以看到，UKF算法对于解决非线性模型滤波问题时， 相对于 EKF算法，它不需要计算雅克比矩阵，具有较好的跟踪精度，而且在非线性严重 或者高阶误差引入时，会推迟或延缓滤波发散，因此在实

10、际中得到了广泛的应用。附：m代码注：UT变换及 UKF函数均来自于 Yi Cao at Cranfield University, 04/01/2008Un sce nted Kalma n Filter for non li near dyn amic systems%nu mer of statesfun ctio n y,Y,P,Y1=ut(f,X,Wm,Wc, n,R) %Unscen ted Tran sformatio n L=size(X,2);y=zeros( n,1);Y=zeros (n 丄)；for k=1:LY(:,k)=f(X(:,k); y=y+Wm(k)*Y(:,

11、k);endY1=Y-y(:,o nes(1,L); P=Y1*diag(Wc)*Y1'+R;fun ctio n x,P=ukf(fstate,x,P,hmeas, z, Q,R) % UKFL=nu mel(x); m=nu mel(z);alpha=1e-2;ki=0;beta=2;lambda=alphaA2*(L+ki)-L; c=L+lambda;Wm=lambda/c 0.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(fstat

12、e,X,Wm ,Wc, L,Q); z1,Z1,P2,Z2=ut(hmeas,X1,Wm,Wc,m,R); measurme ntsP12=X2*diag(Wc)*Z2'K=P12*i nv(P2); x=x1+K*(z-z1);P=P1-K*P12'fun cti on X=sigmas(x,P,c)%Sigma points around reference point A = c*chol(P)'%Cholesky 分解Y = x(:, on es(1, nu mel(x);X = x Y+A Y-A%nu mer of measureme nts%default

13、, tun able%default, tun able%default, tun able%scali ng factor%scali ng factor %weights for mea ns%weights for covaria nee%sigma points around x%unscen ted tran sformati on of process%un sce ntedtran sformatio nof%tra nsformed cross-covaria nee%state update%covaria nee updatefun ctio n P_k,X_k=ekf(f

14、,h,Q,R,Z,x,P) t=1;fx=1 0 t 0 tA2/2 0;0 1 0 -t 0 -tA2/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 值的预测hx= x_1(1)/sqrt(x_1(1)A2+x_1(2)A2)x_1(2)/sqrt(x_1(1)A2+x_1(2)A2) 0 0 0 0;-x_1(2)/(x_1(1)A2+

15、x_1(2)A2)-x_1(1)/(x_1(1)A2+x_1(2)A2) 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.m clear;clc n=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

16、0 0;0 0 0 0 0.0001 0;0 0 0 0 0 0.0001;% 过程噪声协方差阵R = 100 0;0 0.001A2;% 量测噪声协方差阵f=(x)x(1)+t*x(3)+0.5*tA2*x(5);x(2)+t*x(4)+0.5*tA2*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(1)A2+x(2)A2);atan(x(2)/x(1);% measurement equation s=1000;5000;10;

17、50;2;-4;x0=s+sqrtm(Q)*randn(n,1); % initial state with noise P0 =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 covraiance N=50; % total dynamic steps ukV = zeros(n,N); %ukf estmate ekV = zeros(n,N); sV = zeros(n,N); %actual zV = zeros(2,N);% 测量值 ekx

18、=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:Nz = h(sV(:,k) + sqrtm(R)*randn(2,1); % 测量值 measurments zV(:,k) = z; % save measurment ux,

19、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,:)-sV(2,:);end aux=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;for i=1:MC for j