1、第七讲第七讲状态估计状态估计卡尔曼滤波卡尔曼滤波状态估计的主要内容状态估计的主要内容应用：应用： 通过数学方法寻求与观测数据最佳拟合的状态通过数学方法寻求与观测数据最佳拟合的状态向量。向量。 1 1、确定运动目标的当前位置与速度；、确定运动目标的当前位置与速度； 2 2、确定运动目标的未来位置与速度；、确定运动目标的未来位置与速度； 3 3、确定运动目标的固有特征或特征参数。、确定运动目标的固有特征或特征参数。2状态估计主要内容状态估计主要内容：位置与速度估计。：位置与速度估计。位置估计位置估计：距离、方位和高度或仰角的估计；：距离、方位和高度或仰角的估计；速度估计速度估计：速度、加速度估计。
2、：速度、加速度估计。 3状态估计的主要方法状态估计的主要方法 1 1、-滤波滤波 2 2、-滤波滤波 3 3、卡尔曼滤波、卡尔曼滤波 这些方法针对匀速或匀加速目标提出，如目标这些方法针对匀速或匀加速目标提出，如目标真实运动与真实运动与采用的目标模型采用的目标模型不一致，滤波器发散。不一致，滤波器发散。 4算法的改进及适应性算法的改进及适应性状态估计难点：状态估计难点：机动目标的跟踪机动目标的跟踪 1 1、自适应、自适应-滤波和自适应滤波和自适应KalmanKalman滤波均改善滤波均改善对机动目标的跟踪能力。对机动目标的跟踪能力。 2 2、扩展、扩展KalmanKalman滤波针对卡尔曼滤波在
3、笛卡儿坐滤波针对卡尔曼滤波在笛卡儿坐标系中才能使用的局限而提出。标系中才能使用的局限而提出。5卡尔曼滤波器卡尔曼滤波器 卡尔曼滤波器的应用卡尔曼滤波器的应用: : 通信、雷达、导航、自动控制等领域；通信、雷达、导航、自动控制等领域； 航天器的轨道计算、雷达目标跟踪、生产过程的自航天器的轨道计算、雷达目标跟踪、生产过程的自动控制等。动控制等。6卡尔曼滤波器的应用特点卡尔曼滤波器的应用特点 对机动目标跟踪中具有良好的性能；对机动目标跟踪中具有良好的性能； 为最佳估计并能够进行递推计算；为最佳估计并能够进行递推计算； 只需当前的一个测量值和前一个采样周期的预测只需当前的一个测量值和前一个采样周期的预
4、测值就能进行状态估计。值就能进行状态估计。7卡尔曼滤波器的局限性卡尔曼滤波器的局限性 卡尔曼滤波器解决运动目标或实体的状态估计问卡尔曼滤波器解决运动目标或实体的状态估计问题时，题时，动态方程和测量方程均为线性动态方程和测量方程均为线性。8一、数字滤波器作估值器一、数字滤波器作估值器 1 1、非递归估值器、非递归估值器 2 2、递归估值器、递归估值器 9 1 1、非递归估值器、非递归估值器 z1z1hmh3h2h1z1z1z2z3X10 采样平均估值器：采样平均估值器： 采用时域分析方法在掺杂有噪声的测量信号中采用时域分析方法在掺杂有噪声的测量信号中估计信号估计信号x x。11 根据数字信号处理
5、我们知道，所谓非递归数字滤波器是一根据数字信号处理我们知道，所谓非递归数字滤波器是一种只有前馈而没有反馈的滤波器，它的冲击脉冲响应是有限的，种只有前馈而没有反馈的滤波器，它的冲击脉冲响应是有限的， 在许多领域有着广泛的应用。在许多领域有着广泛的应用。 假定用假定用zk表示观测值，表示观测值， zk=x+nk 式中：式中： x 恒定信号或称被估参量恒定信号或称被估参量 nk 观测噪声采样观测噪声采样 假定，假定，E(x)=x0，D(x)=2x，E(nk)=0，E(n2k)=2n。12 h1, h2, , hm是滤波器的脉冲响应是滤波器的脉冲响应hj的采样，或称滤波器的的采样，或称滤波器的加权系数
6、。滤波器的输出加权系数。滤波器的输出 miiizhX1当当h1=h2=hm=1/m时，时， miizmX11该式表明，估计该式表明，估计 是用是用m个采样值的平均值作为被估参量个采样值的平均值作为被估参量x的近的近似值的，故称其为采样平均估值器。似值的，故称其为采样平均估值器。 X13 估计的均方误差以估计的均方误差以P表示，有表示，有 jijinmjmiijmnnmExXEEP221122211)()(当当i=j时时ij=1，当，当ij时时ij=0，有，有 mijmPn2最后得：最后得： 结论结论X14 估计值估计值 是用是用m m个采样值的平均值作为被个采样值的平均值作为被估参量估参量x
7、x的近似值；的近似值； 估值器的均方误差随着估值器的均方误差随着m m的增加而减少；的增加而减少； 该估值器是一个无偏估值器。该估值器是一个无偏估值器。01)()(1)(xxEnxmEXEmii2 2、递归估值器、递归估值器z1zka1 aykkX15一阶递归估值器：一阶递归估值器： a a为滤波器的加权系数，为滤波器的加权系数，a1a1。 16 递归数字滤波器是一种带有反馈的滤波器，它有无限的脉递归数字滤波器是一种带有反馈的滤波器，它有无限的脉冲响应，有阶数少的优点，冲响应，有阶数少的优点， 但其暂态过程较长。关于信号和噪但其暂态过程较长。关于信号和噪声的基本假设与非递归情况相同。上图给出的
8、一阶递归滤波器声的基本假设与非递归情况相同。上图给出的一阶递归滤波器输入输出信号关系如下：输入输出信号关系如下： kkkkknxzzayy1式中，式中，zk与非递归情况相同；与非递归情况相同；a是一个小于是一个小于1的滤波器加权系数，的滤波器加权系数， 如果它大于或等于如果它大于或等于1， 该滤波器就不稳定了。该滤波器就不稳定了。 17k时刻的输出：时刻的输出： yk=ak-1z1+ak-2z2+azk-1+zk 将将zk中的信号和噪声分开，并代入，有输出中的信号和噪声分开，并代入，有输出 kiiikkknaxaay111由于由于a1，故随着，故随着k值的增加，值的增加，yk趋近于趋近于x/(
9、1-a)。这样，如。这样，如果以果以(1-a)yk作为作为x的估计值，的估计值， kkyaX)1 (则则 ikiikkknaaxaX1)1 ()1 (18 此时信号此时信号x和估值之间只差一个噪声项。当和估值之间只差一个噪声项。当k值较大时，值较大时， 估值的均方误差估值的均方误差 2211)(nkaaxXEP而一次取样的均方误差而一次取样的均方误差 2221)()(nkknExnxEP故上一结果的均方误差约为一次采样的（故上一结果的均方误差约为一次采样的（1-a）/（1+a）倍。）倍。 二、线性均方估计二、线性均方估计1 1、最优非递归估计（标量维纳滤波）、最优非递归估计（标量维纳滤波）2
10、2、递归估计、递归估计1920 1. 最优非递归估计最优非递归估计 非递归滤波器的估计值及其估计误差可分别表示为非递归滤波器的估计值及其估计误差可分别表示为 )()(2121miiimiiixzhExXEPzhX21 对对m个参数逐一求导，令等于零，在均值为零的白噪声的情个参数逐一求导，令等于零，在均值为零的白噪声的情况下，可得到最小均方误差和估计：况下，可得到最小均方误差和估计： 221)(nbmxXEPmiizbmX11其中，其中，b=2n/2x，在，在bm时，这种估计近似于采样平均。在噪时，这种估计近似于采样平均。在噪声方差声方差2n较大时，其性能明显优于非最佳情况。这种最小均方较大时，
11、其性能明显优于非最佳情况。这种最小均方误差准则下的线性滤波，通常称作标量维纳滤波。误差准则下的线性滤波，通常称作标量维纳滤波。 hj与非最优情况的不同，这里的滤波器的加权系数为与非最优情况的不同，这里的滤波器的加权系数为bmhhhm12122 2、由最优非递推估计导出递归估计、由最优非递推估计导出递归估计由前可知，由前可知， 非递归估值器可以表示为非递归估值器可以表示为 kiiikiiikzkhzhX11)(条件与前面相同。对条件与前面相同。对k+1次取样，相应的估计量次取样，相应的估计量 11111) 1(kiiikiiikzkhzhX相应的估计误差相应的估计误差 21)(nbkkP2) 1
12、(1) 1(nbkkP23由由b=2n/2x及及hi(k)=1/（k+b），有），有 122) 1() 1()()(kniknibkPkhbkPkh所以有所以有 )/(11111bkbkbkbbkkkkkbbb1124于是于是, 1111kiikkzbX分成二项：分成二项： 111111kkkiikkzbzbX将第一项同时乘、除一个将第一项同时乘、除一个bk，则，则 111111111kkkkkkkkiikkkkzbXbbzbzbbbX25或或 )(111kkkkkXZbXX11111kkkkkkzbbXbX最后有最后有 最优递归估计器最优递归估计器11111kkkkkkzbbXbX26递推公
13、式递推公式最优递归估计器最优递归估计器)(111kkkkkXzbXX27递推公式递推公式28 递推开始时的初始条件递推开始时的初始条件 应满足应满足 ：0X0)(020XXxE以使以使 为最佳值。为最佳值。 解之，得解之，得，这时的，这时的 如果如果E(x)=0，可从零开始递推运算，即，可从零开始递推运算，即 0X)(0 xEX 2xPbbXnx102200三、标量卡尔曼滤波器时变信号三、标量卡尔曼滤波器时变信号 主要作用：主要作用： 对掺杂有噪声的随机信号进行线性估计。对掺杂有噪声的随机信号进行线性估计。2930 1、模型、模型 1） 信号模型信号模型 设要估计的随机信号为由均值为设要估计的
14、随机信号为由均值为0，方差为，方差为2w的白噪声激的白噪声激励的一个一阶递归过程，即信号对时间变化满足动态方程：励的一个一阶递归过程，即信号对时间变化满足动态方程： x(k)=ax(k-1)+w(k-1) 式中，式中，a系统参数；系统参数； w(k-1)白噪声采样。白噪声采样。如果令如果令x(0)=0，Ew(k)=0， 则则 jkjkjwkwEjPww20)()()(31该过程该过程 称作一阶自回归过程。称作一阶自回归过程。x(k)的均值和方差分别为：的均值和方差分别为： 22221)0()()(0)(aPkxEkxDkxEwxx自相关函数自相关函数 )0()()()(|xjxPajPjkxk
15、xE322） 观测模型观测模型观测模型由下式给出：观测模型由下式给出： z(k)=cx(k)+v(k) 式中：式中：c测量因子；测量因子； v(k)E()=0，D()=2n的白噪声。的白噪声。最优递推估值器的信号和观测模型如图所示。最优递推估值器的信号和观测模型如图所示。 33最优递推估值器的信号和观测模型最优递推估值器的信号和观测模型 cz(k)V(k)x(k)z1aW(k1)34 2、标量卡尔曼滤波器、标量卡尔曼滤波器由前将递归估计的形式写成：由前将递归估计的形式写成： )()() 1()()(kzkbkXkakX均方误差均方误差 )()()() 1()()()()(22kxkzkbkXk
16、aEkxkXEkP分别对分别对a(k)和和b(k)求导，并令其等于求导，并令其等于0，求其最佳估计，得出，求其最佳估计，得出a(k)与与b(k)的关系：的关系： a(k)=a1-cb(k) 最后有递归估值器：最后有递归估值器： )1()()() 1()(kXackzkbkXakX35b(k)为滤波器增益为滤波器增益 12121)()()(nkPckcPkb其中其中, 221) 1()(wkPakP均方误差均方误差 )(1)(2kbckPn 对于给定的信号模型和观测模型，上述一组方程便称为一对于给定的信号模型和观测模型，上述一组方程便称为一维标量卡尔曼滤波器，其结构如图所示。维标量卡尔曼滤波器，
17、其结构如图所示。 36标量卡尔曼滤波器结构标量卡尔曼滤波器结构 b(k)z1acz(k)(kX37 3、标量卡尔曼预测器、标量卡尔曼预测器 标量卡尔曼滤波是对掺杂有噪声的随机信号进行线性估计。标量卡尔曼滤波是对掺杂有噪声的随机信号进行线性估计。但经常要对信号的未来值进行预测，特别是在控制系统中。根但经常要对信号的未来值进行预测，特别是在控制系统中。根据预测提前时间的多少，把预测分成据预测提前时间的多少，把预测分成1步、步、2步、步、 m步预测，步预测， 通常把通常把1步预测记作步预测记作 。预测的步数越多，。预测的步数越多， 误差误差越大。越大。 这里讨论这里讨论1步预测问题。步预测问题。 信
18、号模型和观测模型同前：信号模型和观测模型同前： )()()() 1() 1()(kvkcxkzkwkaxkx)/1(kkX38根据前一节，根据前一节， 有一步线性预测递推公式：有一步线性预测递推公式： )()() 1/()()/1(kzkkkXkakkX其中，其中，a(k)和和(k)可以通过使均方预测误差最小来确定。预测的可以通过使均方预测误差最小来确定。预测的均方误差可表示为均方误差可表示为 22)/1()/1()/1()/1(kkXkkxEkkeEkkP将预测方程代入该式，并求导，就会得到一组正交方程：将预测方程代入该式，并求导，就会得到一组正交方程： 0)()/1(0)1/()/1(kz
19、kkeEkkXkkeE39解之，得解之，得 a(k)=a-c(k) 将其代入预测方程，有将其代入预测方程，有 )1/()()() 1/()/1(kkXckzkkkXakkX进一步可求出：进一步可求出： 22)()/1(wnkcakkP其中，其中， 22) 1/() 1/()(nkkPckkacPk 由以上表达式可以看出，可根据均方预测误差由以上表达式可以看出，可根据均方预测误差P(k/k-1)计计算算(k)，然后再给出，然后再给出P(k+1/k)的均方预测误差。的均方预测误差。 最优一步预测器最优一步预测器40最优一步预测及滤波器最优一步预测及滤波器4142 四、向量卡尔曼滤波器四、向量卡尔曼
20、滤波器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维向量分别表示成维向
21、量分别表示成 ,)()()()(21kxkxkxkXq)()()()(21kwkwkwkWq43显然，显然， X(k)=AX(k-1)+W(k-1) 式中，式中，X(k)，X(k-1)，W(k-1)都是都是q维向量，维向量，A是个是个qq阶矩阵，即阶矩阵，即 qaaaA00000021 如果信号不满足一阶递归差分方程，而满足二阶递归差分如果信号不满足一阶递归差分方程，而满足二阶递归差分方程，即方程，即 x(k)=ax(k-1)+bx(k-2)+w(k-1) 44定义两个分量定义两个分量 x1(k)=x(k)x2(k)=x1(k-1)=x(k-1)于是，有于是，有 0) 1() 1() 1(01
22、)()(2121kwkxkxbakxkx最后，有最后，有 X(k)=AX(k-1)+W(k-1) 结果把一个二阶差分方程变成了一个一阶二维向量方程，结果把一个二阶差分方程变成了一个一阶二维向量方程， 该该方程用起来更简单方便方程用起来更简单方便。 45 用用R(k)表示表示k时刻的距离，时刻的距离，R (k)表示表示k时刻的速度，时刻的速度，U(k)表表示示k时刻的加速度，时刻的加速度，T表示采样周期，则表示采样周期，则 )()() 1()()() 1(kTUkRkRkRTkRkR写成一般形式：写成一般形式： )()() 1()()() 1(122211kukxkxkTxkxkx其中，其中，
23、)()(1kTUku.46写成向量形式：写成向量形式： )(0)()(101) 1() 1(12121kukxkxTkxkx最后最后, 有有 )()() 1(kWkAXkX即可写成一阶向量的形式。即可写成一阶向量的形式。 在对信号向量进行估计的过程中，同时产生在对信号向量进行估计的过程中，同时产生r个含有噪声的个含有噪声的测量值，记作测量值，记作z1(k)，z2(k)，, zr(k)。则得到一组观测方程。则得到一组观测方程: )()()()()()()()()(22221111kvkxckzkvkxckzkvkxckzrrrr47其中，其中，vi(k)表示附加噪声，表示附加噪声，ci表示第表示
24、第i个测量参数，于是有个测量参数，于是有 Z(k)=CX(k)+V(k) 式中，式中，Z(k)，V(k)是是r维向量，维向量，X(k)是是q维向量，维向量，C是是rq阶矩阵。阶矩阵。 对于对于r=q，有，有 rcccC00000021C即是观测矩阵。即是观测矩阵。 48 2、向量问题的表示、向量问题的表示 根据前面的讨论，我们完全可以把前面的信号模型动态方根据前面的讨论，我们完全可以把前面的信号模型动态方程和观测方程写成如下形式程和观测方程写成如下形式: )()()()() 1()(kVkCXkYkWkAXkX 采用标量运算和矩阵运算的等价关系，推广到多维情况：采用标量运算和矩阵运算的等价关系
25、，推广到多维情况：1T2)()/(1BAbaABAbaABabBAba矩阵标量49据此，可以将观测噪声的方差变成协方差矩阵据此，可以将观测噪声的方差变成协方差矩阵 )()()(TkVkVEkR对两个信号的情况，则有对两个信号的情况，则有 2222221221212,21 ,22, 11 , 1)()()()()()()(vvvvkvEkvkvEkvkvEkvEkR同理，也可以把系统噪声的方差变成协方差矩阵，同理，也可以把系统噪声的方差变成协方差矩阵， 即即 )()()(TkWkWEkQ由于系统噪声采样互不相关，该协方差矩阵的非对角线元素的由于系统噪声采样互不相关，该协方差矩阵的非对角线元素的值
26、均为零。值均为零。 单一信号均方误差也可变成协方差矩阵，单一信号均方误差也可变成协方差矩阵， )()()(TkEkEEkP50 3、向量卡尔曼滤波器、向量卡尔曼滤波器 利用前面的概念，直接把标量卡尔曼滤波器公式变成向量利用前面的概念，直接把标量卡尔曼滤波器公式变成向量卡尔曼滤波器公式：卡尔曼滤波器公式： )1()()() 1()(kXCAkZkKkXAkX滤波器增益：滤波器增益： 1T1T1)()()()(kRCkCPCkPkK式中，式中， ) 1() 1()(11kQAkAPkPT实际上，实际上， 它是预测协方差。它是预测协方差。 误差协方差矩阵：误差协方差矩阵： )()()()(11kCP
27、kKkPkP51用用K(k)代替了代替了B(k)，因，因K(k)是通用符号，如图：是通用符号，如图：向量卡尔曼滤波器结构向量卡尔曼滤波器结构 z1Acz(k)(kXK(k)52增益矩阵增益矩阵K(k)的计算流程如图所示：的计算流程如图所示： 增益矩阵计算流程增益矩阵计算流程 计算P(k)计算K(k)迟延T计算P1(k)P (k)K(k)C(k)P(k1)C(k)R(k)A(k , k1)Q(k1)53 4、向量卡尔曼预测器、向量卡尔曼预测器根据相同的推导方法，根据相同的推导方法， 可以获得卡尔曼预测器方程组。可以获得卡尔曼预测器方程组。预测方程：预测方程： )1/()()() 1/()/1(k
28、kXCkZkGkkXAkkX预测增益预测增益: )()() 1/() 1/()(1TTkAKkRCkkCPCkkAPkG预测均方误差预测均方误差: )() 1/()()/1(TkQAkkPCkGAkkP 它们与标量的情况是一一对应的，只是用它们与标量的情况是一一对应的，只是用G(k)代替了代替了(k)。就可以将滤波和预测用同一个方框图表示出来。就可以将滤波和预测用同一个方框图表示出来。 54 5、总结、总结 卡尔曼滤波器应用广泛，卡尔曼滤波器应用广泛， 这里只对其进行简单归纳。这里只对其进行简单归纳。 1） 卡尔曼滤波器的主要特性卡尔曼滤波器的主要特性 卡尔曼滤波器是一个递归、卡尔曼滤波器是一
29、个递归、 线性、无偏和方差最小的滤波线性、无偏和方差最小的滤波器，器，如果过程噪声和观测噪声是正态高斯白噪声如果过程噪声和观测噪声是正态高斯白噪声，则它保持最，则它保持最佳特性。佳特性。 552） 卡尔曼滤波器模型卡尔曼滤波器模型目标运动模型：目标运动模型： )()()()()()()()(10000100010001)()()()(212121212121tWtWtWtWtXtXtXtXTTTtXTtXTtXTtX位置测量模型：位置测量模型： )()()()()()(00100001)()(21212121tVtVtXtXtXtXtZtZ56状态方程状态方程: X(t+T)=(t)X(t)+
30、W(t) Q(t)=EW(t)W(t)T 观测方程观测方程:Z(t)=HX(t)+V(t) R(t)=EV(t)V(t)T 573) 卡尔曼滤波器方程组卡尔曼滤波器方程组残差残差: )()()()(kXkHkZkE 预测方程预测方程: )()() 1(kXkkX状态估计状态估计: )1()()()()() 1()()()()()(kXkkHkZkKkXkkEkKkXkX卡尔曼滤波器增益卡尔曼滤波器增益: 1TT)()()()()()()(kRkHkPkHkHkPkK58 预测协方差预测协方差: )()()()() 1(TkQkkPkkP估计协方差估计协方差: )()()()(kPkHkKIkP
31、59 五、卡尔曼滤波器的应用五、卡尔曼滤波器的应用 1. 系统矩阵系统矩阵 假定系统矩阵是四维矩阵，即距离、速度、方位角及其变假定系统矩阵是四维矩阵，即距离、速度、方位角及其变化率化率, 它们分别由它们分别由R， ，和和 表示，距离方向上的加速度和表示，距离方向上的加速度和角度方向的加速度分别由角度方向的加速度分别由ur(k)和和u(k)表示。状态方程为表示。状态方程为 )()()()() 1()()() 1()()()()() 1()()() 1(21kukkTukkkTkkkukRkTukRkRkRTkRkRrR60则系统方程为则系统方程为 )(0)(010001000010001) 1(
32、) 1() 1() 1()()() 1(2143214321kukuxxxxTTkxkxkxkxkWkAXkX用标准符号用标准符号x1，x2 ，x3, x4分别表示分别表示 R，R, , 。式中，。式中， A为系为系统矩阵，统矩阵，W(k)为噪声项。为噪声项。 .61 2. 观测矩阵观测矩阵 假定观测值只有距离和方位两个，即假定观测值只有距离和方位两个，即R和和，分别用，分别用z1和和z2来表示。它们是由状态值和测量噪声组成的，且测量噪声是来表示。它们是由状态值和测量噪声组成的，且测量噪声是相互独立的零均值的白噪声。相互独立的零均值的白噪声。 测量方程：测量方程： )()()()()()(22
33、11kvkkzkvkrkz则有则有 )()()(kVkCXkZ62)()()()()()(01000001)()(21432121kvkvkxkxkxkxkzkz其中，其中， x1(k)=r(k)，x3(k)=(k)。 以上两个问题实际上是建立模型问题。以上两个问题实际上是建立模型问题。 63 3. 观测噪声协方差矩阵观测噪声协方差矩阵 在计算滤波器增益时，需知观测噪声的协方差矩阵。由于在计算滤波器增益时，需知观测噪声的协方差矩阵。由于只有两个参数，因此只有两个参数，因此 222212212100)()()()()()()(rkvEkvkvEkvkvEkvEkR这里利用了方位和距离观测噪声相互
34、独立的条件，故左下角和这里利用了方位和距离观测噪声相互独立的条件，故左下角和右上角项为零。右上角项为零。 64 4. 系统噪声协方差矩阵系统噪声协方差矩阵 假定目标作匀速运动，由于大气湍流等因素的影响，假定目标作匀速运动，由于大气湍流等因素的影响， 目目标产生随机加速度，在距离和方位上都存在随机扰动标产生随机加速度，在距离和方位上都存在随机扰动, 于是有于是有 WWWr00且且 0, 0, 0WWEWEWErr65因为因为 kjkjrrjWkWEjWkWE2221)()()()(得输入扰动的协方差矩阵得输入扰动的协方差矩阵 2221T00000000000000)()()(kWkWEkQ66
35、5. 滤波器的初值滤波器的初值 滤波器初始化时。先利用一种比较简单的方法确定滤波器初始化时。先利用一种比较简单的方法确定 ， 可利用时刻可利用时刻1和时刻和时刻2两点的距离和方位测量值，即两点的距离和方位测量值，即z1(1)，z1(2)，z2(1)， z2(2)， 建立建立 ，而忽略随机加速度。，而忽略随机加速度。 )1 ()2(1)2()2()2()2()2()1 ()2(1)2()2()2()2( )2()2(2242311211zzTXzXzzTrXzrXX)2(X) 2(X676. 均方误差矩阵均方误差矩阵由滤波器初值，有误差矢量由滤波器初值，有误差矢量 TvvuXxvXxTvvuXx
36、vXx) 1 ()2() 1 ()2()2()2()2()2() 1 ()2() 1 ()2()2()2()2()2(222442331112211168从而，从而， TvvuvTvvuvXX) 1 ()2() 1 ()2() 1 ()2() 1 ()2()2()2(2221111169初始误差的协方差矩阵初始误差的协方差矩阵 44434241343332312423222114131211T)2()2()(2()2()2(ppppppppppppppppXXXXEP70由于由于u，v相互独立，且各噪声采样之间也独立，相互独立，且各噪声采样之间也独立， 则则 444334332221121100000000)2(ppppppppP式中式中, Tppprr22112211712222442433423322212222TpTpppTpr这样，所需要的参数均已具备，可以进行迭代运算了。这样，所需要的参数均已具备，可以进行迭代运算了。 小结小结72
