第4讲UKF滤波算法资料下载.pdf

1、随机状态变量沿非线性函数的传播问题是非线性滤波的关键！注意注意新思路“近似非线性函数的概率密度分布比近似非线性函数更容易近似非线性函数的概率密度分布比近似非线性函数更容易”因此，使用采样方法近似非线性分布来解决非线性滤波问题的途径目前得到了人们的广泛关注。粒子滤波粒子滤波使用参考分布，随机产生大量粒子，近似状态的后验概率密度，得到系统的估计。问题：1）计算量甚大，为EKF的若干数量阶；2）若减少粒子数，估计精度下降。UKF滤波滤波以UT变换为基础，采用卡尔曼滤波器框架，采样形式为确定性采样。在减少采样粒子点数的同时保证逼近精度。UnscentedUnscented变换变换变换变换（Unscen

2、ted TransformationUnscented Transformation）Unscented变换（1 1）构造构造构造构造SigmaSigma点点点点根据随机向量 x 的统计量和，构造Sigma点集为尺度参数，调整它可以提高逼近精度。用这组采样点可以近似表示状态 x 的高斯分xxP（）（）（）（）=+=+=+=0,2,.,1,.,1,ixnniPnxniPnxixixii布。（2 2）对）对）对）对SigmaSigma点进行非线性变换点进行非线性变换点进行非线性变换点进行非线性变换对所构造的点集进行非线性变换，得到变换后的Sigma点集变换后的Sigma点集即可近似地表示的分布。i

3、（）f（）nifYii2,.,1,0=iY（）xfy=（3 3）计算计算计算计算 y y 的均值和方差的均值和方差的均值和方差的均值和方差对变换后的Sigma点集进行加权处理，从而得到输出量的均值和方差和分别为计算的均值和方差所用加权 iYy（）=niimiYWy20（）（）（）=niTiiciyyYyYWP20）（miW）（ciWy（）（）+=nWm0（）（）（）+=201nWc（）（）（）ninWWcimi212,.,=+=其中：在均值和方差加权中需要确定、和共3个参数，它们的取值范围分别为：确定周围Sigma点的分布，通常设为一个较小的正数（）；为第二个尺度参数，通常设置为0或3-n；为

4、状态分布参数，对于高斯分布是最优的，如果状态变量是单变量，则最佳的选择是。适当调节、可以提高估计均值的精度；调节可适当调节、可以提高估计均值的精度；调节可以提高方差的精度。以提高方差的精度。（）nn+=2x411e2=0=Unscented变换原理图（）miWa）（fWeighted SampleMeanx（）ciWy+Weighted SampleCovariancePxPy+nPPxxiaxaxx+=iYUT 示意图UTUTUscented变换的特点?（1）对非线性函数的概率密度分布进行近似，而不是对非线性函数进行近似，即使系统的模型复杂，也不增加算法实现的难度。（2）所得到的非线性函数的

5、统计量的准确性可以达到三阶（泰勒展开）。（3）不需要计算Jacobi矩阵，可以处理不可导非线性函数。UKFUKF滤波算法滤波算法滤波算法滤波算法UKF实现思想UKFUnscented transform+Kalman Filter?即 UKF 可以看作是基于 UT 技术的卡尔曼滤波器。在卡尔曼滤波算法中，对于一步预测方程，使用UT变换来处理均值和协方差的非线性传递，就成为UKF算法。状态的时间更新：选定状态的 2n+1 个 Sigma点（n 为状态维数）；利用 UT 技术计算状态的后验均值和方差。状态的测量更新：利用标准的 Kalman 滤波的测量更新，但使用的公式有所不同。UT-EKFLin

6、earized（EKF）Unscented Transform）（iif=APAPxfyxTy=）（Sigma PointsUT meanUT covariancetransformed sigma points）（xfAPAxT两类非线性系统模型（1）加性噪声（）加性噪声（2）噪声隐含）噪声隐含kkkwxfx+=+）（1kkkvxhy+=）（）,（1kkkwxfx=+）,（kkkvxhy=简化UKF滤波算法（加性噪声）?对于非线性系统?假定状态为高斯随机矢量；过程噪声与测量噪声的统计特性为kkkwxfx+=+）（1kkkvxhy+=）（）,0（kkQNw）,0（kkRNv 00 xEx=（1

7、 1）初始化）初始化）初始化）初始化（）（）TxxxxEP00000=（2 2）状态估计）状态估计）状态估计）状态估计1.计算计算Sigma点点（）（）（）（）nniPnxniPnxxikkikikkikkk2,.,1,.,1,111111101+=+=+=2.时间传播方程时间传播方程（）ikikkf11|=niikkmikWx201|）（kkikknikikkcikxQxxWP+=T1|201|）（,ikknimikWy1|20）（=（）ikkikkh1|1|=3.测量更新方程测量更新方程kTkikkkniikkcikyRyyWP+=1|201|）（,TkikkkikknicikxyyxWP

