UKF算法滤波性能分析复习过程Word文件下载.docx
《UKF算法滤波性能分析复习过程Word文件下载.docx》由会员分享，可在线阅读，更多相关《UKF算法滤波性能分析复习过程Word文件下载.docx（17页珍藏版）》请在冰豆网上搜索。
，设
具有协方差阵
，
。
ukf算法步骤如下：
（1）计算
点
，依据
和
生成2n+1个
在UT变换时，取尺度参数
（2）计算
，即
（3）计算
通过量测方程对
的传播，即
（4）计算输出的一步提前预测，即
（5）获得新的量测后，进行滤波更新：
2.扩展卡尔曼滤波算法分析
对于讨论的非线性系统
，由于状态方程为线性的，定义
由于系统状态方程为线性的，则
而量测方程为非线性的，对其关于
求偏导，得到
EKF算法步骤如下：
k时刻的一步提前预测
状态预测误差协方差阵为
卡尔曼滤波增益为
在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;
升级会员 