第7章_卡尔曼滤波

1、1第第7章章 卡尔曼滤波卡尔曼滤波237.1 基于新息过程的递归最小均方误差估计基于新息过程的递归最小均方误差估计7.1.1 标量新息过程及其性质标量新息过程及其性质1n-4( )( )( )( )H1(7.1.4)ne nd nd nz n-=-=- w z ( )( )enna定义为线性预测器在最小均方误差意义下的预测误差。5 从新息过程定义可知， 就是 在最小均方意义下的一步预测误差，根据维纳滤波的正交原理（估计误差与输入信号向量正交）， 与输入信号向量 正交，因此， 包含了存在于当前观测样本中的新的信息，“新息”的含义即在于此。() () () ()()()H11|(7.1.5)nnn

2、z nd nz nz nz na-=-=-=- w zZ( )na( )z n( )na1n-z( )na( )z n6( ) ( )*E0,1,2,1n zkkna轾=-犏臌L( ) ( )*E0,1,2,1nkknaa轾=-犏臌L() ( )( )1 ,2 ,naaaL() ( )( )1 ,2 ,zzz nL序列和包含了相同的信息，即 () ( )( )() ( )( )1 ,2 ,1 ,2 ,zzz nnaaa LL 等价7()()11za=令( )( )()11221zl za=+上式两边乘()*1a，并取数学期望，根据新息过程的性质(2)有( )00z=()0 1|0z=Z由于，所

3、以，由 的表达式，有( )na8( ) ()( ) ()() ()*11E21E21E110zlzaaaa轾轾轾=+=犏犏犏臌臌臌( ) ()() ()( ) ()()*11*2E21E21E11E1zzlzzaaa轾轾犏犏臌臌= -= -轾轾犏臌犏臌()()( )()21223321zl zl za=+()*1a( )*2a() ( )() ()*E32E310aaaa轾轾=犏犏臌臌上式两边分别乘、，并取数学期望，根据，得方程组9( ) ()( ) ()( ) ()() ()( ) ( )( ) ( )( ) ( )() ( )*2122*2122E31E3121110E32E3222120

4、zl zl zzl zl zaaaaaaaaaa 轾轾=+= 犏犏臌臌 轾轾=+=犏犏臌臌 21l22l( )na可解得和的值。依此类推，可以将表示为( )( )()()()()() ()()1 11 211121 ,2nnnnnz nlz nlz nlzna-=+-+-+L()()( )( )()( )( )( )()( )( )( )( )()( )( )()()()()() ()()1121223132331 11 21111221332144321121nnnnzzl zzl zl zzl zl zl znz nlz nlz nlzaaaaa-=+=+=+=+-+-+ML10(7.1.

5、7)nnn= L za a其中() ()() ()() ()1122211112131000100101n nnnnnnnnllllll-轾犏犏犏犏=犏犏犏犏犏臌LLLLMMMOML由此可知，序列()( )( )12naaaL可由序列()( )( )12zzz nL线性表示，性质(3)得证。117.1.2 最小最小均方误差均方误差估计的新息过程表示估计的新息过程表示( )x n的最小均方误差估计可表示为 ()( )H|nnx nn= wzZ根据新息过程性质(3)，上式可表示为()( )( )H1H|(7.1.8)nnnnx nnn-=wLbaaaaZ( )( )HH1nnn-=bwLna a其

6、中，为输入时的权向量，可表示为( )()( )( )T112nnbbb n轾=臌bL12可以证明( )nbna a是输入为时，最小均方误差准则下的最佳权向量，即满足维纳-霍方程( ) ( )( )nnn=Abpa a( )HEnnn轾=犏臌Aa a a ana a( )( )Ennxn*轾=犏臌pa aa a其中矩阵为的自相关矩阵，为互相关向量。根据新息过程的正交性有() ( )()2EE()ijiijaaad*轾轾=-犏犏臌臌因此( )nA为对角矩阵，即( )()( )( )222diag E1,E2,Ennaaa轾轾轾=犏犏犏臌臌臌AL13容易解出权向量( )nb的各元素为()()()2,

