第七讲信息融合状态估计-卡尔曼滤波课件

第七讲第七讲状态估计状态估计卡尔曼滤波卡尔曼滤波状状态估估计的主要内容的主要内容应用：用：通通过数学方法数学方法寻求与求与观测数据最佳数据最佳拟合的状合的状态向量。向量。1、确定运、确定运动目目标的当前位置与速度；的当前位置与速度；2、确定运、确定运动目目标的未来位置与速度；的未来位置与速度；3、确定运、确定运动目目标的固有特征或特征参数。的固有特征或特征参数。2状状态估估计主要内容主要内容：位置与速度估：位置与速度估计。位置估位置估计：距离、方位和高度或仰角的估：距离、方位和高度或仰角的估计；速度估速度估计：速度、加速度估：速度、加速度估计。3状状态估估计的主要方法的主要方法 1、-滤波波 2、-滤波波 3、卡、卡尔曼曼滤波波这些方法些方法针对匀速或匀加速目匀速或匀加速目标提出，如目提出，如目标真真实运运动与与采用的目采用的目标模型模型不一致，不一致，滤波器波器发散。散。4算法的改算法的改进及适及适应性性状状态估估计难点：点：机机动目目标的跟踪的跟踪1、自适、自适应-滤波和自适波和自适应Kalman滤波均改善波均改善对机机动目目标的跟踪能力。的跟踪能力。2、扩展展Kalman滤波波针对卡卡尔曼曼滤波在笛卡儿波在笛卡儿坐坐标系中才能使用的局限而提出。系中才能使用的局限而提出。5卡卡尔曼曼滤波器波器卡卡尔曼曼滤波器的波器的应用用:通信、雷达、通信、雷达、导航、自航、自动控制等控制等领域；域；航天器的航天器的轨道道计算、雷达目算、雷达目标跟踪、生跟踪、生产过程的程的自自动控制等。控制等。6卡卡尔曼曼滤波器的波器的应用特点用特点对机机动目目标跟踪中具有良好的性能；跟踪中具有良好的性能；为最佳估最佳估计并能并能够进行行递推推计算；算；只需当前的一个只需当前的一个测量量值和前一个采和前一个采样周期的周期的预测值就能就能进行状行状态估估计。7卡卡尔曼曼滤波器的局限性波器的局限性卡卡尔曼曼滤波器解决运波器解决运动目目标或或实体的状体的状态估估计问题时，动态方程和方程和测量方程均量方程均为线性性。8一、数字一、数字滤波器作估波器作估值器器 1、非、非递归估估值器器 2、递归估估值器器 9 1、非、非递归估估值器器 采采样平均估平均估值器：器：采用采用时域分析方法在域分析方法在掺杂有噪声的有噪声的测量信号中量信号中估估计信号信号x。10 根根据据数数字字信信号号处处理理我我们们知知道道，所所谓谓非非递递归归数数字字滤滤波波器器是是一一种种只只有有前前馈馈而而没没有有反反馈馈的的滤滤波波器器，它它的的冲冲击击脉脉冲冲响响应应是是有有限限的的，在许多领域有着广泛的应用。在许多领域有着广泛的应用。假定用假定用zk表示观测值，表示观测值，zk=x+nk 式中：式中：x 恒定信号或称被估参量恒定信号或称被估参量 nk 观测噪声采样观测噪声采样 假定，假定，E(x)=x0，D(x)=2x，E(nk)=0，E(n2k)=2n。11 h1,h2,hm是是滤滤波波器器的的脉脉冲冲响响应应hj的的采采样样，或或称称滤滤波波器器的的加权系数。滤波器的输出加权系数。滤波器的输出 当当h1=h2=hm=1/m时，时，该该式式表表明明，估估计计 是是用用m个个采采样样值值的的平平均均值值作作为为被被估估参参量量x的的近近似值的，故称其为采样平均估值器。似值的，故称其为采样平均估值器。12 估计的均方误差以估计的均方误差以P表示，有表示，有 当当i=j时时ij=1，当，当ij时时ij=0，有，有 最后得：最后得：13结论估估计值是用是用m个采个采样值的平均的平均值作作为被被估参量估参量x的近似的近似值；估估值器的均方器的均方误差随着差随着m的增加而减少；的增加而减少；该估估值器是一个无偏估器是一个无偏估值器。器。142、递归估估值器器一一阶递归估估值器：器：a为滤波器的加波器的加权系数，系数，a1。15 递递归归数数字字滤滤波波器器是是一一种种带带有有反反馈馈的的滤滤波波器器，它它有有无无限限的的脉脉冲冲响响应应，有有阶阶数数少少的的优优点点，但但其其暂暂态态过过程程较较长长。关关于于信信号号和和噪噪声声的的基基本本假假设设与与非非递递归归情情况况相相同同。上上图图给给出出的的一一阶阶递递归归滤滤波波器器输入输出信号关系如下：输入输出信号关系如下：式式中中，zk与与非非递递归归情情况况相相同同；a是是一一个个小小于于1的的滤滤波波器器加加权权系系数数，如果它大于或等于如果它大于或等于1，该滤波器就不稳定了。该滤波器就不稳定了。16k时刻的输出：时刻的输出：yk=ak-1z1+ak-2z2+azk-1+zk 将将zk中的信号和噪声分开，并代入，有输出中的信号和噪声分开，并代入，有输出 由由于于a1，故故随随着着k值值的的增增加加，yk趋趋近近于于x/(1-a)。这这样样，如如果以果以(1-a)yk作为作为x的估计值，的估计值，则则 17 此时信号此时信号x和估值之间只差一个噪声项。当和估值之间只差一个噪声项。当k值较大时，值较大时，估值的均方误差估值的均方误差 而一次取样的均方误差而一次取样的均方误差 故上一结果的均方误差约为一次采样的（故上一结果的均方误差约为一次采样的（1-a）/（1+a）倍。）倍。18二、二、线性均方估性均方估计1、最、最优非非递归估估计（标量量维纳滤波）波）2、递归估估计19 1.最优非递归估计最优非递归估计 非递归滤波器的估计值及其估计误差可分别表示为非递归滤波器的估计值及其估计误差可分别表示为 20 对对m个个参参数数逐逐一一求求导导，令令等等于于零零，在在均均值值为为零零的的白白噪噪声声的的情情况下，可得到最小均方误差和估计：况下，可得到最小均方误差和估计：其其中中，b=2n/2x，在在bm时时，这这种种估估计计近近似似于于采采样样平平均均。在在噪噪声声方方差差2n较较大大时时，其其性性能能明明显显优优于于非非最最佳佳情情况况。这这种种最最小小均均方方误差准则下的线性滤波，通常称作标量维纳滤波。误差准则下的线性滤波，通常称作标量维纳滤波。hj与非最优情况的不同，这里的滤波器的加权系数为与非最优情况的不同，这里的滤波器的加权系数为21 2、由最优非递推估计导出递归估计、由最优非递推估计导出递归估计由前可知，由前可知，非递归估值器可以表示为非递归估值器可以表示为 条件与前面相同。对条件与前面相同。对k+1次取样，相应的估计量次取样，相应的估计量 相应的估计误差相应的估计误差 22由由b=2n/2x及及hi(k)=1/（k+b），有），有 所以有所以有 23于是于是,分成二项：分成二项：将第一项同时乘、除一个将第一项同时乘、除一个bk，则，则 24或或 最后有最后有 25最最优递归估估计器器递推公式递推公式26最最优递归估估计器器递推公式递推公式27 递推开始时的初始条件递推开始时的初始条件 应满足应满足：以使以使 为最佳值。为最佳值。解之，得解之，得，这时的，这时的 如果如果E(x)=0，可从零开始递推运算，即，可从零开始递推运算，即 28三、三、标量卡量卡尔曼曼滤波器波器时变信号信号主要作用：主要作用：对掺杂有噪声的随机信号有噪声的随机信号进行行线性估性估计。29 1、模型、模型 1）信号模型信号模型 设设要要估估计计的的随随机机信信号号为为由由均均值值为为0，方方差差为为2w的的白白噪噪声声激激励的一个一阶递归过程，即信号对时间变化满足动态方程：励的一个一阶递归过程，即信号对时间变化满足动态方程：x(k)=ax(k-1)+w(k-1)式中，式中，a系统参数；系统参数；w(k-1)白噪声采样。白噪声采样。如果令如果令x(0)=0，Ew(k)=0，则则 30该过程该过程 称作一阶自回归过程。称作一阶自回归过程。x(k)的均值和方差分别为：的均值和方差分别为：自相关函数自相关函数 312）观测模型观测模型观测模型由下式给出：观测模型由下式给出：z(k)=cx(k)+v(k)式中：式中：c测量因子；测量因子；v(k)E()=0，D()=2n的白噪声。的白噪声。最优递推估值器的信号和观测模型如图所示。最优递推估值器的信号和观测模型如图所示。32最优递推估值器的信号和观测模型最优递推估值器的信号和观测模型 33 2、标量卡尔曼滤波器、标量卡尔曼滤波器由前将递归估计的形式写成：由前将递归估计的形式写成：均方误差均方误差 分别对分别对a(k)和和b(k)求导，并令其等于求导，并令其等于0，求其最佳估计，得出，求其最佳估计，得出a(k)与与b(k)的关系：的关系：a(k)=a1-cb(k)最后有递归估值器：最后有递归估值器：34b(k)为滤波器增益为滤波器增益 其中其中,均方误差均方误差 对对于于给给定定的的信信号号模模型型和和观观测测模模型型，上上述述一一组组方方程程便便称称为为一一维标量卡尔曼滤波器，其结构如图所示。维标量卡尔曼滤波器，其结构如图所示。35标量卡尔曼滤波器结构标量卡尔曼滤波器结构 36 3、标量卡尔曼预测器、标量卡尔曼预测器 标标量量卡卡尔尔曼曼滤滤波波是是对对掺掺杂杂有有噪噪声声的的随随机机信信号号进进行行线线性性估估计计。但但经经常常要要对对信信号号的的未未来来值值进进行行预预测测，特特别别是是在在控控制制系系统统中中。根根据据预预测测提提前前时时间间的的多多少少，把把预预测测分分成成1步步、2步步、m步步预预测测，通通常常把把1步步预预测测记记作作 。预预测测的的步步数数越越多多，误误差差越大。越大。这里讨论这里讨论1步预测问题。步预测问题。信号模型和观测模型同前：信号模型和观测模型同前：37根据前一节，根据前一节，有一步线性预测递推公式：有一步线性预测递推公式：其其中中，a(k)和和(k)可可以以通通过过使使均均方方预预测测误误差差最最小小来来确确定定。预预测测的的均方误差可表示为均方误差可表示为 将预测方程代入该式，并求导，就会得到一组正交方程：将预测方程代入该式，并求导，就会得到一组正交方程：38解之，得解之，得 a(k)=a-c(k)将其代入预测方程，有将其代入预测方程，有 进一步可求出：进一步可求出：其中，其中，由由以以上上表表达达式式可可以以看看出出，可可根根据据均均方方预预测测误误差差P(k/k-1)计计算算(k)，然后再给出，然后再给出P(k+1/k)的均方预测误差。的均方预测误差。39最最优一步一步预测器器40最最优一步一步预测及及滤波器波器41 四、向量卡尔曼滤波器四、向量卡尔曼滤波器1、信号向量和数据向量、信号向量和数据向量 如如果果要要求求对对q个个信信号号进进行行同同时时估估计计，这这q个个信信号号在在k时时刻刻的的采采样样值值记记作作x1(k)、x2(k)、xq(k)。假假设设每每个个信信号号都都是是由由一一阶阶自回归过程产生的，自回归过程产生的，即第即第个信号在时刻个信号在时刻k的采样值为的采样值为:x(k)=ax(k-1)+w(k-1)=1，2，q 每每个个w过过程程都都是是白白的的，零零均均值值的的，与与其其它它过过程程的的采采样样是是独独立立的的。于是把于是把q个信号与个信号与q个白噪声组成的个白噪声组成的q维向量分别表示成维向量分别表示成 42显然，显然，X(k)=AX(k-1)+W(k-1)式中，式中，X(k)，X(k-1)，W(k-1)都是都是q维向量，维向量，A是个是个qq阶矩阵，即阶矩阵，即 如果信号不满足一阶递归差分方程，而满足二阶递归差分如果信号不满足一阶递归差分方程，而满足二阶递归差分方程，即方程，即 x(k)=ax(k-1)+bx(k-2)+w(k-1)43定义两个分量定义两个分量 x1(k)=x(k)x2(k)=x1(k-1)=x(k-1)于是，有于是，有 最后，有最后，有 X(k)=AX(k-1)+W(k-1)结果把一个二阶差分方程变成了一个一阶二维向量方程，结果把一个二阶差分方程变成了一个一阶二维向量方程，该该方程用起来更简单方便方程用起来更简单方便。44 用用R(k)表表示示k时时刻刻的的距距离离，R(k)表表示示k时时刻刻的的速速度度，U(k)表表示示k时刻的加速度，时刻的加速度，T表示采样周期，则表示采样周期，则 写成一般形式：写成一般形式：其中，其中，.45写成向量形式：写成向量形式：最后最后,有有 即可写成一阶向量的形式。即可写成一阶向量的形式。在在对对信信号号向向量量进进行行估估计计的的过过程程中中，同同时时产产生生r个个含含有有噪噪声声的的测量值，记作测量值，记作z1(k)，z2(k)，,zr(k)。则得到一组观测方程。则得到一组观测方程:46其中，其中，vi(k)表示附加噪声，表示附加噪声，ci表示第表示第i个测量参数，于是有个测量参数，于是有 Z(k)=CX(k)+V(k)式式中中，Z(k)，V(k)是是r维维向向量量，X(k)是是q维维向向量量，C是是rq阶阶矩矩阵阵。对于对于r=q，有，有 C即是观测矩阵。即是观测矩阵。47 2、向量问题的表示、向量问题的表示 根根据据前前面面的的讨讨论论，我我们们完完全全可可以以把把前前面面的的信信号号模模型型动动态态方方程和观测方程写成如下形式程和观测方程写成如下形式:采用标量运算和矩阵运算的等价关系，推广到多维情况：采用标量运算和矩阵运算的等价关系，推广到多维情况：48据此，可以将观测噪声的方差变成协方差矩阵据此，可以将观测噪声的方差变成协方差矩阵 对两个信号的情况，则有对两个信号的情况，则有 同理，也可以把系统噪声的方差变成协方差矩阵，同理，也可以把系统噪声的方差变成协方差矩阵，即即 由于系统噪声采样互不相关，该协方差矩阵的非对角线元素的由于系统噪声采样互不相关，该协方差矩阵的非对角线元素的值均为零。值均为零。单一信号均方误差也可变成协方差矩阵，单一信号均方误差也可变成协方差矩阵，49 3、向量卡尔曼滤波器、向量卡尔曼滤波器 利利用用前前面面的的概概念念，直直接接把把标标量量卡卡尔尔曼曼滤滤波波器器公公式式变变成成向向量量卡尔曼滤波器公式：卡尔曼滤波器公式：滤波器增益：滤波器增益：式中，式中，实际上，实际上，它是预测协方差。它是预测协方差。误差协方差矩阵：误差协方差矩阵：50用用K(k)代替了代替了B(k)，因，因K(k)是通用符号，如图：是通用符号，如图：向量卡尔曼滤波器结构向量卡尔曼滤波器结构 51增益矩阵增益矩阵K(k)的计算流程如图所示：的计算流程如图所示：增益矩阵计算流程增益矩阵计算流程 52 4、向量卡尔曼预测器、向量卡尔曼预测器根据相同的推导方法，根据相同的推导方法，可以获得卡尔曼预测器方程组。可以获得卡尔曼预测器方程组。预测方程：预测方程：预测增益预测增益:预测均方误差预测均方误差:它它们们与与标标量量的的情情况况是是一一一一对对应应的的，只只是是用用G(k)代代替替了了(k)。就可以将滤波和预测用同一个方框图表示出来。就可以将滤波和预测用同一个方框图表示出来。53 5、总结、总结 卡尔曼滤波器应用广泛，卡尔曼滤波器应用广泛，这里只对其进行简单归纳。这里只对其进行简单归纳。1）卡尔曼滤波器的主要特性卡尔曼滤波器的主要特性 卡卡尔尔曼曼滤滤波波器器是是一一个个递递归归、线线性性、无无偏偏和和方方差差最最小小的的滤滤波波器器，如如果果过过程程噪噪声声和和观观测测噪噪声声是是正正态态高高斯斯白白噪噪声声，则则它它保保持持最最佳特性。佳特性。542）卡尔曼滤波器模型卡尔曼滤波器模型目标运动模型：目标运动模型：位置测量模型：位置测量模型：55状态方程状态方程:X(t+T)=(t)X(t)+W(t)Q(t)=EW(t)W(t)T 观测方程观测方程:Z(t)=HX(t)+V(t)R(t)=EV(t)V(t)T 563)卡尔曼滤波器方程组卡尔曼滤波器方程组残差残差:预测方程预测方程:状态估计状态估计:卡尔曼滤波器增益卡尔曼滤波器增益:57 预测协方差预测协方差:估计协方差估计协方差:58 五、卡尔曼滤波器的应用五、卡尔曼滤波器的应用 1.系统矩阵系统矩阵 假假定定系系统统矩矩阵阵是是四四维维矩矩阵阵，即即距距离离、速速度度、方方位位角角及及其其变变化化率率,它它们们分分别别由由R，和和 表表示示，距距离离方方向向上上的的加加速速度度和和角度方向的加速度分别由角度方向的加速度分别由ur(k)和和u(k)表示。状态方程为表示。状态方程为 59则系统方程为则系统方程为 用标准符号用标准符号x1，x2，x3,x4分别表示分别表示 R，R,。式中，。式中，A为系统为系统矩阵，矩阵，W(k)为噪声项。为噪声项。.60 2.观测矩阵观测矩阵 假假定定观观测测值值只只有有距距离离和和方方位位两两个个，即即R和和，分分别别用用z1和和z2来来表表示示。它它们们是是由由状状态态值值和和测测量量噪噪声声组组成成的的，且且测测量量噪噪声声是是相互独立的零均值的白噪声。相互独立的零均值的白噪声。测量方程：测量方程：则有则有 61其中，其中，x1(k)=r(k)，x3(k)=(k)。以上两个问题实际上是建立模型问题。以上两个问题实际上是建立模型问题。62 3.观测噪声协方差矩阵观测噪声协方差矩阵 在计算滤波器增益时，需知观测噪声的协方差矩阵。由于在计算滤波器增益时，需知观测噪声的协方差矩阵。由于只有两个参数，因此只有两个参数，因此 这里利用了方位和距离观测噪声相互独立的条件，故左下角和这里利用了方位和距离观测噪声相互独立的条件，故左下角和右上角项为零。右上角项为零。63 4.系统噪声协方差矩阵系统噪声协方差矩阵 假假定定目目标标作作匀匀速速运运动动，由由于于大大气气湍湍流流等等因因素素的的影影响响，目目标产生随机加速度，在距离和方位上都存在随机扰动标产生随机加速度，在距离和方位上都存在随机扰动,于是有于是有 且且 64因为因为 得输入扰动的协方差矩阵得输入扰动的协方差矩阵 65 5.滤波器的初值滤波器的初值 滤滤波波器器初初始始化化时时。先先利利用用一一种种比比较较简简单单的的方方法法确确定定 ，可可利利用用时时刻刻1和和时时刻刻2两两点点的的距距离离和和方方位位测测量量值值，即即z1(1)，z1(2)，z2(1)，z2(2)，建立建立 ，而忽略随机加速度。，而忽略随机加速度。666.均方误差矩阵均方误差矩阵由滤波器初值，有误差矢量由滤波器初值，有误差矢量 67从而，从而，68初始误差的协方差矩阵初始误差的协方差矩阵 69由于由于u，v相互独立，且各噪声采样之间也独立，相互独立，且各噪声采样之间也独立，则则 式中式中,70这样，所需要的参数均已具备，可以进行迭代运算了。这样，所需要的参数均已具备，可以进行迭代运算了。71小小结72