25状态估计-卡尔曼滤波解析课件.ppt

1、智能信息处理技术智能信息处理技术第第5 5讲讲状态估计状态估计卡尔曼滤波卡尔曼滤波NUST自动化学院自动化学院智能信息处理技术智能信息处理技术2 应用：通过数学方法寻求与观测数据最佳拟合的状态向量。通过数学方法寻求与观测数据最佳拟合的状态向量。1、确定运动目标的当前位置与速度；2、确定运动目标的未来位置与速度；3、确定运动目标的固有特征或特征参数。状态估计主要内容：位置与速度估计位置与速度估计位置估计：距离、方位和高度或仰角的估计距离、方位和高度或仰角的估计速度估计：速度、加速度估计速度、加速度估计NUST自动化学院自动化学院智能信息处理技术智能信息处理技术3 1、-滤波2、-滤波3、卡尔曼滤

2、波这些方法针对匀速或 匀加速目标提出，如 目标真实运动与采用 的目标 模型不致，滤波器发散。状态估计的主要方法：算法的改进及适应性状态估计难点：机动目标的跟踪机动目标的跟踪1、自适应-滤波和自适应Kalman滤波均改善对机动目标的跟踪能力。2、扩展Kalman滤波应用于时间非线性的动态系统。NUST自动化学院自动化学院智能信息处理技术智能信息处理技术4卡尔曼滤波器的应用:通信、雷达、导航、自动控制等领域航天器的轨道计算、雷达目标跟踪、生产过程的自动控制等：卡尔曼滤波器的应用特点对机动目标跟踪中具有良好的性能对机动目标跟踪中具有良好的性能为最佳估计并能够进行递归计算为最佳估计并能够进行递归计算只

3、需当前的一个测量值和前一个采样周期的预测值就能进行状态估计只需当前的一个测量值和前一个采样周期的预测值就能进行状态估计卡尔曼滤波器的局限性：卡尔曼滤波器解决运动目标或实体的状态估计问题时，动态方程和测量方程均为线性。卡尔曼滤波器解决运动目标或实体的状态估计问题时，动态方程和测量方程均为线性。NUST自动化学院自动化学院智能信息处理技术智能信息处理技术5 NUST自动化学院自动化学院智能信息处理技术智能信息处理技术6 z1z1hmh3h2h1z1z1z2z3X 采样平均估值器：采用时域分析方法在掺杂有噪声的测量信号中估计信号采用时域分析方法在掺杂有噪声的测量信号中估计信号x。NUST自动化学院自

4、动化学院智能信息处理技术智能信息处理技术7 根据数字信号处理技术，所谓非递归数字滤波器是一种只有前馈而没有反馈根据数字信号处理技术，所谓非递归数字滤波器是一种只有前馈而没有反馈的滤波器。的滤波器。zk=x+nk 式中：x 恒定信号或称被估参量 nk 观测噪声采样 假定，假定，E(x)=x0，D(x)=2x，E(nk)=0，E(n2k)=2n。假定用zk表示观测值NUST自动化学院自动化学院智能信息处理技术智能信息处理技术8h1,h2,hm是滤波器的脉冲响应hj的采样，或称滤波器的加权系数。滤波器的输出 h1=h2=hm=1/m 该式表明，估计 是用m个采样值的 平均值作为被估参 量x的近似值的

5、，故 称其为采样平均估 值器。XNUST自动化学院自动化学院智能信息处理技术智能信息处理技术9均方误差均方误差估计估计NUST自动化学院自动化学院估计值 是用m个采样值的平均值作为被估参量x的近似值；智能信息处理技术智能信息处理技术10X估值器的均方误差随着m的增加而减少；该估值器是一个无偏估值器NUST自动化学院自动化学院智能信息处理技术智能信息处理技术11z1zka1 aykkX一阶递归估值器：a为滤波器的加权系数，为滤波器的加权系数，a1。式中，zk与非递归情况相同；a是一个小于1的滤波器加权系数，如果它大于或等于1，该滤波器就不稳定了。NUST自动化学院自动化学院智能信息处理技术智能信

