UKF算法滤波性能分析.docx

多源信息融合---UKF算法
PAGE\*MERGEFORMAT11

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

其中

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

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

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

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

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

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

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

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

状态预测误差协方差阵为

卡尔曼滤波增益为

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

状态滤波协方差矩阵为

三、实验仿真与结果分析
假设设系统噪声具有协方差阵，具有协方差阵，二者不相关。观测次数N=50，采样时间为t=0.5。初始状态。则生成的运动轨迹如图1所示。

图1M的轨迹图
t=0.5时UKF和EKF滤波结果比较
我们将UKF和EKF滤波算法进行比较，如图2所示。为了方便对比，我们将测量值得到的距离和角度换算到笛卡尔坐标系中得到x-y测量值，直观的可以看到UKF算法滤波结果优于EKF算法。

图2滤波结果对比图
下面定量分析滤波结果。首先计算UKF和EKF滤波值得到的位置、与该时刻的实际位置的距离、。对该模型做50次蒙特卡洛仿真，得到各个测量点（时刻）的距离均方根误差，如图3所示。在各个测量时刻UKF滤波结果优于EKF。

图3t=0.5时各个测量点的距离RMSE对比图
采样间隔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算法也会发散导致无法跟踪目标。
测量误差对滤波结果的影响
取采样间隔不变，如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