UKF算法滤波性能分析Word文档下载推荐.docx

UKF算法滤波性能分析Word文档下载推荐.docx

《UKF算法滤波性能分析Word文档下载推荐.docx》由会员分享，可在线阅读，更多相关《UKF算法滤波性能分析Word文档下载推荐.docx（13页珍藏版）》请在冰豆网上搜索。

状态预测误差协方差阵为

卡尔曼滤波增益为

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

状态滤波协方差矩阵为

三、实验仿真与结果分析

假设设系统噪声具有协方差阵，具有协方差阵，二者不相关。

观测次数N=50，采样时间为t=0.5。

初始状态。

则生成的运动轨迹如图1所示。

图1M的轨迹图

（1）t=0.5时UKF和EKF滤波结果比较

我们将UKF和EKF滤波算法进行比较，如图2所示。

为了方便对比，我们将测量值得到的距离和角度换算到笛卡尔坐标系中得到x-y测量值，直观的可以看到UKF算法滤波结果优于EKF算法。

图2滤波结果对比图

下面定量分析滤波结果。

首先计算UKF和EKF滤波值得到的位置、与该时刻的实际位置的距离、。

对该模型做50次蒙特卡洛仿真，得到各个测量点（时刻）的距离均方根误差，如图3所示。

在各个测量时刻UKF滤波结果优于EKF。

图3t=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函数均来自于YiCaoatCranfieldUniversity,04/01/2008

function[y,Y,P,Y1]=ut（f,X,Wm,Wc,n,R）

%UnscentedTransformation

L=size（X,2）;

y=zeros（n,1）;

Y=zeros（n,L）;

fork=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;

functionX=sigmas（x,P,c）

%Sigmapointsaroundreferencepoint

A=c*chol（P）'

;

%Cholesky分解

Y=x（:

ones（1,numel（x）））;

X=[xY+AY-A]

function[x,P]=ukf（fstate,x,P,hmeas,z,Q,R）

%UKFUnscentedKalmanFilterfornonlineardynamicsystems

L=numel（x）;

%numerofstates

m=numel（z）;

%numerofmeasurements

alpha=1e-2;

%default,tunable

ki=0;

beta=2;

lambda=alpha^2*（L+ki）-L;

%scalingfactor

c=L+lambda;

Wm=[lambda/c0.5/c+zeros（1,2*L）];

%weightsformeans

Wc=Wm;

Wc

（1）=Wc

（1）+（1-alpha^2+beta）;

%weightsforcovariance

c=sqrt（c）;

X=sigmas（x,P,c）;

%sigmapointsaroundx

[x1,X1,P1,X2]=ut（fstate,X,Wm,Wc,L,Q）;

%unscentedtransformationofprocess

[z1,Z1,P2,Z2]=ut（hmeas,X1,Wm,Wc,m,R）;

%unscentedtransformationofmeasurments

P12=X2*diag（Wc）*Z2'

%transformedcross-covariance

K=P12*inv（P2）;

x=x1+K*（z-z1）;

%stateupdate

P=P1-K*P12'

%covarianceupdate

function[P_k,X_k]=ekf（f,h,Q,R,Z,x,P）

t=1;

fx=[10t0t^2/20;

010-t0-t^2/2;

0010t0;

00010t;

000010;

000001];

%一步提前预测值和预测误差的协方差阵分别是：

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）0000;

-x_1

（2）/（x_1

（1）^2+x_1

（2）^2）-x_1

（1）/（x_1

（1）^2+x_1

（2）^2）0000];

%获取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=[100000;

010000;

000.01000;

0000.0100;

00000.00010;

000000.0001];

%过程噪声协方差阵

R=[1000;

00.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））];

%measurementequation

s=[1000;

5000;

10;

50;

2;

-4];

x0=s+sqrtm（Q）*randn（n,1）;

%initialstatewithnoise

P0=[10000000;

01000000;

001000;

000100;

00000.10;

000000.1];

%initialstatecovraiance

N=50;

%totaldynamicsteps

ukV=zeros（n,N）;

%ukfestmate

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）;

fori=1:

N

sV（:

i）=f（s）;

s=sV（:

i）;

plot（sV（1,:

）,sV（2,:

）,'

k-'

）

title（'

M的弹道图'

formc=1:

MC

uP=P0;

eP=P0;

ux=x0;

ek_x=x0;

fork=1:

z=h（sV（:

k））+sqrtm（R）*randn（2,1）;

%测量值measurments

zV（:

k）=z;

%savemeasurment

[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,:

euy（mc,:

）=ukV（2,:

aux=mean（eux,1）;

auy=mean（euy,1）;

akx=mean（ekx,1）;

aky=mean（eky,1）;

a=ekx.^2+eky.^2