7、1,2,Epib iiniaa=轾犏臌L()() ( )Epii xnaa*轾=犏臌( )npa ai其中，为互相关向量的第个元素。()( )() ()() ()( ) ( )()( ) ( )H1111|1|nnninnix nnbiibiibnnx nbnnaaaa*=-*-=+=-+ba aZZ14于是有迭代关系()()( ) ( )()1|1|7.1.11nnx nx nbnna*-=-+ZZ上式表明上式表明:以新息过程作为维纳滤波器的输入，若1n-()1x n-的估计值()11|nx n-Z已获得，则可按上式的迭代方法计算出n时刻期望响应( )x n的估计值()|nx nZ，这种方法

8、带来计算上的极大方便。157.1.3 向量新息过程及其性质向量新息过程及其性质输入信号向量( )( )( )( )T112NNnz nznzn轾=臌zL新息过程向量( )( )( )( )T112NNnnnnaaa轾=臌a aLn( )nz()1|nn-zZ在时刻，对输入向量的最佳线性预测向量可表示为()()()()THHH11211211|nNnnN nn-轾=犏臌zw zw zw zLZ16将标量新息过程的性质推广到向量新息过程，为:(1)n时刻的新息过程向量( )na a和过去所有观测向量() ( )()11 ,2 ,1nn-=-zzzLZ正交，即( ) ( )HE,1,2,1nkkna

9、 a轾=-犏臌z0L(2)n时刻的新息过程向量( )na a和过去所有新息过程向量( )ka a相互正交，即( )( )HE,1,2,1nkkna aa a轾=-犏臌0L17(3)观测数据向量序列() ( )( )1 ,2 ,nzzzL和新息过程向量序列() ( )( )1 ,2 ,na aa aa aL另一个序列，而不丢失任何信息。即之间存在着一一对应关系，可以借助可逆线性变换从其中一个序列得到() ( )( )() ( )( )1 ,2 ,1 ,2 ,nnzzza aa aa a LL 等价187.2 系统状态方程和观测方程的概念系统状态方程和观测方程的概念一个多输入多输出的离散时间LTI

10、系统，可用状态方程和输出方程进行描述:( )( )( )()()()()()()111121111211121222212222221212111111NSNSNNNNNNNSNSNx naaabbbx nfnaaabbbxnxnfnaaabbbxnfnxn轾轾轾轾轾-犏犏犏犏犏犏犏犏犏犏-犏犏犏犏犏=+犏犏犏犏犏犏犏犏犏犏犏犏犏犏-犏犏犏犏臌臌臌臌臌LLLLMMOMMMOMMMMLL个输入 SN个状态变量 状态方程状态方程19将其表示为向量的形式，为( )()()11nnn=-+-xAxBf( )1Nnx为状态变量 ( )1Snf为输入向量NNA为状态转移矩阵N SB为输入控制矩阵20系统输

11、出方程系统输出方程( )( )( )( )( )( )( )( )( )111121111211121222212222221212NSNSMMMNMMMSNSMz ncccdddx nfncccdcdznxnfnccadddxnfnzn轾轾轾轾轾犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏=+犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏臌臌臌臌臌LLLLMMOMMMOMMMMLLM个输出 表示为向量形式，为( )( )( )nnn=+zCxDf( )1Mnz为输出向量，MNC为状态输出矩阵，MSD为输出控制矩阵。21对于一个受到随机干扰的系统，系统状态方程和输出方程可分别表示为( )()()()1111nnn

12、n=-+-+-xAxBfv状态方程状态方程( )( )( )( )2nnnn=+yCxDfv系统输出方程系统输出方程其中，( )11Sn v( )12Mn v和分别为系统状态噪声和输出噪声，N S 为状态噪声输入矩阵。对于一般的时变系统对于一般的时变系统，系统状态方程和观测方程可表示为以下的形式：221 状态（过程）方程（状态（过程）方程（state equation）( )() ()() () ()1,11,117.2.7nn nnn nn=-+-xFxvG G状态向量：( )1Nnx；状态转移矩阵：(),1NNn n-F状态噪声输入矩阵：(),1N Sn n-G G系统状态噪声：()111

