基于统计的融合

基于统计的融合第1页/共115页24.1点估计理论基础应用举例（状态最优估计融合算法在伺服系统中的应用）

4.2线性动态系统的滤波理论与算法

4.3非线性动态系统的滤波理论与算法

4.4应用——滤波及仿真第4章基于统计的融合--统计推断与估计理论第2页/共115页3第4章基于统计的融合--统计推断与估计理论估计：根据测量值z(t)，z(t)=h[x(t)]+w(t)，解算出x(t)的计算值(t)。随机向量v(t)为量测误差，称为x的估计，z称为x的量测。设在[t0,t1]时间内的量测为z，相应的估计为，则当t=t1时，(t)称为x(t)的估计；当t>t1时，(t)称为x(t)的预测；当t<t1时，x(t)称为x(t)的平滑；最优估计是指某一指标值达到最值时的估计。第3页/共115页4第4章基于统计的融合--统计推断与估计理论以量测估计Z的偏差的平方和达到最小为指标，即(Z-Z)T(Z-Z)=min

则所得的估计为最小二乘估计；以状态估计的均方误差集平均达到最小为指标，E[(x-)T(x-)]=min

则所得的估计为最小方差估计；若又是x的线性估计，则为x的线性最小方差估计。以估计值出现的概率作为估计指标，有极大验后估计、贝叶斯估计和极大似然估计。第4页/共115页5点估计：又称定值估计，就是用实际样本指标数值作为总体参数的估计值，实际上它就是总体未知参数的近似值。由样本x1,x2,…xn确定一个统计量用它来估计总体的未知参数，称为总体参数的估计量。当具体的样本抽出后，可求出样本参数的值。衡量标准有：无偏性(unbiasedness)一致性（consistency）有效性4.1点估计的理论基础第5页/共115页64.1点估计的理论基础本小节的主要内容有：一般概念Bayes点估计理论最佳线性无偏（BLUE）估计加权最小二乘（WLS）估计极大似然（ML）估计递推最小二乘（RLS）估计第6页/共115页74.1点估计的理论基础——一般概念

一般概念

定义4.1.1设是一个未知参数向量，量测y是一个m维的随机向量，而y的一组容量为N的样本是设对它的统计量为(4.1.1)

称其为对x的一个估计量，其中称为统计规则或估计算法。

利用样本对参数的估计量本质上是随机的，而当样本值给定时所得到的参数估计值一般与真值并不相同，因而需要用某些准则进行评价。第7页/共115页84.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页/共115页94.1点估计的理论基础——一般概念

定理4.1.1（Cramer-Rao不等式）设是参数x的一个正规无偏估计，则其估计误差的协方差阵满足Cramer-Rao不等式(4.1.5)其中是估计误差，而是Fisher信息矩阵（注意标量对向量求导取行向量），定义为(4.1.6)

其中是给定x时y的条件概率密度函数。第9页/共115页104.1点估计的理论基础——Bayes点估计理论Bayes点估计理论设x也是一个n维随机向量，仍设是y的一组容量为N的样本。设表示量测信息，则x与z的联合概率密度函数是

(4.1.7)

假定表示由量测信息z得到的一个估计，而估计误差定义为

(4.1.8)第10页/共115页114.1点估计的理论基础——Bayes点估计理论定义4.1.4估计误差的标量函数称为一个损失函数，如果（1）零误差的损失为零，即，则；（2）按分量的绝对值单调增，即和的第i个分量满足，其余分量相等，则；

（3）是对称的，即，有。定义4.1.5设，估计误差的损失函数是，则风险函数定义为(4.1.9)

其中估计方法；Bayes风险定义为

(4.1.10)

其中，分别表示按分布或条件分布求期望；第11页/共115页124.1点估计的理论基础——Bayes点估计理论最小Bayes风险估计定义为(4.1.11)

定义4.1.6利用Bayes公式Bayes风险可以改写为(4.1.12)

其中就是损失函数的后验期望。而最小后验用望损失估计定义为

(4.1.13)

其中就是损失函数的后验期望。而最小后验用望损失估计定义为

