第4讲：UKF滤波算法

非线性系统非线性系统UKFUKF滤波算法滤波算法主要内容非线性状态估计原理（MMSE）Uscented变换（UT）UKF滤波算法应用比较非线性状态估计原理非线性状态估计原理（（MMSEMMSE））非线性状态估计原理离散系统：不管条件密度函数的特性如何，最小均方估计就是条件均值。非线性状态滤波过程的实现包括一步预测与测量修正两个阶段。kkkkkkkvxhywxfx+=+=+)()u,(1()y|xy|xp{}y|xy|xE=µ一步预测根据所有过去时刻的测量信息对状态作最小方差估计状态估计质量的优劣利用一步预测误差协方差矩阵描述{}1−=kkkEYxx()(){}1−−−=kTkkkkkEPYxxxx测量修正获得当前时刻的测量信息后，对状态预测估值进行修正，得到状态的最优估计值描述最优状态估值质量优劣的误差协方差阵确定如下{}[]kkkkkkkKEyyxYxx−+==ˆ()()kPkPKyxyk1−=()(){}()TkykkkTkkkkkKkPKPEP−=−−=Yxxxxˆˆˆ{}1−=kkkEYyy()[]()[]{}1−−−=kTkkkkxyhEkPYxyxx()()[]()[]{}1−−−=kTkkkkyhhEkPYxyxyEKF的不足™必须求非线性函数的Jacobi矩阵，对于模型复杂的系统，比较复杂且容易出错；™引入线性化误差，对非线性强度高的系统，容易导致滤波效果下降。基于上述原因，为了提高滤波精度和效率，以满足特殊问题的需要，就必须寻找新的逼近方法。随机状态变量沿非线性函数的传播问题是非线性滤波的关键！注意新思路“近似非线性函数的概率密度分布比近似非线性函数更容易”因此，使用采样方法近似非线性分布来解决非线性滤波问题的途径目前得到了人们的广泛关注。™粒子滤波使用参考分布，随机产生大量粒子，近似状态的后验概率密度，得到系统的估计。问题：1）计算量甚大，为EKF的若干数量阶；2）若减少粒子数，估计精度下降。™UKF滤波以UT变换为基础，采用卡尔曼滤波器框架，采样形式为确定性采样。在减少采样粒子点数的同时保证逼近精度。UnscentedUnscented变换变换（（UnscentedTransformationUnscentedTransformation））Unscented变换（（11））构造构造SigmaSigma点点根据随机向量x的统计量和，构造Sigma点集为尺度参数，调整它可以提高逼近精度。用这组采样点可以近似表示状态x的高斯分xxP()()()()⎪⎪⎩⎪⎪⎨⎧=+=+−=++=0,2,...,1,,...,1,ixnniPnxniPnxixixiκκχκiχ布。（（22）对）对SigmaSigma点进行非线性变换点进行非线性变换对所构造的点集进行非线性变换，得到变换后的Sigma点集变换后的Sigma点集即可近似地表示的分布。{}iχ()⋅f()nifYii2,...,1,0==χ{}iY()xfy=（（33））计算计算yy的均值和方差的均值和方差对变换后的Sigma点集进行加权处理，从而得到输出量的均值和方差和分别为计算的均值和方差所用加权{}iYy()∑=≈niimiYWy20()()()∑=−−≈niTiiciyyYyYWP20)(miW)(ciWy()()κκ+=nWm0()()()βακκ+−++=201nWc()()()[]ninWWcimi212,...,,=+==κκ其中：在均值和方差加权中需要确定、和共3个参数，它们的取值范围分别为：确定周围Sigma点的分布，通常设为一个较小的正数（）；为第二个尺度参数，通常设置为0或3-n；为状态分布参数，对于高斯分布是最优的，如果状态变量是单变量，则最佳的选择是。适当调节、可以提高估计均值的精度；调节可以提高方差的精度。()nn−+=λακ2αβλαx411−≥eαλβ2=β0=βαλβUnscented变换原理图()miWa)(fWeightedSampleMeanx()ciWy+−WeightedSampleCovariancePxPyκ+n]PP[}{xxiaxaxx−+=χiYUT示意图UTUTUscented变换的特点™（1）对非线性函数的概率密度分布进行近似，而不是对非线性函数进行近似，即使系统的模型复杂，也不增加算法实现的难度。™（2）所得到的非线性函数的统计量的准确性可以达到三阶（泰勒展开）。™（3）不需要计算Jacobi矩阵，可以处理不可导非线性函数。UKFUKF滤波算法滤波算法UKF实现思想UKF＝Unscentedtransform+KalmanFilter™即UKF可以看作是基于UT技术的卡尔曼滤波器。在卡尔曼滤波算法中，对于一步预测方程，使用UT变换来处理均值和协方差的非线性传递，就成为UKF算法。状态的时间更新：™选定状态的2n+1个Sigma点（n为状态维数）；™利用UT技术计算状态的后验均值和方差。状态的测量更新：™利用标准的Kalman滤波的测量更新，但使用的公式有所不同。UT-EKFLinearized(EKF)UnscentedTransform)(iifχψ=APAPxfyxTy==)(SigmaPointsUTmeanUTcovariancetransformedsigmapoints)(xfAPAxT两类非线性系统模型（1）加性噪声（2）噪声隐含kkkwxfx+=+)(1kkkvxhy+=)(),(1kkkwxfx=+),(kkkvxhy=简化UKF滤波算法（加性噪声）™对于非线性系统™假定状态为高斯随机矢量；过程噪声与测量噪声的统计特性为kkkwxfx+=+)(1kkkvxhy+=)(),0(~kkQNw),0(~kkRNv[]00xEx=ˆ（（11）初始化）初始化()()[]TxxxxEP00000ˆˆ−−=（（22）状态估计）状态估计1.计算Sigma点()()()()nniPnxniPnxxikkikikkikkk2,...,1,ˆ,...,1,ˆˆ111111101+=+−==++==−−−−−−−−κχκχχ2.时间传播方程()ikikkf11|−−=χχ∑=−−=niikkmikWx201|)(ˆχkkikknikikkcikxQxxWP+−−=−−=−−−∑T1|201|)(,]ˆ[]ˆ[χχikknimikWy1|20)(ˆ−=−∑=γ()ikkikkh1|1|−−=χγ3.测量更新方程kTkikkkniikkcikyRyyWP+−−=−−−=−∑]ˆ][ˆ[1|201|)(,γγTkikkkikknicikxyyxWP]ˆ][ˆ[1|1|20)(,−−−−=−−=∑γχ1,,−=kykxyPPK)ˆ(ˆˆ−−−+=kkkkyyKxxTkykxkxKKPPP,,,−=−计算量™与EKF的计算量在同一个数量阶，对于n维系统，为O(n3)。™UKF和EKF的计算量之比大致为：UKF:EKF＝3:1™UKF的主要计算量在于选取Sigma点时的方根分解运算。所以优化计算可以从分解方式入手，好的分解方式可以减小计算量。1−kPUKF的优点™不必计算Jacobi矩阵，不必对非线性系统函数f(x)进行任何形式的逼近；™预测阶段只是标准的线性代数运算（矩阵方根分解，外积，矩阵和向量求和）；™系统函数可以不连续；™随机状态可以不是高斯的；™计算量和EKF同阶。扩维UKF滤波算法（噪声隐含）™若过程噪声与测量噪声是隐含在系统中的，即系统方程为这时需要对状态变量进行扩展，得增广状态),(1kkkwxfx=+),(kkkvxhy=[]TTkTkTkkavwxx,,,=增广状态的均值为[]TTlTmTkkaxx11,,,ˆˆ××=00其中，和分别为过程噪声和观测噪声的维数。ml增广状态的方差为⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=RQPPkxka000000,,（（11）初始化）初始化[]00xEx=ˆ()()[]TxxxxxEP00000,ˆˆ−−=[]TTlTmTaxEx1100,00ˆ××=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=RQPPxa0000000,0,（（22）状态估计）状态估计1.计算Sigma点根据和，构造增广Sigma点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|,ˆˆγγ()[][]∑=−−−−−−=NiTkikkkikkxcikxyyxWP201|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+⎥⎦⎤⎢⎣⎡=+=[][]{})()()()(12tJttNJtfωωω×−=−偏航角估计误差051015202530-1-0.500.511.5x10-3偏航角误差／degEKFUKF时间／s滚动角估计误差051015202530-14-12-10-8-6-4-202x10-4EKFUKF时间／s滚动角误差／deg俯仰角估计误差051015202530-1.5-1-0.500.51x10-3时间／sEKFUKF俯仰角误差／deg主要参考文献™S.JulierandJ.K.Uhlmann.“AGeneralMethodforApproximatingNonlinearTransformationsofProbabilityDistributions”.TechnicalReport,RoboticsResearchGroup,DepartmentofEngineeringScience,UniversityofOxford.1994.™S.JulierandJ.K.Uhlmann.“ANewApproachforFilteringNonlinearSystems”.Proc.ofthe1995AmericanControlConference,Seattle,Washington.pp.1628-1632™S.JulierandJ.K.Uhlmann.“TheScaledUnscentedTransformation”.Proc.oftheAmericanControlConference.Anchorage,AKMay8-10,2002™S.Julier,J.K.Uhlmann,etc.“ANewMethodfortheNonlinearTransformationofMeansandCovariancesinFiltersandEstimators”.IEEE,Trans.A.C.,2000,45(3):477-482.™S.Julier,J.K.Uhlmann.“ReducedSigmaPointFiltersforthePropagationofM