北航信号分析与处理中的数学方法课堂作业

EKFEKF：ExtendedKalmanFilter的简写，即扩展卡尔曼滤波器。首先来介绍一下卡尔曼滤波。卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器),它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。斯坦利·施密特(StanleySchmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。扩展卡尔曼滤波：卡尔曼最初提出的滤波理论只适用于线性系统,Bucy，Sunahara等人提出并研究了扩展卡尔曼滤波(ExtendedKalmanFilter，简称EKF)，将卡尔曼滤波理论进一步应用到非线性领域。EKF的基本思想是将非线性系统线性化，然后进行卡尔曼滤波，因此EKF是一种次优滤波。其后，多种二阶广义卡尔曼滤波方法的提出及应用进一步提高了卡尔曼滤波对非线性系统的估计性能。二阶滤波方法考虑了Taylor级数展开的二次项，因此减少了由于线性化所引起的估计误差，但大大增加了运算量，因此在实际中反而没有一阶EKF应用广泛。EKF的原理：Kalman滤波能够在线性高斯模型的条件下，可以对目标的状态做出最优的估计，得到较好的跟踪效果。对非线性滤波问题常用的处理方法是利用线性化技巧将其转化为一个近似的线性滤波问题。因此，可以利用非线性函数的局部性特性，将非线性模型局部化，再利用Kalman滤波算法完成滤波跟踪。扩展Kalman滤波就是基于这样的思想，将系统的非线性函数做一阶Taylor展开，得到线性化的系统方程从而完成对目标的滤波估计等处理。非线性系统离散动态方程可以表示为(k1)f[k,X(k)]G(k)W(k)X（1）(k)h[k,X(k)]V(k)Z（2）这里为了便于数学处理，假定没有控制量的输入，并假定过程噪声是均值为零的高斯白噪声，且噪声分布矩阵(k)G是已知的。其中，观测噪声(k)V也是加性均值为零的高斯白噪声。假定过程噪声和观测噪声序列是彼此独立的，并且有初始状态估计ˆ(0|0)X和协方差矩阵(0|0)P。和线性系统的情况一样，我们可以得到扩展Kalman滤波算法如下ˆˆ(k|k1)f(X(k|k))X（3）(k1|k)(k1|k)P(k|k)(k1|k)Q(k1)P•（4）1(k1)P(k1|)H(k1)[H(k1)P(k1|k)H(k1)R(k1)]Kk晻（5）ˆˆˆ(K1|k1)X(K1|k)K(k1)[Z(k1)h(X(K1|k))]X（6）(k1)[IK(k1)H(k1)]P(k1|k)P（7）这里需要重要说明的是，状态转移(k1|)k和量测矩阵(k1)H是由f和h的雅克比矩阵代替的。其雅克比矩阵的求法如下：假如状态变量有n维，即12[xx...x]nX，则对状态方程对各维求偏导，123(k1)...nfffffxxxxx（8）123(k1)...nhhhhhHXxxxx（9）UKF原理如下：传统的非线性滤波的方法主要是扩展卡尔曼滤波算法(EKF),但是该算法存在着精度不高、稳定性差、对目标机动反应迟缓等缺点.近年来,提出了一种非线性滤波算法-Unscented卡尔曼滤波(UnscentedKalmanFilter,即UKF).它是根据Unscented变化和卡尔曼滤波相结合得到的一种算法.这种算法主要运用卡尔曼滤波的思想,但是在求解目标后续时刻的预测值和量测值时,则需要应用采样点来计算.UKF通过设计加权点δ,来近似表示n维目标采样点,计算这些δ点经由非线性函数的传播,通过非线性状态方程获得更新后的滤波值,从而实现了对目标的跟踪.UKF有效地克服了扩展卡尔曼滤波的估计精度低、稳定性差的缺陷.无损卡尔曼滤波是一种新型的滤波估计算法。UKF以UT变换为基础，摒弃了对非线性函数进行线性化的传统做法，采用卡尔曼线性滤波框架，对于一步预测方程，使用无迹(UT)变换来处理均值和协方差的非线性传递，就成为UKF算法。UT变换如下：（1）构造sigma点根据随机向量x的统计量()()|(|1)kfXHKXXkkXx和xp，构造sigma点集：(()),1,...(()),1,...2,0xiixixnkpinxxnkpinnxiK为尺度参数，调整它可以提高精度。用这组采样点ix可以近似表示状态x的高斯分布。(2)对sigma点进行非线性变换对所构造出的点集进行非线性变换，得到变换后的sigma点集：(),0,1,...2iiYfxin变换后的点集Yi可近似地表示为()ifx的分布。(3)计算y的均值和方差对变换后的点集Yi进行加权处理，从而得到输出量y的均值和方差2()0nmiiiyWY2()0TncyiiiipWYyYy()miW和()ciW分别为计算均值方差的加权：()0/()mWknk()20/()(1)cWknk()()/[2()],1,...,2mciiWWknkin其中：2()knn。在均值和方差加权中需要确定、和的参数。粒子滤波原理如下：粒子滤波的思想基于蒙特卡洛方法(MonteCarlomethods)，它是利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表达其分布，是一种顺序重要性采样法(SequentialImportanceSampling)。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差分布的过程。这里的样本即指粒子,当样本数量N→∝时可以逼近任何形式的概率密度分布。尽管算法中的概率分布只是真实分布的一种近似，但由于非参数化的特点，它摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约，能表达比高斯模型更广泛的分布，也对变量参数的非线性特性有更强的建模能力。因此，粒子滤波能够比较精确地表达基于观测量和控制量的后验概率分布，可以用于解决SLAM问题。3.2扩展卡尔曼在一维非线性系统中的应用3.2.1状态方程和观测方程都为非线性的通用系统所谓的非线性方程，就是因变量和自变量的关系不是线性的，这类方程很多，例如平方关系，对数关系，指数关系，三角函数关系等等。这类方程可分为两类，一类是多项式方程，一种是非多项式方程。为了便于说明非线性卡尔曼滤波——扩展Kalman滤波的原理，我们选用一下系统，系统状态为(k)X，它仅包含一维变量，即(k)[x(k)]X，系统状态方程为22.5(k1)(k)0.5X(k1)8cos(1.2k)w(k)1(k1)XXX（2-1）观测方程为2(k)(k)(k)20XYv（2-2）其中，式（1）是包含分式，平方，三角函数在内的严重非线性的方程，(k)w为过程噪声，其均值为0，方差为Q，观测方程中，观测信号(k)Y与状态(k)X的关系也是非线性的，(k)v也是均值为0，方差为R的高斯白噪声。因此关于（1）和（2-2）是一个状态和观测都为非线性的一维系统。以此为通用的非线性方程的代表，接下来讲述如何用扩展Kalman滤波来处理噪声问题。第一步：初始化状态(0),Y(0)X，协方差矩阵0P。第二步：状态预测22.5(k1)(k|k1)0.5X(k1)8cos(1.2k)1(k1)XXX（2-3）第三步：观测预测2(k|k1)(k|k1)20XY（2-4）第四步：一阶线性化状态方程，求解状态转移矩阵(k)2222.5[1X(k|k1)](k)0.5[1X(k|k1)]fX（2-5）第五步：一阶线性化观测方程，求解观测矩阵(k)H(k|k1)(k)10hXHX（2-6）第六步：求协方差矩阵预测(k|k1)P(k|k1)(k)P(k1|k1)(k)PQ晻（2-7）这里需要说明的是，当噪声驱动矩阵不存在的时候，或系统状态方程中，在(k)w前没有任何驱动矩阵，这时候，Q必然和状态的维数一样的方阵，可将式（3-2-7）直接写为(k|k1)(k)P(k1|k1)(k)QP•。第七步：求Kalman增益(k)P(k|k1)H(k)(H(k)P(k|k1)H(k)R)K晻（2-8）第八步：求状态更新(k)X(k|k1)K(Y(k)Y(k|k1))X（2-9）第九步：协方差更新(k)(IK(k)H(k))P(k|k1)nP（2-10）以上九步为扩展卡尔曼年滤波的一个计算周期，如此循环下去就是各个时刻EKF对非线性系统的处理过程。