本科毕业论文-基于UKF的非线性状态估计问题研究.doc

基于UKF的非线性状态估计问题研究吕保强（陕西师范大学 物理学与信息技术学院 陕西 西安 710062）摘 要：介绍了卡尔曼滤波器的理论，UT (Unscented Transformation) 的基本思路与基本算法、以及UKF（Unscented Kalman Filtering）的理论分析和算法。针对非线性、高精度测量的环境，文中选用了一个雷达对目标跟踪的非线性估计的例子进行研究, 并对UKF和EKF(Extended Kalman Filtering)两种跟踪算法进行了仿真，比较了两者在非线性状态估计中的滤波性能和特点，结果表明：在强非线性高斯系统,UKF的滤波精度要高于 EKF。关键词 : 卡尔曼滤波器; UT; UKF；非线性；EKF1 绪论1.1 引言在滤波器的发展过程中，早期的维纳滤波器涉及到对不随时间变化的统计特性的处理，即静态处理。在这种信号处理过程中，有用信号和无用噪声的统计特性可与它们的频率特性联系起来，因此与经典滤波器在概念上还有一定的联系。维纳滤波采用频域设计法，运算复杂，解析求解困难，整批数据处理要求存储空间大，造成其适用范围极其有限，仅适用于一维平稳随机过程信号滤波。维纳滤波的缺陷促使人们寻求时域内直接设计最优滤波器的新方法，其中美国学者R.E.Kalman的研究最具代表性。1960年，R.E.Kalman提出了离散系统的Kalman滤波;次年，他又与布西 (R.5Bucy)合作，把这一滤波方法推广到连续时间系统中，从而形成Kalman滤波估计理论1。与维纳滤波不同，卡尔曼滤波是对时变统计特性进行处理，他不是从频域，而是从时域的角度出发来考虑问题。卡尔曼滤波是属于现代滤波技术的一种状态估计手段，本质上来讲滤波就是一个信号处理与变换，去除或减弱不想要的成分，增强所需成分的过程，这个过程既可以通过硬件来实现，也可以通过软件来实现。卡尔曼滤波属于一种软件滤波方法，其基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，根据该算法建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。目前，卡尔曼滤波理论广泛应用于航空航天、导航定位、目标跟踪、控制等各种领域。由于实际系统大多数是非线性系统，而最初提出的卡尔曼滤波算法仅适用于线性观测的线性系统。为了解决这一问题，人们开始研究把卡尔曼滤波器应用到非线性系统中，为此Bucy等人提出了非线性条件下的EKF (Extended Kalman Filtering) 2。应用于非线性系统的EKF算法对于非线性的系统方程或观测方程进行泰勒展开，并取其一阶近似项。这样做之后，不可避免地引入了线性化误差，当线性化假设不成立时，采用这种算法会导致滤波器性能下降甚至造成发散。另外，在一般情况下计算系统状态方程和观测方程的Jacobian矩阵或Hessians矩阵是很困难的，增加了算法的计算复杂度。为了解决EKF中存在的问题。Julier和Ohlmann提出了一种新的适合于非线性系统的滤波器UKF 3。UKF是针对非线性系统的一种改进型卡尔曼滤波器。UKF处理非线性系统的基本思路在于无味变换，而无味变换从根本上讲是一种描述高斯随机变量在非线性化变换后的概率分布情况的方法。UKF认为，与其将一个非线性化变换线性化、近似化，还不如将高斯随机变量经非线性变换后的概率分布情况用高斯分布来近似那样简单，因而UKF算法没有非线性化这一步骤。UKF按照一套公式产生一系列样点(sigma点)，每一样点均配有一个相应的权重，而这些带权的样点被用来完整地描述系统状态向量估计值的分布情况，它们替代了原先卡尔曼滤波器中的状态向量估计值及协方差。UKF让这些样点一一经历非线性状态方程与测量方程，然后再将这些经非线性变换后的样点按照它们的权重而综合出对当前时刻的系统状态向量估计值。准确的、稳定的、高精度的卡尔曼滤波器，是获取系统状态以及各种信息的必要条件。然而由于种种原因，正如前面所说，一般的UKF滤波器在复杂多变的环境中，现有的卡尔曼滤波器难以起到良好的效果。为了能够在各种复杂环境下，使得传统的卡尔曼滤波器的应用领域得到延伸，人们对传统的卡尔曼滤波器做出了许多改进。高精度、高稳定性的卡尔曼滤波器是实现控制系统状态估计的关键技术之一，因此提高UKF滤波器的精确性、跟踪能力具有重要意义。1.2本文的主要研究内容及结构本文主要针对UKF的基础理论及其在非线性系统中应用做了一些研究。在介绍UKF论的基础上，对其在跟踪目标等方面的滤波性能进行了有益的研究。各小节的主要内容安排如下:论文的主要研究内容如下：第一部分简述了估计理论及卡尔曼滤波理论的提出、发展及应用，以及无味变换的提出及发展过程。第二部分主要介绍UKF基础理论，依次讲述了随机非线性离散系统的卡尔曼滤波理论，并给出了扩展卡尔曼滤波器的数学模型，接着详细介绍了UT (Unscented Transformation)的理论和算法分析，并在此基础上详细推导了UKF基本方程。第三部分对卡尔曼滤波技术在目标跟踪中的应用课题进行了研究。一方面，利用卡尔曼滤波良好的跟踪性能，实现了对目标位置的状态估计;另一方面，从分析UKF和EKF的对目标跟踪性能的研究，进一步归纳和分析了UKF对非线性系统状态估计的性能和滤波特点。第四部分在前文研究的基础上对全文内容作了总结。2 UKF的基本思想及理论研究2.1 非线性状态估计原理如果对非线性系统不作任何假设，那么最小均方误差意义上的最优估计为条件均值3 （2-1） 其中是n时刻以前的观测序列。估计这个期望值需要知道先验概率密度。由它不仅能够确定最小均方误差估计器，而且不论任何特定的性能估计都能得到最优值。而对非线性状态滤波过程的实现包括一步预测与测量修正两个阶段。2.1.1一步预测：根据所有过去时刻的测量信息对状态作最小方差估计 （2-2） 状态估计质量的优劣利用一步预测误差协方差矩阵描述 （2-3）2.1.2测量修正：获得当前时刻的测量信息后，对状态预测值进行修正，得到状态的最优估计 （2-4） （2-5） （2-6） （2-7） （2-8）描述最优状态估值质量优劣的误差协方差阵确定如下 （2-9）2.2非线性的卡尔曼滤波 卡尔曼滤波器估计一个用线性随机差分方程描述的离散时间过程的状态变量。但是在实际的应用中所有的系统都是非线性的，其中许多还是强非线性的，在非线性系中被估计的观测变量与过程变量的关系是非线性的。这时我们可以应用非线性估计领域的经典算法EKF 4-5,15来处理非线性的问题，它是将期望和方差线性化的卡尔曼滤波器。考虑一般的非线性系统，状态方程和观测方程可表示为: （2-10a） （2-10b）式中为维状态向量，为维观测向量，为维控制向量，为系统噪声，且 ，为观测噪声，且。与相互独立且与系统状态无关，并且均可以假设为高斯白噪声，即：均值为零，方差分别为和。我们的目的就是要递推地在每次获得观测量后, 估计状态量。定义状态量的一步预测为，其它类推,则上述问题在线性最小均方误差意义上的线性最优估计子为: （2-11）其中最的最优预测， 为的最优预测，称为卡尔曼滤波增益，卡尔曼滤波器使用来反映新息对估计的重要程度。完整的滤波公式如下所示：2.2.1扩展卡尔曼滤波器时间更新方程： （2-12） （2-13）就像基本的离散卡尔曼滤波器，时间更新方程将状态和协方差估计从 时刻向前推算到时刻。和是时刻的过程雅可比矩阵，是时刻的过程激励噪声协方差矩阵。2.2.2扩展卡尔曼滤波器状态更新方程: （2-14） （2-15） （2-16）就像基本的离散卡尔曼滤波器，上面三式中的测量更新方程利用观测值变量的值校正状态估计和协方差估计。和 是 时刻的测量雅可比矩阵，是中时刻的观测噪声协方差矩阵。2.2.3通过分析，EKF算法具有如下的优点：(1)未知分布的均值和协方差的获得仅需要保存较少的信息量，但却能支持大多数的操作过程，如确定搜索目标的区域等。(2)均值和协方差具有线性传递性。(3)均值和协方差估计的集合能用来表征分布的附加特征，例如重要模式等。正是由于以上优点，人们仍然希望在非线性滤波方法中应用EKF线性估形式。同时，作为对非线性函数线性化所带来的副作用，EKF滤波器的缺点也非常的明显：(1)必须满足小扰动假设,即假设非线性方程的理论解与实际解之差为小量。也就是说EKF只适合弱非线性系统,对于强非线性系统,该假设不成立,此时EKF滤波性能极不稳定,甚至发散; (2)必须计算Jacobian矩阵及其幂,这是一件计算复杂、极易出错的工作。(3)EKF的另外一个缺点是初始状态不太好确定，如果假设的初始状态和初始协方差误差较大，也容易导致滤波器发散。2.3 Uscented变换（UT）卡尔曼滤波方法为非线性高斯滤波提供了一种次优的递推式实现方法，它在每一步的迭代过程中均需求出随机分布经过非线性变换(函数)后的均值和方差，其中EKF滤波方法等非线性滤波器是采用近似非线性函数的方法来求得。有别于传统的实现方法。UT变换的主要思想是“近似概率分布要比近似非线性函数更容易”6，它采用具有确定性的点集Sigma点，来表征输入状态的分布(或部分统计特征)，然后就是对每个Sigma点分别进行非线性变换，通过加权计算捕捉到变换后的统计特性7，它的基本步骤可概括为: 关于 x 的 点( sigma- point) 集的产生不确定性的非线性变换与传递关于 y的统计特性的推算。这种方法把系统当作“黑盒来处理，因而不依赖于具体的非线性，也不必计算Jacobian矩阵。种方法的本质，可以用图2-1来表达：图2-1 sigam点的非线性传递2.3.1构造Sigma点：采用对称采样点策略时，其所选取的 Sigma点集关于 x 的均值对称分布。对于均值，方差为 的n维随机变量x，产生2n+1个列向量（sigma）为 ， ， ， （2-17）其中表示矩阵的第n行向量或者列向量, 而矩阵平方根的常见求法就是用 Cholesky 8-9分解来获得。为尺度参数，调整它可以提高逼近精度，用这组采样点 可以近似表示状态 x 的高斯分布。2.3.2对Sigma点进行非线性变换 对所构造的点集 进行非线性变换，得到变换后的Sigma点集 i=0,1,2n （2-18）变换后的Sigma点集 即可近似地表示的分布。2.3.3计算y的均值和方差对变换后的Sigma点集 进行加权处理，从而得到输出量y 的均值和方差。 （2-19） （2-20） （2-21） （2-22） ,i=1,2,2n （2-23）其中其中和 分别为计算的均值和方差所用加权值 ，标量是自由参数，可以用来捕捉给定分布的高阶信息，对于高斯分布，考虑到4阶距的统计量，通常的取值为。可以证明，该Sigma点集的输入变量x具有相同的均值，方差和高阶奇次中心距10。在均值和方差加权中需要确定 ，和 共3个参数，它们的取值范围分别为：确定 周围Sigma点的分布，通常设为一个较小的正数；为第二个尺度参数，通常设置为0或3-n；为状态分布参数，对于高斯分布 是最优的，如果状态变量是单变量，则最佳的选择是。2.3.4 UT变换的特点如下：（1）对非线性函数的概率密度分布进行近似，而不是对非线性函数进行近似，即使系统的模型复杂，也不增加算法实现的难度。（2）所得到的非线性函数的统计量的准确性可以达到三阶（泰勒展开）。（3）不需要计算Jacobin矩阵，可以处理不可导非线性函数。2.4 UKF滤波算法以上讨论了在一次实现中如何用无味变换估计随机量经非线性映射后的统计特性, 但实际中更多的是要求能够在线、实时、 反复地进行估计, 这就涉及到无味变换的递推实现无味滤波 ( UF, Unscented Filtering ) 。无味滤波的实现很简单,是将无味变换对随机变量经非线性映射后统计信息的估计嵌到其它的滤波算法中。虽然并不局限于卡尔曼滤波, 但与无味变换最常见的结合还是卡尔曼滤波, 并被称为UKF。UKF滤波方法对噪声的处理包含扩展和非扩展两种方式15，前者在系统模型不变的情况下，将过程噪声和观测噪声隐含在系统中，一次迭代过程只需要产生一次Sigma点集，但运算量明显增大；而非扩展法则可以简化Sigma点的个数，滤波实时性更好，较适合于处理加性高斯噪声11-13。本文主要采用非扩展形式的UKF滤波算法，对于式(2-10)描述的非线性动态系统，假设其状态噪声和观测噪声均为高斯白噪声，方差分别为和，则滤波过程如下：2.4.1初始化根据输入变量x的统计量和，选择一种Sigma点采样策略，得到输入变量的Sigma点集，以及相对应的均值加权值和方差加权值： （2-24）2.4.2 状态估计(1)计算sigma点：其中n为选定特定的采样策略所产生的Sigma点的个数，其中均值附近的Sigma点到中心点的距离表达式将会随着不同的采样策略而不同：i=0 （2-25） i=1,2,n （2-26） i=n+1,2n （2-27）(2)时间更新方程（预测方程）：由系统状态方程对各个采样的输入变量Sigma点集中的每一个Sigma点进行非线性变换，得到变换后的Sigma点集： （2-28）对变换后的Sigma点集进行加权处理，从而得到一步预测状态: （2-29）使用同样的方法求取状态的一步预测方差阵： （2-30）根据一步预测值，再次使用UT变换，产生新的Sigma点集： （2-31） , i=1,2,n （2-32） , i=n+1,2n （2-33）由非线性观测方程对Sigma点集进行非线性变换： （2-34）使用加权求和计算得到系统的预测观测值： （2-35）(3)测量更新方程：计算协方差 （2-36）求得系统量测输出变量的方差阵： （2-37）计算滤波增益阵： （2-38）得到状态更新后的滤波值： （2-39）求解状态后验方差阵： （2-40）从以上实现过程可以清楚地看出，UKF滤波方法在式(2-29)、(2-30)、（2-35）、(2-36)以及(2-37)这5个均值和方差的求解上，均通过UT变换方法加权求和得到；而2.2.2小节中的扩展卡尔曼滤波方法则是对系统的状态和观测方程进行线性化求得，这便是两者的最大不同。基于该基本UKF滤波算法，还可以构造平方根UKF滤波器(Square RootUKF，SRUKF)6,14，它可以有效避免滤波误差方差阵和一步预报误差方差阵失去对称性和正定性，较好地解决了计算字长不够而导致的滤波器数值发散等问题。2.4.3通过分析，UKF算法具有如下的特点： (1) UKF是对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后验概率密度，而不是对非线性函数进行近似，不需要求导计算Jacobian矩阵。（2）UKF没有线性化忽略高阶项，因此非线性分布统计量的计算精度较高（3）系统函数可以不连续。（4）随机状态可以不是高斯的。3 UKF对目标位置的状态估计3.1 问题提出考虑一个在二维平面x-y内运动的目标M，假设M在水平方向（x）作近似匀加速直线运动，垂直方向（y）上亦作近似匀加速直线运动。首先，在直角坐标系中建立目标运动模型，在仿真中，观测站和目标都用质点表示。观测站与目标的相对位置关系如图3-1所示： 图 3-1目标运动模型则在笛卡尔坐标系下该质点的运动状态方程为： (3-1) 式中其在前两个变量表示观测站与目标之间的位置，中间两个变量表示相对运动速度，后面两个变量表示相对运动加速度。是系统动态噪声向量，系统矩阵如下:假设一坐标位置为（0,0）的雷达在观测站对M进行测距和测角，实际测量中雷达具有加性测量噪，则在笛卡尔极坐标系下，系统的观测方程为： (3-2) 显然在笛卡尔坐标系下，该模型运动观测方程为非线性的。我们根据雷达测量值使用UKF算法对目标进行跟踪，并与EKF算法结果进行比较。3.2 问题分析3.2.1UKF滤波跟踪分析：考虑一般的非线性系统，状态方程和观测方程可表示为： （3-3a） （3-3b）设具有协方差阵，具有协方差阵，UKF算法为（2-24）至（2-40）。3.2.2 EKF算法分析：对于式（2-10）讨论的非线性系统，由于状态方程为线性的，可以定义： （3-4a） （3-4b）由于系统状态方程为线性的，则，而量测方程为非线性的，对其关于求偏导，则EKF算法为（2-12）至（2-16）。 （3-5）3.3实验仿真与结果分析假设设系统噪声具有协方差阵: 具有协方差阵: 二者是不相关。观测次数N=50，采样时间为t=0.5。初始状态。则生成的运动轨迹如图3-2所示。图3-2 M的轨迹图3.3.1 t=0.1和t=0.5时将UKF和EKF滤波结果进行比较我们将UKF和EKF滤波算法进行比较，如图3-3所示。为了方便对比，我们将测量值得到的距离和角度换算到笛卡尔坐标系中得到x-y测量值，通过分别比较值t=0.1和t=0.5时的滤波值，我们可以直观的看到UKF算法滤波结果优于EKF算法。 图3-3 滤波结果对比图3.3.2下面定量分析滤波结果首先计算UKF和EKF滤波值得到的位置、与该时刻的实际位置的距离、。为了定量地比较 UKF和EKF性能,我们定义一次独立实验的均方根误差15为: （3-6）其中,T 表示一次实验的时间步长, 表示时刻的估计值,表示时刻的真值。对该模型做50次蒙特卡洛仿真，得到各个测量点（时刻）的距离均方根误差，如图3-4所示。在各个测量时刻EKF滤波的RMSE值为3.5,UKF滤波的RMSE值为2.5,由此得出UKF滤波结果优于EKF。图3-4 t=0.5时各个测量点的距离RMSE对比图3.3.3采样间隔t对滤波结果的影响下面讨论不同的采样间隔t对滤波结果的影响。我们分别取x轴方向和y轴方向预测轨迹值的距离均方根误差，取t=0.5,1.0,1.5，得到RMSE仿真结果。如下图所示。图3-5 采样时间t=0.5时结果图3-6 采样时间t=1.0时结果图 3-7 采样时间t=1.5时结果从图3-5可以看到，在采样间隔t不太大时（t=0.5），EKF和UKF算法均能跟踪目标，且UKF算法滤波精度优于EKF算法。从图3-6可以看到，当t=1.0时在x方向，EKF和UKF算法均能跟踪目标，在y方向UKF能跟踪目标，而EKF算法滤波发散，在图3-7中可以看到，当t=1.5时UKF算法跟踪精度变化不大，EKF滤波在x方向和y方向均发散。3.3.4滤波协方差阵对角线比较对于EKF和UKF算法，在不同的t时，我们分别取其滤波协方差阵对角线的第二个元素（即y轴方向位置方差），作出位置方差变化图如下。图3-8 不同采样间隔的y方向位置滤波方差变化图出现上述现象的原因为当采样间隔t增大时，非线性函数Taylor展开式的高阶项无法忽略，EKF算法线性化（一阶展开）使得系统产生较大的误差，导致了滤波的不稳定。由于UKF算法可以精确到二阶或者三阶Taylor展开项，所以这种现象不明显，但是当t进一步增大，尤其是跟踪目标的状态变化剧烈时，更高阶项误差影响不可忽略，进而UKF算法也会发散导致无法跟踪目标。3.3.5测量误差对滤波结果的影响。取采样间隔不变，如t=0.5s，对于不同的测量误差，分析其对EKF和UKF算法滤波结果的影响。分别取，结果如下: 图8 测量误差阵为时滤波结果图3-9 测量误差阵为时滤波结果 图3-10 测量误差阵为时滤波结果由上面四张图对比可知，当测量误差较小时，UKF滤波精度优于EKF；当测量误差较大时，UKF和EKF滤波精度相差不大。3.3.6实验分析和总结： 综合以上分析可以看到，UKF算法对于解决非线性模型滤波问题时，相对于EKF算法，它不需要计算雅克比矩阵，具有较好的滤波精度，而且在非线性严重或者高阶误差引入时，会推迟或延缓滤波发散，在测量误差较大或者采样时间增大时，也会降低UKF的滤波精度。同时 UKF利用确定的离散采样点直接逼近状态的后验概率密度, 由于不需计算量测方程的Jacobian矩阵, 实现也相对简单。以上仿真表明：UKF滤波方法算法较之EKF算法在相同仿真条件下对状态的估计更准确，定位精度更高，算法对非线性系统的适应性更强。4 总结UKF是近年来兴起的非线性滤波方法, 它是将无味变换对随机量经非线性映射后统计信息的估计嵌入到卡尔曼的滤波算法中。对于解决大部分问题，它是最优，效率最高甚至是最有用的，因此它被广泛应用于非线性估计领域及系统辨识与参数估计等领域。本论文围绕UKF的基础理论，对UKF在非线性系统的状态估计应用展开了研究。现将本文所做工作总结如下(1)回顾了卡尔曼滤波的理论基础、发展过程及应用前景，依次讲述了随机非线性离散系统的卡尔曼滤波理论，并给出了扩展卡尔曼滤波器的数学模型，接着详细介绍了UT (Unscented Transformation)的理论和算法分析，并在此基础上介绍了UKF的基础理论并详细推导了UKF基本方程。(2)针对一个强非线性、高斯的系统的跟踪问题进行仿真,将UKF和EKF两种算法的跟踪效果进行比较，从理论分析和实验结果两方面表明：UKF滤波方法算法较之EKF算法在相同仿真条件下对状态的估计更准确，定位精度更高，算法对非线性系统的适应性更强。(3) 由于研究时间比较短、水平有限，本文没有从理论上深入研究UKF的建模问题和系统特性，也没有对UKF的滤波算法进行改进，在今后的研究中，要改善UKF算法，提高UKF滤波器的精确性、跟踪能力。参考文献1Julier S,Uhlmann J,Durrant-Whyte H F.A new methodfor the nonlinear transformation of means and covariancesin filter and estimatorJ.IEEE Transactions on Auto-matic Control，2000，45( 3) : 477- 4822 R.S.Bucy and K.D. Renne.Digital Synthesis of Nonlinear FiltersJ, Automatica,1971，7(3)：2728.3AlspachDL,SorensonHW.Noulinearb,sianestimation using Gaussian Sum aPppro-ximation.IEEE ans.Automat.Control，1972:439一447. 4 彭丁聪.卡尔曼滤波的基本原理及应用J. 软 件 导 刊,2009,8(11):31-34.5敬喜.卡尔曼滤波器及其应用基础 M.北京:国防工业出版社,1973.5-47.6 杨 宏，李亚安，李国辉.种改进扩展卡尔曼滤波新方法J.计算机工程与应用,2010,46(19):18-20.7 S.J.Julier and J.K.Uhlmann.Unscented filtering and nonlinear Estimation .Proc-eedings ofthe IEEE，2004，92(3)：401-,422.8程水英.无味变换与无味卡尔曼滤波J.计算机工程与应用，2008,44（24）:25-35.9万 莉,刘焰春,皮亦鸣 .EKF、UKF、P F目标跟踪性能的比较. 雷达科学与技术J,2007,5(1):13-16.10乔坤,郭朝勇,史进伟.基于卡尔曼滤波的运动人体跟踪算法研究J. 计算机与数字工程,2012,267(1):1-3.11樊红娟.无先导卡尔曼滤波算法分析D. 西南大学研究生院(筹),200712Rambabu Kandepu , Bjarne Foss, Lars Imsland.Applying the unscented Kalman filter for nonlinear state estimationJ. ournal of Process Control,2008,9(1):1-1513曲春晓,陈伟.卡尔曼滤波在飞行器姿态获取系统中的实现. 交通信息与安全,2011,6(29):139-142.14 钱默抒,姜 斌,杜董生,杨 浩.新型 UKF 在非线性系统执行器故障估计中的应用J. 东南大学学报(自 然 科 学 版),2011,41(S1):120-124.15 刘罗仁, 罗金玲.U KF 滤波器在非线性组合信号系统中的应用研究J. 沈阳工程学院学报( 自然科学版),2009,5(4):356-358.UKF based on the problem of nonlinear state estimation research LvBaoQiang(College of Physics and Information