13、Sn-v其中，系统的状态转移矩阵(),1n n-F描述了系统状态从1n-时刻到n时刻的变化规律，它有这样的一些特点。23(1) 乘积律乘积律() ()()1,11,1nnn nnn+-=+-FFF并可类推为() ( )(),m nn lm l=FFF(2) 求逆律求逆律()()1,m nn m-=FF且有(), n n =FI24系统状态噪声( )1nv通常为随机过程向量，并假定为零均值白噪声，其相关矩阵满足( ) ( )( )( )1H111,E(),nknnknnkknd=轾=-=犏臌 QvvQ0上式表明，不同时刻间的状态噪声是统计独立的（即白噪声）；但并未强调同时刻不同状态噪声间的统计独

14、立性，若同时刻不同状态噪声间也是统计独立的，则矩阵( )1nQ是对角阵。25实际应用中，也可将噪声输入矩阵与状态噪声的乘积() ()1,11n nn-vG G看成一个向量，若仍用()11n-v表示，此时状态方程可表示为( )() ()()1,111nn nnn=-+-xFxv(7.2.12)262 观测（测量）方程（观测（测量）方程（measurement equation）( )( ) ( )( )()27.2.13nnnn=+zCxv观测向量：( )1Mnz观测矩阵:( )MNnC观测噪声：( )12Mnv与状态方程类似，观测噪声( )2nv通常也假定为零均值白噪声，其相关矩阵为( ) (

15、 )( )( )2H222,E(),nknnknnkknd=轾=-=犏臌 QvvQ027 同样，不同时刻间的观测噪声是统计独立的；若同时刻不同观测噪声间也是统计独立的，则矩阵( )2nQ是对角阵。 由于系统状态噪声和观测噪声是在系统的不同阶段引入的，它们之间是统计独立的，即有( )( )H12E,nkn k轾=犏臌vv0 上面给出的相关矩阵都是与时间有关的量，因此，可以用来描述非平稳的系统状态噪声和测量噪声。图7.2.1给出了系统状态方程和观测方程的结构。28 系统状态方程是我们对此变化规律的一种假设。通常，状态方程不可能和系统变化规律完全相符合。因此，已知状态向量初始值通过递推是无法有效提取

16、系统状态的29 利用观测量集合() ( )( )1 ,2 ,mzzzL对系统状态变量( )nx进行最优估计。根据mn和之间的关系，可将问题分为以下三类：(1)mn称为平滑问题。下面的讨论将集中在卡尔曼滤波卡尔曼滤波问题上。307.3 卡尔曼滤波原理卡尔曼滤波原理 7.3.1 状态向量的最小均方误差估计状态向量的最小均方误差估计为基于观测向量集合() ( )()1 ,2 ,1n-zzzL对i 时刻的状态向量()ix的最小均方误差估计，可表示为 ()( ) ( )()111|7.3.1nnikikk-=xa anZ其中，( )1Nka a 是新息过程向量。 在此系统状态已经由标量拓展到N维向量，所

17、以( )NNik为待求的最佳权矩阵。()1|ni-xZ31时刻的状态向量估计误差记为i()()()1,1|ni nii-=-xxe enZ可得互相关矩阵()()()()( )( )()11E,1EEniki nlilkkl-=轾轾轾-=-犏犏犏臌臌臌xe ea aa aa aa a 由正交原理可知，在MMSE意义下，状态向量估计误差和新息向量是正交的，因而有()()E,1i nl轾-=犏臌0e ea a32由新息过程的统计特性可知( )()( )( )EE()nlnnnld轾轾=-犏犏臌臌a aa aa aa a由此可得( )()( )( )( )()( )( )11EEEikikkkikk-

18、轾轾轾=犏犏犏臌臌臌xxAaaaaaaaa其中( )( )( )EMMkkk轾=犏臌Aa aa a为新息过程的自相关矩阵。(7.3.3)( )ik仅与k时刻的新息( )ka a有关。33另外，根据式(7.3.1)有：()( ) ( )() ()()() ()2112|11|11nniikniikknninn-=-=+-=+-xxa aa aa annZZ令1in=-，可得()()() ()()()()() ()121121|1|111|E1111nnnnnnnnnnnnn-=-+-轾=-+-犏臌xxBxxAa aa aa annnZZZ进一步将上式表示为()()() ()121|1|11nnn

