UKF算法滤波性能分析.docx

多源信息融合---UKF算法
UKF算法滤波性能分析
高海南 3110038011
一、仿真问题描述
考虑一个在二维平面x-y内运动的质点M，其在某一时刻k的位置、速度和加速度可用矢量表示。假设M在水平方向（x）作近似匀加速直线运动，垂直方向（y）上亦作近似匀加速直线运动。两方向上运动具有加性系统噪声，则在笛卡尔坐标系下该质点的运动状态方程为

其中

假设一坐标位置为（0,0）的雷达对M进行测距和测角，实际测量中雷达具有加性测量噪声，则在传感器极坐标系下，观测方程为

显然在笛卡尔坐标系下，该模型运动观测方程为非线性的。我们根据雷达测量值使用UKF算法对目标进行跟踪，并与EKF算法结果进行比较。
二、问题分析
1. UKF滤波跟踪
对于非线性系统，设具有协方差阵，具有协方差阵。ukf算法步骤如下：
(1) 计算点，依据和生成2n+1个点，。在UT变换时，取尺度参数，，。
(2) 计算点，即

(3) 计算点通过量测方程对的传播，即

(4) 计算输出的一步提前预测，即

(5) 获得新的量测后，进行滤波更新：

2. 扩展卡尔曼滤波算法分析
对于讨论的非线性系统，由于状态方程为线性的，定义

由于系统状态方程为线性的，则，
而量测方程为非线性的，对其关于求偏导，得到

EKF算法步骤如下：
k时刻的一步提前预测

状态预测误差协方差阵为

卡尔曼滤波增益为

在k时刻得到新的量测后，状态滤波的更新公式为

状态滤波协方差矩阵为

三、实验仿真与结果分析
假设设系统噪声具有协方差阵，具有协方差阵，二者不相关。观测次数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时各个测量点的距离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 不同采样间隔的y方向位置滤波方差变化图
出现上述现象的原因为当采样间隔t增大时，非线性函数Taylor展开式的高阶项无法忽略，EKF算法线性化（一阶展开）使得系统产生较大的误差，导致了滤波的不稳定。由于UKF算法可以精确到二阶或者三阶Taylor展开项，所以这种现象不明显，但是当t进一步增大，尤其是跟踪目标的状态变化剧烈时，更高阶项误差影响不可忽略，进而UKF算法也会发散导致无法跟踪目标。
(3) 测量误差对滤波结果的影响
取采样间隔不变，如t=0.5s，对于不同的测量误差，分析其对EKF和UKF算法滤波结果的影响。分别取，结果如下

图8 测量误差阵为R1k时滤波结果

图9 测量误差阵为R2k时滤波结果
由上面两图对比可知，当测量误差较小时，UKF滤波精度优于EKF；当测量误差较大时，UKF和EKF滤波精度相差不大。
综合以上分析可以看到，UKF算法对于解决非线性模型滤波问题时，相对于EKF算法，它不需要计算雅克比矩阵，具有较好的跟踪精度，而且在非线性严重或者高阶误差引入时，会推迟或延缓滤波发散，因此在实际中得到了广泛的应用。

附：m代码
注：UT变换及UKF函数均来自于Yi Cao at Cranfield University, 04/01/2008

11

function [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)
%Unscented Transformation
L=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);
end
Y1=Y-y(:,ones(1,L));
P=Y1*diag(Wc)*Y1'+R;
function X=sigmas(x,P,c)
%Sigma points around reference point
A = c*chol(P)';%Cholesky分解
Y = x(:,ones(1,numel(x)));
X = [x Y+A Y-A]

function [x,P]=ukf(fstate,x,P,hmeas,z,Q,R)
% UKF Unscented Kalman Filter for nonlinear dynamic systems
L=numel(x); %numer of states
m=numel(z); %numer of measurements
alpha=1e-2; %default, tunable
ki=0; %default, tunable
beta2; %default, tunable
lambda=alpha^2*(L+ki)-L; %scaling factor
c=L+lambda; %scaling factor
Wm=[lambda/c 0.5/c+zeros(1,2*L)]; %weights for means
Wc=Wm;
Wc(1)=Wc(1)+(1-alpha^2+beta); %weights for covariance
c=sqrt(c);
X=sigmas(x,P,c); %sigma points around x
[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q); %unscented transformation of process
[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R); %unscented transformation of measurments
P12=X2*diag(Wc)*Z2'; %transformed cross-covariance
K=P12*inv(P2);
x=x1+K*(z-z1); %state update
P=P1-K*P12'; %covariance update

function [P_k,X_k]=ekf(f,h,Q,R,Z,x,P)
t=1;
fx=[1 0 t 0 t^2/2 0;
0 1 0 -t 0 -t^2/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)^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.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 0 0;
0 0 0 0 0.0001 0;
0 0 0 0 0 0.0001];%过程噪声协方差阵
R = [100 0;
0 0.001^2];%量测噪声协方差阵
f=@(x)[x(1)+t*x(3)+0.5*t^2*x(5);x(2)+t*x(4)+0.5*t^2*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 equation
s=[1000;5000;10;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=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);
end

plot( 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(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,:)-sV(2,:);
end
aux=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));
end
end
rmse_ekf=std(dsekf,0,1);rmse_ukf=std(dsukf,0,1);
figure
t=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
plot(t,rmse_ekf(t),'r-',t,rmse_ukf(t),'b-')
legend('ekf RMSE','ukf RMSE')
title('t=1 距离RMSE')
figure
plot(t,Pekf(t),'r-',t,Pukf(t),'b-')
title('t=1 y方向位置方差')