1、多源信息融合-UKF 算法1UKF 算法滤波性能分析高海南 3110038011一、仿真问题描述考虑一个在二维平面 x-y 内运动的质点 M，其在某一时刻 k 的位置、速度和加速度可用矢量 表示。假设 M 在水平方向（x ）作近(),Tkkkxyxy似匀加速直线运动，垂直方向（y）上亦作近似匀加速直线运动。两方向上运动具有加性系统噪声 ，则在笛卡尔坐标系下该质点的运动状态方程为()kw(1)()()()()k kkxfxwFxw其中 22100010001k tttttF假设一坐标位置为（0,0）的雷达对 M 进行测距 和测角 ，实际测量中krk雷达具有加性测量噪声 ，则在传感器极坐标系下，观
2、测方程为()kv 221()()()()() tankkrkrk kxyvv zhxv显然在笛卡尔坐标系下，该模型运动观测方程为非线性的。我们根据雷达测量值使用 UKF 算法对目标进行跟踪，并与 EKF 算法结果进行比较。二、问题分析1. UKF 滤波跟踪对于非线性系统 ，设 具有协方差阵 ，(1)()()kkkxfxwzhv()kkQ多源信息融合-UKF 算法2具有协方差阵 。ukf 算法步骤如下：()kvkR(1) 计算 点 ，依据 和 生成 2n+1 个 点 ，1|ik1|kx1|kp1|ik。在 UT 变换时，取尺度参数 ， ， 。0,2in 0.2(2) 计算 点 ，即|ik| 1|
3、2|1|02|1|1|110(),0,12iikkknmikkin Tci ikikkkkki n fxpxxQ(3) 计算 点 通过量测方程对 的传播，即|1kx|1k k(4) 计算输出的一步提前预测，即(5) 获得新的量测后，进行滤波更新：2. 扩展卡尔曼滤波算法分析对于讨论的非线性系统 ，由于状态方程为线性的，(1)()()kkkxfxwzhv定义 k|1=()kkk xxff多源信息融合-UKF 算法3k|1=()kk xxh由于系统状态方程为线性的，则 ，kkxfF而量测方程为非线性的，对其关于 求偏导，得到k|12222=222200()k k kkk k kxyyxxy xxh
4、EKF 算法步骤如下：k 时刻的一步提前预测 |111|kkkxfx状态预测误差协方差阵为 |11| 11| 1T TkkkkkkkkkxxpfpfQFpQ卡尔曼滤波增益为 |1|1TTkkkkkkKxxhhR在 k 时刻得到新的量测后，状态滤波的更新公式为 |1 |1 kkkkkK xzx状态滤波协方差矩阵为 | |1xkkkpIhp三、实验仿真与结果分析假设设系统噪声 具有协方差阵 ，)(kw222100.00.10.1k Q具有协方差阵 ，二者不相关。观测次数 N=50，采样)(kv2201.5kR时间为 t=0.5。 初始状态 。则生成的运动(),50,15,24Tx轨迹如图 1 所示
5、。多源信息融合-UKF 算法4图 1 M 的轨迹图(1) t=0.5 时 UKF 和 EKF 滤波结果比较我们将 UKF 和 EKF 滤波算法进行比较，如图 2 所示。为了方便对比，我们将测量值得到的距离和角度换算到笛卡尔坐标系中得到 x-y 测量值，直观的可以看到 UKF 算法滤波结果优于 EKF 算法。图 2 滤波结果对比图下面定量分析滤波结果。首先计算 UKF 和 EKF 滤波值得到的位置、 与该时刻的实际位置 的距离),(ukffyx),(ekffyx ),(yx、 。对该模型做 5022dufff 22)(ydekfekfekf 次蒙特卡洛仿真，得到各个测量点（时刻）的距离均方根误差
6、，如图 3 所示。在各个测量时刻 UKF 滤波结果优于 EKF。多源信息融合-UKF 算法5图 3 t=0.5 时各个测量点的距离 RMSE 对比图(2) 采样间隔 t 对滤波结果的影响下面讨论不同的采样间隔 t 对滤波结果的影响。分别取 t=0.1,1.0,1.5，得到滤波结果与 RMSE。如下图所示。图 4 采样时间 t=0.1 时结果图 5 采样时间 t=1.0 时结果多源信息融合-UKF 算法6图 6 采样时间 t=1.5 时结果从上面的 3 张图可以看到，在采样间隔 t 不太大时（0.1,1.0） ，EKF 和 UKF算法均能跟踪目标，且 UKF 算法滤波精度优于 EKF 算法。而当
7、 t=1.5 甚至更大时，EKF 算法滤波不收敛，而 UKF 算法跟踪精度变化不大。对于 EKF 和 UKF 算法，在不同的 t 时，我们分别取其滤波协方差阵对角线的第二个元素（即 y 方向位置方差） ，作出位置方差变化图如下。图 7 不同采样间隔的 y 方向位置滤波方差变化图出现上述现象的原因为当采样间隔 t 增大时，非线性函数 Taylor 展开式的高阶项无法忽略，EKF 算法线性化（一阶展开）使得系统产生较大的误差，导致了滤波的不稳定。由于 UKF 算法可以精确到二阶或者三阶 Taylor 展开项，所以这种现象不明显，但是当 t 进一步增大，尤其是跟踪目标的状态变化剧烈时，更高阶项误差影
8、响不可忽略，进而 UKF 算法也会发散导致无法跟踪目标。(3) 测量误差对滤波结果的影响取采样间隔不变，如 t=0.5s，对于不同的测量误差 ，分析其对 EKF 和kR多源信息融合-UKF 算法7UKF 算法滤波结果的影响。分别取 ，2101.kR201.5kR结果如下图 8 测量误差阵为 R1k 时滤波结果图 9 测量误差阵为 R2k 时滤波结果由上面两图对比可知，当测量误差较小时，UKF 滤波精度优于 EKF；当测量误差较大时，UKF 和 EKF 滤波精度相差不大。综合以上分析可以看到，UKF 算法对于解决非线性模型滤波问题时，相对于 EKF 算法，它不需要计算雅克比矩阵，具有较好的跟踪精
9、度，而且在非线性严重或者高阶误差引入时，会推迟或延缓滤波发散，因此在实际中得到了广泛的应用。多源信息融合-UKF 算法8附：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=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*di
10、ag(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 Kalman Filter for nonlinear dynamic systemsL=numel(x); %numer of statesm=numel(z); %numer of measurementsalph
11、a=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;Wc(1)=Wc(1)+(1-alpha2+beta); %weights for covariancec=sqrt(c);X=sigmas(x,P,c); %sigma points around xx1,X
12、1,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-covarianceK=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)
13、t=1;fx=1 0 t 0 t2/2 0;0 1 0 -t 0 -t2/2;0 0 1 0 t 0;多源信息融合-UKF 算法90 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)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(
14、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.mclear;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
15、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(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); %
16、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 = zeros(n,N);sV = zeros(n,N); %actualzV = zeros(2,N);%测量值ekx=zeros(MC,N);eky=zeros(MC,N);多源信息融合-UKF 算法10
17、eux=zeros(MC,N);euy=zeros(MC,N);for i=1:NsV(:,i)= f(s);s = sV(:,i);endplot( sV(1,:),sV(2,:), k-)title(M 的弹道图)for mc=1:MCuP=P0;eP=P0;ux=x0;ek_x=x0;for k=1:Nz = h(sV(:,k) + sqrtm(R)*randn(2,1); % 测量值 measurmentszV(:,k) = z; % save measurmentux, uP = ukf(f,ux,uP,h,z,Q,R); % ukfPukf(k)=uP(2,2);ukV(:,k)
18、= 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(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:MCfor j=1:Nds
19、ekf(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 估计值,测量值,1)title(t=1 ukf 和 ekf 滤波对比图)figure