19、nnn-=-+-xxKa annZZ(7.3.6)34其中()()()()11E111N Mnnnn-轾-=-犏臌KxAa a可以看出，基于观测向量集合() ( )()1 ,2 ,1n-zzzL对系统状态()1n-x的估计， 可在()21|nn-xnZ的基础上，利用新息向量()1n-a a进行修正。()1n-K其中，矩阵称为卡尔曼增益卡尔曼增益（Kalman gain）下面将讨论增益矩阵()1n-K和新息过程自相关矩阵()1n-A的计算方法。(7.3.7)357.3.2 新息过程的自相关矩阵新息过程的自相关矩阵设观测序列的新息过程向量为( )( )()1|nnnn-=-zza anZ预测值间也

20、应满足观测方程(7.3.8)()( )()()1121|nnnnnnn-=+zCxvnnnZZZ(7.3.9)由于( ) ( )H2E,1,2,1nkkn轾=-犏臌0Lvz所以()21|nn-=v0nZ于是有()( )()11|nnnnn-=zCxnnZZ(7.3.11)将上式代入式(7.3.8)可得36( )( ) ( )()( )12|nnnnnn-轾=-+臌Cxxva anZ定义状态预测误差向量(),1n n-e e()( )()1,1|nn nnn-=-xxe enZ则新息过程又可表示为( )( ) ()( )2,1nnn nn=-+Cva ae e由于(),1n n-e e与( )2

21、nv相互独立( )nA为( )( )() ()( )( )2E,1,1nnn nn nnn轾=-+犏臌ACCQe ee e(7.3.12)(7.3.13)(7.3.14)(7.3.15)37定义一步状态预测误差自相关矩阵(),1n n-P为()() (),1E,1,1N Nn nn nn n轾-=-犏臌Pe ee e则( )nA又可表示为( )( ) ()( )( )2,1nnn nnn=-+ACPCQ上式给出了新息过程自相关矩阵的表示式，但矩阵(),1n n-P待确定。(7.3.16)(7.3.17)387.3.3 卡尔曼滤波增益矩阵卡尔曼滤波增益矩阵上节给出的卡尔曼增益矩阵含有未知的系统状

22、态因而并不能直接使用，下面来导()1n-K的求解方法。()()() () ()()2E11E111,21nnnnnnn轾轾-=-+-犏臌臌xxCva ae e(7.3.18)()()()211,21|nnnnn-=-+-xxe enZ(7.3.19)由式 ，有：( )( ) ()( )2,1nnn nn=-+Cva ae e又由式 ，可得： ()( )()1,1|nn nnn-=-xxe enZ39因此有()()()()()()()HH22E11E1,21|1,211nnnnnnnnnn-轾-犏臌轾轾=-+-+-犏臌臌xxCva aeeeenZ于是() ()() ()()()()2E111,2

23、1E1|1,21nnnnnnnnnn-轾轾-=-+-犏犏臌臌xPCxCa ae enZ此外，由正交原理可知()()2E1|1,2nnnn0-轾-=犏臌xe enZ故有()()()()E111,21nnnnn轾-=-犏臌xPCa a(7.3.20)()()()()11E111N Mnnnn-轾-=-犏臌KxAa a将上式代入可得卡尔曼滤波增益为40()()()()111,211nnnnn-=-KPCA对于n时刻，可以得到( )()( )( )1,1nn nnn-=-KPCA(7.3.21)417.3.4 卡尔曼滤波的黎卡蒂方程卡尔曼滤波的黎卡蒂方程通过上节已知，为求得卡尔曼滤波增益，需得到状态预