6、息处理技术12k时刻的输出：yk=ak-1z1+ak-2z2+azk-1+zk 将将zk中的信号和噪声分开，并代入，有输出中的信号和噪声分开，并代入，有输出 由于由于a1，故随着，故随着k值的增加，值的增加，yk趋近于趋近于x/(1-a)。这样，如果以。这样，如果以(1-a)yk作为作为x的的估计值，估计值，则则NUST自动化学院自动化学院智能信息处理技术智能信息处理技术13当当k值较大时，值较大时，估值的均方误差估值的均方误差 而一次取样的均方误差而一次取样的均方误差 故这一结果的均方误差约为一次采样的（故这一结果的均方误差约为一次采样的（1-a）/（1+a）倍。）倍。NUST自动化学院自动

7、化学院智能信息处理技术智能信息处理技术14NUST自动化学院自动化学院智能信息处理技术智能信息处理技术15 1.最优非递归估计最优非递归估计 非递归滤波器的估计值及其估计误差可分别表示为非递归滤波器的估计值及其估计误差可分别表示为 m个参数逐一求导，令等于零均值为零的白噪声b=2n/2x在bm时最优非递归估计近似于采样平均在噪声方差2n较大时性能明显优于非最佳情况 这种最小均方误差准则下的线性滤波，通常称作标量维纳滤波。NUST自动化学院自动化学院智能信息处理技术智能信息处理技术16由前可知，由前可知，非递归估值器可以表示为非递归估值器可以表示为 k+1次取样次取样误差估计误差估计误差估计误差

8、估计NUST自动化学院自动化学院智能信息处理技术智能信息处理技术17b=2n/2x及及hi(k)=1/（k+b）NUST自动化学院自动化学院智能信息处理技术智能信息处理技术18分成二项分成二项 第一项同时乘、除一个第一项同时乘、除一个bkORNUST自动化学院自动化学院智能信息处理技术智能信息处理技术19递递归归公公式式1kz1kb)1/1kb（1z1kXNUST自动化学院自动化学院智能信息处理技术智能信息处理技术201z1kz1kb1kX递递归归公公式式NUST自动化学院自动化学院为使为使 最佳，递最佳，递推初始条件推初始条件智能信息处理技术智能信息处理技术210X若若E(x)=0从零开始递

9、推从零开始递推应用时要注意初始条件，即递推开始时的初始值应用时要注意初始条件，即递推开始时的初始值NUST自动化学院自动化学院智能信息处理技术智能信息处理技术22 主要作用：对掺杂有噪声的随机信号进行线性估计。对掺杂有噪声的随机信号进行线性估计。NUST自动化学院自动化学院智能信息处理技术智能信息处理技术23 x(k)=ax(k-1)+w(k-1)如果令如果令x(0)=0，Ew(k)=0，则则 1）信号模型设要估计的随机信号为由均值为设要估计的随机信号为由均值为0，方差为，方差为2w的白噪声激励的一个一阶递归过的白噪声激励的一个一阶递归过程，即信号对时间变化满足动态方程：程，即信号对时间变化满

10、足动态方程：式中，a系统参数 w(k-1)白噪 声采样。NUST自动化学院自动化学院智能信息处理技术智能信息处理技术24x(k)的均值和方差分别为：的均值和方差分别为：自相关函数自相关函数 NUST自动化学院自动化学院智能信息处理技术智能信息处理技术25观测模型由下式给出：观测模型由下式给出：z(k)=cx(k)+v(k)式中：c测量因子；v(k)E()=0，D()=2n的白噪声。2）观测模型NUST自动化学院自动化学院智能信息处理技术智能信息处理技术26 均方误差均方误差 分别对分别对a(k)和和b(k)求导，并令其等于求导，并令其等于0，求其最佳估计，得出，求其最佳估计，得出a(k)与与b