(4.1.13)第12页/共115页134.1点估计的理论基础——Bayes点估计理论定理4.1.2设参数x和量测信息z是联合Guass分布的，其均值和协方差阵分别为

(4.1.14)

并假定R和非奇异，那么，给定z时，x也是条件Gauss的，而且对估计误差的任意容许损失函数，最小后验期望损失估计公式为(4.1.15)

估计误差的协方差阵是(4.1.16)第13页/共115页144.1点估计的理论基础——BLUE估计

BLUE估计定义4.1.7设，，对参数x的估计表示为量测信息z的线性函数(4.1.17)

则称为线性估计；进而如果估计误差的均方值达到最小，则称之为线性最小方差估计；如估计还是无偏的，则称为线性无偏最小方差估计。这种线性无偏最小方差估计在多源信息融合领域一般称为最佳线性无偏估计（BestLinearUnbiasedEstimation，BLUE）。第14页/共115页154.1点估计的理论基础——BLUE估计定理4.13设参数x和量测信息z是任意分布，z的协方差阵非奇异，则利用量测信息z对参数x的BLUE估计惟一地表示为

(4.1.18)

此处只是一个记号，不表示条件期望；而估计误差的协方差阵是

(4.1.19)第15页/共115页164.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页/共115页174.1点估计的理论基础——ML估计ML估计定义4.1.9给定参数x时量测信息z的似然函数表示为，极大似然（maximumIikelihood，ML）参数估计问题可以描述为

(4.1.23)

考虑如下参数估计问题

(4.1.24)

其中是时间指标，分别是回归向量和未知参数向量，是k时刻的量测量，而是量测误差。第17页/共115页184.1点估计的理论基础——RLS估计RLS估计

假定是一个零均值的随机过程，对于k时刻的量测总量、回归总量和误差总量，，，可有总量关系

(4.1.25)

所以有最小二乘估计

(4.1.26)第18页/共115页194.1点估计的理论基础——RLS估计引理（矩阵求逆引理）设，均可逆，，则有

(4.1.27)定理4.1.7

设获得第k+1时刻的量测和回归向量之后，令

(4.1.28)

则有递推最小二乘（recursiveleastsquares，RLS）估计是

(4.1.29)第19页/共115页204.1点估计的理论基础——RLS估计递推最小二乘（recursiveleastsquares，RLS）估计其中和分别是Kalman增益矩阵和一步预测误差或参数估计新息（innovation），分别计算为

(4.1.30)

而的递推计算式为

(4.1.31)第20页/共115页214.1点估计的理论基础——应用应用举例（状态最优估计融合算法在伺服系统中的应用）

第21页/共115页224.1点估计理论基础应用举例（状态最优估计融合算法在伺服系统中的应用）

4.2线性动态系统的滤波理论与算法

4.3非线性动态系统的滤波理论与算法

4.4应用——滤波及仿真第4章基于统计的融合第22页/共115页234.2线性动态系统的滤波理论与算法主要内容有：离散时间线性系统状态估计问题的一般描述基本Kalman滤波器Kalman滤波基本方程的推导过程第23页/共115页244.2线性动态系统的滤波理论与算法

——状态估计问题的一般描述

离散时间线性系统状态估计问题的一般描述定义4.2.1考虑离散时间线性随机动态系统(4.2.1)(4.2.2)

其中是时间指标，是k时刻的系统状态向量，是系统状态转移矩阵，而是过程演化噪声，是噪声矩阵，是k时刻对系统状态的量测向量，是量测矩阵，而是量测噪声。假定直到k时刻所有的量测信息是

(4.2.3)

马尔可夫序列大多数实际物理系统成立第24页/共115页25

基于量测信息：对的估计问题，称为状态滤波问题；对的估计问题，称为状态预测问题；对的估计问题，称为状态平滑问题。4.2线性动态系统的滤波理论与算法

——状态估计问题的一般描述第25页/共115页264.2线性动态系统的滤波理论与算法