24、测误差的自相关矩阵， 本小节将介绍利用递推的方法计算(),1n n-P定义状态估计误差向量()1n-e e()()()1111|nnnn-=-xxe enZ(7.3.22)定义状态估计误差的自相关矩阵()1n-P()() ()1E11NNnnn轾-=-犏臌Pe ee e(7.3.23)由于()111|nn-= 0vnZ可得状态预测方程()()()11|,11|nnnn nn-=-xFxnnZZ(7.3.24)因此，状态预测误差可表示为()( )()() ()() ()11,1|,11,11nn nnnn nnn nn-=-=-+-xxFve eeGeGnZ求上式的自相关矩阵可得()() ()(

25、)() ()()1,1,11,1,11,1n nn nnn nn nnn n-=-+-PFPFQGGGG(7.3.25)上式称为黎卡蒂差分方程黎卡蒂差分方程（Riccati difference equation）另外()()( ) ( )()( ) ( ) ()( )112|,1nnnnnnnnnnn nn-=+轾=+-+臌xxKxKCva ae ennnZZZ因此( )( )()()( ) ( ) ()( ) ( )2|,1,1nnnnn nnnn nnn=-=-xxKCKve ee ee enZ对上式求相关矩阵可得( )( ) ( ) ()( ) ( )( )( )( )2,1nnnn

26、nnnnnn轾轾=-臌臌+PIKCPIKCKQK(7.3.26)至此，得到了卡尔曼滤波递推算法的所有方程。44和()0 0|xnZ( )0P(1) 计算下一时刻状态的预测值及状态预测误差自相关矩阵()()()11|,11|nnnn nn-=-xFxnnZZ()() ()()() ()()1,1,11,1,11,1n nn nnn nn nnn n-=-+-PFPFQGGGG(2) 计算新息过程、新息自相关矩阵和卡尔曼增益( )( )( )()1|nnz nnn-=- Cxa anZ( )( ) ()( )( )2,1nnn nnn=-+ACPCQ( )()( )( )1,1nn nnn-=-K

27、PCA45(3) 计算下一时刻状态估计值()()( ) ( )1|nnnnnn-=+xxKa annZZ( )( ) ( ) ()( ) ( )( )( )( )2,1nnnn nnnnnn轾轾=-臌臌+PIKCPIKCKQK(4) (1)1nn=+46卡尔曼增益和状态估计误差自相关矩阵另外的表达形式( )( ) ( ) ()( ) ( ) ()( ) ( )( )( )( )2,1,1nnnn nnnn nnnnnn轾=-臌轾轾=-臌臌+PIKCPIKCPIKCKQK(7.3.28)( )( )( )( )()( )( )121,1nnnnn nnn-=-KPCQPCA(7.3.27)其中，

28、式(7.3.27)和式(7.3.21)是等价的；式(7.3.28)和式(7.3.26)是等价的477.3.5 卡尔曼滤波计算步骤卡尔曼滤波计算步骤算法算法7.1 （卡尔曼滤波算法）已知条件( )() ()() ()1,11,11nn nnn nn=-+-xFxvG G( )( ) ( )( )2nnnn=+zCxv( )( )( )H111Ennn轾=犏臌vvQ( )( )( )H222Ennn轾=犏臌vvQ48初始条件()( )0 0|E0轾=臌xxZ( )( )( )( )( )0E0E00E0轾轾轾轾=-犏犏臌臌臌臌Pxxxx步骤步骤1状态一步预测()()()111|,11|Nnnnn

29、nn-=-xFxnnZZ步骤步骤2新息过程( )( )()( )( )()111|Mnnnnnnnn-=-=-zzzCxa annZZ49步骤步骤3一步预测误差自相关矩阵()() ()()() ()()1,1,11,1,11,1NNn nn nnn nn nnn n=-=-+-PFPFQGGGG步骤步骤4新息过程自相关矩阵( )( ) ()( )( )2,1MMnnn nnn=-+ACPCQ步骤步骤5卡尔曼增益( )()( )( )1,1nn nnn-=-KPCA( )( )( )12N Mnnn-=PCQ50步骤步骤6()()( ) ( )11|Nnnnnnn-=+xxKa annZZ步骤步

30、骤7( )( ) ( ) (),1nnnn n轾=-臌PIKCP( ) ( ) ()( ) ( ),1nnn nnn轾轾=-臌臌IKCPIKC( )( )( )2NNnnn+KQK步骤步骤851527.4 卡尔曼滤波的统计性能卡尔曼滤波的统计性能 上一节从新息过程出发推导得出了卡尔曼滤波算法， 但并未讨论算法的估计结果是否满足最小均方误差准则。 本节中将验证，卡尔曼滤波算法满足最小均方误差准则。 并将看到，从新息过程出发正是基于最小均方误差准则， 两者只是解决问题的出发点不同，其结论是一致的。537.4.1 卡尔曼滤波的无偏性卡尔曼滤波的无偏性由( )() ()() ()1,11,11nn n