8、1|1|20）（,=1,=kykxyPPK）（+=kkkkyyKxxTkykxkxKKPPP,=计算量?与 EKF 的计算量在同一个数量阶，对于 n 维系统，为 O（n3）。UKF 和 EKF 的计算量之比大致为：UKF:EKF 3:1?UKF 的主要计算量在于选取 Sigma 点时的方根分解运算。所以优化计算可以从分解方式入手，好的分解方式可以减小计算量。1kPUKF的优点?不必计算 Jacobi 矩阵，不必对非线性系统函数 f（x）进行任何形式的逼近；预测阶段只是标准的线性代数运算（矩阵方根分解，外积，矩阵和向量求和）；系统函数可以不连续；随机状态可以不是高斯的；计算量和 EKF 同阶。扩

9、维UKF滤波算法（噪声隐含）?若过程噪声与测量噪声是隐含在系统中的，即系统方程为这时需要对状态变量进行扩展，得增广状态）,（1kkkwxfx=+）,（kkkvxhy=TTkTkTkkavwxx,=增广状态的均值为TTlTmTkkaxx11,=00其中，和分别为过程噪声和观测噪声的维数。ml增广状态的方差为=RQPPkxka000000,（1 1）初始化）初始化）初始化）初始化 00 xEx=（）（）TxxxxxEP00000,=TTlTmTaxEx1100,00=RQPPxa0000000,0,（2 2）状态估计）状态估计）状态估计）状态估计1.计算计算Sigma点点根据和，构造增广Sigma

10、点1,kax1,kaP（）（）（）（）NNiPNxNiPNxxikakaikaikakaikakaka2,.,1,.,1,1,1,1,1,1,1,1,01,+=+=+=这里，为增广状态的维数,且lmnN+=TTvTwTxa=2.时间传播方程时间传播方程（）ikwikxikkxf1,1,1|,=（）ikkxNimikWx1|,20=（）=NiTkikkxkikkxcikxxxWP201|,1|,（）ikvikkxikkh1,1|,1|,=（）ikkNimikWy1|20=3.测量更新方程测量更新方程（）=NiTkikkkikkcikyyyWP201|1|,（）=NiTkikkkikkxcikxy

11、yxWP201|1|,1,=kykxyPPK）（+=kkkkyyKxxTkykxkxKKPPP,=两类UKF算法的比较?处理加性噪声的简化UKF的Sigma点较处理隐含噪声的扩维UKF要少许多。简化UKF的Sigma点数：2n+1?扩维UKF的Sigma点数：2N+1=2（n+m+l）+1?由此，简化UKF的计算量较之扩维UKF大大降低。应用实例?模型：卫星姿态确定系统其中，）（）（）（）,（211ttqttqf=）（0）（）（）,（）（）（）（133421tdJItfttqftGdtxftx+=+=&）（）（）（）（）（）（21kvrkqArkqAkvkxhky+=+=）（）（）（）（12t

12、JttNJtf=偏航角估计误差051015202530-1-0.500.511.5x 10-3偏航角误差degEKFUKF时间s滚动角估计误差051015202530-14-12-10-8-6-4-202x 10-4EKFUKF时间s滚动角误差deg俯仰角估计误差051015202530-1.5-1-0.500.51x 10-3时间sEKFUKF俯仰角误差deg主要参考文献?S.Julier and J.K.Uhlmann.“A General Method for Approximating Nonlinear Transformations of Probability Distribut

13、ions”.Technical Report,Robotics Research Group,Department of Engineering Science,University of Oxford.1994.?S.Julier and J.K.Uhlmann.“A New Approach for Filtering Nonlinear Systems”.Proc.of the 1995 American Control Conference,Seattle,Washington.pp.1628-1632?S.Julier and J.K.Uhlmann.“The Scaled Unsc

14、ented Transformation”.Proc.of the American Control Conference.Anchorage,AK May 8-10,2002?S.Julier,J.K.Uhlmann,etc.“A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators”.IEEE,Trans.A.C.,2000,45（3）:477-482.?S.Julier,J.K.Uhlmann.“Reduced Sigma Point Filters f

15、or the Propagation of Means and Covariances Through Nonlinear Transfor-mations”.Proc.of the American Control Conference.Anchorage,AK May 8-10,2002?S.Julier,J.K.Uhlmann.“A New Extension of the KalmanFilter to Nonlinear Systems”.Proc.Aero Sense:11thInt.Symp.Aerosp./Defense Sensing.Simulate Contr.,Orla

16、ndo,FL,1997?R.van der Merwe and A.Wan.“The Unscented KalmanFilter for Nonlinear Estimation”.Proc.of IEEE Symposium 2000,Lake Louise,Alberta,Canada,Oct.2000?R.van der Merwe and A.Wan.“Dual Estimation and the Unscented Transformation”.Advances in Neural Information Processing Systems 12,1999?R.van der Merwe and A.Wan.“Efficient Derivative-Free Kalman Filters for Online Learning”?R.van der Merwe and A.Wan.“The Square-Root Unscented Kalman Filter for State and Parameter-Estimation”.