——状态估计问题的一般描述定义4.2.2仍考虑式(4.2.1)和式(4.2.2)描述的离散时间线性随机动态系统，假定所有随机变量都是Gauss的情况下，考虑对于量测的一步提前预测

(4.2.4)

而预测误差序列

(4.2.5)

称为新息（innovation）序列。随机过程{X(t),t∈D}，其中的向量X(t1),…,X(tm)两两不相关第26页/共115页27

如果假定随机变量是非Gauss的情况下，仍考虑对于量测的一步提前预测

(4.2.6)其中估计采用BLUE准则，而预测误差序列

(4.2.7)称为伪新息序列。4.2线性动态系统的滤波理论与算法

——状态估计问题的一般描述第27页/共115页284.2线性动态系统的滤波理论与算法

——状态估计问题的一般描述定理4.2.1Gauss序列所产生的新息序列是一个零均值的独立过程，它与原量测序列之间存在因果性线性运算，而且包含了原序列的所有信息；同时原量测序列、一步提前预测序列和新息序列构成一个一步提前预测器，这个预测器是一个具有单位反馈的线性系统，如下图所示。第28页/共115页294.2线性动态系统的滤波理论与算法

——状态估计问题的一般描述推论：非Gauss序列所产生的伪新息序列也是一个零均值的独立过程，它与原量测序列之间也存在因果性线性运算，而且包含了原序列的所有信息；同时原量测序列、一步提前预测序列和伪新息序列构成一个一步提前预测器，与Gauss情况相同。定理4.2.2设是由Gauss序列产生的新息序列，假定x是与联合Gauss分布的，且，则

(4.2.8)第29页/共115页304.2线性动态系统的滤波理论与算法

——状态估计问题的一般描述推论如果是由非Gauss序列产生的伪新息序列，并假定x是与具有任意形式的联合分布，且，则

(4.2.9)第30页/共115页314.2线性动态系统的滤波理论与算法

——基本Kalman滤波基本Kalman滤波的主要内容有：卡尔曼滤波所要解决的问题卡尔曼滤波的基本方程卡尔曼滤波基本方程的直观推导卡尔曼滤波基本方程的正交投影推导第31页/共115页324.2线性动态系统的滤波理论与算法

——基本Kalman滤波解决的问题滤波所谓滤波就是从混合在一起的诸多信号中提取出所需要的信号。信号是传递和运载信息的时间或空间函数。有一类信号的变化规律是既定的，具有确定的频谱，称为确定性信号。另一类信号没有既定的变化规律，没有确定的频谱，称为随机信号。第32页/共115页33滤波由于确定性信号具有确定的频潜，所以可根据各信号频带的不同，设置具有相应频率特性的滤波器，如低通、高通、带通、带阻滤波器，使有用信号无衰减地通过，使干扰信号受到抑制。4.2线性动态系统的滤波理论与算法

——基本Kalman滤波解决的问题这类滤波器可用物理方法实现，此即模拟滤波器，也可用计算机通过算法实现，此即数字滤波器。对确定性信号的滤波处理也称常规滤波。随机信号没有确定的频谱，无法用常规滤波提取或抑制信号。但随机信号具有确定的功率谱，所以可根据有用信号和干扰信号的功率谱设计滤波器。第33页/共115页344.2线性动态系统的滤波理论与算法

——基本Kalman滤波解决的问题卡尔曼滤波所要解决的问题卡尔曼滤波从与被提取信号有关的量测量中通过算法估计出所需信号。被估计信号是由白嗓声激励引起的随机响应，激励源与响应之问的传递结构（系统方程）已知，量测量与被估计量之间的函数关系（量测方程）也已知。估汁过程中利用了如下信息：系统方程、量测方程、白噪声激励的统计特性、量测误差的统计特性。由于所用信息都是时域内的量，所以卡尔曼滤波器是在时域内设计的，且适用于多维情况。第34页/共115页354.2线性动态系统的滤波理论与算法