31、nn nn=-+-xFxvG G()()( ) ( )1|nnnnnn-=+xxKa annZZ可得( )( )()() ()() ()()( ) ( )11EE|E,11,11|nnnnnn nnn nnnnn-轾轾=-臌臌=-+-xxFxvxKe eG Ga annZZ54根据()1E10n轾-=臌v()()()11|,11|nnnn nn-=-xFxnnZZ可得( )()()( )( )E,1 E1Enn nnnn轾轾轾=-臌臌臌FKe ee ea a且( )( ) ()()E,1 E1nnn nn轾轾=-臌臌CFa ae e有( )()()( ) ( ) ()()( ) ( ) ()(

32、)E,1 E1,1 E1,1 E1nn nnnnn nnnnn nn轾轾轾=-臌臌臌轾轾=-臌臌FKCFIKCFeeeeeee e55由以上递推方程可知，为使估计误差的均值( )( )()0E0E00|0轾轾=-=臌臌xxe enZ因此，在卡尔曼滤波算法中状态估计的初值常设为()( )0 0|E0轾=臌xxnZ( )( )()EE|nnnn0轾轾=-=臌臌xxe enZ则则卡尔曼滤波算法所得的估计值就是无偏的。则卡尔曼滤波算法所得的估计值就是无偏的。7.4.2 卡尔曼滤波的最小均方误差估计特性卡尔曼滤波的最小均方误差估计特性 根据最小均方误差准则，在n时刻，算法获得的状态估计均方误差取得极小值

33、( )( )( ) ( )trtr EJ nnnn轾=犏臌Peeee( )()( ) ( ) ()( )( )( ) ( ) ()()( )( )( )( )( )2,1,1,1,1nn nnnn nnnnnn nn nnnnnn=-+-+PPKCPCKKCPPCKKQK(7.4.7) 为验证卡尔曼滤波是否为最小均方误差估计，将上式看成( )nK的函数，计算使均方误差( )trnP最小的( )nK是否与卡尔曼增益矩阵具有相同的形式。57由定义可得( )( )()(),1,1nnn nn n=-=-PPPP有( )()( ) ( ) ()( )( )( )( ) ( ) ()( ) ()( )(

34、)( ) ( )( )( ) ( ) ()( ) ()( )2,1,1,1,1,1,1,1nn nnnn nnnnnnn nnn nnn nnnnnnn nnn nn轾=-+-+犏臌轾轾-臌臌轾=-+-臌轾-臌PPKCPCQKKCPCPKPKAKKCPCPK(7.4.8)对新息过程自相关矩阵进行Cholesky分解，得( )( )( )nnn=ALL同时，选取矩阵( )nV满足( )( )( ) (),1nnnn n=-LVCP根据数学中常用的“配方法”思想，有( ) ( )( )( ) ( )( )( ) ( )( )( )( ) ( )( )( )( )( )( )( )Hnnnnnnnn

35、nnnnnnnnnn轾轾轾轾-=-犏犏臌臌臌臌轾-+犏臌KLVKLVKLLKKLVLVKVV即( ) ( )( )( ) ( )( )( ) ( )( )( ) ( ) ()( ) ()( )( )( ),1,1nnnnnnnnnnnn nnn nnnn轾轾-=臌臌轾轾-+臌臌KLVKLVKAKKCPCPKVV(7.4.9)(7.4.10)(7.4.11)59由矩阵理论知识可知( )( )( )( )( )( ) ( )( )( )( ) ( )( )( )( )( )( ) ( )( )( )( )( )( )( )( )( )( )11111nnnnnnnnnnnnnnnnnnnnnnnn

36、nn-轾轾=犏犏臌臌轾=犏臌轾=犏臌轾轾=犏犏臌臌VVVLLLLVVLLLLVVLLLLVLVLLLV将式(7.4.9)和式(7.4.10)代入上式，得( )( )()( )( ) ( ) ()1,1,1nnn nnnnn n-=-VVPCACP60进一步，将式(7.4.11)和上式分别代入(7.4.8) ，可得( )()( ) ( )( )( ) ( )( )( )( )()( ) ( )( )( ) ( )( )()( )( ) ( ) ()1,1,1,1,1nn nnnnnnnnnn nnnnnnnn nnnnn n-轾轾=-+-臌臌轾轾=-+-臌臌-PPKLVKLVVVPKLVKLVP

