【文章內(nèi)容簡介】 1()( ?? ?? kyacky 之差為零，則說明 k 時刻的新測量樣值中不包含任何新信息。因此，我們把 k 時刻的信號實測值 )(ky 與其預估值 )1()( ?? ?? kyacky 之差 )1()( ?? ? kyacky 稱為第k 次測量中的新信息。 顯然，當我們測得 k 時刻的新測量樣值 )(ky 之后，可利用第 k 次測量中的 新信息 )1()( ?? ? kyacky 乘上一個比例系數(shù) )(kb 作為修正項，對未測得 )(ky 前對信號給出的預估值 )1( ?? kxa 進行修正，從而得到 k 時刻對信號的估計值)(kx? ?？梢?，（ 316）式等號右側(cè)的第二項 ?????? ?? ? )1()()( kxackykb 即為對信號預估值的修正項。 標量卡爾曼濾波器的遞推算法 卡爾曼濾波的基本算法是預估加修正， 而公式（ 316）式、（ 315）式和（ 312）式就構(gòu)成了標量卡爾曼濾波器在信號及其測量過程的數(shù)學模型分別為 )1()1()( ???? kwkaxkx 和 )()()( kvkcxky ?? 時對信號進行最優(yōu)估計的一套完整的遞推算法。 （ 36）式可用來推算卡爾曼濾波器在不同取樣時刻 k 對信號 )(kx 的估計值 )(kx? ，即 ?????? ???????? )1()()()1()( kxackykbkxakx （ 315）式可用來推算卡爾曼濾波器在不同取樣 時刻 k 的時變?yōu)V波增益)(kb ，即 ? ? )1()1()(2222222??????kPacckPackbwvw?? ? （ 312）式可用來推算卡爾曼濾波器在不同取樣時刻 k 的均方估計誤差)(kP ，即 )(1)( 2 kbckP v?? 為了便于將標量卡爾曼濾波器的遞推算法直接推廣到向量隨機信號（即多維隨機信號）的卡爾曼濾波中去，給出如下的一套完整的遞推算法： 濾 波估計方程 ?????? ????? ??? )1()()()1()( kxackykbkxakx (317) 濾 波增益方程 ? 212 1 )( )()( vkPc kcPkb ?? (318) 式中： ? 221 )1()( wkPakP ??? (319) 均 方濾波誤差方程 )()()()( 11 kPkcbkPkP ?? (320) 標量卡爾曼預測器 假設(shè)待測標量隨機信號的數(shù)學模型為一階自遞歸過程，即 )1()1()( ???? kwkaxkx 信號測量過程的數(shù)學模型為 )()()( kvkcxky ?? 線性遞歸型預測估計器可用如下的遞推方程來表述： )()()1|()()|1( kykkkxkkkx ?? ???? ?? (321) 為使（ 321）式表述的線性預估器成為一個最優(yōu)線性預測器，就必須依據(jù)最小均方誤差準則對（ 321）式中的時變預測增益 )(k? 和 )(k? 進行最優(yōu)化。用上節(jié)類似的方法，可以求得經(jīng)最優(yōu)化后的 )(k? 和 )(k? 的表達式以及相應的均方預測誤差 )|1( kkP ? 的表達式，即 )()( kcak ?? ?? (322) ? ? ?? 22 1| )1|()( vkkPc kkac Pk ?? ?? (323) ?? ? 22 )()|1( wv kcakkP ??? (324) 其 中：根據(jù)（ 322）式，可由 )1|( ?kkP 推算出；再根據(jù)（ 323）式又可由 )(k?推算出 )|1( kkP ? 。 只要選定了初值，這一遞推過程就可以不斷的進行下去。 將（ 322）式代入（ 321）式，可得 ?????? ?????? ??? )1|()()()1|()|1( kkxckykkkxakkx ? (325) 根據(jù)（ 325）式，可以構(gòu)成一個標量卡爾曼預測器，其框圖如圖 32 所示。 圖 32 標量卡爾曼預測器框圖 圖 32 所示的標量卡爾曼預測器的一套完整的遞推算法有（ 322）式、（ 323）式、（ 324）式組成。但為了便于將這組公式直接推廣到向量隨機信號（即多維隨機信號）的卡爾曼遞推預測中去，當變量隨機信號及其測量過程的數(shù)學模型分別為 )1()1()( ???? kwkaxkx 和 )()()( kvkcxky ? 時，將（ 324）式作下改寫，即利用（ 323）式消去（ 324）式中的 ?2v 。 經(jīng)過改寫后得到卡爾曼預測器對此信號進行遞推預測的一套完整的遞推算法，公式如下： 預 測估計方程 ?????? ?????? ??? )1|()()()1|()|1( kkxckykkkxakkx ? (326) 預 測增益方程 ?? 22 )1|( )1|()( vkkPc kkac Pk ?? ?? (327) 均 方預測誤差方程 )(k? a c T )|1( kkx ?? )(ky + + + + + + ?? 22 )1|()()1|()|1( wkkPkackkPakkP ?????? (328) 向量卡爾曼濾波和預測 與標量情況類似，可用向量 )(kx? 表示 k 時刻對隨機信號向量 )(kx 的最優(yōu)線性濾波估計值，用向量 )|1( kkx ?? 表示在 k時刻對 )1( ?k 時刻的隨機信號向量)1(?kx 的最優(yōu)線性預測估計值。 對向量卡爾曼濾波來說，標量情況下的濾波誤差 )(ke 此時將變成一個濾波誤差向量 )()()( kxkxke ??????????????????)(...)()(21kekekeq （ 329） 標量情況下的均方濾波誤差，此時將變成一個濾波誤差的協(xié)方差矩陣 ? ?)()()( kekekP T? ???????????????)(. . .)()(. . .. . .. . .. . .)(. . .)()()(. . .)()(,2,1,22,21,2,12,11,1kPkPkPkPkPkPkPkPkPqqqqqq （ 330） 同樣 ， 對向量卡爾曼預測來說，其預測誤差向量為 )|1()1()|1( kkxkxkke ????? ? （ 331） 其預測協(xié)方差矩陣為 ? ?)|1()|1()|1( kkekkeEkkP T ???? （ 332） 所謂向量卡爾曼濾波器或預測器，實際上就是一種能對向量隨機信號進行最優(yōu)線性濾波或預測的遞歸型濾波器?！白顑?yōu)”的含義，是指能使每個信號分量的均方估計誤差同時為最小。 考慮到向量卡爾曼濾波器或預測與標量卡爾曼濾波器或預測無論條件上（即信號及其測量過程的數(shù)學模型）還是在要求上（即“最優(yōu)”的含義）都完全相似，因而，可以直接把標量卡爾曼濾波或預測的遞推公式推廣到向量情況。 在直接引用標量卡爾曼濾波或預測的遞推公式時，除了應把標量形式的信號、信號估計值和測量樣值表述成向量形式，把系統(tǒng)參數(shù) a 和量測參數(shù) c表述成 系統(tǒng)矩陣（或稱系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣） A 和量測矩陣 C，把均方估計誤差和有關(guān)噪聲和方差表述成相應的協(xié)方差矩陣外，還應根據(jù)表 31 將遞推公式中的標量運算變換成相應的矩陣運算 。 表 31 從 標量運算到矩陣運算的轉(zhuǎn)換表 標量運算 矩陣運算 ba? BA? ab AB ba2 TABA ba?1 1)( ??BA 注意：在使用表 31 時必須使變換后的沒一步矩陣運算都能進行下去，并使等號兩側(cè)最終得到的矩陣在階數(shù)上保持一致。 我們可以從標量卡爾曼濾波的遞推算法（ 317） ~（ 320）式直接得到向量卡爾曼濾波的遞推算法： 濾 波估計方程 ： ?????? ????? ??? )1()()()1()( kxCAkykKkxAkx (333） 濾 波增益方程 ： ? ? 111 )()()()( ??? kRCkCPCkPkK TT （ 334） 式中： )1()1()(1 ???? kQAkAPkP T （ 335） 濾 波協(xié)方差方程 ： )()()()( 11 kCPkKkPkP ?? （ 336） 和標量卡爾曼濾波器一樣，向量卡爾曼濾波器也是以預測加修正作為其遞推濾波的基本算法的?？柭鼮V波器的這一特性，使得很容易用計算機來實現(xiàn)對信號的實時濾波，為此，可采用軟件芳案來實現(xiàn)卡爾曼濾波。 在向量卡爾曼濾波的一整套遞推算法中，（ 333）式可構(gòu)成向量卡爾曼濾波的主程序算法， 圖 33 給出了主程序算法的框圖。 圖 33 向量卡爾曼濾波的主程序算法框圖 從圖 33 中可以看出，向量卡爾曼濾波的主程序算法主要分三步來進行。 第一步：在已知（ k1）時刻對信號向量 )1( ?kx 的估計值 )1(??kx 的條件下，用系統(tǒng)矩陣 A 乘以 )1(??kx ，得到在（ k1）時刻對 k 時刻信號向量 )(kx 的預測值 )1()1|( ??? ?? kxAkkx 。 第二步： 用量測矩陣 C 乘以 )1|( ?? kkx ， 得到在 （ k1）時刻對 k 時刻的測量數(shù)據(jù)向量 )(ky 的預測值 )1()1|( ??? ?? kxCAkky ； 再用 )(ky 的實測值減去預測值，得到K C A T )(ky )(kx )(kK )1|( ?? kky )1|( ?? kkx )1( ??kx )(39。 ke )(39。 kKe 第一步 第一步 第三步 第一步 第二步 第一步 + + + —— 殘差（新信息） )1()()1|()()(39。 ?????? ?? kxCAkykkykyke ； 最后用濾波增益矩陣 )(kK 乘以 )(39。ke ， 得到修正量 )()( 39。 kekK 。 第三步： 把對信號的預測值 )1|( ?? kkx 加上修正量 )()( 39。 kekK ， 得到信號的濾波估計值 )(kx? 。 值得注意的是：在上述運算過程中，所用的濾波增益矩 陣 )(kK 并 不是在主程序中計算出來的，而是從向量卡爾曼濾波的子程序算法中計算出來的。上述運算過程中所得到的 )(kx? 值 應存儲起來，以供下一次遞推時使用。只要確定了信號估計值的初值 )0(?x ， 例如設(shè) 0)0( ??x ， 則隨著時間的推移，可在測得 )1(y 后算出 )1(?x ， 在測得 )2(y 后算出 )2(?x ， 依次類推。在從（ k1）時刻到 k時刻這段時間內(nèi)，只需把 )1(??kx 存儲起來。隨著遞推 的不斷進行，再對它不斷 更新。當然，如果系統(tǒng)矩陣和量測矩陣是時變的，就需把 )1,( ?kkA 和 )(kC 也存儲起來 。 向量卡爾曼濾波的子程序算法是由（ 333） ~（ 336）式構(gòu)成的，其算法框圖由圖 34 所示。 從圖 34 中可以看出，向量卡爾曼濾波的子程序算法也分三步來進行。 第一步：在以知 )1( ?kP 、 )1( ?kQ 和 )1,( ?kkA 的條件下，利用（ 335）式求出 )(1kP 。 第二步：將 )(1kP 、 )(kC 和 )(kR 代入（ 334）式，求出 )(kK ，供主程序調(diào)用。 第三步：將 )(1kP 、 )(kK 和 )(kC 代入（ 336）式，求出 )(kP 并存儲起來，以供下一次遞推用。 圖 34 向量卡爾曼濾波的子程序算法框圖 由于濾波增益矩陣 )(kK 只與矩陣 )1,( ?kkA