——基本Kalman滤波解决的问题从以上简述中可看出卡尔曼滤波有如下特点：（1）卡尔曼滤波处理的对象是随机信号；（2）被处理信号无有用和干扰之分，滤波的目的是要估计出所有被处理信号；（3）系统的白噪声激励和量测噪声并不是需要滤除的对象，它们的统计特性正是估计过程中需要利用的信息。第35页/共115页36

卡尔曼滤波的基本方程设时刻的被估计状态受系统噪声序列驱动，驱动机理由下述状态方程描述

(4.2.10)4.2线性动态系统的滤波理论与算法

——Kalman滤波的基本方程

对的量测满足线性关系，量测方程为

(4.2.11)

式中：为时刻至时刻的一步转移阵；为系统噪声驱动阵；为量测阵；为量测噪声序列；为系统激励噪声序列。第36页/共115页37

同时，和满足

(4.2.12)

式中：若为系统噪声序列的方差阵，假设为非负定阵；为量测噪声序列的方差阵，假设为正定阵。

4.2线性动态系统的滤波理论与算法

——Kalman滤波的基本方程定理4.3.2

如果被估计状态满足式(4.2.10)，对的量测量满足式(4.2.11)，系统噪声和量测噪声满足式(4.2.12)，系统噪声方差阵非负定，量测噪声方差阵正定，时刻的量测为，则估计按下述方程求解：第37页/共115页38状态一步预测

(4.2.13a)状态估计

(4.2.13b)滤波增益

(4.2.13c)或