37、CACP利用矩阵迹的性质 trtrtr=ABAB，有( )()( ) ( )( )( ) ( )( )()( )( ) ( ) ()1trtr,1trtr,1,1nn nnnnnnnn nnnnn n-轾轾=-+-臌臌-PPKLVKLVPCACP与卡尔曼增益矩阵有关的只有第二项，选取( )nK使之为零矩阵，就可以使( )trnP最小。61因此有( ) ( )( )( )( )( )1nnnnnn-=KLVKVL0由于( )( )( )( )( )( )( )( )( )( )( )1111nnnnnnnnnnn-=轾=犏臌轾轾=犏犏臌臌KVLVLLLLVLL将式(7.4.9)和式(7.4.10

38、)代入，可得( )()( )( )1,1nn nnn-=-KPCA62 当增益满足上式时，代价函数取得最小值。对比式(7.3.21)发现，该式与通过新息过程推导出来的卡尔曼增益矩阵是一致的，因此，卡尔曼滤波算法是满足最小均方误差准则的。63卡尔曼滤波在雷达目标跟踪中的应用卡尔曼滤波在雷达目标跟踪中的应用 当卡尔曼滤波应用于目标跟踪时，用系统状态方程来描述目标的运动特性，其中的状态向量通常由目标的位置、速度和（或）加速度参量构成；观测方程中的观测向量则由雷达测得的目标运动参量构成。64例例 假设被跟踪目标在二维空间中运动，它从初始位置100m100m出发，在x和y方向分别以25m/s和20m/s

39、的速度进行匀速直线运动。观测噪声的统计特性由x和y方向的观测噪声标准差描述，分别为30mxs=20mys=。试用卡尔曼滤波算法实现对该目标的跟踪，给出目标预测和估计的位置和速度方差。其中的系统过程噪声假设为零均值的高斯白噪声，自相关矩阵取为22.5 I对于匀速直线运动目标，在没有任何扰动的情况下，满足( )()()11xx nx nT vn=-+-( )()1xxvnvn=-( )()()11yy ny nT vn=-+-( )()1yyvnvn=-若将两方向的干扰分别记为( )xnd和( )ynd 对于匀速直线运动目标，各方向的干扰可视为相应方向的加速度。因此解解：将目标n时刻在两方向的位置

40、分别记为( )x n和( )y n速度分别记为( )xvn和( )yvn66(7.6.4)( )()()11xxxvnvnTnd=-+-( )()()()21112yyTy ny nT vnnd=-+-+-(7.6.5)( )()()11yyyvnvnTnd=-+-(7.6.6)根据式(7.6.3) 式(7.6.6) ，可以获得如下系统状态方程( )() ()() ()1,11,11nn nnn nn=-+-xFxvG G(7.6.7)其中各参数分别为( )()()()21112xxTx nx nT vnnd=-+-+-(7.6.3)67状态向量：( )( )( )( )( )Txynx nv

41、ny nvn轾=犏臌x状态转移矩阵：()1000100,10010001Tn nT轾犏犏犏-=犏犏犏臌F系统过程噪声输入矩阵：()22200,1020TTn nTT轾犏犏犏-=犏犏犏犏臌G G68系统过程噪声：()()()T1111xynnndd轾-=-犏臌v观测方程为( )( ) ( )( )2nnnn=+zCxv其中各参数分别为观测向量：( )( )( )Txynznzn轾=犏臌z观测矩阵：( )10000010n轾犏=犏臌C69过程噪声和观测噪声的自相关矩阵分别为( )( )22122103002.5,01020nn轾轾犏犏=犏犏臌臌QQ 在给定系统状态方程和观测方程下，为进行卡尔曼滤波

