第四章-基于统计的融合----重庆大学

第四章基于统计的融合,2010.5.,2,4.1点估计理论基础4.2期望极大化（EM）方法4.3线性动态系统的滤波理论与算法4.4非线性动态系统的滤波理论与算法,第4章基于统计的融合-统计推断与估计理论,3,第4章基于统计的融合-统计推断与估计理论,估计：根据测量值z(t)，z(t)=hx(t)+w(t)，解算出x(t)的计算值(t)。随机向量v(t)为量测误差，称为x的估计，z称为x的量测。设在t0,t1时间内的量测为z，相应的估计为，则当t=t1时，(t)称为x(t)的估计；当tt1时，(t)称为x(t)的预测；当tt1时，(t)称为x(t)的平滑；最优估计是指某一指标值达到最值时的估计。,4,第4章基于统计的融合-统计推断与估计理论,以量测估计Z的偏差的平方和达到最小为指标，即(Z-Z)T(Z-Z)=min则所得的估计为最小二乘估计；以状态估计的均方误差集平均达到最小为指标，E(x-)T(x-)=min则所得的估计为最小方差估计；若又是x的线性估计，则为x的线性最小方差估计。以估计值出现的概率作为估计指标，有极大验后估计、贝叶斯估计和极大似然估计。,5,4.1点估计的理论基础,本小节的主要内容有：一般概念Bayes点估计理论最佳线性无偏（BLUE）估计加权最小二乘（WLS）估计极大似然（ML）估计递推最小二乘（RLS）估计,6,4.1点估计的理论基础一般概念,一般概念定义4.1.1设是一个未知参数向量，量测y是一个m维的随机向量，而y的一组容量为N的样本是，设对它的统计量为(4.1.1)称其为对x的一个估计量，其中称为统计规则或估计算法。利用样本对参数的估计量本质上是随机的，而当样本值给定时所得到的参数估计值一般与真值并不相同，因而需要用某些准则进行评价。,7,4.1点估计的理论基础一般概念,定义4.1.2对于式(4.1.1)，所得估计量如果满足(4.1.2)则称是对参数x的一个无偏估计；如果满足(4.1.3)则称是对参数x的一个渐近无偏估什。定义4.1.3对于式(4.1.1)所得估计量，如果依概率收敛于真值，即(4.1.4)则称是对参数x的一个一致估计量。,8,4.1点估计的理论基础一般概念,定理4.1.1（Cramer-Rao不等式）设是参数x的一个正规无偏估计，则其估计误差的协方差阵满足Cramer-Rao不等式(4.1.5)其中是估计误差，而是Fisher信息矩阵（注意标量对向量求导取行向量），定义为(4.1.6)其中是给定x时y的条件概率密度函数。,9,4.1点估计的理论基础Bayes点估计理论,Bayes点估计理论设x也是一个n维随机向量，仍设是y的一组容量为N的样本。设表示量测信息，则x与z的联合概率密度函数是(4.1.7)假定表示由量测信息z得到的一个估计，而估计误差定义为(4.1.8)定义4.1.4估计误差的标量函数称为一个损失函数，如果（1）零误差的损失为零，即，则；（2）按分量的绝对值单调增，即和的第i个分量满足，其余分量相等，则；,10,4.1点估计的理论基础Bayes点估计理论,（3）是对称的，即，有。定义4.1.5设，估计误差的损失函数是，则风险函数定义为(4.1.9)其中估计方法；Bayes风险定义为(4.1.10)其中，分别表示按分布或条件分布求期望；而最小Bayes风险估计定义为(4.1.11)定义4.1.6利用Bayes公式，Bayes风险可以改写为(4.1.12),11,4.1点估计的理论基础Bayes点估计理论,其中就是损失函数的后验期望。而最小后验用望损失估计定义为(4.1.13)定理4.1.2设参数x和量测信息z是联合Guass分布的，其均值和协方差阵分别为(4.1.14)并假定R和非奇异，那么，给定z时，x也是条件Gauss的，而且对估计误差的任意容许损失函数，最小后验期望损失估计公式为,12,4.1点估计的理论基础Bayes点估计理论,(4.1.15)估计误差的协方差阵是(4.1.16),13,4.1点估计的理论基础BLUE估计,BLUE估计定义4.1.7设，对参数x的估计表示为量测信息z的线性函数(4.1.17)则称为线性估计；进而如果估计误差的均方值达到最小，则称之为线性最小方差估计；如估计还是无偏的，则称为线性无偏最小方差估计。这种线性无偏最小方差估计在多源信息融合领域一般称为最佳线性无偏估计（bestlinearunbiasedestimation，BLUE）。,14,4.1点估计的理论基础BLUE估计,定理4.13设参数x和量测信息z是任意分布，z的协方差阵非奇异，则利用量测信息z对参数x的BLUE估计惟一地表示为(4.1.18)此处只是一个记号，不表示条件期望；而估计误差的协方差阵是(4.1.19),15,4.1点估计的理论基础WLS估计,WLS估计定义4.1.8假定量测信息z可以表示为参数x的线性函数，即(4.1.20)其中，是一个零均值的随机向量；设，为对称阵，则如下估计(4.1.21)称为加权最小二乘（weightedleastsquare，WLS）估计；如果，则称为最小二乘（leastsquare，LS）估计。定理4.1.4设可逆，则基于量测信息z和加权矩阵W对参数x的WLS估计为(4.1.22),16,4.1点估计的理论基础ML估计,ML估计定义4.1.9给定参数x时量测信息z的似然函数表示为，极大似然（maximumIikelihood，ML）参数估计问题可以描述为(4.1.23),17,4.1点估计的理论基础RLS估计,考虑如下参数估计问题(4.1.24)其中是时间指标，分别是回归向量和未知参数向量，是k时刻的量测量，而是量测误差。RLS估计假定是一个零均值的随机过程，对于k时刻的量测总量、回归总量和误差总量，可有总量关系(4.1.25)所以有最小二乘估计(4.1.26),18,4.1点估计的理论基础RLS估计,引理（矩阵求逆引理）设，均可逆，则有(4.1.27)定理4.1.7设获得第k+1时刻的量测和回归向量之后，令(4.1.28)则有递推最小二乘（recursiveleastsquares，RLS）估计是(4.1.29),19,4.1点估计的理论基础RLS估计,其中和分别是Kalman增益矩阵和一步预测误差或参数估计新息（innovation），分别计算为(4.1.30)而的递推计算式为(4.1.31),20,4.1点估计理论基础4.2期望极大化（EM）方法4.3线性动态系统的滤波理论与算法4.4非线性动态系统的滤波理论与算法,第4章基于统计的融合,21,4.2期望极大化（EM）的方法,本小节的主要内容有：概述EM算法描述,22,22,4.2期望极大化（EM）的方法概述,概述EM算法是由A.P.Dempster等人于1977年提出来的，是当前统计学领域最为广泛应用的算法之一。定义4.2.1给定某个量测数据z，以及用参数描述的模型族，EM算法的基本形式就是求得，使得似然函数为最大，即(4.2.1)假定已经定义了一个对数似然函数，而且次迭代对于参数的最优估计是，由此可以得到对数似然函数变化量(4.2.2),23,23,4.2期望极大化（EM）的方法EM算法描述,算法描述定义4.2.2设观测数据集合是，而不可观测的数据（或缺失数据）是，二者构成对所考虑模型族适配的完全数据集合。（4.2.3）假设是已知的，则最优的值就容易计算得到。从数学的观点来看，对于离散概率分布，式（4.2.2）变为（4.2.4）这个表达式是对和式求对数，一般难于处理。我们不加证明地给出所谓Jensen不等式（4.2.5）,24,24,4.2期望极大化（EM）的方法EM算法描述,为了利用这个不等式，需要构造，使得。显然，在给定当前观测数据和参数的前提下，缺失数据的条件概率，且有，于是把这些系数引入式（4.2.4）得（4.2.6）,25,25,4.2期望极大化（EM）的方法EM算法描述,现在可以应用Jensen不等式得（4.2.7）重写上式，得到（4.2.8）其中（4.2.9）,26,26,4.2期望极大化（EM）的方法EM算法描述,现在，和都是参数的函数，而且在参数空间中，前者处处大于或等于后者，如图4.2.1所示。进而可证明，如果，则。于是，可以用图4.2.1对EM算法进行说明。图4.2.1,27,27,4.2期望极大化（EM）的方法EM算法描述,算法可以分为如下两个步骤进行：（1）求期望（步）（2）极大化（步）因此，算法的步骤可描述如下：（1）步计算（4.2.11）（2）步计算（4.2.12）定理2.2.1上述算法在满足假设条件时收敛。,28,4.1点估计理论基础4.2期望极大化（EM）方法4.3线性动态系统的滤波理论与算法4.4非线性动态系统的滤波理论与算法,第4章基于统计的融合,29,4.3线性动态系统的滤波理论与算法,本小节的主要内容有：离散时间线性系统状态估计问题的一般描述基本Kalman滤波器Kalmanac滤波基本方程的推导过程,30,4.3线性动态系统的滤波理论与算法状态估计问题的一般描述,离散时间线性系统状态估计问题的一般描述定义4.3.1考虑离散时间线性随机动态系统(4.3.1)(4.3.2)其中是时间指标，是k时刻的系统状态向量，是系统状态转移矩阵，而是过程演化噪声，是噪声矩阵，是k时刻对系统状态的量测向量，是量测矩阵，而是量测噪声。假定直到k时刻所有的量测信息是(4.3.3),31,基于量测信息，对的估计问题，称为状态滤波问题；对的估计问题，称为状态预测问题；对的估计问题，称为状态平滑问题。,4.3线性动态系统的滤波理论与算法状态估计问题的一般描述,32,4.3线性动态系统的滤波理论与算法状态估计问题的一般描述,定义4.3.2仍考虑式(4.3.1)和式(4.3.2)描述的离散时间线性随机动态系统，假定所有随机变量都是Gauss的情况下，考虑对于量测的一步提前预测(4.3.4)而预测误差序列(4.3.5)称为新息（innovation）序列。如果假定随机变量是非Gauss的情况下，仍考虑对于量测的一步提前预测(4.3.6),33,4.3线性动态系统的滤波理论与算法状态估计问题的一般描述,其中估计采用BLUE准则，而预测误差序列(4.3.7)则称为伪新息序列。定理4.3.1Gauss序列所产生的新息序列是一个零均值的独立过程，它与原量测序列之间存在因果性线性运算，而且包含了原序列的所有信息；同时原量测序列、一步提前预测序列和新息序列构成一个一步提前预测器，这个预测器是一个具有单位反馈的线性系统，如下图所示。,34,4.3线性动态系统的滤波理论与算法状态估计问题的一般描述,推论：非Gauss序列所产生的伪新息序列也是一个零均值的独立过程，它与原量测序列之间也存在因果性线性运算，而且包含了原序列的所有信息；同时原量测序列、一步提前预测序列和伪新息序列构成一个一步提前预测器，与Gauss情况相同。定理4.3.2设是由Gauss序列产生的新息序列，假定x是与联合Gauss分布的，且，则(4.3.8),35,4.3线性动态系统的滤波理论与算法状态估计问题的一般描述,推论如果是由非Gauss序列产生的伪新息序列，并假定x是与具有任意形式的联合分布，且，则(4.3.9),36,4.3线性动态系统的滤波理论与算法基本Kalman滤波,本小节的主要内容有：卡尔曼滤波所要解决的问题卡尔曼滤波的基本方程卡尔曼滤波基本方程的直观推导卡尔曼滤波基本方程的正交投影推导,37,4.3线性动态系统的滤波理论与算法基本Kalman滤波解决的问题,滤波所谓滤波就是从混合在一起的诸多信号中提取出所需要的信号。信号是传递和运载信息的时间或空间函数。有一类信号的变化规律是既定的，具有确定的频谱，称为确定性信号。另一类信号没有既定的变化规律，没有确定的频谱，称为随机信号。,38,滤波由于确定性信号具有确定的频潜，所以可根据各信号频带的不同，设置具有相应频率特性的滤波器，如低通、高通、带通、带阻滤波器，使有用信号无衰减地通过，使干扰信号受到扣制。这类滤波器可用物理方法实现，此即模拟滤波器，也可用计算机通过算法实现，此即数字滤波器。对确定性信号的滤波处理也称常规滤波。随机信号没有确定的频谱，无法用常规滤波提取或抑制信号。但随机信号具有确定的功率谱，所以可根据有用信号和干扰信号的功率谱设计滤波器。,4.3线性动态系统的滤波理论与算法基本Kalman滤波解决的问题,39,4.3线性动态系统的滤波理论与算法基本Kalman滤波解决的问题,卡尔曼滤波所要解决的问题卡尔曼滤波从与被提取信号有关的量测量中通过算法估计出所需信号。被估计信号是由白嗓声激励引起的随机响应，激励源与响应之问的传递结构（系统方程）已知，量测量与被估计量之间的函数关系（量测方程）也已知。估汁过程中利用了如下信息：系统方程、量测方程、白噪声激励的统计特性、量测误差的统计特性。由于所用信息都是时域内的量，所以卡尔曼滤波器是在时域内设计的，且适用于多维情况。,40,4.3线性动态系统的滤波理论与算法基本Kalman滤波解决的问题,从以上简述中可看出卡尔曼滤波有如下特点：（1）卡尔曼滤波处理的对象是随机信号；（2）被处理信号无有用和干扰之分，滤波的目的是要估计出所有被处理信号；（3）系统的白噪声激励和量测噪声并不是需要滤除的对象，它们的统计特性正是估计过程中需要利用的信息。,41,4.3线性动态系统的滤波理论与算法基本Kalman滤波解决的问题,确切地说，卡尔曼滤波应称作最优估计理论，此处称谓的滤波与常规滤波具有完全不同的概念和含意。就实现形式而言，卡尔曼滤波器实质上是一套由数字计算机实现的递推算法，每个递推周期中包含对被估计量的时间更新和量测更新两个过程。时间更新由上一步的量测更新结果和设计卡尔曼滤波器时的先验信息确定，量测更新则在时间更新的基础上根据实时获得的量测值确定。因此，量测量可看做卡尔曼滤波器的输人，估计值可看做输出，输入与输出之间由时间更新和量测更新算法联系。,42,卡尔曼滤波的基本方程设时刻的被估计状态受系统噪声序列驱动，驱动机理由下述状态方程描述(4.3.10)对的量测满足线性关系，量测方程为(4.3.11)式中：为时刻至时刻的一步转移阵；为系统噪声驱动阵；为量测阵；为量测噪声序列；为系统激励噪声序列。,4.3线性动态系统的滤波理论与算法Kalman滤波的基本方程,43,同时，和满足(4.3.12)式中：若为系统噪声序列的方差阵，假设为非负定阵；为量测噪声序列的方差阵，假设为正定阵。定理4.3.3如果被估计状态满足式(4.3.10)，对的量测量满足式(4.3.11)，系统噪声和量测噪声满足式(4.3.12)，系统噪声方差阵非负定，量测噪声方差阵正定，时刻的量测为，则估计按下述方程求解：,4.3线性动态系统的滤波理论与算法Kalman滤波的基本方程,44,状态一步预测(4.3.13a)状态估计(4.3.13b)滤波增益(4.3.13c)或(4.3.13c)一步预测均方误差(4.3.13d),4.3线性动态系统的滤波理论与算法Kalman滤波的基本方程,45,估计均方误差(4.3.13e)或(4.3.13e)或(4.3.13e)式(4.3.13)即为离散型卡尔曼滤波基本方程。只要给定初值和，根据时刻的量测，就可递推计算得时刻的状态估计。,4.3线性动态系统的滤波理论与算法Kalman滤波的基本方程,46,式(4.3.13)所示算法可用图4.3.1来表示。从图中可明显看出卡尔曼滤波具有两个计算回路：增益计算回路和滤波计算回路。其中增益计算回路是独立计算回路，而滤波计算回路依赖于增益计算回路。在一个滤波周期内，从卡尔曼滤波在使用系统信息和量测信息的先后次序来看，卡尔曼滤波具有两个明显的信息更新过程，时间更新过程和量测更新过程。式(4.3.13a)说明了根据时刻的状态估计预测时刻状态估计的方法，式(4.3.13d)对这种预测的质量优劣作了定量描述。该两式的计算中仅使用了与系统动态特性有关的信息，如一步转移阵、噪声驱动阵、驱动噪声的方差阵。从时间的推移过程来看，该两式将时间从时刻推进到时刻。所以该两式描述了卡尔曼滤波的时间更新过程。,4.3线性动态系统的滤波理论与算法Kalman滤波的基本方程,47,式(4.3.13)的其余诸式用来计算对时间更新值的修正量，该修正量由时间更新的质量优劣()、量测信息的质量优劣()、量测与状态的关系()以及具体的量测值所确定，所有这些方程围绕一个目的，即正确合理地利用量测，所以这一过程描述了卡尔曼滤波的量测更新过程。该两过程如图4.3.1所示。,4.3线性动态系统的滤波理论与算法Kalman滤波的基本方程,48,4.3线性动态系统的滤波理论与算法Kalman滤波的基本方程,49,卡尔曼滤波基本方程的直观推导为了加深对离散型卡尔曼滤波基本方程的理解，此处根据直观理解和物理概念推导出式(4.3.13)。1.一步预测方程推导一步预测是根据时刻的状态估计预测时刻的状态，即根据个量测，对作线性最小方差估计根据线性最小方差估计的性质，有由式(4.2.10)知，只影响，所以与不相关，且。,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的直观推导,50,则有：而因此,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的直观推导,51,2.状态估计方程推导用一步预测代替真实状态引起的误差为引起对量测的估计误差为滤波理论中称为残差（也称新息）。从上式可看出，残差包含有一步预测误差信息，对作适当的加权处理就能将分离出来，用来修正即可得到状态的估计式中为对残差的加权阵，称为滤波增益阵。,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的直观推导,52,3.滤波增益阵和估计均方误差阵的推导增益阵的选取准则是使估计的均方误差阵达到最小，其中为估计误差。而所以由于是根据时刻前的量测对时刻的状态所作的估计，而是时刻的量测噪声。所以与不相关，并注意到，因此有,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的直观推导,53,代入的表达式，得式中，下面根据极值原理从式(4.3.13e)推导出滤波增益阵。设是使估计的均方误差阵达到最小的最佳增益阵，并设该最小均方误差阵为。显然，若滤波增益阵偏离最佳增益阵的偏离量为，则由式(4.3.13e)确定的估计的均方误差将偏离最小值而达到，且为非负定阵，即。和满足的方程为,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的直观推导,54,其中和满足式(4.3.13e)。将式(4.3.13e)代入上式，得式中若取即则，,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的直观推导,55,由于为正定阵，至少为非负定阵，所以至少为非负定阵。若为非零阵，则至少为非负定阵，即。这说明，若按式（4.3.13c）确定，则对于相对增益阵的任何偏离，估计的均方误差将产生非负的偏差，因此是使估计的均方误差达到最小的最佳增益阵。,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的直观推导,56,4一步预侧均方误差阵的推导一步预测产生的误差为所以一步预测均方误差阵为由于,只影响，不影响,所以与不相关，又因，因此,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的直观推导,57,卡尔曼滤波基本方程除利用物理概念经直观推导获得外,还可用严密的数学概念推导获得。1.正交投影定理设和都是随机向量，如果(4.3.14)则称与正交。应注意随机向量间正交、不相关和独立三者的区别。如果随机向量和的协方差阵为零，即即(4.3.15)式中和分别为和的均值，则称和不相关。,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,58,如果和的联合分布密度(4.3.16)式中和分别为和的分布密度，则称和互相独立。从上述定义可得出如下结论：（1）若与独立，则与一定不相关。但逆命题一般并不成立，只有当和都服从正态分布时才成立。（2）如果和的数学期望至少有一个为零，则不相关与正交等价。（3）如果和都服从正态分布。且至少有一个数学期望为零，则不相关、正交和独立三者等价。,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,59,如果存在某矩阵和某向量，对任意矩阵和任意向量都能使下式成立(4.3.17)则称为在上的正交投影。式(4.3.17)也可改写成如下形式由于为任意矩阵。为任意向量，要使上式恒能成立，须有(4.3.18)式(4.2.18)是正交投影定义的另一种形式。从该式可看出：,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,60,（1）正交投影是量测量和常值向量的线性组合，位于张成的空间内，该空间也称量测空间。（2）若用正交投影作为的估计。则估计误差与量测空间正交。（3）正交投影是的无偏估计。,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,61,定理4.3.4在上的正交投影即为在上的线性最小方差估计。反之亦然。即(4.3.19)证明（1）设是在上的线性最小方差估计，有，则有：由于同理,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,62,所以此外，由于线性最小方差估计是无偏估计，即根据式(4.3.18)所给定义知是在上的正交投影。（2）设为在上的正交投影，则根据式(4.3.18)，有(1)(2)由(1)证明了线性最小方差估计是在上的正交投影，即有(3)(4),4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,63,式(1)式(3)，得由于，因此(5)式(2)式(4)，得(6)将式(6)代入式(5)，得在估计过程中，为了充分利用量测信息，的各分量必须是独立的，即有，,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,64,即满秩，因此有将此关系式代入(6)式，得从而有这就证明了定理4.3.4,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,65,2.更新信息定理定理4.3.5（更新信息定理）设为随机向量，为第次量测，和分别为前次量测和前次量测，则(4.3.20)即(4.3.21)式中，,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,66,更新信息定理说明只需用对作修正即可获得，所以是卡尔曼滤波中的一个重要信息源。由于是前次量测已提供的关于的信息，而含有第次量测新提供的信息，所以常将称为新息。这一定理也由此而得名。证明而(1)式中即是关于的线性函数，由式(1)可推知，可由和线性表示，所以可由和线性表示(2),4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,67,由于，由定理4.3.4知与正交，即(3)且(4)根据线性最小方差估计性质及式(3)、式(4)，有,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,68,注意到是确定性向量，所以,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,69,3基本方程的正交投影推导下面根据更新信息定理推导卡尔曼滤波的状态估计方程和最佳增益阵。由于并注意到时刻的量测噪声不会影响时刻前的估计，所以,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,70,将上述关系式代人式(4.3.21)，得令则一步预测方程、一步预测的均方误差阵方程和估计的均方误差阵方程的推导与直观推导方法中所述的一样，此处不再重复。从上述推导过程可看出，是量测空间上的正交投影，根据定理4.3.4，正交投影与线性最小方差估计两者等价，所以卡尔曼滤波是在上的线性最小方差估,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,71,计，利用了时刻及无时刻以前的所有量测信息，离散型卡尔曼滤波基本方程有如下优点：（1）由于采用了递推算法，不同时刻的量测值不必储存起来，而是经实时处理提炼成被估汁状态的信息，随着滤波步数的增加，提取出的信息浓度逐渐增加。（2）不必了解被估计量和量测量在不同时刻的一、二阶矩，而只须知道驱动被估计量的驱动噪声的统计特性、描述这种驱动作用的系统状态方程及量测噪声的统计特性。驱动噪声和量测噪声都是白噪声，是平稳过程，统计特性不随时间而变，系统的状态方程又是准确已知的，所以卡尔曼滤波能对非平稳的被估计量作估计。,4.3线性动态系统的滤波理论与算法Kalman滤波基本方程的正交投影推导,72,4.1点估计理论基础4.2期望极大化（EM）方法4.3线性动态系统的滤波理论与算法4.4非线性动态系统的滤波理论与算法,第4章基于统计的融合,73,4.4非线性动态系统的滤波理论与算法,本小节的主要内容有：扩展Kalman滤波（EKF）UKF滤波,74,4.4非线性动态系统的滤波理论与算法扩展Kalman滤波器（EKF）,扩展Kalman滤波器（EKF）考虑离散时间非线性动态系统：(4.4.1)(4.4.2)其中是时间指标，是时刻的系统状态向量，是系统状态演化映射，而是n维过程演化噪声，是k时刻对系统状态的量测向量，是量测映射，而是m维量测噪声。假定和对其变元连续可微（当用二阶扩展Kalman滤波时假定二阶连续可微），同时假定初始状态为任意分布，具有均值和协方差矩阵分别为(4.4.3),75,4.4非线性动态系统的滤波理论与算法扩展Kalman滤波器（EKF）,过程噪声是一个零均值的独立过程，分布任意，具有协方差矩阵为（4.44）量测噪声也是一个零均值的独立过程，分布任意，具有协方差矩阵为（4.45）过程噪声、量测噪声以及初始状态之间都相互独立。扩展Kalman滤波算法实质上是一种在线线性儿的算法，即按名义轨线进行线性化处理，再利用Kalman滤波公式进行计算。这种算法已经不再是按某个指标进行优化的最优化算法，其性能取决于非线性系统的复杂度以及算法的优劣等。,76,4.2卡尔曼滤波正交投影推导,一阶EKF算法的步骤描述如下：（1）在时刻，假定已经获得时刻的状态估计值和估计误差的协方差阵(4.4.6)而此时对演化方程的线性化方程为(4.4.7)其中(4.4.8)(4.4.9),4.4非线性动态系统的滤波理论与算法扩展Kalman滤波器（EKF）,77,4.2卡尔曼滤波正交投影推导,4.4非线性动态系统的滤波理论与算法扩展Kalman滤波器（EKF）,（2）对时刻状态的一步提前预测(4.4.10)状态预测误差是(4.4.11)状态预测误差的协方差阵是(4.4.12)（3）对时刻量测的线性化量测方程(4.4.13)其中，(4.4.14),78,4.2卡尔曼滤波正交投影推导,4.4非线性动态系统的滤波理论与算法扩展Kalman滤波器（EKF）,(4.4.15)（4）对时刻量测的一步提前预测为(4.4.16)量测预测误差是(4.4.17)量测预测误差的协方差阵是(4.4.18)状态预测误差与量测预测误差的协方差阵是(4.4.19),79,4.2卡尔曼滤波正交投影推导,在时刻k得到新的量测状态滤波的更新公式(4.4.20)预测误差的协方差阵是(4.4.21)而k时刻Kalman增益阵为(4.4.22)二阶EKF算法的步骤描述如下：(1)在时刻假定已经获得时刻的状态估计值和估计误差的协方差阵(4.4.23),4.4非线性动态系统的滤波理论与算法扩展Kalman滤波器（EKF）,80,4.2卡尔曼滤波正交投影推导,4.4非线性动态系统的滤波理论与算法扩展Kalman滤波器（EKF）,而此时对演化方程的线性化方程为(4.4.24)其中是第个标准基向量，是第分量，而(4.4.25)(4.4.26)(4.4.27),81,4.2卡尔曼滤波正交投影推导,4.4非线性动态系统的滤波理论与算法扩展Kalman滤波器（EKF）,（2）对时刻k状态的一步提前预测(4.4.28)其中表示矩阵求迹，状态预测误差是(4.4.29)状态预测误差的协方差阵是(4.4.30)（3）对k时刻量测的线性化量测方程(4.4.31),82,4.2卡尔曼滤波正交投影推导,4.4非线性动态系统的滤波理论与算法扩展Kalman滤波器（EKF）,其中是第个标准基向量，是的第分量，而(4.4.32)(4.4.33)(4.4.34)（4）对时刻k量测的一步提前预测(4.4.35)量测预测误差是,83,4.2卡尔曼滤波正交投影推导,4.4非线性动态系统的滤波理论与算法扩展Kalman滤波器（EKF）,(4.4.36)量测预测误差的协方差阵是(4.4.37)状态预测误差与量测预测误差的协方差阵是(4.4.38)（5）在时刻k得到新的量测，状态滤波的更新公式(4.4.39)预测误差的协方差阵是(4.4.40)而时刻k的Kalman增益阵为(4.4.41),84,84,4.4非线