(4.2.13c')一步预测均方误差(4.2.13d)4.2线性动态系统的滤波理论与算法

——Kalman滤波的基本方程第38页/共115页39估计均方误差

(4.2.13e)或(4.2.13e′)或

(4.2.13e〞)

式(4.2.13)即为离散型卡尔曼滤波基本方程。只要给定初值和，根据时刻的量测，就可递推计算得时刻的状态估计。

4.2线性动态系统的滤波理论与算法

——Kalman滤波的基本方程第39页/共115页404.2线性动态系统的滤波理论与算法

——Kalman滤波的基本方程第40页/共115页414.2线性动态系统的滤波理论与算法

——Kalman滤波的基本方程式(4.2.13)所示算法可用图来表示。卡尔曼滤波具有两个计算回路：增益计算回路和滤波计算回路。增益计算回路是独立计算回路滤波计算回路依赖于增益计算回路。式(4.2.13)所示算法可用图来表示。卡尔曼滤波具有两个计算回路：增益计算回路和滤波计算回路。增益计算回路是独立计算回路滤波计算回路依赖于增益计算回路。第41页/共115页424.2线性动态系统的滤波理论与算法

——Kalman滤波的基本方程在一个滤波周期内：系统信息和量测信息卡尔曼滤波具有两个明显的信息更新过程，时间更新过程和量测更新过程。根据k-1时刻的状态估计，预测k时刻状态估计的方法对这种预测的质量优劣作了定量描述两式的计算中仅使用了与系统动态特性有关的信息，如一步转移阵、噪声驱动阵、驱动噪声的方差阵。从时间的推移过程来看，该两式将时间从时刻推进到时刻。所以该两式描述了卡尔曼滤波的时间更新过程。第42页/共115页434.2线性动态系统的滤波理论与算法

——Kalman滤波的基本方程其余诸式用来计算对时间更新值的修正量，该修正量由时间更新的质量优劣()、量测信息的质量优劣()、量测与状态的关系()以及具体的量测值所确定，所有这些方程围绕一个目的，即正确合理地利用量测。这一过程描述了卡尔曼滤波的量测更新过程。第43页/共115页444.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的直观推导卡尔曼滤波基本方程的直观推导

为了加深对离散型卡尔曼滤波基本方程的理解，此处根据直观理解和物理概念推导出式(4.2.13)。

第44页/共115页454.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的直观推导初始值状态预测值协方差矩阵预测值滤波增益矩阵状态估计值协方差矩阵估计值第45页/共115页461.一步预测方程推导

一步预测是根据时刻的状态估计预测时刻的状态，即根据个量测，对作线性最小方差估计。4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的直观推导

根据线性最小方差估计的性质，有由式(4.2.10)知，只影响，所以与不相关，且。

则有：因此采用BLUE估计（4.2.4）第46页/共115页472.状态估计方程推导4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的直观推导用一步预测代替真实状态引起的误差为引起对量测的估计误差为滤波理论中称为残差（也称新息）。从上式可看出，残差包含有一步预测误差信息，对作适当的加权处理就能将分离出来，用来修正即可得到状态的估计式中为对残差的加权阵，称为滤波增益阵。第47页/共115页483.滤波增益阵和估计均方误差阵的推导（1）增益阵的选取准则是使估计的均方误差阵达到最小，其中为估计误差。而

4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的直观推导所以状态估计第48页/共115页49由于是根据时刻前的量测对时刻的状态所作的估计，而是时刻的量测噪声。所以与不相关，并注意到，因此有4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的直观推导代入的表达式，得式中第49页/共115页50（2）下面根据极值原理从式(4.2.13e)推导出滤波增益阵4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的直观推导设是使估计的均方误差阵达到最小的最佳增益阵，并设该最小均方误差阵为。显然，若滤波增益阵偏离最佳增益阵的偏离量为，则由式(4.2.13e)确定的估计的均方误差将偏离最小值而达到，且为非负定阵，即。

和满足的方程：第50页/共115页51由，可得

式中4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的直观推导

若取即则第51页/共115页52

由于为正定阵，至少为非负定阵，所以至少为非负定阵。若为非零阵，则至少为非负定阵，即。4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的直观推导

这说明，若按式（4.2.13c）确定，则对于相对增益阵的任何偏离，估计的均方误差将产生非负的偏差，因此是使估计的均方误差达到最小的最佳增益阵。（4.2.13c）第52页/共115页534．一步预侧均方误差阵的推导

一步预测产生的误差为

4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的直观推导

所以一步预测均方误差阵为由于,只影响，不影响，所以与不相关，又因，因此

第53页/共115页544.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的直观推导第54页/共115页55初始值状态预测值协方差矩阵预测值滤波增益矩阵状态估计值协方差矩阵估计值4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的直观推导第55页/共115页56计算步骤4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的直观推导时间Hk-1,Rk-1Xk-1/k-2Pk-1/k-2Pk-1Xk-1tk-1Hk,RkXk/k-1Pk/k-1PkXktkΦk/k-1Qk-1Φk+1/kQk第56页/共115页574.2线性动态系统的滤波理论与算法

——Kalman滤波的基本方程卡尔曼滤波应称作最优估计理论，此处称谓的滤波与常规滤波具有完全不同的概念和含意。就实现形式而言，卡尔曼滤波器实质上是一套由数字计算机实现的递推算法，每个递推周期中包含对被估计量的时间更新和量测更新两个过程。时间更新由上一步的量测更新结果和设计卡尔曼滤波器时的先验信息确定，量测更新则在时间更新的基础上根据实时获得的量测值确定。因此，量测量可看做卡尔曼滤波器的输人，估计值可看做输出，输入与输出之间由时间更新和量测更新算法联系。第57页/共115页58滤波中的发散现象引起滤波器发散的主要原因有两点：描述系统动力学特性的数学模型和噪声的统计模型不准确，使模型与获得的量测值不匹配。卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差逐渐积累。渐消kalman滤波kalman滤波4.2线性动态系统的滤波理论与算法

——Kalman滤波的基本方程第58页/共115页59渐消因子4.2线性动态系统的滤波理论与算法

——Kalman滤波的基本方程第59页/共115页60卡尔曼滤波基本方程除利用物理概念经直观推导获得外，还可用严密的数学概念推导获得

4.2.1正交投影定理设和都是随机向量，如果(4.2.14)

则称与正交。应注意随机向量间正交、不相关和独立三者的区别。

4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第60页/共115页61随机向量间正交、不相关和独立三者的区别。如果随机向量和的协方差阵为零，即即(4.2.15)

式中和分别为和的均值，则称和不相关。

4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导

如果和的联合分布密度(4.2.16)

式中和分别为和的分布密度，则称和互相独立。第61页/共115页62从上述定义可得出如下结论：（1）若X与Z独立，则X与Z一定不相关。但逆命题一般并不成立，只有当X和Z都服从正态分布时才成立。（2）如果X和Z的数学期望至少有一个为零，则不相关与正交等价。（3）如果X和Z都服从正态分布。且至少有一个数学期望为零，则不相关、正交和独立三者等价。4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第62页/共115页63

如果存在某矩阵和某向量，对任意矩阵和任意向量b都能使下式成立

(4.2.17)

则称为在上的正交投影。式(4.2.17)也可改写成如下形式由于为任意矩阵。为任意向量，要使上式恒能成立，须有

(4.2.18)式(4.2.18)是正交投影定义的另一种形式。从该式可看出：4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第63页/共115页64（1）正交投影是量测量和常值向量的线性组合，位于张成的空间内，该空间也称量测空间。（2）若用正交投影作为的估计。则估计误差与量测空间正交。（3）正交投影是的无偏估计。4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第64页/共115页65定理4.2.4

在上的正交投影即为在上的线性最小方差估计。反之亦然。即

(4.2.19)

证明（1）设是在上的线性最小方差估计，有，，则有：由于同理4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第65页/共115页66

所以此外，由于线性最小方差估计是无偏估计，即根据式(4.2.18)所给定义知是在上的正交投影。（2）设为在上的正交投影，则根据式(4.2.18)，有

(1)(2)

由(1)证明了线性最小方差估计是在上的正交投影，即有

(3)(4)4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第66页/共115页67

式(1)—式(3)，得由于，因此

(5)

式(2)—式(4)，得

(6)

将式(6)代入式(5)，得在估计过程中，为了充分利用量测信息，的各分量必须是独立的，即有，4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第67页/共115页68

即满秩，因此有将此关系式代入(6)式，得从而有这就证明了定理4.2.44.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第68页/共115页692.更新信息定理定理4.2.5（更新信息定理）设为随机向量，为第次量测，和分别为前次量测和前次量测，则

(4.2.20)

即

(4.2.21)

式中，4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第69页/共115页70

更新信息定理说明只需用对作修正即可获得，所以是卡尔曼滤波中的一个重要信息源。由于是前次量测已提供的关于的信息，而含有第次量测新提供的信息，所以常将称为新息。这一定理也由此而得名。

证明：而(1)

式中即是关于的线性函数，由式(1)可推知，可由和线性表示，所以可由和线性表示

(2)4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第70页/共115页71由于，由定理4.2.4知与正交，即

(3)且

(4)根据线性最小方差估计性质及式(3)、式(4)，有4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第71页/共115页72注意到是确定性向量，所以4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第72页/共115页733．基本方程的正交投影推导下面根据更新信息定理推导卡尔曼滤波的状态估计方程和最佳增益阵。由于并注意到时刻的量测噪声不会影响时刻前的估计，所以4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第73页/共115页74

将上述关系式代人式(4.2.21)，得令则一步预测方程、一步预测的均方误差阵方程和估计的均方误差阵方程的推导与直观推导方法中所述的一样，此处不再重复。从上述推导过程可看出，是量测空间上的正交投影，根据定理4.2.4，正交投影与线性最小方差估计两者等价，所以卡尔曼滤波是在上的线性最小方差估。4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第74页/共115页75

利用该时刻及该时刻以前的所有量测信息，离散型卡尔曼滤波基本方程有如下优点：（1）由于采用了递推算法，不同时刻的量测值不必储存起来，而是经实时处理提炼成被估汁状态的信息，随着滤波步数的增加，提取出的信息浓度逐渐增加。（2）不必了解被估计量和量测量在不同时刻的一、二阶矩，而只须知道驱动被估计量的驱动噪声的统计特性、描述这种驱动作用的系统状态方程及量测噪声的统计特性。驱动噪声和量测噪声都是白噪声，是平稳过程，统计特性不随时间而变，系统的状态方程又是准确已知的，所以卡尔曼滤波能对非平稳的被估计量作估计。4.2线性动态系统的滤波理论与算法

——Kalman滤波基本方程的正交投影推导第75页/共115页764.1点估计理论基础应用举例（状态最优估计融合算法在伺服系统中的应用）

4.2线性动态系统的滤波理论与算法

4.3非线性动态系统的滤波理论与算法

4.4应用——滤波及仿真第4章基于统计的融合第76页/共115页774.3非线性动态系统的滤波理论与算法本小节的主要内容有：扩展Kalman滤波（EKF）UKF滤波(无迹（Unscented）卡尔曼滤波

)第77页/共115页784.3非线性动态系统的滤波理论与算法

——扩展Kalman滤波器（EKF）

扩展Kalman滤波器（EKF）考虑离散时间非线性动态系统：(4.4.1)

(4.4.2)

其中是时间指标，是时刻的系统状态向量，

是系统状态演化映射，而是n维过程演化噪声，

是k时刻对系统状态的量测向量，是量测映射，而是m维量测噪声。

假定和对其变元连续可微（当用二阶扩展Kalman滤波时假定二阶连续可微），同时假定初始状态为任意分布，具有均值和协方差矩阵分别为(4.4.3)

第78页/共115页794.3非线性动态系统的滤波理论与算法

——扩展Kalman滤波器（EKF）

过程噪声是一个零均值的独立过程，分布任意，具有协方差矩阵为（4.44）量测噪声也是一个零均值的独立过程，分布任意，具有协方差矩阵为（4.45）过程噪声、量测噪声以及初始状态之间都相互独立。扩展Kalman滤波算法实质上是一种在线线性的算法，即按名义轨线进行线性化处理，再利用Kalman滤波公式进行计算。这种算法已经不再是按某个指标进行优化的最优化算法，其性能取决于非线性系统的复杂度以及算法的优劣等。第79页/共115页804.2卡尔曼滤波——正交投影推导一阶EKF算法的步骤描述如下：（1）在时刻，假定已经获得时刻的状态估计值和估计误差的协方差阵(4.4.6)而此时对演化方程的线性化方程为(4.4.7)其中(4.4.8)(4.4.9)4.3非线性动态系统的滤波理论与算法

——扩展Kalman滤波器（EKF）第80页/共115页814.2卡尔曼滤波——正交投影推导4.3非线性动态系统的滤波理论与算法

——扩展Kalman滤波器（EKF）（2）对时刻状态的一步提前预测(4.4.10)状态预测误差是

(4.4.11)状态预测误差的协方差阵是(4.4.12)（3）对时刻量测的线性化量测方程(4.4.13)其中，(4.4.14)第81页/共115页824.2卡尔曼滤波——正交投影推导4.3非线性动态系统的滤波理论与算法

——扩展Kalman滤波器（EKF）(4.4.15)（4）对时刻量测的一步提前预测为(4.4.16)量测预测误差是

(4.4.17)量测预测误差的协方差阵是

(4.4.18)状态预测误差与量测预测误差的协方差阵是(4.4.19)第82页/共115页834.2卡尔曼滤波——正交投影推导在时刻

k得到新的量测状态滤波的更新公式

(4.4.20)预测误差的协方差阵是(4.4.21)而k时刻Kalman增益阵为(4.4.22)二阶EKF算法的步骤描述如下：(1)在时刻假定已经获得时刻的状态估计值和估计误差的协方差阵(4.4.23)4.3非线性动态系统的滤波理论与算法

——扩展Kalman滤波器（EKF）第83页/共115页844.2卡尔曼滤波——正交投影推导4.3非线性动态系统的滤波理论与算法

——扩展Kalman滤波器（EKF）而此时对演化方程的线性化方程为(4.4.24)其中是第个标准基向量，是第分量，而(4.4.25)(4.4.26)(4.4.27)第84页/共115页854.2卡尔曼滤波——正交投影推导4.3非线性动态系统的滤波理论与算法

——扩展Kalman滤波器（EKF）（2）对时刻

k状态的一步提前预测

(4.4.28)其中表示矩阵求迹，状态预测误差是(4.4.29)

状态预测误差的协方差阵是(4.4.30)（3）对k时刻量测的线性化量测方程(4.4.31)第85页/共115页864.2卡尔曼滤波——正交投影推导4.3非线性动态系统的滤波理论与算法

——扩展Kalman滤波器（EKF）其中是第个标准基向量，是

的第分量，而(4.4.32)(4.4.33)(4.4.34)（4）对时刻k量测的一步提前预测(4.4.35)量测预测误差是

第86页/共115页874.2卡尔曼滤波——正交投影推导4.3非线性动态系统的滤波理论与算法

——扩展Kalman滤波器（EKF）(4.4.36)量测预测误差的协方差阵是(4.4.37)状态预测误差与量测预测误差的协方差阵是(4.4.38)（5）在时刻k得到新的量测，状态滤波的更新公式(4.4.39)预测误差的协方差阵是(4.4.40)而时刻k的Kalman增益阵为(4.4.41)第87页/共115页88884.3非线性动态系统的滤波理论与算法—UKF滤波UKF滤波(无迹（Unscented）卡尔曼滤波)虽然把扩展Kalman滤波应用于非线性系统状态估计已经得到学术界和工程界的认可，但这种应用却存在着明显的缺陷。因为EKF为了求取估计误差协方差的传播，将动力学模型在当前状态估值处进行Taylor展开线性化，并将测量模型在状态一步预测处进行Taylor展开线性化，而基于Taylor级数展开的方法存在函数的整体特性（它的平均值）被局部特性（其导数）所代替的缺点，而且噪声的存在使之进一步恶化。因此，按Taylor级数进行高阶展开的可用性也值得怀疑。为提高近似迫近的精度。为了改善对非线性问题进行滤波的效果，Julier等人提出了采用基于unscented变换的unscentedKalmanfilter（UKF）方法。该方法在处理状态方程时，首先进行第88页/共115页894.3非线性动态系统的滤波理论与算法—UKF滤波无迹（Unscented）卡尔曼滤波

Julier和Uhlman等利用无迹变换（UT，UnscentedTransform）变换方法，用一组确定的取样点来近似后验概率，提出了Unscented卡尔曼滤波（UnscentedKalmanFilter）。

UKF不线性化非线性状态方程和观测方程，直接利用非线性状态方程来估算状态向量的概率密度函数（pdf），规定一组确定的取样点，当状态向量的概率密度函数是高斯型的，利用这组取样点能获取高斯密度函数的均值和协方差。当高斯型状态向量经由非线性系统进行传递时，对任何一种非线性系统，利用这组取样点能获取精确到三阶矩的后验均值和协方差。与EKF相比，UKF的误差仅仅出现在三阶以上的矩中，而EKF仅仅精确到一阶矩。第89页/共115页90904.3非线性动态系统的滤波理论与算法—UKF滤波无迹（Unscented）变换unscented变换（简称U变换），然后使用U变换后的状态变量进行滤波估计，以减小估计误差。首先，考虑如下非线性模型(4.4.42)(4.4.43) 在进行滤波前先作如下假设：过程噪声和测量噪声为互不相关零均值白噪声，且过程噪声

具有协方差阵

，量测噪声

具有协方差阵

，初始状态

与所有噪声独立，其先验均值和协方差阵是 ， (4.4.44)第90页/共115页91914.3非线性动态系统的滤波理论与算法—UKF滤波 定义2.4.1设n维随机向量

，m维随机向量 z为x的某一非线性函数 。

(4.4.45) x的统计特性是，通过非线性函数进行传播得到z的统计特性。Unscented变换（UT）就是根据，设计一系列的点称其为 点;对设定的点计算其经过传播所得的结果 然后基于，计算。 通常点的数量取为2n+1，即L=2n。第91页/共115页92924.3非线性动态系统的滤波理论与算法—UKF滤波 UT与线性化方法的比较可用图2.4.1表示，UT的具体过程可描述如下： 计算2n+1个点及其权系数

(4.4.46) (4.4.47)第92页/共115页93934.3非线性动态系统的滤波理论与算法—UKF滤波