42、，需给出状态估计和估计误差自相关矩阵的初始值，下面给出目标跟踪时，工程上常用的状态向量估计初始化方法。70 两坐标雷达中，状态向量由目标的位置和速度分量构成，即( )( )( )( )( )Txynx nvny nvn轾=犏臌x()( )( )()( )( )()T22121 2|22yyxxxyzzzzzzTT轾-犏=犏犏臌xnZ 可利用前两个时刻的观测值 和 来确定该向量估计的初始状态()1z( )2z71状态估计误差自相关矩阵按下式进行初始化( )( )( )( )( )( )( )( )( )( )( )( )( )( )( )( )( )22222222222222222222222

43、22xxxxxyxyxxxxxyxyxyxyyyyyxyxyyyyyqqTqqTqTqTqTqTqqTqqTqTqTqTqT轾犏犏犏=犏犏犏犏臌P其中，( )xxqn、( )xyqn和( )yyqn为观测噪声自相关矩阵的元素。( )( )( )( )( )2xxxyxyyyqnqnnqnqn轾犏=犏犏臌Q72在本例中，( )( )( )2230 ,0,20 xxxyyyqnqnqn= 若状态向量由目标的位置、速度和加速度分量构成，即( )( )( )( )( )( )( )Txxyynx nvnany nvnan轾=犏臌x其中，( )xan和( )yan分别表示n时刻目标在两个方向上的加速度，

44、该向量估计的初始状态需要利用前三个时刻的观测值来确定73()( )( )( )( )( )( )()( )( )( )( )( )( )()33323221 3|3323221xxxxxxxyyyyyyyzzzTzzzzTTTzzzTzzzzTTT轾犏犏-犏犏犏犏-犏-犏犏犏犏=犏犏犏-犏犏犏犏-犏-犏犏犏臌xnZ74估计误差自相关矩阵按下式进行初始化( )3xxxyxyyy轾犏=犏臌PPPPP其中( )( )( )( )( )( )( )( )( )( )( )( )( )()22323433333232233223421klklklklklklklklklklklklklklklqqqTT

45、qqqqqTTTqqqqqqTTT轾犏犏犏犏+犏=犏犏犏+犏犏犏臌P75同样，( )xxqn、( )xyqn和( )yyqn为观测噪声自相关矩阵( )2nQ的元素。在上述两种情况下，滤波的迭代过程分别从3n =和4n =时刻开始。 本例中状态向量由目标的位置和速度分量构成，因此，利用前两个时刻的观测值便可初始化状态估计，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 卡尔曼滤波在数据融合中的应用卡尔曼滤波在数据融合中的应用 数据融合(da

46、ta fusion)是针对使用多个或多类传感器的系统而开展的一种信息处理新方法。按照信息提取的层次，融合可以分成五级，包括检测级融合、位置级融合、属性级融合、态势评估与威胁估计。卡尔曼滤波算法是实现多传感器位置融合的主要技术手段之一，根据位置融合系统结构的不同，利用卡尔曼滤波理论对多传感器数据进行最优融合有两种途径：集中式滤波和分布式滤波。81图7.6.8 集中式融合系统82 集中式融合系统，融合单元可以利用所有传感器的观测信息进行状态估计。i()( )()( ) ( )()( )2,1,2,iiinnnniL=+=zCxvL同时，将各传感器的观测方程组合在一起，用一个方程表示，可以得到如下的

47、广义观测方程( )( ) ( )( )2nnnn=+zCxv集中式融合系统利用一个滤波器集中处理所有观测数据，无任何信息损失，因此，可以获得最优估计。但随着传感器数目的增多，运算量也会相应增大，当传感器数目达到一定时，将无法保证滤波器的实时性。83图7.6.9 分布式融合系统84 图7.6.9所示的分布式融合系统中，各个传感器首先要进行局部滤波（与融合单元的滤波相比，各分布传感器的滤波称为局部滤波），并将当前的状态估计结果送至融合单元，融合单元对各传感器提供的局部估计进行融合给出最终的全局估计。局部滤波实质为分布传感器的卡尔曼滤波过程，而融合单元输出的全局估计是局部估计的线性组合，可见，融合单元在此的作用与其在集中式融合系统中的不同，它不进行卡尔曼滤波而仅实现局部估计的优化组合。85首先，考虑仅有两个传感器的情况。假设n时刻的局部状态估计分别为()()()11|nnxZ和( )( )()22|nnxZ。相应