11、(k)的关系的关系?a(k)=a1-cb(k)最后有递归估值器：由前将递归估计的形式写成：NUST自动化学院自动化学院智能信息处理技术智能信息处理技术27滤波器增益b(k)？均方误差均方误差 对于给定的信号模型和观测模型，上述一组方程便称为一维标量卡尔曼滤波器，对于给定的信号模型和观测模型，上述一组方程便称为一维标量卡尔曼滤波器，其结构如图所示。其结构如图所示。NUST自动化学院自动化学院智能信息处理技术智能信息处理技术28标量卡尔曼滤波器结构标量卡尔曼滤波器结构 b(k)z1acz(k)(kXNUST自动化学院自动化学院标量卡尔曼滤波是对掺杂有噪声的随机信号进行线性估计。但经常要对信号的未来

12、值进行预测，特别是在控制系统中。根据预测提前时间的多少，把预测分成1步、2步、m步预测，通常把1步预测记作 。预测的步数越多，误差越大。这里讨论1步预测问题。智能信息处理技术智能信息处理技术29 )/1(kkX 信号模型和观测模型同前：NUST自动化学院自动化学院智能信息处理技术智能信息处理技术30根据前一节，有一步线性预测递推公式：其中，其中，(k)和和(k)可以通过使预测均方误差最小来确定。预测的均方误差可表示为可以通过使预测均方误差最小来确定。预测的均方误差可表示为 将预测方程代入该式，并求导，就会得到一组正交方程：NUST自动化学院自动化学院a(k)=？智能信息处理技术智能信息处理技术

13、31解之，得 a(k)=a-c(k)将其代入预测方程，有将其代入预测方程，有 进一步可求出：由以上表达式可以看出，可根据预测均方误差由以上表达式可以看出，可根据预测均方误差P(k/k-1)计算计算(k)，然后再给出，然后再给出P(k+1/k)的预测均方误差。的预测均方误差。NUST自动化学院自动化学院智能信息处理技术智能信息处理技术32)(kz1zac)(k)/1(kkXNUST自动化学院自动化学院智能信息处理技术智能信息处理技术33NUST自动化学院自动化学院智能信息处理技术智能信息处理技术34 每个每个w过程都是白的，零均值的，与其它过程的采样是独立的。过程都是白的，零均值的，与其它过程的

14、采样是独立的。于是把于是把q个个信号与信号与q个白噪声组成的个白噪声组成的q维向量分别表示成维向量分别表示成 1、信号向量和数据向量如果要求对如果要求对q个信号进行同时估计，这个信号进行同时估计，这q个信号在个信号在k时刻的采样值记作时刻的采样值记作x1(k)、x2(k)、xq(k)。假设每个信号都是由一阶自回归过程产生的，。假设每个信号都是由一阶自回归过程产生的，即第即第个个信号在时刻信号在时刻k的采样值为的采样值为:x(k)=ax(k-1)+w(k-1)=1，2，q NUST自动化学院自动化学院智能信息处理技术智能信息处理技术35显然 X(k)=AX(k-1)+W(k-1)如果信号不满足一

15、阶递归差分方程，而满足二阶递归差分方程，即如果信号不满足一阶递归差分方程，而满足二阶递归差分方程，即 x(k)=ax(k-1)+bx(k-2)+w(k-1)q维向量NUST自动化学院自动化学院智能信息处理技术智能信息处理技术36定义两个分量 x1(k)=x(k)x2(k)=x1(k-1)=x(k-1)X(k)=AX(k-1)+W(k-1)结果把一个二阶差分方程变成了一个一阶二维向量方程，结果把一个二阶差分方程变成了一个一阶二维向量方程，该方程用起来更简单方该方程用起来更简单方便。便。NUST自动化学院自动化学院智能信息处理技术智能信息处理技术37 用R(k)表示k时刻的距离，R(k)表示k时刻

16、的速度，U(k)表示k时刻的加速度，T表示采样周期，则 写成一般形式：.NUST自动化学院自动化学院智能信息处理技术智能信息处理技术38写成向量形式：在对信号向量进行估 计的过程中，同时产 生r个含有噪声的测 量值，记作z1(k)，z2(k)，,zr(k)。则 得到一组观测方程:一阶向量形式一阶向量形式NUST自动化学院自动化学院智能信息处理技术智能信息处理技术Z(k)=CX(k)+V(k)C即是观测矩阵。q维向量维向量rq阶矩阵第阶矩阵第i个个测量参数测量参数附加噪声NUST自动化学院自动化学院智能信息处理技术智能信息处理技术40 采用标量运算和矩阵运算的等价关系，推广到多维情况：根据前面的

17、讨论，我们完全可以把前面的信号模型动态方程和观测方程写成如下形式:NUST自动化学院自动化学院智能信息处理技术智能信息处理技术41据此，可以将观测噪声的方差变成协方差矩阵 对两个信号的情况，则有 同理，也可以把系统噪声的方差变成协方差矩阵，即 由于系统噪声采样互不相关，该协方差矩阵的非对角线元素的值均为零。单一信号均方误差也可变成协方差矩阵，NUST自动化学院自动化学院智能信息处理技术智能信息处理技术42 滤波器增益：实际上，它是预测协方差。误差协方差矩阵：利用前面的概念，直接把标量卡尔曼滤波器公式变成向量卡尔曼滤波器公式：NUST自动化学院自动化学院智能信息处理技术智能信息处理技术43向量卡

18、尔曼滤波器结构向量卡尔曼滤波器结构 z1Acz(k)(kXK(k)NUST自动化学院自动化学院智能信息处理技术智能信息处理技术44增益矩阵K(k)的计算流程如图所示：增益矩阵计算流程增益矩阵计算流程 P(k-1)根据k-1时刻的误差协方差矩阵p(k-1)计算预测协方差p1(k),进而计算得到滤波器增益K(k)，通过滤波器增益K(k)，预测协方差p1(k),计算出k时刻的误差协方差矩阵p(k)，为计算k+1时刻的p1(k+1)创造了条件。这样就形成了一个递归计算流程，可以计算不同时刻的增益矩阵。NUST自动化学院自动化学院智能信息处理技术智能信息处理技术45 预测增益:预测均方误差:它们与标量的

19、情况是一一对应的，只是用它们与标量的情况是一一对应的，只是用G(k)代替了代替了(k)。就可以将滤波和预测用。就可以将滤波和预测用同一个方框图表示出来。同一个方框图表示出来。预测方程：NUST自动化学院自动化学院智能信息处理技术智能信息处理技术46 卡尔曼滤波器的主要特性卡尔曼滤波器是一个递归、卡尔曼滤波器是一个递归、线性、无偏和方差最小的滤波器，线性、无偏和方差最小的滤波器，如果过程噪声和观测噪声是正态高斯白噪声，则它保持最佳特性。如果过程噪声和观测噪声是正态高斯白噪声，则它保持最佳特性。NUST自动化学院自动化学院智能信息处理技术智能信息处理技术47 卡尔曼滤波器模型卡尔曼滤波器模型 位置

20、测量模型：目标运动模型：NUST自动化学院自动化学院智能信息处理技术智能信息处理技术48状态方程: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 NUST自动化学院自动化学院假定系统矩阵是四维矩阵，即距离、速度、方位角及其变化率,它们分别由R，和 表示，距离方向上的加速度和角度方向的加速度分别由ur(k)和u(k)表示。状态方程为 智能信息处理技术智能信息处理技术49 R 1.系统矩阵NUST自动化学院自动化学院智能信息处理技术智能信息处理技术50则系统方程为 用标准符号x1(k)，x2(k)，x3(

21、k),x4(k)分别表 、。式中，A为系统矩阵，W(k)为噪声项。.()R k()R k()k()kNUST自动化学院自动化学院智能信息处理技术智能信息处理技术51 2.观测矩阵假定观测值只有距离和方位两个，即R和，分别用z1和z2来表示。它们是由状态值和测量噪声组成的，且测量噪声是相互独立的零均值的白噪声。测量方程测量方程NUST自动化学院自动化学院智能信息处理技术智能信息处理技术52x1(k)=R(k)x3(k)=(k)以上两个问题实际上是建立模型问题。NUST自动化学院自动化学院智能信息处理技术智能信息处理技术53 3.观测噪声协方差矩阵在计算滤波器增益时，需知观测噪声的协方差矩阵。由于

22、只有两个参数，因此方位和距离观测噪声相互独立的条件NUST自动化学院自动化学院智能信息处理技术智能信息处理技术54 4.系统噪声协方差矩阵假定目标作匀速运动，由于大气湍流等因素的影响，目标产生随机加速度，在距离和方位上都存在随机扰动,于是有 and系统噪声的协方差矩阵NUST自动化学院自动化学院滤波器初始化时,先利用一种比较简单的方法确定 ，可利用时刻1和时刻2两点的距离和方位测量值，即z1(1)，z1(2)，z2(1)，z2(2)，建立 ，而忽略随机加速度。智能信息处理技术智能信息处理技术55 )2(X)2(X5.滤波器的初值NUST自动化学院自动化学院智能信息处理技术智能信息处理技术566

23、.均方误差矩阵误差矢量NUST自动化学院自动化学院智能信息处理技术智能信息处理技术57初始误差的协方差矩阵 u，v相互独立，均值为0，各噪声采样之间独立迭迭代代所所需需参参数数NUST自动化学院自动化学院智能信息处理技术智能信息处理技术58 对于匀速和匀加速运动的目标，有目标运动模型 式中，w(k)为均值为0、方差为2的高斯白噪声，T 为对目标的采样周期。一、目标运动模型NUST自动化学院自动化学院智能信息处理技术智能信息处理技术59 预测方程：滤波方程：常系数-滤波器定义如下对于匀速运动的目标，可以采用均方误差最小的准则进行滤波和预测，即-和-滤波器。NUST自动化学院自动化学院智能信息处理

24、技术智能信息处理技术60滤波方程和预测方程也可以分别写成如下形式：NUST自动化学院自动化学院智能信息处理技术智能信息处理技术61常系数-滤波器定义如下：预测方程：滤波方程：NUST自动化学院自动化学院智能信息处理技术智能信息处理技术62对-滤波器，滤波和预测方程也可写成，为系统增益，分别称为位置增益、速度增益和加速度增益。NUST自动化学院自动化学院智能信息处理技术智能信息处理技术63 2.-滤波器的系数1.-滤波器的系数-和-滤波器的系数可以通过频域分析得到式中：0滤波器的固有频率；d滤波器的阻尼固有频率；阻尼系数；d滤波器的实根。满足满足保证滤波器的稳定工作保证滤波器的稳定工作NUST自

25、动化学院自动化学院智能信息处理技术智能信息处理技术64对-滤波器，其参数如下：参数均为对应模拟滤波器的参数，使用起来比较复杂，这里直接给出一组用临界阻尼法、最佳选择法给出的系数。对-滤波器，通常在给定值的情况下，计算值:一般取一般取=0.30.5。NUST自动化学院自动化学院智能信息处理技术智能信息处理技术65给定，得R，最后得和。式中R是系统特征方程三重正实根。这组系数也是在给定这组系数也是在给定的情况下，计算的情况下，计算和和NUST自动化学院自动化学院智能信息处理技术智能信息处理技术66 -滤波器-滤波器延 迟组 合)(kXZ(k)(),(211kdkX)(),(222kdkX对于匀速和

26、匀加速运动的目标，可以将-和-滤波器联合使用。在滤波参数不变的情况下，可以得到更高的跟踪精度。组合滤波器结构如图所示：NUST自动化学院自动化学院智能信息处理技术智能信息处理技术67图中输出 组合滤波器的滤波输出-滤波器的滤波输出-滤波器的滤波输出-滤波器的残差滤波器的残差-滤波器的残差滤波器的残差NUST自动化学院自动化学院智能信息处理技术智能信息处理技术68 构建自适应-滤波器：可采用变参选择方法来决定滤波参数基本思想是：目标稳态时，滤波器采用稳态滤波参数；目标机动后，为防止目标丢失，增加滤波器增益，并使滤波器进入暂态过程，这就是变参的基本思想。目标机动与否，可根据一步预测方差的大小来判断

27、。NUST自动化学院自动化学院自适应自适应-滤波器系数的获取滤波器系数的获取定义目标机动指数定义目标机动指数 是机动加速度方差，是机动加速度方差，是观测误差方差是观测误差方差 T 是雷达天线扫描周期是雷达天线扫描周期r的值可反映目标的机动程度的值可反映目标的机动程度定义残差定义残差 不仅反映了观测误差的大小，也反映了目标机动的不仅反映了观测误差的大小，也反映了目标机动的情况。设残差有情况。设残差有N个采样值，可知个采样值，可知为了准确反映目标机动性，必须用最近时间内残差值来为了准确反映目标机动性，必须用最近时间内残差值来求解，所以求解，所以N一般选取一般选取35。计算出。计算出 ，代替，代替

28、，代入，代入r的计算公式，再根据滤波增益公式算出的计算公式，再根据滤波增益公式算出 22()2wnTr2w2n44(k)1241rrrr(k)2(2(k)4 1(k)(k)Z(k)X(k/k 1)V(k)V2211(k)(i)NviVN2w(k)(k)2vNUST自动化学院自动化学院智能信息处理技术智能信息处理技术70滤波器乘法次数加法次数Kalman滤波器238159常增益 滤波器99自适应 滤波器1113-实验表明，自适应-滤波器在目标以匀速直线运动时的性能与常增益-滤波器相当，当目标机动时，常增益-滤波器会发散，而自适应-滤波器仍有良好的跟踪性能，它能跟踪转弯加速度小于3g的机动目标。计算量略有增加，但与卡尔曼滤波器相比,运算量少一个数量级。三种滤波器的计算量见表