第7章_卡尔曼滤波ppt课件

1,第7章卡尔曼滤波,2,本章要回答的问题是：采用迭代方法直接利用观测数据进行运算，可得到原系统状态向量的估计。,介绍新息过程的概念然后导出卡尔曼滤波算法，讨论其统计性能和推广介绍卡尔曼滤波在目标跟踪等方面的应用,3,7.1基于新息过程的递归最小均方误差估计,7.1.1标量新息过程及其性质,图7.1.1阶线性预测器结构,4,在最小均方误差意义下，当式中的权向量满足维纳-霍夫方程时，预测的均方误差最小，为最佳线性预测。,由上图获得的预测误差为,新息过程,定义为线性预测器在最小均方误差意义下的预测误差。,5,从新息过程定义可知，就是在最小均方意义下的一步预测误差，根据维纳滤波的正交原理（估计误差与输入信号向量正交），与输入信号向量正交，因此，包含了存在于当前观测样本中的新的信息，“新息”的含义即在于此。,6,根据正交原理，新息过程具有如下的统计特性:(1),(2),(3),序列,和,包含了相同的信息，即,7,性质(1)和(2)可由维纳滤波的正交原理证明得到，下面对性质(3)进行推导：,令,8,再令,9,有,10,将上式改写成矩阵的形式，有,其中,11,7.1.2最小均方误差估计的新息过程表示,维纳滤波的期望响应,的最小均方误差估计可表示为,根据新息过程性质(3)，上式可表示为,12,可以证明,是输入为,时，最小均方误差准则下的,最佳权向量，即满足维纳-霍方程,其中矩阵,为,的自相关矩阵，,为互相关向量。,根据新息过程的正交性有,13,容易解出权向量,的各元素为,其中，,为互相关向量,的第,个元素。,14,于是有迭代关系,上式表明:以新息过程作为维纳滤波器的输入，若,时刻期望响应,的估计值,已获得,，则可按上式的迭代方法计算出n时刻期望响应,的估计值,，这种方法带来计算上的,极大方便。,15,7.1.3向量新息过程及其性质,输入信号向量,新息过程向量,在,时刻，对输入向量,的最佳线性预测向量,可表示为,16,将标量新息过程的性质推广到向量新息过程，为:,17,(3),观测数据向量序列,和新息过程向量,序列,另一个序列，而不丢失任何信息。即,之间存在着一一对应关系，,可以借助可逆线性变换从其中一个序列得到,18,7.2系统状态方程和观测方程的概念,一个多输入多输出的离散时间LTI系统，可用状态方程和输出方程进行描述:,个输入,个状态变量,状态方程,19,将其表示为向量的形式，为,为状态变量,为输入向量,为状态转移矩阵,为输入控制矩阵,20,系统输出方程,个输出,表示为向量形式，为,为输出向量，,为状态输出矩阵，,为输出控制矩阵。,21,对于一个受到随机干扰的系统，系统状态方程和输出方程可分别表示为,状态方程,系统输出方程,其中，,和,分别为系统状态噪声,和输出噪声，,为状态噪声输入矩阵。,对于一般的时变系统，系统状态方程和观测方程,可表示为以下的形式：,22,1状态（过程）方程（stateequation）,状态向量：,；,状态转移矩阵：,状态噪声输入矩阵：,系统状态噪声：,其中，系统的状态转移矩阵,描述了系统状态,从,时刻到,时刻的变化规律，它有这样的一些,特点。,23,(1)乘积律,并可类推为,(2)求逆律,且有,24,系统状态噪声,通常为随机过程向量，并假定,为零均值白噪声，其相关矩阵满足,上式表明，不同时刻间的状态噪声是统计独立的（即白噪声）；但并未强调同时刻不同状态噪声间的统计独立性，若同时刻不同状态噪声间也是统计独立的，则矩阵,是对角阵。,25,实际应用中，也可将噪声输入矩阵与状态噪声的乘积,看成一个向量，若仍用,表示，此时状态方程可表示为,(7.2.12),26,2观测（测量）方程（measurementequation）,观测向量：,观测矩阵:,观测噪声：,与状态方程类似，观测噪声,通常也假定,为零均值白噪声，其相关矩阵为,27,同样，不同时刻间的观测噪声是统计独立的；若同时刻不同观测噪声间也是统计独立的，则矩阵,是对角阵。,由于系统状态噪声和观测噪声是在系统的不同阶段引入的，它们之间是统计独立的，即有,上面给出的相关矩阵都是与时间有关的量，因此，可以用来描述非平稳的系统状态噪声和测量噪声。图7.2.1给出了系统状态方程和观测方程的结构。,28,图7.2.1系统状态方程与观测方程结构图,系统状态方程是我们对此变化规律的一种假设。通常，状态方程不可能和系统变化规律完全相符合。因此，已知状态向量初始值通过递推是无法有效提取系统状态的。,29,(1),称为预测问题；,(2),(3),称为滤波或者估计问题；,称为平滑问题。,下面的讨论将集中在卡尔曼滤波问题上。,30,7.3卡尔曼滤波原理,7.3.1状态向量的最小均方误差估计,为基于观测向量集合,对,时刻的状态向量,的最小均方误差估计，可表示为,其中，,是新息过程向量。,在此系统状态已经,由标量拓展到,维向量，所以,为待求的,最佳权矩阵。,31,时刻的状态向量估计误差记为,可得互相关矩阵,由正交原理可知，在MMSE意义下，状态向量估计误差和新息向量是正交的，因而有,32,由新息过程的统计特性可知,由此可得,(7.3.3),33,另外，根据式(7.3.1)有：,进一步将上式表示为,(7.3.6),34,其中,可以看出，基于观测向量集合,对系统状态,的估计，,可在,的基础上，,利用新息向量,进行修正。,其中，矩阵,称为卡尔曼增益（Kalmangain）,下面将讨论增益矩阵,和新息过程自相关矩阵,的计算方法。,(7.3.7),35,7.3.2新息过程的自相关矩阵,设观测序列的新息过程向量为,预测值间也应满足观测方程,(7.3.8),(7.3.9),由于,所以,于是有,(7.3.11),将上式代入式(7.3.8)可得,36,定义状态预测误差向量,则新息过程又可表示为,(7.3.12),(7.3.13),(7.3.14),(7.3.15),37,定义一步状态预测误差自相关矩阵,为,(7.3.16),(7.3.17),38,7.3.3卡尔曼滤波增益矩阵,上节给出的卡尔曼增益矩阵含有未知的系统状态，,因而并不能直接使用，下面来导,的求解方法。,(7.3.18),(7.3.19),由式，有：,又由式，可得：,39,因此有,于是,此外，由正交原理可知,故有,(7.3.20),将上式代入可得卡尔曼滤波增益为,40,(7.3.21),图7.3.1计算卡尔曼增益的结构图,41,7.3.4卡尔曼滤波的黎卡蒂方程,通过上节已知，为求得卡尔曼滤波增益，需得到状态,预测误差的自相关矩阵，,本小节将介绍利用递推的方法,计算,定义状态估计误差向量,(7.3.22),定义状态估计误差的自相关矩阵,(7.3.23),由于,可得状态预测方程,(7.3.24),因此，状态预测误差可表示为,求上式的自相关矩阵可得,(7.3.25),上式称为黎卡蒂差分方程（Riccatidifferenceequation）,另外,因此,对上式求相关矩阵可得,(7.3.26),至此，得到了卡尔曼滤波递推算法的所有方程。,44,(1)计算下一时刻状态的预测值及状态预测误差自相关矩阵,(2)计算新息过程、新息自相关矩阵和卡尔曼增益,45,(3)计算下一时刻状态估计值和估计误差自相关矩阵,(4)令，从(1)再次循环。,46,卡尔曼增益和状态估计误差自相关矩阵另外的表达形式,(7.3.28),(7.3.27),其中，式(7.3.27)和式(7.3.21)是等价的；,式(7.3.28)和式(7.3.26)是等价的,47,7.3.5卡尔曼滤波计算步骤,算法7.1（卡尔曼滤波算法）,已知条件,48,初始条件,步骤1,状态一步预测,步骤2,新息过程,49,步骤3,一步预测误差自相关矩阵,步骤4,新息过程自相关矩阵,步骤5,卡尔曼增益,50,步骤6,状态估计,步骤7,状态估计误差自相关矩阵,步骤8,重复以上步骤17，进行递推滤波计算。,51,图7.3.2卡尔曼滤波算法递推流程框图,52,7.4卡尔曼滤波的统计性能,上一节从新息过程出发推导得出了卡尔曼滤波算法，但并未讨论算法的估计结果是否满足最小均方误差准则。本节中将验证，卡尔曼滤波算法满足最小均方误差准则。并将看到，从新息过程出发正是基于最小均方误差准则，两者只是解决问题的出发点不同，其结论是一致的。,53,7.4.1卡尔曼滤波的无偏性,由,可得,54,根据,可得,且,有,55,由以上递推方程可知，为使估计误差的均值为零，还需,因此，在卡尔曼滤波算法中状态估计的初值常设为,则,则卡尔曼滤波算法所得的估计值就是无偏的。,7.4.2卡尔曼滤波的最小均方误差估计特性,根据最小均方误差准则，在,时刻，算法获得的,状态估计均方误差取得极小值,(7.4.7),为验证卡尔曼滤波是否为最小均方误差估计，将上式看成,的函数，计算使均方误差,最小的,是否与卡尔曼增益矩阵具有相同的形式。,57,由定义可得,有,(7.4.8),对新息过程自相关矩阵进行Cholesky分解，得,同时，选取矩阵,满足,根据数学中常用的“配方法”思想，有,即,(7.4.9),(7.4.10),(7.4.11),59,由矩阵理论知识可知,将式(7.4.9)和式(7.4.10)代入上式，得,60,进一步，将式(7.4.11)和上式分别代入(7.4.8)，可得,利用矩阵迹的性质,，有,与卡尔曼增益矩阵有关的只有第二项，选取,使之,为零矩阵，就可以使,最小。,61,因此有,由于,将式(7.4.9)和式(7.4.10)代入，可得,62,当增益满足上式时，代价函数取得最小值。对比式(7.3.21)发现，该式与通过新息过程推导出来的卡尔曼增益矩阵是一致的，因此，卡尔曼滤波算法是满足最小均方误差准则的。,63,卡尔曼滤波在雷达目标跟踪中的应用,当卡尔曼滤波应用于目标跟踪时，用系统状态方程来描述目标的运动特性，其中的状态向量通常由目标的位置、速度和（或）加速度参量构成；观测方程中的观测向量则由雷达测得的目标运动参量构成。,64,例假设被跟踪目标在二维空间中运动，它从初始位置,出发，在,和,方向分别以,和,的速度进行匀速直线运动。观测噪声的统计特,性由,和,方向的观测噪声标准差描述，分别为,。试用卡尔曼滤波算法实现对该目标的跟踪，,给出目标预测和估计的位置和速度方差。其中的系统,过程噪声假设为零均值的高斯白噪声，自相关矩阵取为,对于匀速直线运动目标，在没有任何扰动的情况下，满足,若将两方向的干扰分别记为,和,对于匀速直线运动目标，各方向的干扰可视为相应方向的加速度。因此,解：,将目标,时刻在两方向的位置分别记为,和,速度分别记为,和,66,(7.6.4),(7.6.5),(7.6.6),根据式(7.6.3)式(7.6.6)，可以获得如下系统状态方程,(7.6.7),其中各参数分别为,(7.6.3),67,状态向量：,状态转移矩阵：,系统过程噪声输入矩阵：,68,系统过程噪声：,观测方程为,其中各参数分别为,观测向量：,观测矩阵：,69,过程噪声和观测噪声的自相关矩阵分别为,在给定系统状态方程和观测方程下，为进行卡尔曼滤波，需给出状态估计和估计误差自相关矩阵的初始值，下面给出目标跟踪时，工程上常用的状态向量估计初始化方法。,70,两坐标雷达中，状态向量由目标的位置和速度分量构成，即,可利用前两个时刻的观测值和来确定该向量估计的初始状态,71,状态估计误差自相关矩阵按下式进行初始化,其中，,、,和,为观测噪声自相关矩阵,的元素。,72,在本例中，,若状态向量由目标的位置、速度和加速度分量构成，即,其中，,和,分别表示,时刻目标在两个方向上,的加速度，该向量估计的初始状态需要利用前三个时刻的观测值来确定,73,74,估计误差自相关矩阵按下式进行初始化,其中,75,同样，,、,和,为观测噪声自相关矩阵,的元素。在上述两种情况下，滤波的迭代过程,分别从,和,时刻开始。,本例中状态向量由目标的位置和速度分量构成，因此，利用前两个时刻的观测值便可初始化状态估计，以下给出的是100次蒙特卡罗仿真的平均结果。,76,图7.6.3(a)x方向的预测和估计位置方差,77,图7.6.3(b)x方向的预测和估计速度方差,78,图7.6.3(c)y方向的预测和估计位置方差,79,图7.6.3(d)y方向的预测和估计速度方差,80,卡尔曼滤波在数据融合中的应用,数据融合(datafusion)是针对使用多个或多类传感器的系统而开展的一种信息处理新方法。按照信息提取的层次，融合可以分成五级，包括检测级融合、位置级融合、属性级融合、态势评估与威胁估计。卡尔曼滤波算法是实现多传感器位置融合的主要技术手段之一，根据位置融合系统结构的不同，利用卡尔曼滤波理论对多传感器数据进行最优融合有两种途径：集中式滤波和分布式滤波。,81,图7.6.8集中式融合系统,82,集中式融合系统，融合单元可以利用所有传感器的观测信息进行状态估计。假设目标运动规律可由卡尔曼滤波状态方程描述，且第个传感器的观测方程为,同时，将各传感器的观测方程组合在一起，用一个方程表示，可以得到如下的广义观测方程,集中式融合系统利用一个滤波器集中处理所有观测数据，无任何信息损失，因此，可以获得最优估计。但随着传感器数目的增多，运算量也会相应增大，当传感器数目达到一定时，将无法保证滤波器的实时性。,83,图7.6.9分布式融合系统,84,图7.6.9所示的分布式融合系统中，各个传感器首先要进行局部滤波（与融合单元的滤波相比，各分布传感器的滤波称为局部滤波），并将当前的状态估计结果送至融合单元，融合单元对各传感器提供的局部估计进行融合给出最终的全局估计。局部滤波实质为分布传感器的卡尔曼滤波过程，而融合单元输出的全局估计是局部估计